Note: All comments and opinions expressed on "MATLAB for everyone" are mine alone and do not necessarily reflect those of my employers, past or present.

본 포스팅은 아래의 MathWorks 김종헌 프로님의 Path Planning 시리즈 중 아래의 비디오를 정리한 것입니다.

이 비디오에서는 모바일 패스 플레이닝 시리즈 미디어의 세 번째 파트 중 두 번째 시간으로 Navigation toolbox에서 제공하는 global path planning algorithm 중에서도 Ackermann 조향 제한조건을 가진 로봇을 대상으로 연속 공간에서 검색 기반으로 경로를 계획하는 Hybrid A*에 대해 알아보도록 하자.

그림 1. Path Planning 알고리즘의 구분과 이번에 알아볼 Hybrid A* 알고리즘

경로 계획 알고리즘의 분류에 대해 알고 싶다면 4. Route Planning 편을 참고하기 바란다.

Hybrid A* Path Planner

자율주행 시스템의 경로 계획 중 주차장에서 목표하는 주차공간까지 경로를 계획하는데 가장 많이 사용되는 유명한 알고리즘이 바로 Hybrid A*이다.

이름에서 알 수 있듯이 기본적인 뿌리는 A*이다. 하지만, A*의 경우 grid로 공간을 이산화하기 때문에 전륜 조향 mechanism을 갖는 차량과 같은 로봇은 Path planning을 한다해도 계획한 그 node에 도달할 수 없고, 좁은 주차 공간에서 공간을 효율적으로 사용하려면 grid를 너무 잘게 나누어야 하기 때문에 비효율적이다. 따라서 hybrid A*는 다음과 같은 특징을 가지고 있다.

State space가 아니라 control space에서 sampling을 수행해서 motion primitive를 생성한다. 즉, 공간을 이산화해서 그 공간을 검색하는 대신 로봇의 기구적 특징을 고려해 도달가능한 pose를 몇 개 sampling해서 이 pose들을 연결하는 형태로 path plannin을 수행한다.

그림 2. Hybrid A*는 제어 공간(control space)을 샘플링하면서 motion primitive를 생성한다

Pose를 sampling 할 때는 그림 2와 같이 전진 방향으로 조향 제어량에 따라 몇 개의 pose, 후진 방향으로 또 몇 개의 pose를 sampling한다. 여기서 pose란 위치 position과 방위 orientation을 합친 로봇의 상태이다. 그럼 새롭게 sampling된 위치는 그림 3의 오른쪽과 같이 grid 상의 점위에 위치하지는 않을 것이다? 이렇게 grid와 상관없이 연속 공간에서 A*를 수행하는 것이 hybrid A*이다.

그림 3. Hybrid A*를 통해 계획된 경로는 grid와 관계없이 연속 공간에서 얻어진다.

하지만, 이렇게만 하면 무한한 공간을 탐색해서 목적지까지 정확하게 연결하는데 너무 오랜 시간이 소모되기 때문에 아래와 같은 3가지 기법을 적용해서 planning 효율성을 높이고있다.

Heuristic Cost
Analytic Expansion
Path Smoothing

Heuristic Cost

첫번째 방법은 Heuristic cost를 조금 특별하게 설계해서 탐색횟수를 줄이는 방법이다. Hybrid A*에서는 Heuristic cost에 두 가지 목적 함수를 동시에 고려한다.

그림 4. Hybrid A*에 활용되는 Heuristic cost로 장애물을 고려하지 않는 non-holonomic cost와 장애물을 고려하는 holonomic cost를 중 하나를 활용한다.

한가지는 장애물을 고려하지 않는 non-holonomic heuristic 이다. Heuristic cost로 현재 고려중인 위치에서 목적지까지 장애물이 없다고 가정하고 Reeds-shepp같은 non-holonomic 모션을 이용해서 도달하는 거리를 heuristic cost로 사용하는 방법이다. 이렇게 하면 후진을 고려하면서 검색방향을 목적지쪽으로 설정할 수 있는 효과를 갖는다.

또 다른 한가지는 장애물을 고려한 holonomic heuristic이다. 이는 로봇의 기구적 특징을 고려하지 않고 단순히 장애물을 회피하는데 도움이 되는 방향으로 경로를 이끄는 효과를 가지게 된다.

그리고, 이 두 heuristic은 더하거나 하는 것이 아닌 그때 그때 둘 중 큰 값을 사용하게 된다. 사실, 그냥 단순히 생각해보면 장애물도 고려하면서 기구적인 제한조건도 고려하는 heuristic이 있다면 그게 제일 좋을 것 같은데 왜 이렇게 두 개로 분리해서 사용할까 하고 생각할 수 있다. 그러나 현실적으로 이것이 너무 어렵기 때문에 사용할 수가 없다.

장애물을 고려하고 기구적 특성을 고려하지 않는 holonomic heuristic은 Dijkstra 같은 방법을 사용해서 목표점으로부터 grid의 각 cell들이 얼마나 떨어져 있는지 그림 4의 오른쪽 하단 같이 계산해서 사용한다.

장애물을 고려하지 않고 기구적 특성을 고려하는 non-holonomic heuristic은 이 시리즈 비디오의 2번째 비디오에서 설명한 motion primitive를 이용해서 계산하는데 그 중에서도 Reeds-Shepp 모델을 사용하여 heuristic을 고려하게 된다. Reeds-Shepp 모델은 자동차가 움직일 때 있어서의 제약사항을 고려하여 특정 시작 포즈와 종료 포즈를 만족시킬 수 있는 motion primitive으 조합을 찾게 하는 방법이다. 예를 들어, 아래의 그림 5와 같은 상황에서 주차를 하기 위해 두 개의 pose를 연결하려면 이렇게 R+ L- S- R- N 순서의 움직임으로 표현할 수 있다. 이러한 움직임을 구현하기 위해 사용되는 노력을 바로 heuristic cost로 사용하는 것이며, 아무래도 이 값이 작을 수록 작은 노력, 시간에 구현되는 좋은 움직임이 된다.

그림 5. Reeds-Shepp Motion Primitives의 조합 예시 (R+ L- S- R- N)

Analytic Expansion

두번째는 Analytical expansion이다. hybrid A*는 처음에 말씀드린 것처럼 공간을 sampling한다. 아래의 그림6의 비디오가 바로 시작점에서 sampling을 통해 공간을 탐색하는 과정이다.

그림 6. 시작점부터 Sampling을 통해 공간을 탐색하는 과정

하지만 목표지점 근처에 도달해도 공간에 연속적으로 노드를 만드는 것이 아닌 Sampling한 노드를 사용하므로 이 노드가 정확하게 목표지점에 도달하기 어렵다.

그래서 이에 대한 해결방법으로 새롭게 검색을 수행할 노드에서 목표지점까지 좀전에 봤던 reeds-shepp 기법을 통해 장애물을 고려하지 않는 경로를 만들고 이 경로에 대해 장애물 충돌 검사를 수행해, 만약 충돌이 없다면 더 이상 sampling하는 과정없이 Reeds-Shepp로 만들어진 경로를 바로 지금까지의 경로 끝에 붙여버리면서 Planning을 끝내는 방법이다.

그림 7. 종점에 다가갔을 때 추가 sampling 없이 바로 붙여버리는 모습

이 방법을 사용하면 추가적인 sampling과정 없이 종점 부근에서 충돌 없는 경로를 빨리 찾을 수 있어 planning 시간을 줄일 수 있고, Reeds-Shepp를 통해 최종 목적지에서 로봇이 가져야 하는 정확한 위치와 방위에 대해 path planning할 수 있다는 장점을 가지고 있다.

Path Smoothing

마지막 방법은 path smoothing이다. Sampling을 하는 것은 공간을 효율적으로 빠르게 검색하는데는 유리하지만 경로를 다 찾고 나면 그 경로의 품질에 악영향을 미칠 수 있다.

그림 8. Sampling으로 계획한 경로 (파란색)과 Smoothing 처리하여 얻은 부드러운 경로 (빨간색 실선, 노란색 점선 등)

이를 해결하기 위해 hybrid A*에서는 planning을 마치고 난 뒤 시작점에서 종료점까지 이어진 경로를 장애물을 고려하면서 그 곡률의 변화를 최소화하는 형태로 최적화를 수행해 가장 작은 조향 입력 effort를 이용해서 가장 부드럽고 편안한 경로로 smoothing하는 작업을 수행한다.

그림 9. Smoothing 전, 후의 Path 비교 예시

처음부터 최적화를 이용해서 planning을 하는것보다 sampling을 통해 시간을 단축하고 이후 최적화를 통해 경로 품질도 확보하는 방법이다.

Hybrid A*를 이용한 자동 주차 코드 예시

그림 10. 자율주행 자동차가 자동 주차를 하기 위해 경로를 생성하는 코드 예제

그럼 예제 코드를 이용해 자율주행차가 자동으로 주차를 하기 위해 경로를 생성하는 예시를 확인해보자. 순서는 아래와 같다.

처음에는 주차장 환경을 occupancy grid map을 이용해 모델링하고
planner object를 몇가지 옵션과 함께 생성한다.
그리고는 시작pose와 목표 pose를 지정해서,
Planner를 동작시켜서
그 결과를 시각화한다.

% Configure parking environment
costmap = helperGetVehicleCostpMap();
validator = validatorVehicleCostmap(stateSpaceSE2);
validator.Map = costmap;

planner = plannerHybridAStar(validator,...% Path Planner Hybrid A*
'AnalyticExpansionInterval',5,...
'MinTurningRadius',4,...
'MotionPrimitiveLength',6,...
‘NumMotionPrimitives’,7,...
'ForwardCost’,1, 'ReverseCost',3,'DirectionSwitchingCost',1);

startP = [25 13 180]; % Set start pose
goalP = [24.3 25 90]; % Set goal pose

refpath = plan(planner, startPose, goalPose); % Execute path planner
[refPoses, directions] = interpolate(refPath);

figure;show(planner); % Plot the results

그럼 첫번째 항목부터 더 자세히 살표보도록 하자.

Modeling Occupancy Grid Map

Occupancy grid의 또 다른 표현 방식중 하나인 vehicleCostmap은 map위에 장애물을 설정하여 path planning을 할 때, 이 장애물을 회피할 수 있도록 내 로봇의 크기를 효율적으로 표현하는 방법이다. 사실 vehicleCostmap은 path planning을 위한 환경 표현 방법 및 충돌 검출편에서 자세히 다룬 바 있다.

그림 11. vehicleCostmap을 활용해 map의 충돌 가능성을 확인하는 예시

간단하게 정리해보면 차와 주변 장애물과의 충돌을 검출하는 작업을 가장 적은 연산으로 수행하기 위해 차를 원으로 표현하는 방법이라고 할 수 있다. 장애물이 있는 셀이 원 안으로 들어오는지 확인하는데 차 전체를 감싸는 하나의 원은 길고 좁은 주차 구역에서는 충돌이 없지만, 충돌로 판단하여공간을 효율적으로 활용하기 어려우니 몇 개의 작은 원으로 차량을 표현함으로써 충돌 검출을 위한 연산은 최소화하면서 공간을 효율적으로 활용하는 방법이다.

그림 12. inflation을 통해 차량을 단순화하여 모델링하는 경우 여러개의 원으로 차량의 형상을 커버할 수도 있다.

아래의 코드에서처럼 장애물이 표현된 occupancy map을 Vehicle cost map을 이용해 차를 표현하는 원 크기만큼 inflate하고 이를 planner 내부에서 충돌 검출에 사용하는 validator에 입력한다.

그림 13. vehicleCostmap을 이용해 Occupancy Grid Map을 모델링 하는 스크립트

Creating a Planner Object

이제 planner object 를 만든다.

그림 14. planner 객체를 생성하는 스크립트

공간을 정의하고 충돌을 검출하는 validator를 입력하고 몇가지 planner 파라미터를 정한다. 전진 cost, 후진 cost, 방향 전환 cost가 기본이다. 전진보다 후진 cost를 높이면 planner는 후진보다는 전진이 많은 경로를 출력할 것이고, 방향 전환 cost값이 낮으면 여러 번 전후진이 반복되는 경로를 높으면 전후진 방향 전환을 최소화하는 형태로 경로를 출력하게 된다. min turning radius는 sampling을 할 때 전체적인 조향각의 범위를 결정하게 되고 motion primitive length는 sampling 시의 primitive motion의 길이이다. 너무 길면, 공간을 효율적으로 탐색하지 못하고, 너무 짧으면 불필요하게 많은 횟수의 탐색을 수행해야 한다.

마지막으로 numMotionPrimitives는 전체적인 조향각 범위 안에서 몇 개의 조향각에 대해 sampling할 것인지를 결정한다. 이 값이 크면 더 많은 sample에 대해 충돌검사 및 공간 검색을 수행하므로 planning 속도가 느려질 수도 있지만, 반대로 좀 더 촘촘히 공간을 검색해서 좀 더 효율적인 경로를 더 빨리 찾을 수도 있다.

Defining Start and End Pose

시작점과 끝점에서의 pose를 정의한다. pose는 x, y 좌표와 방향으로 구성되어 있다.

그림 15. 시점과 종점의 pose를 정의하자

Visualization

경로를 생성하면 planner 결과를 받아 제어를 수행하는데 제어의 편의를 위해 생성된 경로를 동일 길이로 짧게 잘라낸다. 이 때 interpolate 함수가 사용된다. 최종적으로 생성 경로를 plot하면 오른쪽과 같은 그림을 확인할 수 있다.

그림 16. interpolation과 시각화 과정

Technical Resources

정리하면 이 비디오에서는 자동차와 같은 Ackermann 조향 제한 조건이 있는 로봇에 사용하는 Hybrid A* path planner와 이 planner를 구현하는데 사용된 여러 부가적인 내용에 대해 알아보았다. Path Planning 분야 외의 로봇 개발을 위한 다른 내용이 궁금하다면 아래의 매스웍스 코리아에서 제공하는 모바일 로봇틱스 그리고 자율주행 웹 포털을 통해 정보를 얻어갈 수 있으니 참고하기 바란다.

MATLAB Mobile Robotics Web Portal

그림 17. MATLAB을 이용한 육상 이동 로봇 개발 Web Portal (링크)

MATLAB ADAS Web Portal

그림 18. MATLAB을 이용한 자율주행/ADAS 개발 Web Portal (링크)

MATLAB Onramp Series

매트랩 기초 사용법을 학습하고 싶은 경우 MathWorks 홈페이지 내의 Onramp 라는 무료 트레이닝 코스를 활용할 수 있다. Onramp는 웹상으로 진행하는 온라인 무료 교육으로, 컴퓨터에 매트랩을 설치 할 필요 없이 온라인으로 매트랩 관련된 여러 기초 내용을 학습할 수 있다.

그림 19. MATLAB을 무료로 배울 수 있는 Onramp 시리즈 (링크)

Search-based Path Planning