fix(trajectory_follower_nodes): mpc_follower does not send proper con…

…verged data under low steering rate limit (#2454)
autowarefoundation · Jun 2, 2023 · 9ca4ce4 · 9ca4ce4
1 parent dc76ed4
commit 9ca4ce4
Show file tree

Hide file tree

Showing 4 changed files with 118 additions and 14 deletions.
diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md
@@ -34,12 +34,15 @@ For the optimization, a Quadratic Programming (QP) solver is used and two option
 <!-- cspell: ignore ADMM -->
 
 - unconstraint_fast : use least square method to solve unconstraint QP with eigen.
-- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) algorithm (for more details see the related papers at the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section):
+- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html)
+  algorithm (for more details see the related papers at
+  the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section):
 
 ### Filtering
 
 Filtering is required for good noise reduction.
-A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as input of the MPC as well as for
+A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as
+input of the MPC as well as for
 the output steering angle.
 Other filtering methods can be considered as long as the noise reduction performances are good
 enough.
@@ -105,6 +108,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | keep_steer_control_until_converged           | bool   | keep steer control until steer is converged                                                                                                       | true          |
 | new_traj_duration_time                       | double | threshold value of the time to be considered as new trajectory                                                                                    | 1.0           |
 | new_traj_end_dist                            | double | threshold value of the distance between trajectory ends to be considered as new trajectory                                                        | 0.3           |
+| mpc_converged_threshold_rps                  | double | threshold value to be sure output of the optimization is converged, it is used in stopped state                                                   | 0.3           |
 
 (\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state.
 
@@ -143,11 +147,13 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 ### How to tune MPC parameters
 
 1. Set appropriate vehicle kinematics parameters for distance to front and rear axle and max steer angle.
-   Also check that the input `VehicleKinematicState` has appropriate values (speed: vehicle rear-wheels-center velocity [km/h], angle: steering (tire) angle [rad]).
+   Also check that the input `VehicleKinematicState` has appropriate values (speed: vehicle rear-wheels-center
+   velocity [km/h], angle: steering (tire) angle [rad]).
    These values give a vehicle information to the controller for path following.
    Errors in these values cause fundamental tracking error.
 
-2. Set appropriate vehicle dynamics parameters of `steering_tau`, which is the approximated delay from steering angle command to actual steering angle.
+2. Set appropriate vehicle dynamics parameters of `steering_tau`, which is the approximated delay from steering angle
+   command to actual steering angle.
 
 3. Set `weight_steering_input` = 1.0, `weight_lat_error` = 0.1, and other weights to 0.
    If the vehicle oscillates when driving with low speed, set `weight_lat_error` smaller.
@@ -156,7 +162,8 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
    One of the simple way for tuning is to increase `weight_lat_error` until oscillation occurs.
    If the vehicle is unstable with very small `weight_lat_error`, increase terminal weight :
    `weight_terminal_lat_error` and `weight_terminal_heading_error` to improve tracking stability.
-   Larger `prediction_horizon` and smaller `prediction_sampling_time` is effective for tracking performance, but it is a trade-off between computational costs.
+   Larger `prediction_horizon` and smaller `prediction_sampling_time` is effective for tracking performance, but it is a
+   trade-off between computational costs.
    Other parameters can be adjusted like below.
 
 - `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID.
@@ -165,8 +172,10 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 - `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.
+- `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.
 
 ## References / External links
 

diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp
@@ -47,6 +47,7 @@
 #include <deque>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace autoware::motion::control::mpc_lateral_controller
@@ -99,16 +100,32 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   double m_stop_state_entry_ego_speed;
   double m_stop_state_entry_target_speed;
   double m_converged_steer_rad;
-  double m_new_traj_duration_time;  // check trajectory shape change
-  double m_new_traj_end_dist;       // check trajectory shape change
+  double m_mpc_converged_threshold_rps;  // max mpc output change threshold for 1 sec
+  double m_new_traj_duration_time;       // check trajectory shape change
+  double m_new_traj_end_dist;            // check trajectory shape change
   bool m_keep_steer_control_until_converged;
 
+  /* parameter to store the actual steering rate limit */
+  double m_steer_rate_lim;
+
   // trajectory buffer for detecting new trajectory
   std::deque<autoware_auto_planning_msgs::msg::Trajectory> m_trajectory_buffer;
 
   // MPC object
   MPC m_mpc;
 
+  // Check is mpc output converged
+  bool m_is_mpc_history_filled{false};
+
+  //!< @brief store the last mpc outputs for 1 sec
+  std::vector<std::pair<autoware_auto_control_msgs::msg::AckermannLateralCommand, rclcpp::Time>>
+    m_mpc_steering_history{};
+  //!< @brief set the mpc steering output to history
+  void setSteeringToHistory(
+    const autoware_auto_control_msgs::msg::AckermannLateralCommand & steering);
+  //!< @brief check if the mpc steering output is converged
+  bool isMpcConverged();
+
   //!< @brief measured kinematic state
   nav_msgs::msg::Odometry m_current_kinematic_state;
   //!< @brief measured steering

diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml
@@ -62,6 +62,7 @@
     keep_steer_control_until_converged: true
     new_traj_duration_time: 1.0
     new_traj_end_dist: 0.3
+    mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s]
 
     # steer offset
     steering_offset:

diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp
@@ -69,14 +69,16 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
     node_->declare_parameter<bool>("keep_steer_control_until_converged");
   m_new_traj_duration_time = node_->declare_parameter<double>("new_traj_duration_time");  // [s]
   m_new_traj_end_dist = node_->declare_parameter<double>("new_traj_end_dist");            // [m]
+  m_mpc_converged_threshold_rps =
+    node_->declare_parameter<double>("mpc_converged_threshold_rps");  // [rad/s]
 
   /* mpc parameters */
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo();
   const double wheelbase = vehicle_info.wheel_base_m;
   const double steer_rate_lim_dps = node_->declare_parameter<double>("steer_rate_lim_dps");
   constexpr double deg2rad = static_cast<double>(M_PI) / 180.0;
   m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad;
-  m_mpc.m_steer_rate_lim = steer_rate_lim_dps * deg2rad;
+  m_steer_rate_lim = steer_rate_lim_dps * deg2rad;
 
   /* vehicle model setup */
   const std::string vehicle_model_type =
@@ -202,6 +204,17 @@ trajectory_follower::LateralOutput MpcLateralController::run(
     m_is_ctrl_cmd_prev_initialized = true;
   }
 
+  const bool is_vehicle_stopped = std::fabs(m_current_kinematic_state.twist.twist.linear.x) < 0.01;
+
+  // if the vehicle is stopped, set steering angle rate limit to a large value to get a proper
+  // steering value from mpc.
+  // TODO(someone): solve mpc cannot create output in low steering rate limit problem
+  if (is_vehicle_stopped) {
+    m_mpc.m_steer_rate_lim = std::numeric_limits<double>::max();
+  } else {
+    m_mpc.m_steer_rate_lim = m_steer_rate_lim;
+  }
+
   const bool is_mpc_solved = m_mpc.calculateMPC(
     m_current_steering, m_current_kinematic_state.twist.twist.linear.x,
     m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values);
@@ -213,6 +226,8 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   // the actual steer angle, and it may make the optimization result unstable.
   if (!is_mpc_solved) {
     m_mpc.resetPrevResult(m_current_steering);
+  } else {
+    setSteeringToHistory(ctrl_cmd);
   }
 
   if (enable_auto_steering_offset_removal_) {
@@ -225,10 +240,17 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   publishPredictedTraj(predicted_traj);
   publishDebugValues(debug_values);
 
-  const auto createLateralOutput = [this](const auto & cmd) {
+  const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
     trajectory_follower::LateralOutput output;
     output.control_cmd = createCtrlCmdMsg(cmd);
-    output.sync_data.is_steer_converged = isSteerConverged(cmd);
+    // To be sure current steering of the vehicle is desired steering angle, we need to check
+    // following conditions.
+    // 1. At the last loop, mpc should be solved because command should be optimized output.
+    // 2. The mpc should be converged.
+    // 3. The steer angle should be converged.
+    output.sync_data.is_steer_converged =
+      is_mpc_solved && isMpcConverged() && isSteerConverged(cmd);
+
     return output;
   };
 
@@ -239,7 +261,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
     }
     // Use previous command value as previous raw steer command
     m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
-    return createLateralOutput(m_ctrl_cmd_prev);
+    return createLateralOutput(m_ctrl_cmd_prev, false);
   }
 
   if (!is_mpc_solved) {
@@ -250,7 +272,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   }
 
   m_ctrl_cmd_prev = ctrl_cmd;
-  return createLateralOutput(ctrl_cmd);
+  return createLateralOutput(ctrl_cmd, is_mpc_solved);
 }
 
 bool MpcLateralController::isSteerConverged(
@@ -410,6 +432,61 @@ void MpcLateralController::publishDebugValues(
   m_pub_steer_offset->publish(offset);
 }
 
+void MpcLateralController::setSteeringToHistory(
+  const autoware_auto_control_msgs::msg::AckermannLateralCommand & steering)
+{
+  const auto time = node_->now();
+  if (m_mpc_steering_history.empty()) {
+    m_mpc_steering_history.emplace_back(steering, time);
+    m_is_mpc_history_filled = false;
+    return;
+  }
+
+  m_mpc_steering_history.emplace_back(steering, time);
+
+  // Check the history is filled or not.
+  if (rclcpp::Duration(time - m_mpc_steering_history.begin()->second).seconds() >= 1.0) {
+    m_is_mpc_history_filled = true;
+    // remove old data that is older than 1 sec
+    for (auto itr = m_mpc_steering_history.begin(); itr != m_mpc_steering_history.end(); ++itr) {
+      if (rclcpp::Duration(time - itr->second).seconds() > 1.0) {
+        m_mpc_steering_history.erase(m_mpc_steering_history.begin());
+      } else {
+        break;
+      }
+    }
+  } else {
+    m_is_mpc_history_filled = false;
+  }
+}
+
+bool MpcLateralController::isMpcConverged()
+{
+  // If the number of variable below the 2, there is no enough data so MPC is not converged.
+  if (m_mpc_steering_history.size() < 2) {
+    return false;
+  }
+
+  // If the history is not filled, return false.
+
+  if (!m_is_mpc_history_filled) {
+    return false;
+  }
+
+  // Find the maximum and minimum values of the steering angle in the past 1 second.
+  double min_steering_value = m_mpc_steering_history[0].first.steering_tire_angle;
+  double max_steering_value = m_mpc_steering_history[0].first.steering_tire_angle;
+  for (size_t i = 1; i < m_mpc_steering_history.size(); i++) {
+    if (m_mpc_steering_history[i].first.steering_tire_angle < min_steering_value) {
+      min_steering_value = m_mpc_steering_history[i].first.steering_tire_angle;
+    }
+    if (m_mpc_steering_history[i].first.steering_tire_angle > max_steering_value) {
+      max_steering_value = m_mpc_steering_history[i].first.steering_tire_angle;
+    }
+  }
+  return (max_steering_value - min_steering_value) < m_mpc_converged_threshold_rps;
+}
+
 void MpcLateralController::declareMPCparameters()
 {
   m_mpc.m_param.prediction_horizon = node_->declare_parameter<int>("mpc_prediction_horizon");