plannerRRTStar

최적 RRT 경로 플래너(RRT*) 생성

R2019b 이후

설명

plannerRRTStar 객체는 점근적 최적 RRT 플래너인 RRT*를 생성합니다. RRT* 알고리즘은 상태공간 거리 측면에서 최적해로 수렴합니다. 또한 그 실행 시간은 RRT 알고리즘 실행 시간의 상수 배입니다. RRT*는 기하 계획 문제를 풀기 위해 사용됩니다. 기하 계획 문제에서는 상태공간에서 가져온 두 개의 무작위 상태가 연결될 수 있어야 합니다.

생성

구문

planner = plannerRRTStar(stateSpace,stateVal)

planner = plannerRRTStar(___,Name=Value)

설명

planner = plannerRRTStar(stateSpace,stateVal)은 state space 객체 stateSpace와 state validator 객체 stateVal에서 RRT* 플래너를 생성합니다. stateVal의 상태공간은 stateSpace와 동일해야 합니다. stateSpace와 stateVal은 planner 객체의 StateSpace 속성과 StateValidator 속성도 설정합니다.

planner = plannerRRTStar(___,Name=Value)는 위에 열거된 구문의 입력 인수 외에 하나 이상의 이름-값 인수를 사용하여 속성을 설정합니다. StateSampler, BallRadiusConstant, ContinueAfterGoalReached, MaxNumTreeNodes, MaxIterations, MaxConnectionDistance, GoalReachedFcn, GoalBias 속성을 이름-값 인수로 지정할 수 있습니다.

속성

StateSpace — 플래너를 위한 상태공간

state space 객체

플래너를 위한 상태공간으로, state space 객체로 지정됩니다. stateSpaceSE2, stateSpaceDubins, stateSpaceReedsShepp, stateSpaceSE3과 같은 state space 객체를 사용할 수 있습니다. 또한 nav.StateSpace 객체를 사용하여 state space 객체를 사용자 지정할 수 있습니다.

StateValidator — 플래너를 위한 상태 유효성 검사기

state validator 객체

플래너를 위한 상태 유효성 검사기로, state validator 객체로 지정됩니다. validatorOccupancyMap, validatorVehicleCostmap, validatorOccupancyMap3D와 같은 state validator 객체를 사용할 수 있습니다.

StateSampler — 입력 공간 샘플링을 위한 상태공간 샘플러

stateSamplerUniform 객체 (디폴트 값) | stateSamplerGaussian 객체 | stateSamplerMPNET 객체 | nav.StateSampler 객체

R2023b 이후

입력 공간에서 상태 샘플을 찾는 데 사용되는 상태공간 샘플러로, stateSamplerUniform, stateSamplerGaussian, stateSamplerMPNET 또는 nav.StateSampler 객체로 지정됩니다. 기본적으로 plannerRRTStar 객체는 상태에 균일 샘플링을 사용합니다.

BallRadiusConstant — 근접 이웃 탐색 반경을 추정하는 데 사용되는 상수

100 (디폴트 값) | 양의 스칼라

근접 이웃 탐색 반경을 추정하는 데 사용되는 상수로, 양의 스칼라로 지정됩니다. 반경은 다음과 같이 추정됩니다.

$$r=\min \left(\gamma {\left(\frac{\log (n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

각 요소는 다음과 같습니다.

• γ — BallRadiusConstant 속성의 값

• n — 트리에서의 현재 노드 개수

• d — 상태공간의 차원

• η — MaxConnectionDistance 속성의 값

γ는 다음과 같이 정의됩니다.

$$\gamma ={{\displaystyle 2}}^d\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

각 요소는 다음과 같습니다.

• V_Free — 탐색 공간 내 대략의 자유 부피(free volume)

• V_Ball — d 차원의 단위 볼의 부피

위의 식은 주어진 공간에 대해 "적절한" 크기의 BallRadiusConstant를 정의합니다. 이는 공간을 채우는 노드 개수가 늘어나고 반경이 줄어들수록 예상 이웃 개수가 로그적으로 늘어남을 의미합니다. 값이 클수록 매 반복당 d차원 볼 내의 평균 이웃 개수가 늘어나고 재연결할 후보가 많아집니다. 그러나 값이 제안된 최솟값보다 작으면 단일 근접 이웃이 구해질 수 있으며 점근적으로 최적의 결과를 도출하지 못합니다.

예: BallRadiusConstant=80

데이터형: single | double

ContinueAfterGoalReached — 목표에 도달한 후 최적화 계속

false (디폴트 값) | true

목표에 도달한 후 플래너가 최적화를 계속할지 여부로, false 또는 true로 지정됩니다. 또한 플래너는 최대 반복 횟수 또는 최대 트리 노드 개수에 도달하면 이 속성값과 관계없이 종료됩니다.

예: ContinueAfterGoalReached=true

데이터형: logical

MaxNumTreeNodes — 탐색 트리의 최대 노드 개수

1e4 (디폴트 값) | 양의 정수

탐색 트리의 최대 노드 개수(루트 노드 제외)로, 양의 정수로 지정됩니다.

예: MaxNumTreeNodes=2500

데이터형: single | double

MaxIterations — 최대 반복 횟수

1e4 (디폴트 값) | 양의 정수

최대 반복 횟수로, 양의 정수로 지정됩니다.

예: MaxIterations=2500

데이터형: single | double

MaxConnectionDistance — 모션의 최대 길이

0.1 (디폴트 값) | 양의 스칼라

트리에서 허용되는 모션의 최대 길이로, 스칼라로 지정됩니다.

예: MaxConnectionDistance=0.3

데이터형: single | double

GoalReachedFcn — 목표 도달 여부를 확인하는 콜백 함수

@nav.algs.checkIfGoalIsReached | 함수 핸들

목표 도달 여부를 확인하는 콜백 함수로, 함수 핸들로 지정됩니다. 자체 목표 도달 함수를 만들 수 있습니다. 함수는 다음 구문을 따라야 합니다.

 function isReached = myGoalReachedFcn(planner,currentState,goalState)

각 요소는 다음과 같습니다.

• planner — 생성된 planner 객체로, plannerRRTStar 객체로 지정됩니다.

• currentState — 현재 상태로, 요소를 3개 가진 실수형 벡터로 지정됩니다.

• goalState — 목표 상태로, 요소를 3개 가진 실수형 벡터로 지정됩니다.

• isReached — 현재 상태가 목표 상태에 도달했는지 여부를 나타내는 부울 변수로, true 또는 false로 반환됩니다.

코드 생성 워크플로에서 사용자 지정 GoalReachedFcn을 사용하려면 plan 함수를 호출하기 전에 이 속성을 사용자 지정 함수 핸들로 설정해야 하며 초기화 후에는 변경할 수 없습니다.

데이터형: function handle

GoalBias — 상태 샘플링 중 목표 상태를 선택할 확률

0.05 (디폴트 값) | 범위 [0,1] 내에 있는 실수형 스칼라

상태 샘플링 중 목표 상태를 선택할 확률로, 범위 [0,1] 내에 있는 실수형 스칼라로 지정됩니다. 이 속성은 상태공간에서 상태를 무작위로 선택하는 과정에서 실제 목표 상태를 선택할 확률을 정의합니다. 확률을 0.05와 같은 작은 값으로 설정하여 시작할 수 있습니다.

예: GoalBias=0.1

데이터형: single | double

객체 함수

plan	Plan path between two states
copy	Create copy of planner object

예제

두 상태 간의 최적 경로 계획하기

상태공간을 생성합니다.

ss = stateSpaceSE2;

생성된 상태공간을 사용하여 occupancyMap 기반 상태 유효성 검사기를 생성합니다.

sv = validatorOccupancyMap(ss);

예제 맵에서 점유 맵을 만들고 맵 해상도를 10셀/미터로 설정합니다.

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

유효성 검사기의 유효성 검사 거리를 설정합니다.

sv.ValidationDistance = 0.01;

상태공간 경계가 맵 제한과 동일하도록 업데이트합니다.

ss.StateBounds = [map.XWorldLimits; map.YWorldLimits; [-pi pi]];

RRT* 경로 플래너를 생성하고 목표에 도달한 후 추가 최적화를 허용합니다. 최대 반복 횟수를 줄이고 최대 연결 거리를 늘립니다.

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

출발 상태와 목표 상태를 설정합니다.

start = [0.5 0.5 0];
goal = [2.5 0.2 0];

디폴트 설정으로 경로를 계획합니다.

rng(100,'twister') % repeatable result
[pthObj,solnInfo] = plan(planner,start,goal);

결과를 시각화합니다.

map.show
hold on
% Tree expansion
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-')
% Draw path
plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

RRT 스타(*) 플래너를 사용하여 3차원 점유 맵을 통과하는 경로 계획하기

도시 블록의 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]);

생성된 상태공간을 사용하여 3차원 점유 맵 상태 유효성 검사기를 생성합니다. 점유 맵을 state validator 객체에 할당합니다. 샘플링 거리 간격을 지정합니다.

sv = validatorOccupancyMap3D(ss, ...
     Map = omap, ...
     ValidationDistance = 0.1);

최대 연결 거리를 늘리고 최대 반복 횟수를 줄인 RRT 스타 경로 플래너를 생성합니다. 목표까지의 유클리드 거리가 임계값 1미터 미만인 경우에 경로가 목표에 도달했다고 결정하는 사용자 지정 목표 함수를 지정합니다.

planner = plannerRRTStar(ss,sv, ...
          MaxConnectionDistance = 50, ...
          MaxIterations = 1000, ...
          GoalReachedFcn = @(~,s,g)(norm(s(1:3)-g(1:3))<1), ...
          GoalBias = 0.1);

출발 자세와 목표 자세를 지정합니다.

start = [40 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

반복 가능한 결과를 위해 난수 생성기를 구성합니다.

rng(1,"twister");

경로를 계획합니다.

[pthObj,solnInfo] = plan(planner,start,goal);

계획된 경로를 시각화합니다.

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

세부 정보

볼 반경 상수

RRT와 RRT*의 주요 차이점은 RRT* 알고리즘의 점근적 최적성을 보장하는 재연결 동작입니다. RRT 기반 플래너가 새로운 노드를 생성할 때 플래너는 트리에서 가장 가까운 노드를 찾습니다. 노드 간의 경로에 충돌이 없고 그밖에 측면에서 유효하다면 RRT 알고리즘은 두 노드를 연결하지만, RRT* 알고리즘은 노드를 연결한 후 트리를 최적화하는 추가 단계를 수행합니다. RRT*는 먼저 새로운 노드와 일정 거리 내에 있는 트리의 모든 노드를 찾은 다음, 시작 노드로 돌아가는 가장 짧은 유효한 경로를 새로운 노드에 제공하는 노드를 찾고, 이 노드와 새로운 노드 사이에 간선을 추가합니다. 마지막으로 플래너는 재연결 작업을 수행해서 새로운 노드가 가장 가까운 노드 각각에 대해 시작 노드까지 더 짧은 경로를 제공할 수 있는지 확인합니다. 더 짧은 경로가 있는 경우 해당 노드는 현재 부모 노드와 연결이 끊어지고 더 가까운 노드를 부모 노드로 갖는 재연결이 이루어집니다.

재연결이 일어나는 반경을 볼 반경 상수라고 합니다. RRT*의 목표는 점근적 최적성을 보장하는 동시에 추가적인 오버헤드 계산을 제한하는 것이기 때문에 적절한 볼 반경 상수를 선택하는 것이 중요합니다. 볼 반경 상수가 너무 크면 RRT*의 런타임이 증가합니다. 볼 반경 상수가 너무 작으면 알고리즘이 최적의 결과에 수렴하지 못할 수 있습니다.

plannerRRTStar 객체는 [1]에서 채택한 다음의 거리 공식을 사용하여 최근접이웃을 구합니다.

$$r=\min \left(\gamma {\left(\frac{\log (n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

각 요소는 다음과 같습니다.

• n — 트리의 노드 개수

• d — 상태공간의 차원 수

• η — 최대 연결 거리

• γ — 볼 반경 상수로, 다음과 같이 정의됩니다.

  $$\gamma ={{\displaystyle 2}}^d\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

  각 요소는 다음과 같습니다.

  • V_Free — 르베그 측도, 탐색 공간 내 대략의 자유 부피(free volume)

  • V_Ball — d 차원의 단위 볼의 부피

볼 반경 상수의 의미와 직관

r과 γ의 공식은 주어진 공간과 샘플링 밀도에 적절한 크기의 반경을 정의합니다. 공간을 채우는 노드 개수가 선형적으로 늘어날수록 반경은 줄어들고, 줄어든 볼 내부의 이웃 개수는 로그적으로 늘어납니다.

이 직관은 트리에서 새로 샘플링된 모든 점이 컨피규레이션 공간의 자유 영역에서 균일하고 독립적으로 샘플링되었다는 예측에 따른 것입니다. 이와 같은 방식으로 점을 샘플링하면 동질 푸아송 점 과정을 사용하여 생성되었다고 말할 수 있습니다. 이는 RRT*의 각 반복에서 n개의 점이 자유 공간에서 균일하게 샘플링되었으므로 단위 부피당 점의 평균 밀도 λ가 있어야 한다는 의미입니다. 임의 차원의 공간에서는 단위 측정당 점의 강도(intensity)가 존재합니다.

$$\lambda =\frac{N}{V_{Free}}$$

따라서 계획 공간의 어떠한 임의의 영역에서든 해당 영역의 부피에 밀도를 곱한 것이 이 영역에서 볼 수 있는 점의 개수입니다. RRT*에서는 반경이 r인 d차원 볼 내부의 점 개수에 초점이 있습니다.

$$n_{1,d}=V_{Ball}·\lambda =\frac{V_{Ball}}{V_{Free}}·n$$

$${\text{n}}_{1,d}={\text{n}}_{1,d}·r^d$$

여기서

• n_1,d — d차원의 단위 볼 내부에 있을 것으로 예상되는 점 개수

• n_r,d — 반경이 r인 d차원의 볼 내부에 있을 것으로 예상되는 점 개수

또한 n이 무한대에 가까워질수록 이웃 개수의 목표가 로그적으로 증가한다는 점을 상기하면, n_r,d=log(n)으로 설정하고 r에 대해 해를 구할 수 있습니다.

$$\text{r=}{\left(\frac{V_{Free}}{V_{Ball}}·\frac{\log (n)}{n}\right)}^{\frac{1}{d}}$$

공식 2에서 나머지 계수는 [1]의 수렴 증명에서 유도됩니다. 그러나 n을 제거하면 볼 반경 상수가 샘플 영역의 자유 공간 대비 단위 볼 측정의 비율에 차원별 상수를 곱한 것임을 알 수 있습니다.

참고 문헌

[1] Karaman, S. and E. Frazzoli. "Sampling-Based Algorithms for Optimal Motion Planning." The International Journal of Robotics Research. Vol. 30, Number 7, 2011, pp 846 – 894.

확장 기능

C/C++ 코드 생성

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

사용법 관련 참고 사항 및 제한 사항:

• 코드 생성 워크플로에서 사용자 지정 GoalReachedFcn을 사용하려면 plan 함수를 호출하기 전에 이 속성을 사용자 지정 함수 핸들로 설정해야 하며 초기화 후에는 변경할 수 없습니다.

버전 내역

R2019b에 개발됨

R2023b: 경로 계획의 샘플링 방식을 지정함

이제 균일 샘플링, 가우스 샘플링, MPNet 샘플링 또는 사용자 지정 샘플링 방식을 지정하여 경로 계획을 위한 샘플을 생성할 수 있습니다. 샘플링 방식을 지정하려면 이름, 값 인수 StateSampler를 사용합니다.

참고 항목

객체

• plannerRRT | plannerBiRRT | stateSpaceReedsShepp | stateSpaceDubins | stateSpaceSE2 | stateSpaceSE3 | validatorOccupancyMap | validatorVehicleCostmap | validatorOccupancyMap3D | stateSamplerUniform

함수

• plan | copy

도움말 항목

• Calculating an Appropriate BallRadiusConstant for PlannerRRStar