RRT를 사용한 이동 로봇 경로 계획

이 예제는 RRT(Rapidly-exploring Random Tree) 알고리즘을 사용하여 알려진 맵에서 움직이는 이동체의 경로를 계획하는 방법을 보여줍니다. 특수한 이동체 제약 조건은 사용자 지정 상태공간에도 적용됩니다. 모든 내비게이션 응용 분야에서 사용자 지정 state space 객체와 path validation 객체를 이용해 사용자 자신의 플래너를 조정할 수 있습니다.

점유 맵 불러오기

소규모 사무실 공간을 나타내는 점유 맵을 불러옵니다. 맵에서 로봇의 출발 자세와 목표 자세를 정의합니다.

load("office_area_gridmap.mat","occGrid")
show(occGrid)

% Set start and goal poses.
start = [-1.0,0.0,-pi];
goal = [14,-2.25,0];

% Show start and goal positions of robot.
hold on
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')

% Show start and goal heading angle using a line.
r = 0.5;
plot([start(1),start(1) + r*cos(start(3))],[start(2),start(2) + r*sin(start(3))],'r-')
plot([goal(1),goal(1) + r*cos(goal(3))],[goal(2),goal(2) + r*sin(goal(3))],'m-')
hold off

로봇은 기본 원형 모델로 가정해 보겠습니다. 로봇이 장애물에 너무 가까이 가지 않도록 맵의 장애물을 약간 확장합니다.

% Make a copy of the original map and infate it by 0.1 meters. Use this inflated map for path planning. 
% Use the original map for visualization purpose. 
inflatedMap = copy(occGrid);
inflate(inflatedMap,0.1); 

상태공간 정의하기

stateSpaceDubins 객체를 사용하고 상태 범위를 설정하여 이동체의 상태공간을 지정합니다. 이 객체는 상태 범위 내에서 이동체를 조향할 수 있도록 샘플링된 상태를 실현 가능한 Dubins 곡선으로 제한합니다. 이러한 소규모 환경에서의 급선회를 감안하여 0.4미터의 회전 반경을 지정합니다.

bounds = [inflatedMap.XWorldLimits; inflatedMap.YWorldLimits; [-pi pi]];

ss = stateSpaceDubins(bounds);
ss.MinTurningRadius = 0.4;

경로 계획하기

경로를 계획하기 위해 RRT 알고리즘은 상태공간 내의 임의의 상태를 샘플링하여 경로에 연결하려고 시도합니다. 이러한 상태와 연결은 맵 제약 조건을 기준으로 제외하거나 유효성을 검사해야 합니다. 이동체는 맵에 정의된 장애물과 충돌하지 않아야 합니다.

지정된 상태공간으로 validatorOccupancyMap 객체를 생성합니다. Map 속성을 불러온 occupancyMap 객체로 설정합니다. ValdiationDistance를 0.05미터로 설정합니다. 이 유효성 검사 거리는 경로 연결을 이산화하고 이를 기반으로 맵에서 장애물을 확인합니다.

stateValidator = validatorOccupancyMap(ss); 
stateValidator.Map = inflatedMap;
stateValidator.ValidationDistance = 0.05;

경로 플래너를 생성하고 최대 연결 거리를 늘려 더 많은 상태를 연결합니다. 샘플링한 상태에 대한 최대 반복 횟수를 설정합니다.

planner = plannerRRT(ss,stateValidator);
planner.MaxConnectionDistance = 2.5;
planner.MaxIterations = 30000;

GoalReached 함수를 사용자 지정합니다. 이 예시 헬퍼 함수는 설정된 임계값 내에서 실현 가능한 경로가 목표 지점에 도달하는지 여부를 확인합니다. 목표 지점에 도달하면 이 함수는 true를 반환하고 플래너는 중지됩니다.

planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

function isReached = exampleHelperCheckIfGoal(planner, goalState, newState)
    isReached = false;
    threshold = 0.1;
    if planner.StateSpace.distance(newState, goalState) < threshold
        isReached = true;
    end
end

재현 가능한 결과를 위해 난수 생성기를 재설정합니다. 출발 자세부터 목표 자세에 이르는 경로를 계획합니다.

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

경로 단축하기

shortenpath 함수를 사용하여 경로에서 중복되는 노드를 제거합니다. 이 함수는 원치 않는 노드를 제거하고 충돌이 발생하지 않는 비순차적 노드를 연결하여 충돌 없는 경로를 생성합니다.

shortenedPath = shortenpath(pthObj,stateValidator);

원래 경로와 단축 경로의 경로 길이를 계산합니다.

originalLength = pathLength(pthObj)

originalLength = 
33.8183

shortenedLength = pathLength(shortenedPath)

shortenedLength = 
29.0853

단축으로 인해 경로 길이가 줄어들었음을 확인할 수 있습니다.

원래 경로와 단축 경로 플로팅하기

점유 맵을 표시합니다. solnInfo에서 탐색 트리를 플로팅하십시오. 최종 경로를 보간하고 겹쳐 놓습니다.

show(occGrid)
hold on

% Plot entire search tree.
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),plannerLineSpec.tree{:})

% Interpolate and plot path.
interpolate(pthObj,300)
plot(pthObj.States(:,1),pthObj.States(:,2),plannerLineSpec.path{:})

% Interpolate and plot path.
interpolate(shortenedPath,300);
plot(shortenedPath.States(:,1),shortenedPath.States(:,2),'g-','LineWidth',3)

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
legend('search tree','original path','shortened path')
hold off

Dubins 이동체 제약 조건 사용자 지정하기

사용자 지정 이동체 제약 조건을 지정하려면 state space 객체를 사용자 지정합니다. 이 예제에서는 stateSpaceDubins 클래스를 기반으로 하는 ExampleHelperStateSpaceOneSidedDubins를 사용합니다. 이 헬퍼 클래스는 부울 속성 GoLeft를 기준으로 오른쪽 또는 왼쪽으로 회전 방향을 제한합니다. 이 속성은 기본적으로 dubinsConnection 객체의 경로 유형을 비활성화합니다.

예시 헬퍼를 사용하여 state space 객체를 생성합니다. 동일한 상태 범위를 지정하고 새로운 부울 파라미터를 true로 지정합니다(좌회전만 가능).

% Only making left turns
goLeft = true;

% Create the state space
ssCustom = ExampleHelperStateSpaceOneSidedDubins(bounds,goLeft);
ssCustom.MinTurningRadius = 0.4;

경로 계획

이러한 제약 조건을 기반으로 하는 사용자 지정 Dubins 제약 조건과 유효성 검사기로 새로운 planner 객체를 생성합니다. 동일한 GoalReached 함수를 지정합니다.

stateValidator2 = validatorOccupancyMap(ssCustom); 
stateValidator2.Map = inflatedMap;
stateValidator2.ValidationDistance = 0.05;

planner = plannerRRT(ssCustom,stateValidator2);
planner.MaxConnectionDistance = 2.5;
planner.MaxIterations = 30000;
planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

출발 지점과 목표 지점 간의 경로를 계획합니다. 난수 생성기를 재설정합니다.

rng default
[pthObj2,solnInfo] = plan(planner,start,goal);

경로 단축하기

사용자 지정 모션 제약 조건을 유지함으로써 경로를 단축합니다.

shortenedPath2 = shortenpath(pthObj2,stateValidator2);

원래 경로와 단축 경로의 경로 길이를 계산합니다.

originalLength2 = pathLength(pthObj2)

originalLength2 = 
46.7841

shortenedLength2 = pathLength(shortenedPath2)

shortenedLength2 = 
45.7649

단축으로 인해 경로 길이가 줄어들었음을 확인할 수 있습니다.

경로 플로팅하기

이 경로에서는 목표 지점에 도달하기 위해 좌회전만 실행합니다.

figure
show(occGrid)
hold on
% Interpolate and plot path.
interpolate(pthObj2,300)
h1 = plot(pthObj2.States(:,1),pthObj2.States(:,2),plannerLineSpec.path{:});

% Interpolate and plot path.
interpolate(shortenedPath2,300)
h2 = plot(shortenedPath2.States(:,1),shortenedPath2.States(:,2),'g-','LineWidth',3);

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
legend([h1 h2],'original path','shortened path')
hold off