[matlab] plannerRRT , plannerRRTStar

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

plannerRRTStar - Create an optimal RRT path planner (RRT*) - MATLAB

 

% SE2 상태공간 객체를 생성합니다.
ss = stateSpaceSE2;

% 생성한 상태 공간에 대해서, 점유 지도 기반의 상태 유효성 검사기를 생성한다.
sv = validatorOccupancyMap( ss );

% 예제 지도를 불러온다. 
load exampleMaps.mat

% simpleMap을 해상도 10의 occupancyMap 객체로 변환한다.
map = occupancyMap( simpleMap , 10 )

% 상태 유효성 검사기에 occupancyMap을 설정 
sv.Map = map;

% 유효성 검사시에 경로에서 충돌 여부를 검사하는 간격을 설정
sv.ValidationDistance = 0.01;

% 상태 공간의 바운드를 지도의 x,y 한계와 회전 한계로 설정
% [ 로봇의 x좌표 범위 , 로봇의 y좌표 범위 , 로봇의 회전 범위 ]
% 즉, 로봇이 취할 수 있는 상태이다.
ss.StateBounds = [map.XLocalLimits; map.YLocalLimits; [ -pi pi ]];

% maxConnectionDistance는 '한번에 얼마나 멀리'확장할지의 최대거리를 의미
% 0.3이라면, 한번에 최대로 0.3m만큼 뻗어나갈 수 있다.
planner = plannerRRT( ss , sv , MaxConnectionDistance=0.3 )


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

% Random Number Generator이다. 즉, 난수 생성기
% 시드값은 100이며, twister는 난수 생성기 알고리즘이다.
rng(100,'twister');

% plannerRRT객체를 사용해서, 실제 경로 검색을 수행한다.
% pathObj은 경로 객체이다. 주로 .State를 프로퍼티로 waypoints( 경유 좌표 )를 가진다.
% solution info는 탐색 과정을 요약한 구조체이다.
[pthObj,solnInfo] = plan(planner,start,goal);

show(map)

% 현재의 플롯을 고정해두고, 이후에 그리는 그래픽 요소들이 같은 축에 겹쳐지도록 설정
hold on

% 생성된 트리의 노드 좌표를 순서대로 꺾은선과 점으로 그린다.
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-')

plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

 

 

pthObj ( path object )

• .State
  • N x 3의 행렬이며, 각 행이 [ x , y , theta ]형태의 로봇 상태를 나타냄
• .NumStates
  • 경로 상에 포함된 waypoint의 갯수
• .Cost
  • 경로의 총 기이나 다른 비용 함수 값을 저장

 

solnInfo ( solution info )

• .PlanningTime
  
  • 플래너가 경로를 찾는데 걸린 총 시간 ( 초 단위 )
• .NumNodes
  • 검색중에 생성한 RRT트리의 노드의 총 갯수
• .IsPathFound
  • 목표 지점까지 경로를 찾았는지의 여부
• .TreeData
  • 전체 트리 구조를 담은 데이터이며, 시각화나 추가 분석에 사용한다.

 

 

 

 

 

---

ss = stateSpaceSE2;

sv = validatorOccupancyMap( ss );

load exampleMaps.mat

map = occupancyMap( simpleMap , 6 );

sv.Map = map;

sv.ValidationDistance = 0.01;

ss.StateBounds = [
    map.XLocalLimits; map.YLocalLimits; [ -pi pi ]
];

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)