Three Methods of Vehicle Lateral Control: Pure Pursuit, Stanley and MPC
Updates:
Hi all, I have updated this blog in our website SHUFFLE. I also corrected some pictures which hope can bring you much precisely understanding of lateral control models. Moreover, I added some sample codes for these three methods so you can also try yourself in Carla simulator.
- Introduction
In this article, we will discuss three methods of vehicle lateral control: Pure pursuit, Stanley, and MPC combined with the result of a project of controlling the vehicle to follow a race track. The source of this project is the final assignment of the course “Introduction to self-driving cars” on Coursera[1].
2. Pure Pursuit Controller
Pure pursuit is the geometric path tracking controller. A geometric path tracking controller is any controller that tracks a reference path using only the geometry of the vehicle kinematics and the reference path. Pure Pursuit controller uses a look-ahead point which is a fixed distance on the reference path ahead of the vehicle as follows. The vehicle needs to proceed to that point using a steering angle which we need to compute.
In this method, the center of the rear axle is used as the reference point on the vehicle.
The target point is selected as the red point in the above figure. And the distance between the rear axle and the target point is denoted as 𝑙𝑑. Our target is to make the vehicle steer at a correct angle and then proceed to that point. So the geometric relationship figure is as follows, the angle between the vehicle’s body heading and the look-ahead line is referred to as 𝛼. Because the vehicle is a rigid body and proceeds around the circle. The instantaneous center of rotation(ICR) of this circle is shown as follows and the radius is denoted as R.
2.1 Pure Pursuit Formulation
From the law of sines:
k is the curvature. Using the bicycle model (If you have no idea about the kinematic bicycle model, you can refer to another article named “Simple Understanding of Kinematic Bicycle Model”),
So the steering angle 𝛿 can be calculated as:
The pure pursuit controller is a simple control. It ignores dynamic forces on the vehicles and assumes the no-slip condition holds at the wheels. Moreover, if it is tuned for low speed, the controller would be dangerously aggressive at high speeds. One improvement is to vary the look-ahead distance 𝑙𝑑 based on the speed of the vehicle.
So the steering angle changed as:
2.2 Pure Pursuit Simulation in CARLA
Now we know how to control the steering wheel. Let’s see how the pure pursuit controller behaves in the CARLA simulator.
As you can see in the above result, we have successfully followed the race track and completed 100.00% of waypoints.
2.3 Why Pure Pursuit Controller is effective?
Let’s see what is the cross-track error in this case. First, the cross-track error is defined as the lateral distance between the heading vector and the target point as follows. If the cross-track error is smaller, that means our vehicle follows the path better.
What is the relationship between the cross-track error and the curvature k? Before we have already known,
we can easily arrive,
The above equation shows that the curvature k is proportional to the cross-track error. As the error increases, so does the curvature, bringing the vehicle back to the path more aggressively. The proportional gain 2/ld² can be tuned by yourself. In short, pure pursuit control works as a proportional controller of the steering angle operating on the cross-track error. The cross-track error can be reduced by controlling the steering angle, so this method works.
3. Stanley Controller
Secondly, we will discuss Stanley Controller. It is the path tracking approach used by Standford University’s Darpa Grand Challenge team. Different from the pure pursuit method using the rear axle as its reference point, Stanley method use the front axle as its reference point. Meanwhile, it looks at both the heading error and cross-track error. In this method, the cross-track error is defined as the distance between the closest point on the path with the front axle of the vehicle.
3.1 Stanley formulation
As you can see in Figure5, 𝜓(t) is the angle between the trajectory heading and the vehicle heading. The steering angle is denoted as 𝛿. There are three intuitive steering laws of Stanley method,
Firstly, eliminating the heading error. 𝛿 (t)= 𝜓(𝑡)
Secondly, eliminating the cross-track error. This step is to find the closest point between the path and the vehicle which is denoted as e(t). The steering angle can be corrected as follows,
The last step is to obey the max steering angle bounds. That means 𝛿(𝑡)∈ [𝛿𝑚𝑖𝑛,𝛿𝑚𝑎𝑥].
So we can arrive,
One adjustment of this controller is to add a softening constant to the controller. It can ensure the denominator be non-zero.
3.2 Stanley Simulation in CARLA
After knowing how to control the steering angle, we now can make the vehicle follow a path. Let’s first see how the Stanley method behaves in the CARLA simulator. As same as the pure pursuit before, we implement the above formulation to python and connect it with the CARLA simulator.
Using the Stanley controller, we can also complete 100.00% of waypoints. Moreover, looking at the video, the vehicle proceeds much more steadily than the Pure Pursuit controller, especially when it comes to a turn. We will discuss why the Stanley controller is effective and steady.
3.3 Why Stanley Controller is effective and steady?
Let’s recap its formulation,
Stanley controller not only considers the heading error but also corrects the cross-track error. Let’s look at these two scenarios,
Firstly, if the heading error is large and cross-track error is small, that means 𝜓 is large, so the steering angle 𝛿 will be large as well and steer in the opposite direction to correct the heading error, which can bring the vehicle orientation as same as the trajectory.
The process of this scenario can be drawn as below,
Secondly, if the cross-track error is large with small heading error, that can makes,
so the steering angle will be,
Supposing the heading error 𝜓(t) =0, 𝛿(t) will be 𝜋/2. That makes the vehicle run towards the path as follows,
As the heading changes due to the steering angle, the heading correction counteracts the cross-track correction and drives the steering angle back to zero. When the vehicle approaches the path, cross-track error drops and the steering angle starts to correct the heading alignment as follows,
The process of this scenario can be drawn as follows,
In short, the Stanley controller is a simple but effective and steady method for later control. These two methods are both geometric controller. We will discuss another non-geometric controller which is the Model Predictive Controller known as MPC.
4. Model Predictive Controller
4.1 Cost Function
We should first know the cost function. For example, in this project, we want to control the vehicle to follow a race track. So the cost function should contain the deviation from the reference path, smaller deviation better results. Meanwhile, minimization of control command magnitude in order to make passengers in the car feel comfortable while traveling, smaller steering better results. This is similar to the optimization problem of optimal control theory and trades off control performance and input aggressiveness. Above these two targets, we can arrive the cost function as,
In the above equation, given the input of the steering angle, 𝛿x is the distance between the predictive point and the reference point as follows,
𝑢 is the steering input. So how can we know 𝛿x? We must have the predictive model of the plant first.
4.2 Predictive Model
The main concept of MPC is to use a model of the plant to predict the future evolution of the system[2]. In this case, we can use the simple kinematic bicycle model as follows, if you are not familiar with it, you can refer to my another blog.
x_dot = v * cos(𝛿 + 𝜃)
y_dot = v * sin(𝛿 + 𝜃)
𝜃_dot = v / R = v / (L/sin(𝛿)) = v * sin(𝛿)/L
𝛿_dot = 𝜑
x_(t+1) = x_t + x_dot * ∆𝑡
y_(t+1) = y_t + y_dot * ∆𝑡
𝜃_(t+1) = 𝜃_t + 𝜃_dot * ∆𝑡
𝛿_(t+1) = 𝛿_t + 𝛿_dot * ∆𝑡
The states X are [x, y, 𝜃, 𝛿], 𝜃 is heading angle, 𝛿 is steering angle. Our inputs U are [𝑣, 𝜑], 𝑣 is velocity, 𝜑 is steering rate.
4.3 MPC Structure
Now, we have the cost function and the predictive model. The next step is to seek the best inputs to optimize our cost function. We can summarize the whole MPC process as follows,
So how to find the best control policy U? In this case, U is the steering angle.
- Firstly, suppose our steering angle bounds are 𝛿(𝑡)∈ [𝛿𝑚𝑖𝑛,𝛿𝑚𝑎𝑥]. I use a simple method that discrete the input of the model, which is the steering angle 𝛿 into values with the same interval.
- Then we can get the predicted outputs which are [x, y, 𝜃, 𝛿] using the above model and the input 𝛿.
- The last step is to select the smallest value of the cost function and its corresponding inputs 𝛿. (In this case, we divided steering angle with 0.1 intervals from 𝛿𝑚𝑖𝑛 -1.2 to 𝛿𝑚𝑎𝑥 1.2 radians. Then put it into the cost function and for loop to find the minimum value and its corresponding input 𝛿.)
- Repeat the above process in each time step.
4.4 MPC Implementation in CARLA simulator
Now we have our steering angle and know how to control the vehicle. Let’s see how the MPC behaves in the CARLA simulator.
As you can see in the above figure, we can also complete 100.00% of waypoints with the MPC controller. But looking at the video, the vehicle runs not so steadily as using the Stanley Controller. Recap our cost function, we set the input 𝛿 in it because we do not want too big actions which may lead passengers feeling not good. I guess it is not appropriate just set like that. I think one method that can improve it is to make the action more continuous. If you are interested in it, you can try yourself. In this article, we just focus on the basic idea of MPC.
MPC has a lot of advantages. It can also be applied to linear or nonlinear models. It has a straightforward formulation and it can handle multiple constraints. For example, it can incorporate the low-level controller, adding constraints for Engine map, Fully dynamic vehicle model, Actuator models, Tire force models. And the cost function can be designed for different targets. For example, it can penalize collision, distance from the pre-computed offline trajectory, and the lateral offset from the current trajectory and so on. MPC is much more flexible and general. But it also has disadvantages of computationally expensive. Especially for the non-linear model, which is very general and even our bicycle model is also in this category, MPC must be solved numerically and cannot provide a closed-form solution.
5. Summary
In this article, we discussed three methods of lateral control and analyzed the project of trajectory tracking using these three methods. I hope it can give you some basic ideas for vehicle lateral control.
References
[1] Steven Waslander, Jonathan Kelly, “Introduction to Self-Driving Cars”, Coursera.
[2] Gabriel M. Hoffmann, Claire J. Tomlin, “ Autonomous Automobile Trajectory Tracking for Off-Road Driving: Controller Design, Experimental Validation and Racing”, 2007.
[3] P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng, D. Hrovat, “Predictive Active Steering Control for Autonomous Vehicle Systems”, 2007.
