PID Longitudinal Controller

Purpose / Use cases

The longitudinal_controller computes the target acceleration to achieve the target velocity set at each point of the target trajectory using a feed-forward/back control.

It also contains a slope force correction that takes into account road slope information, and a delay compensation function. It is assumed that the target acceleration calculated here will be properly realized by the vehicle interface.

Note that the use of this module is not mandatory for Autoware if the vehicle supports the “target speed” interface.

Design / Inner-workings / Algorithms

States

This module has four state transitions as shown below in order to handle special processing in a specific situation.

• DRIVE
  • Executes target velocity tracking by PID control.
  • It also applies the delay compensation and slope compensation.
• STOPPING
  • Controls the motion just before stopping.
  • Special sequence is performed to achieve accurate and smooth stopping.
• STOPPED
  • Performs operations in the stopped state (e.g. brake hold)
• EMERGENCY.
  • Enters an emergency state when certain conditions are met (e.g., when the vehicle has crossed a certain distance of a stop line).
  • The recovery condition (whether or not to keep emergency state until the vehicle completely stops) or the deceleration in the emergency state are defined by parameters.

The state transition diagram is shown below.

Logics

Control Block Diagram

FeedForward (FF)

The reference acceleration set in the trajectory and slope compensation terms are output as a feedforward. Under ideal conditions with no modeling error, this FF term alone should be sufficient for velocity tracking.

Tracking errors causing modeling or discretization errors are removed by the feedback control (now using PID).

Brake keeping

From the viewpoint of ride comfort, stopping with 0 acceleration is important because it reduces the impact of braking. However, if the target acceleration when stopping is 0, the vehicle may cross over the stop line or accelerate a little in front of the stop line due to vehicle model error or gradient estimation error.

For reliable stopping, the target acceleration calculated by the FeedForward system is limited to a negative acceleration when stopping.

Slope compensation

Based on the slope information, a compensation term is added to the target acceleration.

There are two sources of the slope information, which can be switched by a parameter.

• Pitch of the estimated ego-pose (default)
  • Calculates the current slope from the pitch angle of the estimated ego-pose
  • Pros: Easily available
  • Cons: Cannot extract accurate slope information due to the influence of vehicle vibration.
• Z coordinate on the trajectory
  • Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory
  • Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained
  • Pros: Can be used in combination with delay compensation (not yet implemented)
  • Cons: z-coordinates of high-precision map is needed.
  • Cons: Does not support free space planning (for now)

We also offer the options to switch between these, depending on driving conditions.

Notation: This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system.

This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller. For instance, if the vehicle is attempting to start with an acceleration of 1.0 m/s^2 and a gravity correction of -1.0 m/s^2 is applied, the output value will be 0. If this output value is mistakenly treated as the target acceleration, the vehicle will not start.

A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise.

Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition.

PID control

For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system.

This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity.

This PID logic has a maximum value for the output of each term. This is to prevent the following:

• Large integral terms may cause unintended behavior by users.
• Unintended noise may cause the output of the derivative term to be very large.

Note: by default, the integral term in the control system is not accumulated when the vehicle is stationary. This precautionary measure aims to prevent unintended accumulation of the integral term in scenarios where Autoware assumes the vehicle is engaged, but an external system has immobilized the vehicle to initiate startup procedures.

However, certain situations may arise, such as when the vehicle encounters a depression in the road surface during startup or if the slope compensation is inaccurately estimated (lower than necessary), leading to a failure to initiate motion. To address these scenarios, it is possible to activate error integration even when the vehicle is at rest by setting the enable_integration_at_low_speed parameter to true.

When enable_integration_at_low_speed is set to true, the PID controller will initiate integration of the acceleration error after a specified duration defined by the time_threshold_before_pid_integration parameter has elapsed without the vehicle surpassing a minimum velocity set by the current_vel_threshold_pid_integration parameter.

File truncated at 100 lines see the full file

Changelog for package autoware_pid_longitudinal_controller

0.53.0 (2026-09-29)

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

• fix(pid_long): fix slope sign (#13247)

• refactor(pid_logitudinal_controller): cleanup core logic (#13071)

  * refactor(autoware_pid_longitudinal_controller): remove unused m_current_time member m_current_time was assigned once per run() but never read anywhere in the class.

  * refactor(autoware_pid_longitudinal_controller): remove unused m_prev_nearest_time member m_prev_nearest_time was only ever written to (set or reset) and never read, so it had no effect on behavior.

  * refactor(autoware_pid_longitudinal_controller): pass is_steer_converged as parameter is_steer_converged was cached as a member for one control cycle only to be read inside updateControlState(), the sole function that used it. Pass it as a parameter to updateControlState() instead of holding it as class state.

  * refactor(autoware_pid_longitudinal_controller): remove unimplemented member function declarations getCurrentMotion() and calcFilteredAcc() were declared in the header but never defined or called anywhere in the class.

  * refactor(autoware_pid_longitudinal_controller): remove redundant m_received_invalid_trajectory member The value was only set and read within a single run() call, so it can be a local variable derived directly from is_valid_trajectory() instead of a persistent member.

  * refactor(autoware_pid_longitudinal_controller): extract debug/slope message creation into free helper functions Move the inline debug_message and slope_message construction in run() into createDebugMessage() and createSlopeMessage() free functions.

  * refactor(autoware_pid_longitudinal_controller): return emergency stop reason from updateControlState Have changeControlState() and updateControlState() return the emergency stop reason instead of writing it to the m_emergency_stop_reason member, removing the member variable and the reset-before-use pattern in run().

  * refactor(autoware_pid_longitudinal_controller): add const to side-effect-free member functions Moves the m_prev_ctrl_cmd update out of createCtrlCmdMsg (which should only format the output message) and into calcCtrlCmd where the state transition belongs, allowing createCtrlCmdMsg and getTimeUnderControl to be marked const.

  * refactor(autoware_pid_longitudinal_controller): make createCtrlCmdMsg a free helper function Converts createCtrlCmdMsg into a free function in the anonymous namespace, matching createDebugMessage/createSlopeMessage, since it does not need access to any member state. Argument order places current_time first to align with the other message-creation helpers.

  * fix(autoware_pid_longitudinal_controller): avoid clock-type mismatch in delay-compensation time arithmetic PidLongitudinalController compared current_time (whose clock type is caller-supplied) against timestamps implicitly converted from builtin_interfaces::msg::Time, which defaults to RCL_ROS_TIME. When current_time used a different clock type, rclcpp::Time subtraction threw "can't subtract times with different time sources". Replace the ROS-message-backed m_ctrl_cmd_vec with a dedicated TimestampedAcceleration struct that stores rclcpp::Time directly, so stored stamps always share current_time's clock type without needing an explicit conversion. The remaining message-derived timestamp (trajectory header stamp) is now converted using current_time.get_clock_type() instead of the implicit RCL_ROS_TIME default.

  * test(autoware_pid_longitudinal_controller): drop clock-type overrides now that current_time flows through unchanged make_time() no longer needs an rcl_clock_type_t parameter, and no test needs to force RCL_ROS_TIME, since PidLongitudinalController no longer implicitly assumes RCL_ROS_TIME for message-derived timestamps. ---------Co-authored-by: Takahisa.Ishikawa <<takahisa.ishikawa@tier4.jp>>

• test(pid_longitudinal_controller): add unit tests (#13064)

  • test(autoware_pid_longitudinal_controller): add unit tests for PidLongitudinalController

  * test(autoware_pid_longitudinal_controller): trim redundant PidLongitudinalController test cases Removes several overlapping/redundant test cases while keeping the temporal-trajectory coverage (TemporalTrajectoryProducesValidCommand and its make_temporal_trajectory helper), which exercises the use_temporal_trajectory branch in getControlData.

  * test(autoware_pid_longitudinal_controller): assert control_cmd

File truncated at 100 lines see the full file