MPC Lateral Controller#
This is the design document for the lateral controller node
in the autoware_trajectory_follower_node package.
Purpose / Use cases#
This node is used to general lateral control commands (steering angle and steering rate) when following a path.
Design#
The node uses an implementation of linear model predictive control (MPC) for accurate path tracking. The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command. The optimization of the control command is formulated as a Quadratic Program (QP).
Different vehicle models are implemented:
- kinematics : bicycle kinematics model with steering 1st-order delay.
- kinematics_no_delay : bicycle kinematics model without steering delay.
- dynamics : bicycle dynamics model considering slip angle. The kinematics model is being used by default. Please see the reference [1] for more details.
For the optimization, a Quadratic Programming (QP) solver is used and two options are currently implemented:
- unconstraint_fast : use least square method to solve unconstraint QP with eigen.
- osqp: run the following ADMM algorithm (for more details see the related papers at the Citing OSQP section):
Filtering#
Filtering is required for good noise reduction. A Butterworth filter is employed for processing the yaw and lateral errors, which are used as inputs for the MPC, as well as for refining the output steering angle. Other filtering methods can be considered as long as the noise reduction performances are good enough. The moving average filter for example is not suited and can yield worse results than without any filtering.
Assumptions / Known limits#
The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose.
Inputs / Outputs / API#
Inputs#
Set the following from the controller_node
autoware_planning_msgs/Trajectory: reference trajectory to follow.nav_msgs/Odometry: current odometryautoware_vehicle_msgs/SteeringReport: current steering
Outputs#
Return LateralOutput which contains the following to the controller node
autoware_control_msgs/Lateral- LateralSyncData
- steer angle convergence
Publish the following messages.
| Name | Type | Description |
|---|---|---|
~/output/predicted_trajectory |
autoware_planning_msgs::Trajectory | Predicted trajectory calculated by MPC. The trajectory size will be empty when the controller is in an emergency such as too large deviation from the planning trajectory. |
MPC class#
The MPC class (defined in mpc.hpp) provides the interface with the MPC algorithm.
Once a vehicle model, a QP solver, and the reference trajectory to follow have been set
(using setVehicleModel(), setQPSolver(), setReferenceTrajectory()), a lateral control command
can be calculated by providing the current steer, velocity, and pose to function calculateMPC().
Parameter description#
The default parameters defined in param/lateral_controller_defaults.param.yaml are adjusted to the
AutonomouStuff Lexus RX 450h for under 40 km/h driving.
System#
| Name | Type | Description | Default value |
|---|---|---|---|
| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 |
| use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false |
| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory | true |
Path Smoothing#
| Name | Type | Description | Default value |
|---|---|---|---|
| enable_path_smoothing | boolean | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | false |
| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 25 |
| curvature_smoothing_num_traj | int | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 |
| curvature_smoothing_num_ref_steer | int | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 |
Trajectory Extending#
| Name | Type | Description | Default value |
|---|---|---|---|
| extend_trajectory_for_end_yaw_control | boolean | trajectory extending flag for end yaw control | true |
MPC Optimization#
| Name | Type | Description | Default value |
|---|---|---|---|
| qp_solver_type | string | QP solver option. described below in detail. | "osqp" |
| mpc_prediction_horizon | int | total prediction step for MPC | 50 |
| mpc_prediction_dt | double | prediction period for one step [s] | 0.1 |
| mpc_weight_lat_error | double | weight for lateral error | 1.0 |
| mpc_weight_heading_error | double | weight for heading error | 0.0 |
| mpc_weight_heading_error_squared_vel | double | weight for heading error * velocity | 0.3 |
| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 |
| mpc_weight_steering_input_squared_vel | double | weight for steering error (steer command - reference steer) * velocity | 0.25 |
| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) * velocity | 0.1 |
| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 |
| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.000001 |
| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.1 |
| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 |
| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error * velocity | 0.3 |
| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 1.0 |
| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) * velocity | 0.25 |
| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) * velocity | 0.0 |
| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 |
| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.000001 |
| mpc_low_curvature_thresh_curvature | double | threshold of curvature to use "low_curvature" parameter | 0.0 |
| mpc_weight_terminal_lat_error | double | terminal lateral error weight in matrix Q to improve mpc stability | 1.0 |
| mpc_weight_terminal_heading_error | double | terminal heading error weight in matrix Q to improve mpc stability | 0.1 |
| mpc_zero_ff_steer_deg | double | threshold that feed-forward angle becomes zero | 0.5 |
| mpc_acceleration_limit | double | limit on the vehicle's acceleration | 2.0 |
| mpc_velocity_time_constant | double | time constant used for velocity smoothing | 0.3 |
| mpc_min_prediction_length | double | minimum prediction length | 5.0 |
Vehicle Model#
| Name | Type | Description | Default value |
|---|---|---|---|
| vehicle_model_type | string | vehicle model type for mpc prediction | "kinematics" |
| input_delay | double | steering input delay time for delay compensation | 0.24 |
| vehicle_model_steer_tau | double | steering dynamics time constant (1d approximation) [s] | 0.3 |
| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [40.0, 50.0, 60.0] |
| curvature_list_for_steer_rate_lim | [double] | curvature list for steering angle rate limit interpolation in ascending order [/m] | [0.001, 0.002, 0.01] |
| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [60.0, 50.0, 40.0] |
| velocity_list_for_steer_rate_lim | [double] | velocity list for steering angle rate limit interpolation in ascending order [m/s] | [10.0, 15.0, 20.0] |
| acceleration_limit | double | acceleration limit for trajectory velocity modification [m/ss] | 2.0 |
| velocity_time_constant | double | velocity dynamics time constant for trajectory velocity modification [s] | 0.3 |
Lowpass Filter for Noise Reduction#
| Name | Type | Description | Default value |
|---|---|---|---|
| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 |
| error_deriv_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for error derivative [Hz] | 5.0 |
Stop State#
| Name | Type | Description | Default value |
|---|---|---|---|
| stop_state_entry_ego_speed *1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.001 |
| stop_state_entry_target_speed *1 | double | threshold value of the target speed used to the stop state entry condition | 0.001 |
| converged_steer_rad | double | threshold value of the steer convergence | 0.1 |
| keep_steer_control_until_converged | boolean | keep steer control until steer is converged | true |
| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 |
| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 |
| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.01 |
(*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state.
Steer Offset#
Defined in the steering_offset namespace. This logic is designed as simple as possible, with minimum design parameters.
| Name | Type | Description | Default value |
|---|---|---|---|
| enable_auto_steering_offset_removal | boolean | Estimate the steering offset and apply compensation | true |
| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation | 5.56 |
| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 |
| average_num | int | The average of this number of data is used as a steering offset. | 1000 |
| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 |
For dynamics model (WIP)#
| Name | Type | Description | Default value |
|---|---|---|---|
| cg_to_front_m | double | distance from baselink to the front axle[m] | 1.228 |
| cg_to_rear_m | double | distance from baselink to the rear axle [m] | 1.5618 |
| mass_fl | double | mass applied to front left tire [kg] | 600 |
| mass_fr | double | mass applied to front right tire [kg] | 600 |
| mass_rl | double | mass applied to rear left tire [kg] | 600 |
| mass_rr | double | mass applied to rear right tire [kg] | 600 |
| cf | double | front cornering power [N/rad] | 155494.663 |
| cr | double | rear cornering power [N/rad] | 155494.663 |
Debug#
| Name | Type | Description | Default value |
|---|---|---|---|
| publish_debug_trajectories | boolean | publish predicted trajectory and resampled reference trajectory for debug purpose | true |
How to tune MPC parameters#
Set kinematics information#
First, it's important to set the appropriate parameters for vehicle kinematics. This includes parameters like wheelbase, which represents the distance between the front and rear wheels, and max_steering_angle, which indicates the maximum tire steering angle. These parameters should be set in the vehicle_info.param.yaml.
Set dynamics information#
Next, you need to set the proper parameters for the dynamics model. These include the time constant steering_tau and time delay steering_delay for steering dynamics, and the maximum acceleration mpc_acceleration_limit and the time constant mpc_velocity_time_constant for velocity dynamics.
Confirmation of the input information#
It's also important to make sure the input information is accurate. Information such as the velocity of the center of the rear wheel [m/s] and the steering angle of the tire [rad] is required. Please note that there have been frequent reports of performance degradation due to errors in input information. For instance, there are cases where the velocity of the vehicle is offset due to an unexpected difference in tire radius, or the tire angle cannot be accurately measured due to a deviation in the steering gear ratio or midpoint. It is suggested to compare information from multiple sensors (e.g., integrated vehicle speed and GNSS position, steering angle and IMU angular velocity), and ensure the input information for MPC is appropriate.
MPC weight tuning#
Then, tune the weights of the MPC. One simple approach of tuning is to keep the weight for the lateral deviation (weight_lat_error) constant, and vary the input weight (weight_steering_input) while observing the trade-off between steering oscillation and control accuracy.
Here, weight_lat_error acts to suppress the lateral error in path following, while weight_steering_input works to adjust the steering angle to a standard value determined by the path's curvature. When weight_lat_error is large, the steering moves significantly to improve accuracy, which can cause oscillations. On the other hand, when weight_steering_input is large, the steering doesn't respond much to tracking errors, providing stable driving but potentially reducing tracking accuracy.
The steps are as follows:
- Set
weight_lat_error= 0.1,weight_steering_input= 1.0 and other weights to 0. - If the vehicle oscillates when driving, set
weight_steering_inputlarger. - If the tracking accuracy is low, set
weight_steering_inputsmaller.
If you want to adjust the effect only in the high-speed range, you can use weight_steering_input_squared_vel. This parameter corresponds to the steering weight in the high-speed range.
Descriptions for weights#
weight_lat_error: Reduce lateral tracking error. This acts like P gain in PID.weight_heading_error: Make a drive straight. This acts like D gain in PID.weight_heading_error_squared_vel_coeff: Make a drive straight in high speed range.weight_steering_input: Reduce oscillation of tracking.weight_steering_input_squared_vel_coeff: Reduce oscillation of tracking in high speed range.weight_lat_jerk: Reduce lateral jerk.weight_terminal_lat_error: Preferable to set a higher value than normal lateral weightweight_lat_errorfor stability.weight_terminal_heading_error: Preferable to set a higher value than normal heading weightweight_heading_errorfor stability.
Other tips for tuning#
Here are some tips for adjusting other parameters:
- In theory, increasing terminal weights,
weight_terminal_lat_errorandweight_terminal_heading_error, can enhance the tracking stability. This method sometimes proves effective. - A larger
prediction_horizonand a smallerprediction_sampling_timeare efficient for tracking performance. However, these come at the cost of higher computational costs. - If you want to modify the weight according to the trajectory curvature (for instance, when you're driving on a sharp curve and want a larger weight), use
mpc_low_curvature_thresh_curvatureand adjustmpc_low_curvature_weight_**weights. - If you want to adjust the steering rate limit based on the vehicle speed and trajectory curvature, you can modify the values of
steer_rate_lim_dps_list_by_curvature,curvature_list_for_steer_rate_lim,steer_rate_lim_dps_list_by_velocity,velocity_list_for_steer_rate_lim. By doing this, you can enforce the steering rate limit during high-speed driving or relax it while curving. - In case your target curvature appears jagged, adjusting
curvature_smoothingbecomes critically important for accurate curvature calculations. A larger value yields a smooth curvature calculation which reduces noise but can cause delay in feedforward computation and potentially degrade performance. - Adjusting the
steering_lpf_cutoff_hzvalue can also be effective to forcefully reduce computational noise. This refers to the cutoff frequency in the second order Butterworth filter installed in the final layer. The smaller the cutoff frequency, the stronger the noise reduction, but it also induce operation delay. - If the vehicle consistently deviates laterally from the trajectory, it's most often due to the offset of the steering sensor or self-position estimation. It's preferable to eliminate these biases before inputting into MPC, but it's also possible to remove this bias within MPC. To utilize this, set
enable_auto_steering_offset_removalto true and activate the steering offset remover. The steering offset estimation logic works when driving at high speeds with the steering close to the center, applying offset removal. - If the onset of steering in curves is late, it's often due to incorrect delay time and time constant in the steering model. Please recheck the values of
input_delayandvehicle_model_steer_tau. Additionally, as a part of its debug information, MPC outputs the current steering angle assumed by the MPC model, so please check if that steering angle matches the actual one.
References / External links#
- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", Robotics Institute, Carnegie Mellon University, February 2009.