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 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_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.

System

Path Smoothing

File truncated at 100 lines see the full file