Vehicle MPC Controller

An MPC, or Model Predictive Control, is a process control designed to automate a sensor/actuator system such industrial power systems, robotics, or in this case: driving a vehicle around a track. We can sense the car and the position (x/y coordinates) relative to the track, and then we can control it via steering.

To help illustrate, you can see in the diagram below the light blue line on the bottom is the actuation, in this case a discrete input, along with the trajectory (both reference and predicted) at the top. The controller will take in the sensed state of the system and optimize an ideal control plan based on a cost function defined within the model.

Here we only have one actuator, which for example could be the control rods that moderate the fission rate in a nuclear plant, but in my controller I will have two.

My Model

Through using a Unity game engine simulation of a vehicle on a track, the vehicle will be controlled using MPC (model predictive control) process control. This is similar to my other project on GitHub that used a PID controller for this same task. The difference is that an MPC controller (forgive the redundancy of words here, I prefer this phrasing) can effectively model future time states and plan ahead. The model was optimize throughout this finite time-horizon each actuator, which in this case is steering and throttle/brake. Throttle and braking are controlled via the same actuation via values with a range of [1,1].

Steps Involved:

• Understand methods of accessing and controlling the car.
• Convert the MPC method to C++ code and implement in both main.cpp and mpc.cpp to control the vehicle
• Observe and take into account the simulated latency (in this case 100ms) of the sensor stack and optimize the model using this knowledge.

Dependencies and Compiling the C++ code

If you care for the technical aspects of implementation see my GitHub ReadMe here for more detailed instructions.

Vehicle Control API

Using a WebSocket server, the C++ console program communicates with the driving simulator through a JSON file that takes in and reads out various telemetry variables. These include: CTE, speed, angle, throttle, and steering angle.

In this MPC controller I will need to observe the CTE, heading, and speed, while pushing throttle (including braking for negative values under 0) and steering angle in an attempt to keep up the highest possible speed while staying safely within the lane. Though in this case I have locked speed to a specific value to aim for at 60mph. Below is the code for reading the initial telemetry into C++:

MPC Controller Algorithm

The aim of this is to model the behavior of dynamical systems, in this case the vehicle positioning and trajectory. This is a kinematic model that is a more advanced version of the PID controller from my other repository that does not take into account future time states. This will model the change in dependent variables that are caused by changes in the independent variables.

This is an iterative process based on a finite-horizon optimization of states. We will attempt to minimize the cost (CTE, heading, velocity) of the vehicle on the track. This is what is called on online model, in that it calculates on-the-fly.

This is a multi-variable control algorithm that uses:

• a dynamical model of the process.
• a history of past actuation.
• an optimization of a cost function (CTE, heading, velocity) over the prediction horizon.

Below is the mathematical notation for some of the state prediction updates:

The code to implement the above is as follows:

Variables above, explained:

• X — Vehicle location in X coordinate.
• Y — Vehicle location in Y coordinate.
• PSI — Vehicle heading (simulation works with radians, algorithm uses degrees). The two formula used to convert back and forth between the two are:

• CTE — Cross-Track-Error (distance of vehicle from ideal (middle) line on track)
• EPSI — Vehicle heading error (difference from ideal and actual, ideal is tangent to road curve)

Tuning the Model

For me personally, I found these variables to work best with my model at a target speed of 60mph:

• Timestep Length — N - This is the count of way-points to predict actuation for the vehicle in the future. For this project I used a length of 7 as it predicted enough around turns but did not run too slowly for my computer. Though I have a feeling on slower CPUs it may struggle. In the image below you can see an example of a controller with 6 way-points:

• Duration — dt - Elapsed time at 100 should coincide very cleanly with the 100ms delay built in to the model. Quicker is ideally better, but will be more taxing on the system CPU.

Fitting the Polynomial

Using the code below, a 3rd order polynomial is fitted to the ideal vehicle path for the future to best reduce the total error value. Using 3 polynomials allows for a more advanced curvature on some tighter sections of road.

In regards to the image above of the vehicle making a turn, you notice that it does not follow the ideal path exactly. Due to the nature of the algorithm it balances multiple goals all at the same time, which include: velocity, CTE (distance from the blue line), and heading (orientation tangent to this blue line). It also has a sort of buffering system for the actuators similar to the shock overbears in your car, it is designed to keep from changing throttle or steering too quickly. This is sometimes referred to in physics as the Jerk, which you can calculate as the second derivative of the velocity (first derivative being acceleration).

Simulating the Sensor Stack Latency

In the main source code I incremented the position forward in time with latency = 0.1 to represent 100ms delay in positioning detection. This was done by re-evaluating the vehicle position that was sensed by the following code:

In Closing

This is an expansion of my PID controller from earlier, which I have a GitHub repository for here. To really get to enjoy the fun, apply for the Udacity Self-Driving Car NanoDegreee program. This project will come up in Term 2 near the end. It has taught me an amazing amount so far of curated skills that will be useful in such a career. Beforehand, much of this material was relegated to scientific papers and engineering textbooks. Having it all together on a responsive website, with videos and code examples, is a huge benefit. The projects then help you really hammer all this information in.