Note: All comments and opinions expressed on "MATLAB for everyone" are mine alone and do not necessarily reflect those of my employers, past or present.

본 포스팅은 아래의 MathWorks 김종헌 프로님의 Path Planning 시리즈 중 아래의 비디오를 정리한 것입니다.

이 비디오는 모바일 패스 플레이닝 시리즈의 세번째 파트의 세번째 시간으로 Navigation toolbox에서 제공하는 global path planning algorithm 중에서도 연속 공간에서 sampling 기반으로 경로를 계획하는 path planner들에 대해 알아보도록 하자.

그림 1. Path Planning 알고리즘의 구분과 이번에 알아볼 Sampling-Based Path Planning 알고리즘

경로 계획 알고리즘의 분류에 대해 알고 싶다면 4. Route Planning 편을 참고하기 바란다.

PRM (Probabilistic RoadMap)

그림 2. holonomic한 움직임을 갖는 이동 로봇의 PRM 경로 계획 예시

첫번째 방법은 PRM(Probabilistic RoadMap)이다. 이 방법은 이동물체가 없는 정적인 공간에서 holonomic robot을 위한 경로를 계획할 때 주로 사용되는 가장 간단한 형태의 path planner이다.

이 방법은 control space에 균등하게 분포하도록 random한 sample state를 만들어 장애물을 표현한 map에 충돌하지 않는 edge들로 sample state들을 연결하여 시작점에서 제일 가까운 sample state, 종료점에서 가장 가까운 sample state 사이의 연결성을 찾아 Global path를 찾는 방법이다.

MATLAB의 PRM 사용 예제

map = load("exampleMaps.mat").simpleMap; 
map = occupancyMap(map,10);

ss = stateSpaceSE2; 
sv = validatorOccupancyMap(ss,Map=map); 
sv.ValidationDistance = 0.01;

planner = plannerPRM(ss,sv);

start = [0.5 0.5 0]; goal = [2.5 0.2 0]; 
[pthObj, solnInfo] = plan(planner,start,goal);

방법은 우선 맵을 로드해서 occupancy map으로 만든다. (Map은 plannerPRM의 문서에서 확인 가능) 그리고, 2차원 state space로 지정하고 이 맵에서 충돌 여부를 확인하기 위해 occupancy map을 validator로 정의한다. state space와 validator object를 기반으로 planner를 정의하고 시작점 종료점 정의하여 plan 함수를 수행하면 다음과 같은 결과를 얻을 수 있다.

그림 3. PRM 예시 MATLAB 코드의 결과물

RRT (Rapidly-exploring Random Tree)

두번째 방법은 RRT(Rapidly-exploring Random Tree)이다. 이름에서 알 수 있듯이 무작위로 샘플링한 상태를 search tree에 추가하면서 확장하여 목적지 까지 경로를 찾는 방법이다.

계획을 수행할 공간에서 랜덤하게 sampling한 state 방향으로 기존의 search tree에서 가장 가까운 점으로부터 edge를 연결한다. 이 때 그림 4에서 $\epsilon$ 길이 만큼 떨어진 곳에 새로운 상태 x를 만들어 기존의 tree에 추가한다. 여기서 상태 공간은 많은 로봇 상태의 집합이고, 충돌을 피해야 하는 물체의 위치도 상태공간 안에 표현한다. 이를 반복해서 목적지까지 도달하는 tree를 생성하면 계획을 종료한다.

그림 4. RRT의 알고리즘 설명

RRT는 상태공간 안에서 다양하게 장애물 혹은 constraint를 표현할 수 있고, 이를 회피하는 경로를 만들 수 있기 때문에 자유도가 높은 로봇이나 복잡한 공간에서 사용되는 로봇의 경로계획에 적합하다.

그림 5. 로봇을 이용한 경로 계획

MATLAB의 RRT 사용 예제

그럼 이해를 돕기 위해 예제를 통해 MATLAB에서 RRT를 사용하는 방법을 알아보자. Path planner의 구성은 이 시리즈 세미나의 다른 비디오에서 이미 여러 번 설명한 것과 같이 다음과 같은 방법으로 구성된다.

그림 6. MATLAB에서 RRT 사용 방법의 순서

우선, 경로계획에 필요한 맵을 워크스페이스에 로드한다. 그리고는 경로계획을 수행할 상태공간을 정의하고 경로 계획 중 생성되는 여러 부분 경로들이 장애물과 충돌하는지를 확인하는 충돌을 확인을 위한 validator를 정의한다. 그리고, 상태공간과 validator object에 정의된 정보들을 기반으로 planner object를 생성 후 경로를 생성한다. 마지막으로 생성된 경로를 map위에 그림으로 나타내면 다음과 같이 왼쪽 하단의 시작점에서 오른쪽 하단의 목적지로 생성된 붉은 색 경로가 나타나는 것이 보인다.

이처럼 RRT는 임의의 상태를 샘플링하여 경로를 생성하고 도착지까지 경로가 생성되면 더 이상의 검색을 수행하지 않으므로 울퉁불퉁하기도 하고, 목적지에서 먼쪽으로 돌아오기도 한다.

그림 7. RRT를 활용하여 경로계획한 결과물

load exampleMaps
map = occupancyMap(simpleMap,10);

ss = stateSpaceSE2;

sv = validatorOccupancyMap(ss); 
sv.Map = map;
sv.ValidationDistance = 0.01;

planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 0.3;

start = [0.5,0.5,0];
goal = [2.5,0.2,0];
rng(100,'twister'); % for repeatable result
[pthObj,solnInfo] = planner.plan(start,goal);

figure;
Show(map); hold on;
% tree expansion
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-’); 
% draw path 
plot(pthObj.States(:,1), pthObj.States(:,2),'r-','LineWidth’,2);

RRT*

하지만, RRT 알고리즘은 반드시 최단 경로를 제공하지는 않는다. 앞서 말한 것처럼 이 알고리즘의 종료 조건은 목적지에 도달하는 것이기에 경로를 찾으면 그것이 최적이던 최단이던 검색을 종료한다.

그림 8. RRT*의 알고리즘 설명

이 때문에 RRT의 개선된 버전인 RRT*가 더 많이 사용된다. RRT와 유사하지만 주어진 검색 반경에서 트리가 연결된 이후에도 트리를 다시 연결하고 더 짧은 경로를 찾으려고 시도한다.

그림 9. RRT*의 적용 예시

이로 인해 RRT*는 최적까지는 아니지만 RRT보다는 좀 더 개선된 솔루션을 제공할 수 있다.

MATLAB의 RRT* 사용 예제

RRT*를 사용하여 경로 검색을 수행하는 방법은 RRT를 사용하는 방법과 크게 다르지 않다. planner를 plannerRRT 에서 plannerRRTStar로 바꾸고 모든 준비작업을 동일하게 수행한다.

RRT*는 목적지까지의 경로를 찾은 후에도 더 좋은 경로를 계속해서 검색하기 때문에 얼마나 더 검색을 수행할지 option과 횟수를 지정한다. 그럼 다음 검색을 추가하여 수행한 후, 더 최적화된 결과를 보여준다.

그림 10. RRT*를 활용하여 경로계획한 결과물

확실히 그림 7과 그림 10을 비교하면 RRT*의 결과물에서는 지그재그한 경로가 더 정돈된 것이 보인다. 하지만, 역시나 그리 부드럽지는 않고 불필요하게 돌아가는 부분도 보이기도 한다. 이런 부분들은 검색으로 조정하는 것이 아니라 smoothing 기법을 이용하여 조정하게 된다. 이 smoothing 기법은 이후 내용에서 추가로 알아보도록 하자.

load exampleMaps
map = occupancyMap(simpleMap,10);

ss = stateSpaceSE2;

sv = validatorOccupancyMap(ss); 
sv.Map = map;
sv.ValidationDistance = 0.01;
ss.StateBounds = [map.XWorldLimits; map.YWorldLimits; [-pi pi]];

planner = plannerRRTStar(ss,sv);
planner.MaxConnectionDistance = 0.3;
planner.ContinueAfterGoalReached = true;
planner.MaxIterations = 2500;

start = [0.5,0.5,0];
goal = [2.5,0.2,0];
rng(100,'twister'); % for repeatable result
[pthObj,solnInfo] = planner.plan(start,goal);

figure;
Show(map); hold on;
% tree expansion
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-’); 
% draw path 
plot(pthObj.States(:,1), pthObj.States(:,2),'r-','LineWidth’,2);

PRM, RRT, RRT*의 결과물 비교

그림 11. Sampling-Based 경로 계획 알고리즘 중 PRM, RRT, RRT*의 결과물 비교

Bi-directional RRT

고차원이나 복잡한 형상의 공간에서도 경로를 빠르게 찾을 수 있다는 RRT의 장점 때문에 RRT의 여러 variation들이 개발되었다.

그 중에 하나가 바로 bi-directional RRT이다. Bi-directional RRT는 이름 그대로 시작점과 종료점 양쪽에서 동시에 search tree를 확장시켜 두 tree가 연결되는 경로를 찾는 RRT 알고리즘으로 복잡한 공간에서 더 빨리 경로를 찾기 위하여 개발되었다.

그림 12. Bidirectional RRT에서 장애물이 있는 경우(connect1 & extend1)와 장애물이 없는 경우(connect2 & extend2). 결국 장애물이 없는 경우에만 연결된다.

그림 12에서 설명하고 있는 것 처럼, 방법은 search tree를 확장하기 위해 RRT와 같은 방법으로 MaxConnectionDistance 거리만큼 가까운 노드에서 한 단계 확장한다. 각 트리의 확장 후, 플래너는 두 트리 간의 연결을 시도한다. 두 트리를 연결할 때는 새 확장 노드와 반대쪽 트리에서 가장 가까운 노드를 사용한다. 충돌이 검출되는 경우 그 연결은 트리에 추가되지 않고, 최종적으로는 그림 12에서 extend2와 connect2가 연결되는 것 처럼 마무리될 수 있다.

여기에 추가로 경로 단축 알고리즘을 적용하면 무작위 단축 전략을 실행하여 지정된 경로를 단축할 수 있다.

그림 13. Bidirectional RRT에 경로 단축 알고리즘을 추가 적용한 경우

일단, 그림 13의 0초에서와 같이 초기 계획된 경로가 존재한다고 했을 때, 두 개의 인접하지 않은 edge를 선택한다. 선택한 edge에서 중간 노드를 생성해서 연결했을 때 유효하지 않는다면 무시하고, 충돌없이 유효한 edge라면 이 edge를 트리에 추가하여 경로를 단축시킬 수 있다.

MATLAB의 Bi-directional RRT 사용 예제 (1)

아래 그림처럼 이전보다 훨씬 복잡한 미로와 같은 맵에 Bi-directional RRT을 적용해보자.

그림 14. Bidirectional RRT을 이용한 미로에서의 경로 계획

planner에서 달라진 것은 양쪽 tree에 대한 연결 한계 거리를 설정하는 것과 planner object의 이름이 plannerBiRRT로 수정된 것 외에는 없다. 그리고 실행을 해보면, 양쪽에서 tree가 확장되어 경로가 생성되는 것이 보인다.

load exampleMaps.mat
map = occupancyMap(simpleMap, 10);

ss = stateSpaceSE2;
sv = validatorOccupancyMap(ss);
sv.Map = map;
sv.ValidationDistance = 0.01;

planner = plannerBiRRT(ss,sv);
planner.MaxConnectionDistance = 0.3;

start = [20 10 0]; goal = [40 40 0]; 
[pthObj, solnInfo] = plan(planner,start,goal);

MATLAB의 Bi-directional RRT 사용 예제 (2)

그럼 이제 자동 주차라는 좀 더 실질적인 예제를 이용해 주차위치까지 어떻게 path planning을 할 수 있는지 보도록 하자. 그림 15에는 path planning을 위한 코드가 있다. 이를 분석해보자면,

처음에는 path planning을 수행하는 환경에 대한 설정을 진행한다. 장애물을 표현하기 위한 맵을 설정하고, 충돌을 검출하기 위한 로봇의 크기도 지정한다
이 정보들을 기반으로 path planning을 위한 상태공간과 충돌 검출을 위한 object를 생성한다.
그럼 이제는 이 정보들을 모두 사용해서 planner object를 생성하고, 시작상태와 목표상태를 지정하여 경로를 검색한다. 이 부분은 코드 상에 3, 4, 5번 플로우에 위치한다.
마지막에는 이 결과들을 시각화하여 결과를 확인한다.

그림 15. Bidirectional RRT을 이용한 자율 주차 예제 코드의 분석

아래는 텍스트로 쓴 MATLAB 코드이다.

%% Define vehicle cost map
mapImage = double(imread('mapImage.bmp')/255);     
costmap = vehicleCostmap(mapImage,"CellSize",0.068,"MapLocation",[5,3]); 
costmap.CollisionChecker.NumCircles = 5;
costmap.CollisionChecker = inflationCollisionChecker();
length = 4.7; width = 1.8; height = 1.4;
vehicleDims = vehicleDimensions(length,width,height,FrontOverhang=0.9,RearOverhang=1);
costmap.CollisionChecker.VehicleDimensions = vehicleDims;

%% Define state space
ss = stateSpaceReedsShepp;                                                                                                    
ss.StateBounds = [5,50;0 65;-pi,pi];
ss.MinTurningRadius = 4;
ss.ReverseCost = 1.2;

%% Define state validator
sv = validatorVehicleCostmap(ss,map=costmap);                                                          
sv.ValidationDistance = 0.1;

%% Path Planning Part
planner = plannerBiRRT(ss,sv,"MaxConnectionDistance",1,"MaxIterations",5000); % Create path planner
startP = [25 13 pi]; goalP = [25.6 42 pi*3/2]; % Set start/goal pose
[pathObj,solnInfo] = plan(planner, startP, goalP);  % Execute path planner
interpolate(pathObj,250);

%% Plot the results
figure; plot(costmap); hold on;
plot(solnInfo.StartTreeData(:,1),solnInfo.StartTreeData(:,2),'.-'); % Draw start tree expansion 
plot(solnInfo.GoalTreeData(:,1),solnInfo.GoalTreeData(:,2),'.-');  % Draw goal tree expansion 
plot(pathObj.States(:,1),pathObj.States(:,2),'r-','LineWidth',2); % Draw path

Configuring planning environments

Vehicle Cost Map을 만들 때, Bitmap image를 사용하여 표현한 map을 가져온다. Environment & Collision Check 편에서 이미 설명한 적이 있는 vehicleCostmap 형태로 그리드 cell size와 중심 좌표를 설정하여 occupancy grid를 설정한다.

그림 16. vehicleCostmap을 활용해 map의 충돌 가능성을 확인하는 예시

이 방법에 대해 간략하게 다시 설명하자면, 경로 계획을 위해 로봇의 크기를 고려할 때, 지정한 개수만큼의 원을 이용해 로봇의 크기를 정의함으로써 한번의 경로계획을 위해 수천/수만 번의 수행이 필요한 충돌검출에 소모하는 연산량을 최소화하는 방법이다. 이 코드에서는 5개의 원을 이용해 길이 4.7, 폭 1.8 미터의 로봇을 정의하고 충돌 검출을 하도록 지정하였다. 자세한 설명은 이전에 검색 기반의 path planning 방법 편에서 설명한 내용을 참고해도 좋을 것이다.

Define state space and state validator

그리고 나면 상태 공간을 설정해야 하는데, 이 예제에서 사용하는 로봇의 형태는 Ackermann constraint, 즉 조향각에 의한 움직임에 제한이 있는 차량형 로봇이고 전, 후진을 모두 고려하여야 하기 때문에 2차원 공간상에서 이러한 제한 조건을 반영할 수 있는 stateSpaceReedsShepp object를 이용해서 상태 공간을 정의한다.

그림 17. 조향각에 대한 움직임 제약: Ackermann constraint

state space의 “StateBounds” property 에는 x, y 위치, 방위에 대해 상태를 sampling하는 한계를 설정해서 효율성을 높였다. 또, state space의 “MinTurningRadius” property 에는 로봇의 제한조건을 정확하게 반영하기 위해 최소 회전 반경을 지정하였다. 마지막으로, 비용함수를 생성할 때 실제 상황을 최대한 반영하기 위해 후진보다는 전진을 장려할 수 있게 후진방향의 cost를 default 값인 1 보다 약간 높은 갚으로 지정하였다.

그리고, 이제 충돌 검출을 위한 state validator를 미리 지정해 놓은 vehicle cost map으로 지정할 수 있도록 validatorVehicleCostmap object로 설정하였다.

Path Planning Part

그럼 이제 플래너 object를 생성하고 시작 상태와 종료 상태를 입력하여 경로를 생성한다.

그림 18. 경로 계획의 시작 상태(초록색)와 종료 상태(주황색)

interpolate 함수는 생성된 경로의 각 segment는 길이가 일정하지 않으므로 지정된 숫자만큼 경로를 일정 길이로 나누는 작업을 해주는 것이다.

Plot the generated path

생성된 경로를 시각화하기 위해 계획된 경로 출력 중에 Bidrectional RRT를 사용했으니, 시작상태에서 확장된 트리와 목표 상태에서 확장된 트리를 그리고 최종 결과도 같이 그려주었다.

결과는 그림 19와 같다. 전진하여 장애물을 회피하면서 출발해서 목표지점 근처에서 후진 주차를 하기 위해 방향을 바꾸고 충돌을 피하기 위해 다시 전후진을 한번 반복하는걸 보니 사람이 주차하는 모습과 유사하게 느껴지기도 한다.

그림 19. 계획된 경로의 시각화 결과

Sampling Methods

이제는 지금까지 보아온 sampling 기반 경로 계획 알고리즘의 성능을 높일 수 있는 방법에 대해 말해보고자 한다. sampling 기반 방법들은 무작위로 sampling되는 상태를 연결하기 때문에 sampling되는 상태에 따라 계획되는 경로의 품질이 결정되고, 공간을 검색하는데 소요되는 시간도 결정된다.

이러한 이유로 sampling 기반 알고리즘의 성능을 좌우하는 sampling도 문제의 형태에 조정하여 사용할 수 있도록 Navigation toolbox에서는 몇 가지 옵션을 제공하고 있다.

기본적인 방법으로는 크게 uniform sampling과 Gaussian sampling이 있다. uniform sampling을 default sampling 방법으로 configuration space 전체에 대해 균일하게 상태가 sampling 되도록 하는 방법이다. 간단하고, 표현하기 간단하고, 무엇보다도 넓게 펼쳐진 공간에서 성능이 좋다. 하지만, 좁은 골목과 같이 장애물을 제외한 열린 공간이 작은 곳에서 골목을 통과하는 sample을 얻는 작업이 쉽지 않기 때문에 이런 환경에서는 경로 생성에 실패할 수도 있다.

그림 20. Uniform Sampling 방식으로 샘플링한 경우

Gaussian sampling은 Gaussian distribution을 이용해 장애물의 경계 근처에 집중적으로 sampling하는 방법으로 장애물의 밀도가 높은 복잡한 환경에서 굉장히 효과적이다. 물론, 그렇기에 넓게 펼쳐진 공간에서는 성능이 떨어진다. 그림 21에서도 볼 수 있듯이 공간을 가로지르는데 필요한 샘플이 없는 것을 알 수 있다.

그림 21. Gaussian Sampling 방식으로 샘플링한 경우

혹시, 제공되는 sampler을 사용자 정의하고 싶다면, nav.stateSampler class를 이용해 원하는 sampler를 직접 설계할 수도 있다. sampler를 수정하는 방법은 아래 그림 23의 코드에서와 같이 기존의 path planning하는 코드에서 planner object를 생성할 때 option으로 stateSampler를 지정하면 된다. sampler로 uniform sampling을 하거나 Gaussian sampling을 하거나 아니면 uniform 반, Gaussian 반을 sampling해서 공간을 좀 더 효율적으로 탐색하도록 custom sampler를 만들수 도 있다.

그림 22. Hybrid 방식으로 샘플링한 경우

그림 23. MATLAB 코드에서 Sampler의 선택

Deep-Learning Based Planner: Motion Planning Networks (MPNet)

또 다른 방법도 있다. Map을 학습해서 시작상태와 목표상태가 주어지면, 시작상태에서 목표상태로 가는 최적 경로 상에 sampling을 함으로써 검색 속도를 높이고, 생성된 경로의 품질을 높이는 방법이다.

그림 24. uniform sampling과 학습된 최적 경로 상의 sampling 수행 결과 비교

이 방법을 사용하면 오른쪽 그림 24에서와 같이 기존의 방법은 무작위로 sampling하는 반면에 이 방법은 최적 경로 상에서 sampling을 수행한다. 이 방법을 motion planning networks 줄여서 MPNet이라고 한다.

그림 25. Motion Planning Network의 구조

이 딥러닝 모델은 map을 학습에 맞게 인코딩하는 부분과 시작상태, 종료상태, 인코딩된 맵을 이용해 추론을 하는 feedforward network로 구성된다. 학습은 기존의 전통적인 planner가 수행한 경로 탐색 결과를 이용한 지도학습(supervised learning) 형태로 이루어진다. 이 방법을 구현하기 위해 Navigation Toolbox에서는 MPNet을 이용한 state space를 지정하는 mpnetSE2 object를 지원하고 있다. 또, 학습을 통해 새로운 뉴럴 네트워크 모델을 만들고 싶을 때 MPNet의 기본 구성을 template 형태로 마련해 놓은 mpnetLayers도 제공하고 있다.

학습 시에는 학습을 위한 데이터도 필요하니까 이 데이터들을 encoding 해주는 mpnetPrepareData 함수도 지원한다. 학습을 완료한 mpnet은 state sampler를 이용해서 sampling에만 사용할 수도 있고, plannerMPNet을 이용해서 planner object를 만들어 사용할 수도 있다.

그리고, 사용자가 MPNet을 좀 더 손쉽게 사용할 수 있도록 미리 트레이닝 해둔 모델도 제공한다. OfficeMapTrainedMPNET은 하나의 map에서 Dubin 카 모델을 사용해서 path planning 하도록 만들어 둔 모델이고, mazeMapTrainedMPNet은 무작위로 자동 생성되는 미로 맵에서 path planning을 하도록 만들어 둔 모델로 사용자가 따로 학습할 필요없이 쉽게 접근하여 사용이 가능하다.

MATLAB의 MPNet 사용 예제

그럼 이 MPNet을 사용해서 path planning을 수행해보자.

Path planning을 위한 기본적인 코드는 다른 path planning 코드와 동일하다. State space와 state validator를 정의하고 Planner object를 생성하여 시작상태와 목표상태를 입력해 경로를 생성한다.

달라진 것은 Navigation Toolbox에서 제공하는 mazeMapTrainedMPNet 모델을 로드해서 Planner 생성 시에, 입력 인자인 mpnetSE2 object를 생성하는 것만 추가 되었다는 점이다.

plannerMPNet으로 만들어지는 planner는 MPNet을 이용해 sampling을 수행하는 bi-directional planner이다. 결과를 보면 충돌이 없는 경로를 만들기 위해 최적 경로 주변에 최소한의 sample만 발생하는 것이 보인다. 단, 생성된 sample이 효과적으로 충돌이 없는 경로를 생성하지 못하는 위치(그림 26의 확대하여 본 부분)에서는 RRT로 전환되어 임의의 sample (그림 26의 연두색 점)을 생성함으로써 안정되게 경로를 생성하는 것이 보인다.

그림 26. MPNet을 이용해 수행한 Path Planning 결과

% Load a MPNet pre-trained using various 2-D maze maps 
% Widths/heights: 10 meters, Resolutions: 2.5 cells/m 
% Wall thickness: 1 grid cell
data = load("mazeMapTrainedMPNET.mat")

% Create Maze Map for Motion Planning
map = mapMaze(5,1,MapSize=[10 10],MapResolution=2.5);
% Create State Validator
stateSpace = stateSpaceSE2();
stateValidator = validatorOccupancyMap(stateSpace,Map=map);
stateValidator.ValidationDistance = 0.1;

% Compute Path Using MPNet Path Planner
mpnet = mpnetSE2(Network=data.trainedNetwork, ...
                 EncodingSize=data.encodingSize);
% Create MPNet path planner
planner = plannerMPNET(stateValidator,mpnet);
[pathObj,solutionInfo] = plan(planner,start,goal);

Navigation Toolbox 에서는 아래의 문서들처럼 state sampler 형태로 MPNet을 적용하는 예제나, 다양한 맵에 대해 학습을 통해 새로운 MPNet을 만드는 예제도 제공하고 있으니 참고해도 좋을 것이다.

PlannerControlRRT

Navigation Toolbox에서 제공하는 또다른 RRT의 variation은 plannerControlRRT이다.

RRT에서는 새로운 상태를 sampling할 때, 로봇의 constraints를 고려하지 않기 때문에 복잡한 kinematics나 dynamics를 가지는 상태의 로봇을 위한 경로를 생성하는 경우, 로봇이 생성한 경로를 추종하지 못할 수 있다.

kino-dynamic planner는 일반적인 geometrical planner들이 가진 자유도를 희생하는 대신 시스템의 kinematic model과 제어기를 반영하여 추종 가능한 경로를 생성한다. kino-dynamic planner에서는 planner 내부에서 로봇의 모델을 정의하고 sampling된 상태로 도달하기 위한 제어기를 사용해 새로운 상태의 유효성을 검사하게 되는데 이 작업을 nav.StatePropagator라는 class가 담당하게 된다.

MATLAB의 plannerControlRRT 사용 예제

load('exampleMaps','ternaryMap')
map = occupancyMap(ternaryMap,10);

% Bicycle model
propagator = mobileRobotPropagator(Environment=map); 

% Create the path planner from the state propagator.
planner = plannerControlRRT(propagator);

% Specify the start and 
% goal states.
start = [10 15 0];
goal  = [40 30 0];
path = plan(planner,start,goal);

코드를 한번 보면, 다른 것은 동일하고, mobileRobotPropagator라는 bicycle혹은 Ackermann 모델 둘 중 하나를 선택할 수 있는 바퀴형 이동 로봇을 위한 전용 propagator를 이용해 state space, state validator, propagator를 모두 정의하여 plannerControlRRT object에 입력하는 것이 보인다.

그림 27. plannerControlRRT 예제 결과물

결과를 보면, tree 자체는 직선으로 표현하였지만, 최종 경로 계획 결과는 bicycle 모델이 추종할 수 있는 곡률이 완만한 형태의 경로가 생성된 것이 보인다.

추가로, Navigation Toolbox에서는 mobilerobot propagator외에도 사용자가 자신의 로봇이 가진 복잡한 kinematics를 표현할 수 있도록 nav.StatePropagator라는 class를 지원하고 있다. 제품에 포함된 예제에서는 그림 28과 같은 tractor-trailer propagator를 만들어 경로를 계획하는 예제도 포함하고 있으니 참고하기 바란다.

그림 28. tractor-trailer propagator를 만들어 경로를 계획하는 예제

Technical Resources

정리하면 PRM과 RRT 그리고 몇가지 RRT의 variation 알고리즘과 RRT의 성능을 개선할 수 있는 몇 가지 기법에 대해 알아보았다. Path Planning 분야 외의 로봇 개발을 위한 다른 내용이 궁금하다면 아래의 매스웍스 코리아에서 제공하는 모바일 로봇틱스 그리고 자율주행 웹 포털을 통해 정보를 얻어갈 수 있으니 참고하기 바란다.

MATLAB Mobile Robotics Web Portal

그림 29. MATLAB을 이용한 육상 이동 로봇 개발 Web Portal (링크)

MATLAB ADAS Web Portal

그림 30. MATLAB을 이용한 자율주행/ADAS 개발 Web Portal (링크)

MATLAB Onramp Series

매트랩 기초 사용법을 학습하고 싶은 경우 MathWorks 홈페이지 내의 Onramp 라는 무료 트레이닝 코스를 활용할 수 있다. Onramp는 웹상으로 진행하는 온라인 무료 교육으로, 컴퓨터에 매트랩을 설치 할 필요 없이 온라인으로 매트랩 관련된 여러 기초 내용을 학습할 수 있다.

그림 31. MATLAB을 무료로 배울 수 있는 Onramp 시리즈 (링크)