Skip to content

MPC Lateral Controller#

This is the design document for the lateral controller node in the 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 used for the yaw and lateral errors used as input of the MPC as well as for 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_auto_planning_msgs/Trajectory : reference trajectory to follow.
  • nav_msgs/Odometry: current odometry
  • autoware_auto_vehicle_msgs/SteeringReport current steering

Outputs#

Return LateralOutput which contains the following to the controller node

  • autoware_auto_control_msgs/AckermannLateralCommand
  • LateralSyncData
    • steer angle convergence

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.

Name Type Description Default value
show_debug_info bool display debug info false
traj_resample_dist double distance of waypoints in resampling [m] 0.1
enable_path_smoothing bool path smoothing flag. This should be true when uses path resampling to reduce resampling noise. true
path_filter_moving_ave_num int number of data points moving average filter for path smoothing 35
path_smoothing_times int number of times of applying path smoothing filter 1
curvature_smoothing_num_ref_steer double 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. 35
curvature_smoothing_num_traj double index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. 1
extend_trajectory_for_end_yaw_control bool trajectory extending flag for end yaw control. true
steering_lpf_cutoff_hz double cutoff frequency of lowpass filter for steering output command [hz] 3.0
admissible_position_error double stop vehicle when following position error is larger than this value [m]. 5.0
admissible_yaw_error_rad double stop vehicle when following yaw angle error is larger than this value [rad]. 1.57
stop_state_entry_ego_speed *1 double threshold value of the ego vehicle speed used to the stop state entry condition 0.0
stop_state_entry_target_speed *1 double threshold value of the target speed used to the stop state entry condition 0.0
converged_steer_rad double threshold value of the steer convergence 0.1
keep_steer_control_until_converged bool 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.3

(*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state.

MPC algorithm#

Name Type Description Default value
qp_solver_type string QP solver option. described below in detail. unconstraint_fast
mpc_vehicle_model_type string vehicle model option. described below in detail. kinematics
mpc_prediction_horizon int total prediction step for MPC 70
mpc_prediction_sampling_time double prediction period for one step [s] 0.1
mpc_weight_lat_error double weight for lateral error 0.1
mpc_weight_heading_error double weight for heading error 0.0
mpc_weight_heading_error_squared_vel_coeff double weight for heading error * velocity 5.0
mpc_weight_steering_input double weight for steering error (steer command - reference steer) 1.0
mpc_weight_steering_input_squared_vel_coeff 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.0
mpc_weight_terminal_lat_error double terminal cost weight for lateral error 1.0
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.0
mpc_weight_terminal_heading_error double terminal cost weight for heading error 0.1
mpc_low_curvature_thresh_curvature double trajectory curvature threshold to change the weight. If the curvature is lower than this value, the low_curvature_weight_** values will be used. 0.0
mpc_low_curvature_weight_lat_error double [used in a low curvature trajectory] weight for lateral error 0.0
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.0
mpc_low_curvature_weight_steering_input double [used in a low curvature trajectory] weight for steering error (steer command - reference steer) 0.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.0
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.0
mpc_zero_ff_steer_deg double threshold of feedforward angle [deg]. feedforward angle smaller than this value is set to zero. 2.0

Steering offset remover#

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 bool 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 double 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

Vehicle model#

Name Type Description Default value
input_delay double steering input delay time for delay compensation 0.24
vehicle_model_steer_tau double steering dynamics time constant 0.3
steer_rate_lim_dps_list_by_curvature [double] steering angle rate limit list depending on curvature [deg/s] [10.0, 20.0, 30.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] [40.0, 30.0, 20.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
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

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:

  1. Set weight_lat_error = 0.1, weight_steering_input = 1.0 and other weights to 0.
  2. If the vehicle oscillates when driving, set weight_steering_input larger.
  3. If the tracking accuracy is low, set weight_steering_input smaller.

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 weight weight_lat_error for stability.
  • weight_terminal_heading_error: Preferable to set a higher value than normal heading weight weight_heading_error for stability.

Other tips for tuning#

Here are some tips for adjusting other parameters:

  • In theory, increasing terminal weights, weight_terminal_lat_error and weight_terminal_heading_error, can enhance the tracking stability. This method sometimes proves effective.
  • A larger prediction_horizon and a smaller prediction_sampling_time are 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_curvature and adjust mpc_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_smoothing becomes 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_hz value 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_removal to 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_delay and vehicle_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.
  • [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", Robotics Institute, Carnegie Mellon University, February 2009.