Model Predictive Control with Physics Engine in the loop

The repo for this work can be found in Here

Here I have a high-level explanation of the method but you can find more details in my PhD thesis.

Overall structure

Hierarchy of this motion planning alorithm is showin bellow. Global planner hasn’t been studied in this research since there already many sampling based canditates in the field.

Local Planner

Local planner consumes waypoints from a higher level global planning algorithm and construcs an optimization based boundary value problem where solution to that outputs a trajectory(red line) and modified reachable waypoint(yellow square). Output trajectory (in red) can later be used as a reference trajectory for the MPC.

The robot trajecotry in state space (positions+velocities) is parameterized with bezier curves, where some guarantees can be achieved when solving the optimization problem.

In solving the optimization problem with some numerical descent method jacobian has the following sparsity pattern.

Local Planner In Action

Here are results of running BVP solver in three different senarios.

First when second waypoint is accessable

Second when there isn’t enough ground friction so the second waypoint is not accessable

Third when target velocity is to high and not reachable for the vehicle from given initial point.

MPC

model predictive controller uses the reference trajectory (blue) to construct following cost function

the jacobian for this pattern looks like

Each column of mpc problem is independent and a physics engine simulation needs to be executed per each column. Because of this a multi-threaded function can simultaneously compute all columns. This way calculation of the whole jacobian takes as long as execution of one trajectory in simulation.

Local Planner + MPC in Action

Here is a full simulation of running local planner followed but controling the car on desired trajectory (red) using MPC design above.