Model Predictive Path Integral Control

Overview

This implementation of model predictive path integral control (MPPI) was designed for a TurtleBot3. The average control update occurs at a rate of 50Hz.

See the code in my GitHub repository.

MPPI Algorithm

An iterative path integral control update law is applied followed by a generalized importance sampling term. Optimization and execution occur simultaneously. A trajectory is optimized and then a single control is executed. The trajectory is re-optimized using the un-executed portion of the previous trajectory.

Control perturbations are sampled from a zero mean normal distribution with specified sampling variance. The trajectories for a kinematic differential drive robot are propagated forward using a 4th order Runge-Kutta method. The cost function used is the same as the LQR cost. There is a matrix Q that penalizes the error in states, a matrix R penalizes controls, and a matrix P1 penalizes the terminal error. The cost at each state in the trajectory is a summation of the loss at each future state meaning early control decisions are more costly than future decisions.

Video

This video is in real time. The TurtleBot appears to be moving slowly due to the wheel velocity limits.

Credits

[1] Williams, Grady, Andrew Aldrich, and Evangelos Theodorou. “Model predictive path integral control using covariance variable importance sampling.” arXiv preprint arXiv:1509.01149 (2015). PDF

[2] Abraham, Ian, et al. “Model-Based Generalization Under Parameter Uncertainty Using Path Integral Control.” IEEE Robotics and Automation Letters 5.2 (2020): 2864-2871. PDF