Bug report - Controller bugs that would affect control performance

Overview

Thanks for maintaining the project. Recently we have identified several bugs in the Apollo controller (mainly in MPC) that would affect the controller performance. Here we detail these bugs we found, hoping to improve the quality of this project.

We have tried to fix the bugs we found and conduct experiments in Apollo-Carla co-simulation. The fixed controller code has been acknowledged by the official maintainer of the simulation bridge and merged into the project (https://github.com/guardstrikelab/carla_apollo_bridge/blob/master/docs/RefinedController.md). We experimentally demonstrated that the controller performance improved after fixing the bugs.

We guess the controller bugs were largely ignored because the famous Apollo-LGSVL co-simulation does not use the Apollo controller (LGSVL directly transport vehicle perfectly according to the planning output). However, LGSVL has long been abandoned. Moreover, Carla, as one of the mose competetive open-source simulator, seems not well-connected to the competetive open-source ADS Apollo.

We hope these bugs could be examined by the Apollo official team and better fixed. We believe that they are not only helpful for Carla co-simulation, but also helpful for Apollo itself.

Many thanks for the attention!

Detailed bugs

Followings are the detailed description of each bug, and the suggestions on fixing. There is also an already refined version for reference.

Bug.01: Problematic calibration table.

The calibration_table configuration file is the key component in control module. It reflects at a certain speed, to achieve a particular accerlation, what throttle value (command) should be applied. However, there are the following bugs in the provided calibration_table.

First, in many places, a positive accerlation is mapped to a negative throttle, or vice versa, which violate the physical rules. This is an error because when you want acceleration, you would not press brake (nagative command). Example - wrong mapping 1:
```
calibration {
speed: 0.0
acceleration: 0.41
command: -13.0
}
```
Second, sometimes a higher accerlation is mapped to a lower command, which also violate physical rules. For example, in (wrong mapping 2), accerlation=-1.22 needs a brake=-25%, but accerlation=-0.87 needs a brake=-33%:

calibration {
  speed: 0.2
  acceleration: -1.22
  command: -25.0
}
calibration {
  speed: 0.2
  acceleration: -0.87
  command: -33.0
}

Third, we found that overall, the provided mapping relation would fail to accelerate the car effectively, and significantly lacks smoothness.

Suggestion: Overall, althought this configuration file is just for demonstration, I think it necessary to fix the factoral error within, which will provide more smooth performance in co-simulation such as with Carla. We have fixed the above problems and constructed a better calibration table in refined calibration table for reference (optimized for Apollo 8.0+Carla simulation).

Bug.02: Redundant filtering.

At line 555 of the mpc_controller, the steer angle is filtered once. However, out of the code body of the if condition, this value is filtered again at line 559. There is no reason to filter twice and this can be a bug. It would be sufficient to just keep the filtering operation outside the if condition code body (line 559).

    double steer_angle_limited =
        common::math::Clamp(steer_angle, -steer_limit, steer_limit);
    steer_angle_limited = digital_filter_.Filter(steer_angle_limited);
    steer_angle = steer_angle_limited;
    debug->set_steer_angle_limited(steer_angle_limited);
  }
  steer_angle = digital_filter_.Filter(steer_angle);

Suggestion: delete redundant filtering in mpc_controller.

Bug.03: Inconsistent querying of target following point.

In the current mpc controller code, the target point (that the controller will follow) is quried using different methods for computing tha lateral error and longtitudinal error, respectively. Specifically, the lateral error is calculated using QueryNearestPointByPosition (query by position):

  const auto matched_point =
      trajectory_analyzer.QueryNearestPointByPosition(x, y);

However, the longtitudinal error is calculated using QueryNearestPointByAbsoluteTime (query by time):

  TrajectoryPoint reference_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(
          current_control_time);

Such implementation makes sense when using the LQR+PID control, in which case two controller functions in an individual manner. However, for MPC control, it will calculate the best controller output based ONE target following point, but the above implementation could lead to TWO different points selected for error calculation. In this case, the MPC controller would following a none-existing planning point, leading to potential performance downgradation.

Suggestion: Make sure lateral error and longtitudinal error are calculated based on one target point that exists in the planning trajectory. This can be done by using QueryNearestPointByPosition only or QueryNearestPointByAbsoluteTime only.

Bug.04: Lack of looking ahead time.

When using QueryNearestPointByAbsoluteTime (query by time), there is no looking ahead time added after the querying time (the same bug is observed in the lon_controller):

  TrajectoryPoint reference_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(
          current_control_time);

While in lat_controller, the looking ahead time is correctly added after current time:

    target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
        Clock::NowInSeconds() + query_relative_time_);

Failed to add a looking ahead time could lead to the selection of a target point that is temporally too close, reducing control performance.

Suggestion: Make sure the looking ahead time is correctly added when using QueryNearestPointByAbsoluteTime.

Bug.05: Problematic base time for querying.

When using QueryNearestPointByAbsoluteTime, the target point is selected based on the current program running time ((query by time)):

  const double current_control_time = Clock::NowInSeconds();

  TrajectoryPoint reference_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(
          current_control_time);

However, the selection of the target point in planning trajectory should be based on the real-time state of the vehicle. This is because the controller is computing output based on both the vehicle state and planning trajectory, and the control output needs to be updated only when either of them has changed.

In our experiments, we found that the frequency of state update is 20Hz and planning is 10Hz, while the controller is running in 100Hz, which is significantly higher than state updare or planning.

Suggestion: Using the state update time as the timestamp for temporal querying:

  TrajectoryPoint reference_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(
          injector_->vehicle_state()->timestamp() + looking_ahead_time);

Bug.06: Redundant feedback on controller output.

There are redundant feedbacks on the final controller output. For example, for the final acceleration command (acceleration command):

  double acceleration_cmd =
      acc_feedback + debug->acceleration_reference() +
      control_conf_.enable_slope_offset() * debug->slope_offset_compensation();

The final value acceleration_cmd is consist of three parts, in which only the acc_feedback is calculated by MPC. Slope compensation is reasonalbe, but debug->acceleration_reference() is purely the acceleration of the acceleration of target point (line 955):

  debug->set_acceleration_reference(reference_point.a());

This is a very big acceleration, and should not be directly added to the final MPC output, which will significantly weaken the MPC's utility.

The same problem is observed in lateral control, in which only the first variable is the MPC output:

  double steer_angle =
      steer_angle_feedback + steer_angle_feedforwardterm_updated_ +
      steer_angle_ff_compensation + steer_angle_feedback_augment;

Suggestion: Delete redundant feedbacks to make sure MPC controller work properly.

ApolloAuto / apollo