Photo by Campbell Boulanger

Lane keeping in autonomous driving with Model Predictive Control & PID

The yellow path is the target trajectory and the green path is how our car moves using MPC.

Three major components of autonomous driving are localization (where am I), perception (what is around me), and control (how to drive around). Using lane recognition, we can plan a trajectory for an autonomous car to follow. In this article, we discuss the control of the vehicle’s acceleration, brake, and steering with Model Predictive Control (MPC) using a kinematic bicycle model. The purpose is not only following a target trajectory as close as possible but also as smooth as possible to avoid motion sickness or frequent braking.

MPC is a process to minimize costs while satisfying a set of constraints. For example, we want to adjust the steering and the speed every 100 ms such that the cost function, defined as the difference between the target trajectory point (the yellow dot) and ours (the green dot), is minimized under the constraint that the wheel cannot be steered more than 25°. In MPC, we read from the sensors to determine the current states of the car like speed. Then we consider possible actions within a short period of time (say 1 sec.) based on these readings. Let’s say, we consider steering the wheel by 20° clockwise and then reduce it by 1° every 100 ms. Assuming these actions result in the lowest cost at the end of the one second period, we will then apply the first action of stirring the wheel 20°. But instead of performing the remaining actions later, we wait for 100 ms and read the sensors again. With the new readings, we recompute the next optimal action again. MPC makes the next action by taking advantage of viewing the results of a longer future plan (1 sec.). So it is less vulnerable to short-sighted gain in a greedy method and therefore, plan better.

This is part of a 5-series self-driving. Other articles includes

• Self-driving perception: Sensor fusion with Kalman Filter.
• Self-driving perception: Extended Kalman Filter and Unscented Kalman Filter.
• Self-driving localization: Localization with Particle Filter.
• Self-driving control: Control with Model Predictive Control & PID.
• Self-driving Path finding.

Kinematic bicycle model

First, we define a kinematic model to describe our car.

Source

(x, y) is the vehicle’s center of mass. ψ is the current heading of the car and v is the speed of the vehicle. lf is the distance from the center of the mass to the front axle. β is the angle of v with respect to the car axis. In our example, we assume this is zero. (i.e. the car is not sliding)

In our model, we can control the front wheel steering angle δf and the car acceleration a. For simplicity, we assume it is a front-wheel-drive car and we will write δf as δ for now.

MPC in details

At each period (100 ms), we read from the sensors to determine the current state of the vehicle including:

• location of the vehicle,(x, y)
• speed v,
• heading ψ,
• the steering angle δ, and
• the acceleration a.

Trajectory model

Our lane detection system should also give us a trajectory, say, in the form of next 6 waypoints (6 coordinates). In our example, we use the 6 waypoints to fit a 3-rd order polynomial function. We use this model to compute the y coordinate and the heading ψ from x.

Dynamic model

Next, we will create a dynamic model for predicting the state at time t+1 of our car from the last state at time t. Using the kinematic bicycle model, we can easily deduce the location, the heading and the speed from the last state.

We add 2 more states to measure the cross-track error (cte — the distance between the green and yellow dot) and the heading error for ψ.

Cost function

In MPC, we define a cost function to optimize our path with the trajectory. For speed, we penalize the model if the car cannot maintain at a target speed. We want no acceleration and zero steering if possible. But since it is unavoidable, we want the rate change to be as low as possible if it happens. This reduces motion sickness and saves gas. In our model, our cost includes:

• Cross-track error,
• Heading error,
• Speed cost (prefer staying at say 100 miles/hr ),
• Steering cost (prefer zero steering)
• Acceleration cost (prefer zero acceleration)
• Steering rate change (prefer small values)
• Acceleration rate change (prefer small values)

Since those objectives may be contradictory to others, we add weights to the cost to reflect its priority. Here is the cost function:

Putting it together

Recall that to find our optimal path in MPC, we need a dynamic model to predict the next state, a cost function and the system constraints.

Optimize MPC

We solve the control problem as an optimization problem using a cost function under constraints include those for actuators controlling the gas/brake pedal and the steering wheel.

1. We read the next 6 waypoints from the lane detection and compute a 3rd order polynomial to model our trajectory.
2. We read the current speed v, heading ψ, steering angle δ, and acceleration a from the censors.
3. We compute our first vehicle state using these readings and the dynamic model.
4. We want to optimize the actions for the next 1 sec while adjusting steering angle and acceleration every 100 ms. i.e. we have 10 time periods.
5. We define an MPC with 2 independent variables: one controls the acceleration (positive for the gas pedal and negative for the brake) and the other for the steering angle.
6. We define the allowable range of values (constraints) for acceleration and steering.
7. We unroll the dynamic model 9 times, so we can compute the next 9 states.
8. We also define the cost from each time period.
9. We use an optimizer for MPC to find the best actions for time period 1 to time period 9 that have the minimum total cost under the defined constraints. (Note, in our notation, time period does not start from 0. Instead, we start from 1: period 1 to 10.)
10. We will only take the actions suggested in the period 1 and ignore others.
11. But, we delay 100 ms before issuing the actions to the simulator. This imitates the real world which commands take time to process and execute.
12. We repeat step 1 again to find the next optimized actions.

Tunable

In our example, we compute the optimal solution for a period of 1 sec. This parameter is tunable. Longer period gives us a better picture of our actions but the accumulated errors may be too much. In fact, if this number is too large, the car will go off-track instead.

For those that survive the maths, here is a video on what can go wrong with autonomous driving.

PID

A proportional–integral–derivative controller (PID) is another controller for self-driving cars. The video below explains how it works.

In our example, our control function composes of

• Cross-track error (cte) for the proportional term,
• The derivative of cte as the derivative term to smooth out motion, and
• The accumulated sum of cte as the integral term to handle any steady disturbance.

Source: Wikipedia

We run the same simulation with the PID controller below. Since the cost function is far less sophisticated, the motion is more jerky.

Credit

The video is generated by MPC using the simulator provided by the Udacity self-driving class.