[ROS2::Navigation2] MPPI Controller

G. Williams, P. Drews, B. Goldfain, J. M. Rehg and E. A. Theodorou, "Aggressive driving with model predictive path integral control," 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, 2016, pp. 1433-1440, doi: 10.1109/ICRA.2016.7487277. keywords: {Trajectory;Optimal control;Entropy;Vehicles;Prediction algorithms;Q measurement;Stochastic processes},

https://ieeexplore.ieee.org/document/7487277

Aggressive driving with model predictive path integral control

https://navigation.ros.org/configuration/packages/configuring-mppic.html

Model Predictive Path Integral Controller — Nav2 1.0.0 documentation

 

아이디어

기본적으로 ModelPredictiveControl(MPC)기법은 설정한 receding horizon과 시스템 모델에 대해 사전에 정의한 제약조건을 만족하면서 비용함수를 최소화 할 수 있는 제어 명령을 찾아내는 기법입니다.

 

기존에 Hamilton-Jacobi-Bellman equation을 이용해 optimal control input을 찾는 방법의 경우 dynamics, cost function이 nonlinear한 경우에도 해결이 가능하지만 계산량이 지수적으로 증가합니다. 이러한 문제는 path integral control 기법을 이용해 optimal control input을 찾는 것으로 해결될 수 있습니다. (path integral control의 경우 어떠한 입자가 이동가능한 모든 경로를 고려하는 것을 의미하지만 모든 경로를 고려하는 것은 사실상 불가능하고 이를 위해 몬테카를로 샘플링 기법을 이용해 확률적으로 샘플링)

 

여기서는 몬테카를로 샘플링 기법을 이용해 제어입력을 확률적으로 샘플링하고 각 샘플링에 대한 페널티는 목적 함수에의해 평가 되며 페널티가 작은 샘플링일 수록 샘플된 제어입력이 지배적이게 되어 새로운 제어입력을 만들어냅니다.

이때 충분히 많은 제어입력이 샘플링 되어야 최적 결과를 도출해낼 수 있다고 합니다.  (각 궤적에 가중치를 적용한 평균을 새로운 제어입력으로서 사용)

 

해당 아이디어 알고리즘의 수행 순서는 다음과 같습니다.

 

 1. 가장 먼저 정규분포로부터 몬테카를로 샘플링 기법을 이용해 제어입력을 생성합니다.

 2. Dynamics에 이를 적용하여 다음 상태를 예측합니다.

 3. 사전에 정의한 목적함수로부터 예측한 궤적의 상태를 평가합니다.

 4. 평가된 예측 궤적의 비용을 이용해 새로운 최적 제어 입력을 계산합니다.

 5. 최적으로 도출된 제어 값을 입력합니다. (위 과정 반복)

장점은 시스템 모델을 이용해 현재 상태부터 미래 상태까지 고려하여 비용을 최소로 만드는 최적의 제어 입력을 선정할 수 있습니다. 모바일 로봇의 미래 궤적을 매 순간 예측하고 그때 시스템 상태를 평가함으로서 안정적인 움직임을 제공할 수 있습니다.

 

단점은 DWB에 비해 상대적으로 수식이 복잡할 뿐만 아니라 미래 상태까지 고려하기 때문에 계산량이 적지 않아보입니다. 타임스텝 및 샘플링 개수가 많아질수록 계산량 비례해서 증가합니다.

---

파라미터

 

PathAlign

로봇이 목표 지점에 근접하지 않을 때 전역 경로를 따라 움직이도록 장려합니다. 해당 비용이 가중될 수록 경로 주변에 전역 경로점을 정확하게 따르려고 하는 움직임을 보입니다.

max_path_occupancy_ratio = 0.05 (default)

장애물이 로컬 영역 범위 내 전역 경로와 지역경로의 초기점 사이 5%이상 겹쳐진다면 평가하지 않습니다. 이는 전역 경로상의 경로점이 장애물 혹은 인플레이션 영역에 겹쳐져 있어 로봇이 충돌 가능성이 생기는 지점으로의 지역 경로를 생성하지 않도록 합니다.

trajectory_point_step = 3 (default)

지역 경로를 고려할 때 경로점들을 몇칸 간격으로 고려할 것인지에 대한 파리미터, 파라미터 값이 작을수록 전역 경로 상 로봇의 헤딩 회전이 발생하도록 하는 구간에서 더욱 정렬될 수 있도록 유도합니다.

 

PathAngle

로봇이 목표 지점에 근접하지 않을 때 지역경로가 전역 경로를 향하도록 장려합니다.

PathFollow

로봇이 목표 지점에 근접하지 않을 때 전역 경로를 더욱 추종하도록 장려합니다.

지역 경로점이 전역 경로점을 따르지 않을 때 비용이 높아지며, 지역 경로점들이 전역 경로 점들을 따르는 지역 경로를 선정하도록 유도하여 로봇이 전역 경로를 따르도록 합니다. 이에 대한 비용이 높아진다면 전역 경로의 목적지점에 가깝도록 하는 제어입력이 지배적이게 되며 이는 로봇이 목적지를 향해 더욱 빠르게 움직이는 것을 유도할 수 있습니다.

GoalAngle

로봇이 목표 지점에 근접할 때 목표 포즈를 달성하도록 하며 전역 경로의 목표 지점에서의 각도와 지역 경로 계획에서 생성된 각 경로의 마지막 경로점과의 상대적인 각도의 거리를 비용으로서 정의하고 있습니다.

 

Goal

로봇이 목표 지점에 근접할 때 목표 지점을 향해 가도록 장려하며 전역 경로의 목표 지점과 지역 경로 계획에서 생성된 각 경로의 모든 경로점과의 거리를 비용으로서 정의하고 있습니다.

 

PreferForward

로봇이 전방 방향으로 주행하도록 유도하며 로봇의 헤딩 방향으로의 속도성분을 비용으로서 정의하고 있기 때문에 각 생성된 지역 경로의 경로점이 후방 방향으로의 속도 성분을 포함할 때 비용이 높아집니다. 이를 평가하여 로봇이 전방 방향의 움직임을 가지는 지역경로를 생성하도록 장려합니다.