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 odometry
• autoware_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

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/system.json”) }}

Path Smoothing

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/path_smoothing.json”) }}

Trajectory Extending

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/trajectory_extending.json”) }}

MPC Optimization

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/mpc_optimization.json”) }}

Vehicle Model

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/vehicle_model.json”) }}

Lowpass Filter for Noise Reduction

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/lowpass_filter.json”) }}

Stop State

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/stop_state.json”) }}

(stop_state_entry_ego_speed and stop_state_entry_target_speed) 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.

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/steering_offset.json”) }}

For dynamics model (WIP)

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/dynamics_model.json”) }}

publish debug predicted trajectory in Frenet coordinate

{{ json_to_markdown(“control/autoware_mpc_lateral_controller/schema/sub/debug_publish.json”) }}

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:

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.

References / External links

• [1] Jarrod M. Snider, “Automatic Steering Methods for Autonomous Automobile Path Tracking”, Robotics Institute, Carnegie Mellon University, February 2009.

Related issues

Changelog for package autoware_mpc_lateral_controller

0.44.0 (2025-04-18)

• Merge remote-tracking branch 'origin/main' into humble
• fix(control): apply THROTTLE to frequent log (#10418)
  • fix(control): apply THROTTLE to frequent log
  • fix
  • fix
  • fix

  * fix

• refactor(mpc_lateral_controller): rework parameter (#8935)
• Contributors: Prakash Kannaiah, Ryohsuke Mitsudome, Takayuki Murooka

0.43.0 (2025-03-21)

• Merge remote-tracking branch 'origin/main' into chore/bump-version-0.43
• chore: rename from [autoware.universe]{.title-ref} to [autoware_universe]{.title-ref} (#10306)
• feat(path_optimizer): additional failure logging and failure mode handling (#10276) MRM when MPT fails
• fix(autoware_mpc_lateral_controller): replace Eigen::VectorXd with Eigen::Vector3d for state representation (#10235)
  • fix(autoware_mpc_lateral_controller): replace Eigen::VectorXd with Eigen::Vector3d for state representation

  * docs(autoware_mpc_lateral_controller): update comments for state representation and discretization considerations ---------

• chore(mpc_lateral_controller): add package maintainer (#10239) add package maintainer
• Contributors: Arjun Jagdish Ram, Hayato Mizushima, Kyoichi Sugahara, Yutaka Kondo, mkquda

0.42.0 (2025-03-03)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat(autoware_utils): replace autoware_universe_utils with autoware_utils (#10191)
• Contributors: Fumiya Watanabe, 心刚

0.41.2 (2025-02-19)

• chore: bump version to 0.41.1 (#10088)
• Contributors: Ryohsuke Mitsudome

0.41.1 (2025-02-10)

0.41.0 (2025-01-29)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (#9846)
  • feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files ontrol/autoware_mpc_lateral_controller

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com>

• chore(autoware_mpc_lateral_controller): fix formula description in vehicle_model_bicycle_kinematics.hpp (#8971) fix formula description in vehicle_model_bicycle_kinematics.hpp
• fix(mpc_lateral_controller): prevent unstable steering command while stopped (#9690)
  • modify logic of function isStoppedState
  • use a constant distance margin instead of wheelbase length

  * add comment to implementation ---------

• feat(mpc_lateral_controller): remove trans/rot deviation validation since the control_validator has the same feature (#9684)
• docs: modified minor sign error (#8140)
• Contributors: Autumn60, Fumiya Watanabe, Takayuki Murooka, Vishal Chauhan, Yuki Kimura, mkquda

0.40.0 (2024-12-12)

• Merge branch 'main' into release-0.40.0
• Revert "chore(package.xml): bump version to 0.39.0 (#9587)" This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.
• fix: fix ticket links in CHANGELOG.rst (#9588)
• chore(package.xml): bump version to 0.39.0 (#9587)
  • chore(package.xml): bump version to 0.39.0
  • fix: fix ticket links in CHANGELOG.rst

  * fix: remove unnecessary diff ---------Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

• fix: fix ticket links in CHANGELOG.rst (#9588)
• fix(cpplint): include what you use - control (#9565)
• fix(autoware_mpc_lateral_controller): fix clang-tidy errors (#9436)
• 0.39.0
• update changelog
• Merge commit '6a1ddbd08bd' into release-0.39.0
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• feat(mpc_lateral_controller): suppress rclcpp_warning/error (#9382)
  • feat(mpc_lateral_controller): suppress rclcpp_warning/error
  • fix

  * fix test ---------

• fix(autoware_mpc_lateral_controller): fix variableScope (#9390)
• feat: suppress warning/error of the empty predicted trajectory by MPC (#9373)
• chore(autoware_mpc_lateral_controller): add maintainer (#9374)
• feat(trajectory_follower): publsih control horzion (#8977)
  • feat(trajectory_follower): publsih control horzion
  • fix typo
  • rename functions and minor refactor
  • add option to enable horizon pub
  • add tests for horizon
  • update docs

  * rename to ~/debug/control_cmd_horizon ---------

• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• fix(autoware_mpc_lateral_controller): fix bugprone-misplaced-widening-cast (#9224)
  • fix: bugprone-misplaced-widening-cast

  * fix: consider negative values ---------

• chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
  • unify package.xml version to 0.37.0
  • remove system_monitor/CHANGELOG.rst
  • add changelog

  * 0.38.0

• fix(mpc_lateral_controller): correctly resample the MPC trajectory yaws (#9199)
• Contributors: Esteve Fernandez, Fumiya Watanabe, Kosuke Takeuchi, Kyoichi Sugahara, M. Fatih Cırıt, Maxime CLEMENT, Ryohsuke Mitsudome, Ryuta Kambe, Takayuki Murooka, Yutaka Kondo, kobayu858

0.39.0 (2024-11-25)

• Merge commit '6a1ddbd08bd' into release-0.39.0
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• fix(autoware_mpc_lateral_controller): fix bugprone-misplaced-widening-cast (#9224)
  • fix: bugprone-misplaced-widening-cast

  * fix: consider negative values ---------

• chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
  • unify package.xml version to 0.37.0
  • remove system_monitor/CHANGELOG.rst
  • add changelog

  * 0.38.0

• fix(mpc_lateral_controller): correctly resample the MPC trajectory yaws (#9199)
• Contributors: Esteve Fernandez, Maxime CLEMENT, Yutaka Kondo, kobayu858

0.38.0 (2024-11-08)

• unify package.xml version to 0.37.0
• refactor(osqp_interface): added autoware prefix to osqp_interface (#8958)

• fix(autoware_mpc_lateral_controller): fix calculation method of predicted trajectory (#9048) * fix(vehicle_model): fix calculation method of predicted trajectory ---------

• refactor(autoware_interpolation): prefix package and namespace with autoware (#8088) Co-authored-by: kosuke55 <<kosuke.tnp@gmail.com>>
• chore(mpc_lateral_controller): consistent parameters with autoware_launch (#8914)
• chore: remove duplicate line in mpc_lateral_controller.cpp (#8916) remove duplicate line in mpc_lateral_controller.cpp
• feat(autoware_mpc_lateral_controller): add predicted trajectory acconts for input delay (#8436)
  • feat: enable delayed initial state for predicted trajectory

  * feat: enable debug publishing of predicted and resampled reference trajectories ---------

• fix(autoware_mpc_lateral_controller): fix cppcheck warnings (#8149)
  • fix(autoware_mpc_lateral_controller): fix cppcheck warnings

  * Update control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp Co-authored-by: Takayuki Murooka <<takayuki5168@gmail.com>> ---------Co-authored-by: Takayuki Murooka <<takayuki5168@gmail.com>>

• fix(autoware_mpc_lateral_controller): add timestamp and frame ID to published trajectory (#8164) add timestamp and frame ID to published trajectory
• fix(controller): revival of dry steering (#7903) * Revert "fix(autoware_mpc_lateral_controller): delete the zero speed constraint (#7673)" This reverts commit 69258bd92cb8a0ff8320df9b2302db72975e027f.
  • dry steering
  • add comments

  * add minor fix and modify unit test for dry steering ---------

• fix(autoware_mpc_lateral_controller): delete the zero speed constraint (#7673)
  • delete steer rate limit when vel = 0
  • delete unnecessary variable

  * pre-commit ---------

• fix(autoware_mpc_lateral_controller): relax the steering rate constraint at zero speed (#7581)
  • constraint for zero velocity updated

  * correct the comment ---------

• fix(autoware_mpc_lateral_controller): fix duplicateExpression warning (#7542)
  • fix(autoware_mpc_lateral_controller): fix duplicateExpression warning

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com>

• fix(autoware_mpc_lateral_controller): fix duplicateAssignExpression warning (#7572)
• refactor(universe_utils/motion_utils)!: add autoware namespace (#7594)
• fix(mpc_lateral_controller): align the MPC steering angle when the car is controlled manually. (#7109)
  • align the MPC steering angle when the car is controlled manually.
  • update the condition for is_driving_manually
  • STOP mode included
  • comment the is_driving_manually
  • align the steering outside (after) the solver.
  • use the flag input_data.current_operation_mode.is_autoware_control_enabled
  • correct a typo
  • correct the under control condition check
  • undo the space delete
  • unchange the unrelevant line

  * pre-commit ---------

• feat(mpc_lateral_controller): signal a MRM when MPC fails. (#7016)
  • mpc fail checker diagnostic added
  • fix some scope issues
  • member attribute added.
  • shared pointer added.
  • member attribute (diag_updater_) added
  • dependency added.
  • implementation of the MpcLateralController corrected!
  • typo in comment corrected!
  • member method argument corrected

  * delete unnecessary reference mark Co-authored-by: Takamasa Horibe <<horibe.takamasa@gmail.com>>

  • rebase
  • correct the include

  * pre-commit ---------Co-authored-by: Takamasa Horibe <<horibe.takamasa@gmail.com>>

• refactor(motion_utils)!: add autoware prefix and include dir (#7539) refactor(motion_utils): add autoware prefix and include dir
• feat(autoware_universe_utils)!: rename from tier4_autoware_utils (#7538) Co-authored-by: kosuke55 <<kosuke.tnp@gmail.com>>
• refactor(control)!: refactor directory structures of the trajectory followers (#7521)
  • control_traj
  • add follower_node

  * fix

• refactor(trajectory_follower_node): trajectory follower node add autoware prefix (#7344)
  • rename trajectory follower node package
  • update dependencies, launch files, and README files
  • fix formats

  * remove autoware_ prefix from launch arg option ---------

• refactor(trajectory_follower_base): trajectory follower base add autoware prefix (#7343)
  • rename trajectory follower base package
  • update dependencies and includes

  * fix formats ---------

• refactor(vehicle_info_utils)!: prefix package and namespace with autoware (#7353)
  • chore(autoware_vehicle_info_utils): rename header
  • chore(bpp-common): vehicle info
  • chore(path_optimizer): vehicle info
  • chore(velocity_smoother): vehicle info
  • chore(bvp-common): vehicle info
  • chore(static_centerline_generator): vehicle info
  • chore(obstacle_cruise_planner): vehicle info
  • chore(obstacle_velocity_limiter): vehicle info
  • chore(mission_planner): vehicle info
  • chore(obstacle_stop_planner): vehicle info
  • chore(planning_validator): vehicle info
  • chore(surround_obstacle_checker): vehicle info
  • chore(goal_planner): vehicle info
  • chore(start_planner): vehicle info
  • chore(control_performance_analysis): vehicle info
  • chore(lane_departure_checker): vehicle info
  • chore(predicted_path_checker): vehicle info
  • chore(vehicle_cmd_gate): vehicle info
  • chore(obstacle_collision_checker): vehicle info
  • chore(operation_mode_transition_manager): vehicle info
  • chore(mpc): vehicle info
  • chore(control): vehicle info
  • chore(common): vehicle info
  • chore(perception): vehicle info
  • chore(evaluator): vehicle info
  • chore(freespace): vehicle info
  • chore(planning): vehicle info
  • chore(vehicle): vehicle info
  • chore(simulator): vehicle info
  • chore(launch): vehicle info
  • chore(system): vehicle info
  • chore(sensing): vehicle info

  * fix(autoware_joy_controller): remove unused deps ---------

• refactor(mpc_lateral_controller, trajectory_follower_node)!: prefix package and namespace with autoware (#7306)
  • add the prefix to the folder
  • named to autoware_mpc_lateral_controller
  • rename the folder in the include
  • correct the package name in xml and CMakeLists
  • correct the namespace and include
  • change namespace and include in src/
  • change namespace and include in test/
  • fix the trajectory_follower_node
  • undo rename to the namespace
  • change the trajectory_follower_node, Controller.drawio.svg, and README.md
  • fixed by pre-commit

  * suppress the unnecessary line length detect ---------

• Contributors: Autumn60, Esteve Fernandez, Kosuke Takeuchi, Kyoichi Sugahara, Ryuta Kambe, Satoshi OTA, Takayuki Murooka, Yuki TAKAGI, Yutaka Kondo, Zhe Shen, mkquda

0.26.0 (2024-04-03)