Hybrid A Star를 사용하여 경로 계획에 대한 코드 생성하기

이 예제에서는 Hybrid A* 알고리즘을 사용하여 맵을 통과하는 이동체를 위한 충돌 없는 경로를 계획하기 위해 코드를 생성하는 방법을 보여줍니다. MATLAB®에서 알고리즘을 확인한 후 codegen (MATLAB Coder) 함수를 사용하여 MEX 함수를 생성합니다. 알고리즘에서 생성된 MEX 파일을 사용하여 계획된 경로를 시각화합니다.

경로 계획을 위한 알고리즘 쓰기

plannerHybridAStar 객체를 사용해 맵의 출발 자세부터 목표 자세에 이르는 경로를 계획하는 함수 codegenPathPlanner를 생성합니다.

function path = codegenPathPlanner(mapData,startPose,goalPose)
    %#codegen
       
    % Create a binary occupancy map
    map = binaryOccupancyMap(mapData);

    % Create a state space object
    stateSpace = stateSpaceSE2;

    % Update state space bounds to be the same as map limits.
    stateSpace.StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]];

    % Construct a state validator object using the statespace and map object
    validator = validatorOccupancyMap(stateSpace,Map=map);
    
    % Set the validation distance for the validator
    validator.ValidationDistance = 0.01;
    
    % Assign the state validator object to the plannerHybridAStar object
    planner = plannerHybridAStar(validator);
    
    % Compute a path for the given start and goal poses
    pathObj = plan(planner,startPose,goalPose);
    
    % Extract the path poses from the path object
    path = pathObj.States;

end

이 함수는 표준 Hybrid A* 경로 계획 호출에 대한 래퍼 역할을 합니다. 함수는 표준 입력을 받고 충돌 없는 경로를 배열로 반환합니다. 핸들 객체를 코드 생성에 지원되는 함수의 입력 또는 출력으로 사용할 수 없으므로 함수 내부에 planner 객체를 생성합니다. 현재 폴더에 codegenPathPlanner 함수를 저장합니다.

MATLAB에서 경로 계획 알고리즘 확인하기

코드를 생성하기 전에 MATLAB에서 경로 계획 알고리즘을 확인합니다.

무작위 2차원 미로 맵을 생성합니다.

map = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapData = occupancyMatrix(map);

출발 자세와 목표 자세를 [x, y, theta] 벡터로 정의합니다. 여기서 x와 y는 위치를 지정하고(단위: 미터) theta는 방향 각도를 지정합니다(단위: 라디안).

startPose = [3 3 pi/2];
goalPose = [22 22 pi/2];

지정된 출발 자세, 목표 자세, 맵에 대한 경로를 계획합니다.

path = codegenPathPlanner(mapData,startPose,goalPose);

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

show(binaryOccupancyMap(mapData))
hold on
% Start state
scatter(startPose(1,1),startPose(1,2),"g","filled")
% Goal state
scatter(goalPose(1,1),goalPose(1,2),"r","filled")
% Path
plot(path(:,1),path(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","MATLAB Generated Path")
legend(Location="northwest")

경로 계획 알고리즘에 대한 코드 생성하기

codegen (MATLAB Coder) 함수 또는 MATLAB Coder (MATLAB Coder) 앱을 사용하여 코드를 생성할 수 있습니다. 이 예제에서는 MATLAB 명령줄에서 codegen을 호출하여 MEX 파일을 생성합니다. -args 옵션과 func_inputs 입력 인수를 사용하여 함수에 대한 입력마다 샘플 입력 인수를 지정합니다.

codegen 함수를 호출하고 셀형 배열에 입력 인수를 지정합니다. 이 함수는 사용할 별도의 codegenPathPlanner_mex 함수를 생성합니다. options 입력 인수를 사용하여 C 코드를 생성할 수도 있습니다. 이 단계는 다소 시간이 걸릴 수 있습니다.

codegen -config:mex codegenPathPlanner -args {mapData,startPose,goalPose}

Code generation successful.

생성된 MEX 함수를 사용하여 결과 확인하기

지정된 출발 자세, 목표 자세, 맵에 대한 경로 계획 알고리즘의 MEX 버전을 호출하여 경로를 계획합니다.

mexPath = codegenPathPlanner_mex(mapData,startPose,goalPose);

경로 계획 알고리즘의 MEX 버전에 의해 계산된 경로를 시각화합니다.

scatter(mexPath(:,1),mexPath(:,2),...
        Marker="o",...
        MarkerFaceColor="b",...
        MarkerEdgeColor="b")
legend("Start Pose","Goal Pose","MATLAB Generated Path","MEX Generated Path")
legend(Location="northwest")
hold off

생성된 코드의 성능 검사하기

timeit을 사용하여, 생성된 MEX 함수의 실행 시간을 원래 함수의 실행 시간과 비교합니다.

time = timeit(@() codegenPathPlanner(mapData,startPose,goalPose))

time = 0.3735

mexTime = timeit(@() codegenPathPlanner_mex(mapData,startPose,goalPose))

mexTime = 0.0474

time/mexTime

ans = 7.8872

이 예제에서 MEX 함수는 5배 이상 빠르게 실행됩니다. 시스템에 따라 결과가 다를 수도 있습니다.

생성된 MEX 함수를 사용하여 새 맵에서 경로 계획하기

새 맵에서 새로운 출발 자세와 새로운 목표 자세에 대한 경로를 계획합니다. 새 맵의 크기는 MEX 함수를 생성하는 데 사용된 맵과 동일해야 합니다.

무작위 2차원 미로 맵을 생성합니다.

mapNew = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapDataNew = occupancyMatrix(mapNew);

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

startPoseNew = [22 3 pi/2];
goalPoseNew = [3 22 pi/2];

지정된 출발 자세, 목표 자세, 맵에 대한 경로를 계획합니다.

pathNew = codegenPathPlanner_mex(mapDataNew,startPoseNew,goalPoseNew);

MEX 함수에 의해 계산된 새 경로를 시각화합니다.

show(binaryOccupancyMap(mapDataNew))
hold on
% Start state
scatter(startPoseNew(1,1),startPoseNew(1,2),"g","filled")
% Goal state
scatter(goalPoseNew(1,1),goalPoseNew(1,2),"r","filled")
% Path
plot(pathNew(:,1),pathNew(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","MEX Generated Path")
legend(Location='northeast')