경로 계획을 위한 예제 지도 불러오기

이 예제에서는 MATLAB 환경에서 제공하는 예제 지도 데이터를 로드합니다. 지도 파일에는 세 가지 유형의 지도 데이터가 포함되어 있습니다.

mapDataPath = fullfile(matlabroot, 'toolbox', 'robotics', 'robotexamples', 'data', 'exampleMaps.mat');
load(mapDataPath);

로드된 데이터에서 지도 관련 변수를 확인합니다.

whos *Map*

  Name            Size                Bytes  Class      Attributes

  complexMap      41x52                2132  logical              
  simpleMap       26x27                 702  logical              
  ternaryMap     501x501            2008008  double

simpleMap 데이터를 사용하여 2 cells/meter 해상도의 이진 점유 격자 지도를 생성합니다.

occupancyMap = robotics.BinaryOccupancyGrid(simpleMap, 2);
disp(occupancyMap);

  BinaryOccupancyGrid with properties:

                GridSize: [26 27]
             Resolution: 2
           XWorldLimits: [0 13.5000]
           YWorldLimits: [0 13]
    GridLocationInWorld: [0 0]

생성된 지도를 시각화합니다.

show(occupancyMap);

로봇 크기 고려를 위한 지도 확장

PRM 경로 계획기는 로봇의 물리적 크기를 고려하지 않습니다. 따라서 로봇의 충돌을 방지하기 위해 지도를 로봇 반경만큼 확장해야 합니다. 여기서는 반경 0.2미터의 원형 로봇을 가정합니다.

botRadius = 0.2;

원본 지도를 복사한 후 inflate 함수를 사용하여 로봇 크기를 반영합니다.

expandedMap = copy(occupancyMap);
inflate(expandedMap, botRadius);
show(expandedMap);

PRM 경로 계획기 생성 및 매개변수 설정

robotics.PRM 객체를 생성하고 기본 속성을 확인합니다.

pathPlanner = robotics.PRM;
disp(pathPlanner);

  PRM with properties:

                 Map: [0x1 robotics.BinaryOccupancyGrid]
           NumNodes: 50
    ConnectionDistance: Inf

확장된 지도를 PRM 객체에 할당합니다.

pathPlanner.Map = expandedMap;

PRM이 생성할 로드맵의 노드 수를 설정합니다. 노드 수가 많을수록 경로 탐색 성공 가능성이 높아지지만 계산 시간이 증가합니다.

pathPlanner.NumNodes =102;

노드 간 연결을 허용할 최대 거리를 정의합니다. 이 값이 클수록 노드 연결성이 향상되어 경로 발견이 쉬워집니다.

pathPlanner.ConnectionDistance = 10;

실행 가능한 경로 탐색

시작점과 목표점을 지도 상에 정의합니다.

startPos = [2.0 1.0];
goalPos = [12.0 10.0];

findpath 함수를 사용하여 두 지점 간의 경로를 계산합니다. PRM의 확률적 특성으로 인해 매 실행 시 경로가 다를 수 있습니다.

plannedRoute = findpath(pathPlanner, startPos, goalPos);
disp(plannedRoute);

    2.0000    1.0000
    1.9569    1.0546
    1.8369    2.3856
    3.2389    6.6106
    7.8260    8.1330
   11.4632   10.5857
   12.0000   10.0000

계획된 경로와 PRM 그래프를 시각화합니다.

show(pathPlanner);

복잡한 대규모 지도에서의 경로 계획

complexMap 데이터를 사용하여 해상도 1 cell/meter의 이진 점유 격자 지도를 생성합니다.

largeMap = robotics.BinaryOccupancyGrid(complexMap, 1);
disp(largeMap);
show(largeMap);

로봇 크기를 고려하여 지도를 확장합니다.

largeExpandedMap = copy(largeMap);
inflate(largeExpandedMap, botRadius);
show(largeExpandedMap);

기존 PRM 객체를 새로운 확장 지도로 업데이트하고 매개변수를 조정합니다.

pathPlanner.Map = largeExpandedMap;
pathPlanner.NumNodes = 40;
pathPlanner.ConnectionDistance = 20;
show(pathPlanner);

새로운 시작점과 목표점을 설정하고 경로를 탐색합니다. 복잡한 지도에서는 초기 노드 수로 경로를 찾지 못할 수 있습니다.

newStart = [3 3];
newGoal = [45 35];
plannedRoute = findpath(pathPlanner, newStart, newGoal);
disp(plannedRoute);

     []

경로가 발견될 때까지 노드 수를 점진적으로 증가시키는 루프를 구성합니다.

while isempty(plannedRoute)
    pathPlanner.NumNodes = pathPlanner.NumNodes + 15;
    update(pathPlanner);
    plannedRoute = findpath(pathPlanner, newStart, newGoal);
end
disp(plannedRoute);
show(pathPlanner);

확률적 점유 격자 지도와의 통합

ternaryMap 데이터를 사용하여 해상도 20 cells/meter의 확률적 점유 격자 지도를 생성합니다. 이 지도는 확률 값을 사용하며, 0은 자유 공간, 1은 장애물, 0.5는 미지의 공간을 나타냅니다.

probabilisticMap = robotics.OccupancyGrid(ternaryMap, 20);
disp(probabilisticMap);
show(probabilisticMap);

로봇 크기만큼 지도를 확장합니다.

probExpandedMap = copy(probabilisticMap);
inflate(probExpandedMap, botRadius);
show(probExpandedMap);

PRM 객체를 확률 지도로 업데이트하고 매개변수를 설정합니다. PRM은 FreeThreshold 값을 기준으로 자유 공간을 판단합니다.

pathPlanner.Map = probExpandedMap;
pathPlanner.NumNodes = 120;
pathPlanner.ConnectionDistance = 8;
show(pathPlanner);

최종 시작점과 목표점을 정의하고 경로를 탐색합니다. 필요한 경우 노드 수를 증가시킵니다.

finalStart = [7 22];
finalGoal = [15 5];
finalRoute = findpath(pathPlanner, finalStart, finalGoal);
while isempty(finalRoute)
    pathPlanner.NumNodes = pathPlanner.NumNodes + 20;
    update(pathPlanner);
    finalRoute = findpath(pathPlanner, finalStart, finalGoal);
end
disp(finalRoute);
show(pathPlanner);