# Simulating Trajectory Tracking Controllers for Driverless Cars

Hi, everyone! In this blog, we will see how trajectory tracking controllers can be implemented in MATLAB and Simulink to make the vehicle follow a pre-defined path. Before we get started, a big thank you to my amazing colleagues, David Barnes, Munish Raj, and Roshan Hingnekar for their efforts to build the content of this blog.

## Motivation

As a part of the Student Programs team at MathWorks, we regularly interact with students. During a few of these interactions, students suggested that similar to other Racing Lounge videos, we should build some content on how to simulate bicycle models for path tracking applications. They also mentioned that it can be further utilized for lap time simulation models in Simulink. So inspired by the students and two of our powerful toolboxes, Vehicle dynamics blockset, and Automated Driving Toolbox, we decided to come up with a series of videos to demonstrate the workflow that can be easily implemented.
That was the motivation that students gave us. Now, let’s check out the technical motivation!
Trajectory tracking is one of the essential parts of autonomous driving where the goal of the vehicle is to control the lateral and longitudinal motion to follow a pre-defined path. For example, in the Formula Student Driverless competitions, the vehicle needs to simultaneously plan and track the path in the first lap. Further, in the subsequent laps, the vehicle tracks the optimized path at a much higher speed. Hence, the tracking accuracy of the controllers needs to be high to avoid major deviation from the path.
Currently, there are multiple control strategies to address the trajectory tracking problem. However, in this blog, we will focus on the following three trajectory tracking controllers:
• Pure pursuit controller
• Stanley controller
• Model predictive controller (MPC)
To summarize the upcoming sections, we will first generate a reference trajectory. Then, we will build a vehicle model. Finally, we will show how easily we can implement the above three control strategies to control the longitudinal and lateral motion of the vehicle.

## Reference Trajectory

You can skip this step if you already have recorded vehicle pose data. However, If you want to generate a reference trajectory, you can use the Driving Scenario Designer app. The app enables you to design synthetic driving scenarios for testing your autonomous driving systems. In this case, we have utilized a section of the US Highway scene present in the prebuilt scenarios. Then, we have specified a vehicle and its trajectory and have exported the trajectory data to a MAT-file.

## Vehicle Model

The accuracy of the trajectory tracking problem depends upon the vehicle dynamics model. Based on the fidelity of the vehicle dynamics model, you can decide which controller is best suited for different driving conditions. Vehicle dynamics blockset provides a range of vehicle body blocks to implement vehicle models to calculate vehicle motion. In the model, we have used the vehicle body 3DOF dual track block where the tire forces are obtained from the combined slip wheel 2DOF block. Further, vectorized tire subsystem accepts the inputs from a simplified powertrain subsystem.
Now, in the subsequent sections, we have used the same vehicle dynamics model for all the controller models and these controllers will aim to generate the required steering angle, acceleration, and deceleration commands.
To start with a basic longitudinal and lateral vehicle dynamics model, please check out this video tutorial:

## Controllers

### Pure Pursuit Controller

As the first choice for implementing control strategies, we went ahead with the pure pursuit controller. In simple terms, it’s a path tracking algorithm that calculates the trajectory curvature to move the vehicle from one point to another. As per the formulation, you can compute the vehicle wheel steering angle to follow the reference trajectory.
As can be seen in the steering angle formulation, one of the variables is the lookahead distance. In general, it is how far along the path the vehicle should look from the current location to compute the angular velocity commands. It is one of the important tuning factors in ensuring the controller’s tracking accuracy. A small lookahead distance can lead to an oscillatory path and accurate tracking whereas large lookahead distance results in a less oscillatory path, however, it might result in poor tracking.
Now that we have provided a brief introduction to the pure pursuit controller, the image below shows its implementation in Simulink.
For the implementation, we have used the Pure Pursuit block that calculates the target direction angle based on two input ports, the pose, and the reference trajectory. In the block, you can tune the controller by setting the lookahead distance. Further, based on the above formula, the required wheel steering angle is obtained allowing the vehicle to follow the trajectory as shown in the figure below.
Reference video tutorial:

### Stanley Controller

Moving to our next controller, the Stanley controller was implemented in the DARPA Grand Challenge by the Stanford Racing Team. It is a nonlinear control law to minimize the cross-track error and the heading angle of the front wheel relative to the reference path. It computes the steering angle command that adjusts a vehicle’s current pose to match a reference pose.
To model the controller in Simulink, we have used the Lateral Controller Stanley block. The block computes the steering angle based on the above formula. What is interesting is that you can select the type of vehicle model using the controller settings. For example, for low-speed tracking such as parking lots you can use the kinematic bicycle model, and for high-speed tracking such as race cars it is preferable to use the dynamic bicycle model. Further, you can adjust the position, yaw rate, and steering gains to improve the accuracy of the controller.
Like the pure pursuit controller model, we have built the vehicle path tracking model using the Stanley controller. The vehicle model and the reference trajectory are kept the same. On simulating the model it is evident that in a higher-speed environment the vehicle is successfully tracking the desired trajectory achieving a maximum velocity of 30 m/s.
Reference video tutorial:

### Model Predictive Controller (MPC)

Finally, let us check out the implementation of MPC for path tracking applications in Simulink. In short, MPC solves an online optimization algorithm to find the optimal control action that drives the predicted output to the reference. It can handle multi-input multi-output systems that may have interactions between their inputs and outputs. It can also handle input and output constraints. However, the downside is that it is computationally expensive.
Please note we won’t go much in detail in this section as we already have an amazing MATLAB Tech Talk video series titled, Understanding Model Predictive Control. You can watch the videos to learn how MPC works and will also discover the benefits of this multivariable control technique.
To implement the MPC controller, initially, we started with the generic MPC Controller block. However, the Path Following Control System block is a better alternative for following a trajectory. In general, the block is meant to keep the ego vehicle traveling along the center of a straight or curved road while tracking a set velocity and maintaining a safe distance from a lead vehicle. However, we have implemented it to follow a trajectory by setting the set velocity to the reference velocity of the vehicle and by distance based curvature to the curvature port. Further, as the output of the block is longitudinal acceleration, we connected a pedal map to obtain the required acceleration and deceleration commands.
Finally, after selecting the appropriate design parameters such as prediction horizon, control horizon, etc., we simulated the model and can see the vehicle tracking the path perfectly.
Reference video tutorial:

## Time to build your own model

The blog does not cover all the technical details of the above controllers but rather provides an overview of implementing these controllers in MATLAB and Simulink. We hope we provided enough background and resources to get you started with the trajectory tracking models. So now it is your turn to use these resources as a reference to drive a car on a more challenging track.
Also, in case of any queries related to this blog please feel free to reach out to us at racinglounge@mathworks.com.
|

### コメント

コメントを残すには、ここ をクリックして MathWorks アカウントにサインインするか新しい MathWorks アカウントを作成します。