interpolate

경로를 따라 점 보간

구문

interpolate(path,numStates)

설명

interpolate(path,numStates)는 여러 상태를 경로에 삽입하고 경로에 있는 모든 점이 균일하게 분포되도록 합니다. 이 함수는 경로에 있는 모든 기존 상태를 유지합니다. numStates의 값은 경로에 있는 기존 상태 개수보다 크거나 같아야 합니다.

예제

여러 웨이포인트를 기준으로 navPath 생성하기

Dubins 공간에서 여러 웨이포인트를 기준으로 navPath 객체를 생성합니다.

dubinsSpace = stateSpaceDubins([0 25; 0 25; -pi pi])

dubinsSpace = 
  stateSpaceDubins with properties:

   SE2 Properties
                 Name: 'SE2 Dubins'
          StateBounds: [3×2 double]
    NumStateVariables: 3

   Dubins Vehicle Properties
     MinTurningRadius: 1

pathobj = navPath(dubinsSpace)

pathobj = 
  navPath with properties:

      StateSpace: [1×1 stateSpaceDubins]
          States: [0×3 double]
       NumStates: 0
    MaxNumStates: Inf

waypoints = [8 10 pi/2;
             7 14 pi/4;
             10 17 pi/2;
             10 10 -pi];
append(pathobj,waypoints)

정확히 250개의 점을 포함하도록 경로를 보간합니다.

interpolate(pathobj,250)

보간된 경로와 원래 웨이포인트를 시각화합니다.

figure
grid on
axis equal
hold on
plot(pathobj.States(:,1),pathobj.States(:,2),".b")
plot(waypoints(:,1),waypoints(:,2),"*r","MarkerSize",10)

경로의 길이를 계산합니다.

len = pathLength(pathobj);
disp("Path length = " + num2str(len))

Path length = 19.4722

SE(3) 상태공간에서 여러 웨이포인트를 기준으로 navPath 생성하기

도시 블록의 3D 점유 맵을 작업 공간으로 불러옵니다. 장애물이 없는 셀로 간주할 임계값을 지정합니다.

mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;

장애물 주변에서 안전하게 작동할 수 있도록 점유 맵을 확장하여 완충 지대를 추가합니다.

inflate(omap,1)

상태 변수의 범위가 지정된 SE(3) state space 객체를 생성합니다.

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);

SE(3) 상태공간에서 여러 웨이포인트를 기준으로 navPath 객체를 생성합니다.

path = navPath(ss);
waypoints = [40 180 15 0.7 0.2 0 0.1;
             55 120 20 0.6 0.2 0 0.1;
             100 100 25 0.5 0.2 0 0.1;
             130 90 30 0.4 0 0.1 0.6;
             150 33 35 0.3 0 0.1 0.6];
append(path,waypoints)

정확히 250개의 점을 포함하도록 경로를 보간합니다.

interpolate(path,250)

보간된 경로와 원래 웨이포인트를 시각화합니다.

show(omap)
axis equal
view([-10 55])
hold on
% Start state
scatter3(waypoints(1,1),waypoints(1,2),waypoints(1,3),"g","filled")
% Goal state
scatter3(waypoints(end,1),waypoints(end,2),waypoints(end,3),"r","filled")
% Intermediate waypoints
scatter3(waypoints(2:end-1,1),waypoints(2:end-1,2), ...
         waypoints(2:end-1,3),"y","filled")
% Path
plot3(path.States(:,1),path.States(:,2),path.States(:,3), ...
      "r-",LineWidth=2)

경로의 길이를 계산합니다.

len = pathLength(path);
disp("Path length = " + num2str(len))

Path length = 204.1797

입력 인수

path — path 객체

navpath 객체

path 객체로, navPath 객체로 지정됩니다.

numStates — 상태 개수

음이 아닌 정수

경로에 삽입된 상태 개수로, 음이 아닌 정수로 지정됩니다. 이 값은 경로에 있는 기존 상태 개수보다 크거나 같아야 합니다.

데이터형: double

확장 기능

C/C++ 코드 생성
MATLAB® Coder™를 사용하여 C 코드나 C++ 코드를 생성할 수 있습니다.

버전 내역

R2019b에 개발됨

참고 항목

객체

• navPath

함수

• append | copy | pathLength