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):

Trajectory Reference Mode

The trajectory_reference_mode parameter selects how the MPC tracks the reference trajectory:

• spatial (default): the reference trajectory is indexed by distance, and the time step of each prediction point is calculated from distance and velocity.
• temporal: the reference trajectory’s time_from_start field is used directly, and the ego’s position along the trajectory is estimated from a predicted time (based on the previous nearest time and the control period) corrected by an observed nearest time from the current pose.

This parameter is normally declared once at the autoware_trajectory_follower_node level (see trajectory_follower_node.param.yaml) and shared with the longitudinal controller. It is also declared here with the same spatial default so that this node keeps working when run standalone (e.g. in tests). The default value keeps the existing spatial behavior unchanged.

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.

MPC class

File truncated at 100 lines see the full file

Changelog for package autoware_mpc_lateral_controller

0.53.0 (2026-09-29)

• Merge remote-tracking branch 'origin/main' into prepare-0.53.0-changelog

• test(autoware_mpc_lateral_controller): add characterization tests (#13356) test(autoware_mpc_lateral_controller): add characterization tests for MpcLateralController (#13356) MpcLateralController has no test of its own, so the parameter declaration, the vehicle model built from it, the steering offset compensation, the state kept between cycles and the command reset are untested. Later work moves these responsibilities between classes, and pinning the behaviour first makes a change visible. This adds 37 tests that drive the controller through LateralControllerBase and assert on LateralOutput alone. Three of them record a value or pin a behaviour that a later change is meant to break, so they carry the DISABLED_ prefix and stay out of the default run.

• refactor(mpc_lateral_controller): decouple mpc logic from rclcpp::Node (#13090)

  * refactor(autoware_mpc_lateral_controller): decouple QPSolverOSQP from rclcpp::Logger/Clock Change QPSolverInterface::solve() to return a QPSolverResult (success flag and warning message) instead of performing RCLCPP_WARN/RCLCPP_WARN_THROTTLE internally. Logging is now done by the caller in MPC::executeOptimization, allowing QPSolverOSQP to be constructed without a logger or clock.

  * refactor(autoware_mpc_lateral_controller): use tl::expected for MPC helper return values Replace std::pair<ResultWithReason, T> in getData, executeOptimization, resampleMPCTrajectoryByTime, and updateStateForDelayCompensation with tl::expected<T, std::string>, matching the Error-as-Value pattern already used elsewhere in autoware_universe and avoiding the need to construct dummy values on failure.

  * refactor(autoware_mpc_lateral_controller): return predicted trajectory via MpcResult Merge ResultWithReason into MpcResult and add a predicted_trajectory field, so MPC::calculateMPC() returns the predicted trajectory instead of writing it through an output parameter.

  * refactor(autoware_mpc_lateral_controller): return ctrl_cmd via MpcResult Add a ctrl_cmd field to MpcResult so MPC::calculateMPC() returns the computed control command instead of writing it through an output parameter.

  * refactor(autoware_mpc_lateral_controller): move diagnostic into MpcResult Add a Float32MultiArrayStamped field to MpcResult and remove the diagnostic output parameter from calculateMPC, so callers get the diagnostic data as part of the returned result instead of via a mutable reference argument.

  * refactor(autoware_mpc_lateral_controller): move ctrl_cmd_horizon into MpcResult Add a LateralHorizon field to MpcResult and remove the ctrl_cmd_horizon output parameter from calculateMPC, so callers get the control command horizon as part of the returned result instead of via a mutable reference argument.

  • refactor(autoware_mpc_lateral_controller): change fields order

  * refactor(autoware_mpc_lateral_controller): publish Frenet predicted trajectory from MpcLateralController::run() Move the Frenet-coordinate predicted trajectory out of MPC's internal debug publisher and expose it via MpcResult::debug_msgs instead, so MpcLateralController::run() is responsible for publishing it alongside the other MPC outputs.

  * refactor(autoware_mpc_lateral_controller): publish resampled reference trajectory from MpcLateralController::run() Move the resampled reference trajectory debug message creation out of MPC::resampleMPCTrajectoryByTime and into MPC::calculateMPC, exposing it via MpcResult::debug_msgs so MpcLateralController::run() publishes it alongside the other debug topics.

  * refactor(autoware_mpc_lateral_controller): publish nearest pose from MpcLateralController::run() Move the nearest pose debug message creation out of MPC::publishNearestDebug and into MPC::calculateMPC, exposing it via MpcResult::debug_msgs so MpcLateralController::run() publishes it alongside the other debug topics.

  * refactor(autoware_mpc_lateral_controller): publish nearest segment trajectory from MpcLateralController::run() Extract the ego-nearest segment index computation and trajectory construction out of MPC::publishNearestDebug and into MPC::getData, storing the result in MPCData/MpcResult::debug_msgs so MpcLateralController::run() publishes it alongside the other debug topics. MPC::publishNearestDebug keeps only the nearest_info publish, reusing the already-computed segment indices.

  * refactor(autoware_mpc_lateral_controller): make buildNearestInfoMessage a member function returning the debug message Turn MPC::publishNearestDebug into MPC::buildNearestInfoMessage, a private member function that builds

File truncated at 100 lines see the full file