feat(mpc): calculate mpc predicted trajectory in the world coordinate…

… to improve accuracy (#5576)

* remake

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove maybe_unused in decleration

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* support two predicted trajectories in world and Frenet coordinate

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix test

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

---------

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp

@@ -221,6 +221,7 @@ class MPC
 
   double m_min_prediction_length = 5.0;  // Minimum prediction distance.
 
+  rclcpp::Publisher<Trajectory>::SharedPtr m_debug_frenet_predicted_trajectory_pub;
   /**
    * @brief Get variables for MPC calculation.
    * @param trajectory The reference trajectory.
@@ -333,6 +334,19 @@ class MPC
     const MPCMatrix & m, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered,
     const float current_steer, const double predict_dt) const;
 
+  /**
+   * @brief calculate predicted trajectory
+   * @param mpc_matrix The MPC matrix used in the mpc problem.
+   * @param x0 initial state used in the mpc problem.
+   * @param Uex optimized input.
+   * @param mpc_resampled_ref_traj reference trajectory resampled in the mpc time-step
+   * @param dt delta time used in the mpc problem.
+   * @return predicted path
+   */
+  Trajectory calculatePredictedTrajectory(
+    const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & mpc_resampled_ref_traj, const double dt) const;
+
   /**
    * @brief Check if the MPC matrix has any invalid values.
    * @param m The MPC matrix to check.
@@ -352,18 +366,6 @@ class MPC
              : m_param.nominal_weight;
   }
 
-  /**
-   * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result.
-   * @param mpc_resampled_reference_trajectory The resampled reference trajectory.
-   * @param mpc_matrix The MPC matrix used for optimization.
-   * @param x0_delayed The delayed initial state vector.
-   * @param Uex The optimized input vector.
-   * @return The predicted trajectory.
-   */
-  Trajectory calcPredictedTrajectory(
-    const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix,
-    const VectorXd & x0_delayed, const VectorXd & Uex) const;
-
   /**
    * @brief Generate diagnostic data for debugging purposes.
    * @param reference_trajectory The reference trajectory.
@@ -424,8 +426,10 @@ class MPC
   double ego_nearest_dist_threshold = 3.0;  // Threshold for nearest index search based on distance.
   double ego_nearest_yaw_threshold = M_PI_2;  // Threshold for nearest index search based on yaw.
 
+  bool m_debug_publish_predicted_trajectory = false;  // Flag to publish debug predicted trajectory
+
   //!< Constructor.
-  MPC() = default;
+  explicit MPC(rclcpp::Node & node);
 
   /**
    * @brief Calculate control command using the MPC algorithm.

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -91,7 +91,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   // trajectory buffer for detecting new trajectory
   std::deque<Trajectory> m_trajectory_buffer;
 
-  MPC m_mpc;  // MPC object for trajectory following.
+  std::unique_ptr<MPC> m_mpc;  // MPC object for trajectory following.
 
   // Check is mpc output converged
   bool m_is_mpc_history_filled{false};

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp

@@ -85,6 +85,12 @@ class MPCTrajectory
    */
   size_t size() const;
 
+  /**
+   * @brief get trajectory point at index i
+   * @return trajectory point at index i
+   */
+  MPCTrajectoryPoint at(const size_t i) const;
+
   /**
    * @return true if the components sizes are all 0 or are inconsistent
    */

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp

@@ -92,6 +92,13 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input);
  */
 void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<double> & arc_length);
 
+/**
+ * @brief calculate the arc length of the given trajectory
+ * @param [in] trajectory trajectory for which to calculate the arc length
+ * @return total arc length
+ */
+double calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory);
+
 /**
  * @brief resample the given trajectory with the given fixed interval
  * @param [in] input trajectory to resample
@@ -194,6 +201,14 @@ double calcStopDistance(const Trajectory & current_trajectory, const int origin)
 void extendTrajectoryInYawDirection(
   const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj);
 
+/**
+ * @brief clip trajectory size by length
+ * @param [in] trajectory original trajectory
+ * @param [in] length clip length
+ * @return clipped trajectory
+ */
+MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length);
+
 /**
  * @brief Updates the value of a parameter with the given name.
  * @tparam T The type of the parameter value.

control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp

@@ -103,6 +103,16 @@ class DynamicsBicycleModel : public VehicleModelInterface
 
   std::string modelName() override { return "dynamics"; };
 
+  MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate(
+    const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d,
+    const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & reference_trajectory, const double dt) const override;
+
+  MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate(
+    const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d,
+    const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & reference_trajectory, const double dt) const override;
+
 private:
   double m_lf;    //!< @brief length from center of mass to front wheel [m]
   double m_lr;    //!< @brief length from center of mass to rear wheel [m]

control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp

@@ -91,6 +91,16 @@ class KinematicsBicycleModel : public VehicleModelInterface
 
   std::string modelName() override { return "kinematics"; };
 
+  MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate(
+    const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d,
+    const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & reference_trajectory, const double dt) const override;
+
+  MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate(
+    const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d,
+    const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & reference_trajectory, const double dt) const override;
+
 private:
   double m_steer_lim;  //!< @brief steering angle limit [rad]
   double m_steer_tau;  //!< @brief steering time constant for 1d-model [s]

control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp

@@ -87,6 +87,16 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface
 
   std::string modelName() override { return "kinematics_no_delay"; };
 
+  MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate(
+    const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d,
+    const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & reference_trajectory, const double dt) const override;
+
+  MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate(
+    const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d,
+    const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & reference_trajectory, const double dt) const override;
+
 private:
   double m_steer_lim;  //!< @brief steering angle limit [rad]
 };

control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp

@@ -15,6 +15,8 @@
 #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
 #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
 
+#include "mpc_lateral_controller/mpc_trajectory.hpp"
+
 #include <Eigen/Core>
 
 #include <string>
@@ -55,25 +57,25 @@ class VehicleModelInterface
    * @brief get state x dimension
    * @return state dimension
    */
-  int getDimX();
+  int getDimX() const;
 
   /**
    * @brief get input u dimension
    * @return input dimension
    */
-  int getDimU();
+  int getDimU() const;
 
   /**
    * @brief get output y dimension
    * @return output dimension
    */
-  int getDimY();
+  int getDimY() const;
 
   /**
    * @brief get wheelbase of the vehicle
    * @return wheelbase value [m]
    */
-  double getWheelbase();
+  double getWheelbase() const;
 
   /**
    * @brief set velocity
@@ -109,6 +111,42 @@ class VehicleModelInterface
    * @brief returns model name e.g. kinematics, dynamics
    */
   virtual std::string modelName() = 0;
+
+  /**
+   * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in world
+   * coordinate
+   * @param a_d The MPC A matrix used for optimization.
+   * @param b_d The MPC B matrix used for optimization.
+   * @param c_d The MPC C matrix used for optimization.
+   * @param w_d The MPC W matrix used for optimization.
+   * @param x0 initial state vector.
+   * @param Uex The optimized input vector.
+   * @param reference_trajectory The resampled reference trajectory.
+   * @param dt delta time used in the optimization
+   * @return The predicted trajectory.
+   */
+  virtual MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate(
+    const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d,
+    const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & reference_trajectory, const double dt) const = 0;
+
+  /**
+   * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in Frenet
+   * Coordinate
+   * @param a_d The MPC A matrix used for optimization.
+   * @param b_d The MPC B matrix used for optimization.
+   * @param c_d The MPC C matrix used for optimization.
+   * @param w_d The MPC W matrix used for optimization.
+   * @param x0 initial state vector.
+   * @param Uex The optimized input vector.
+   * @param reference_trajectory The resampled reference trajectory.
+   * @param dt delta time used in the optimization
+   * @return The predicted trajectory.
+   */
+  virtual MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate(
+    const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d,
+    const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const MPCTrajectory & reference_trajectory, const double dt) const = 0;
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
 #endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_

control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml

@@ -83,3 +83,5 @@
     mass_rr: 600.0
     cf: 155494.663
     cr: 155494.663
+
+    debug_publish_predicted_trajectory: false  # publish debug predicted trajectory in Frenet coordinate

control/mpc_lateral_controller/src/mpc.cpp

@@ -17,6 +17,7 @@
 #include "interpolation/linear_interpolation.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "mpc_lateral_controller/mpc_utils.hpp"
+#include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 
 #include <algorithm>
@@ -28,6 +29,12 @@ using tier4_autoware_utils::calcDistance2d;
 using tier4_autoware_utils::normalizeRadian;
 using tier4_autoware_utils::rad2deg;
 
+MPC::MPC(rclcpp::Node & node)
+{
+  m_debug_frenet_predicted_trajectory_pub = node.create_publisher<Trajectory>(
+    "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1));
+}
+
 bool MPC::calculateMPC(
   const SteeringReport & current_steer, const Odometry & current_kinematics,
   AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
@@ -97,9 +104,9 @@ bool MPC::calculateMPC(
   m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev;
   m_raw_steer_cmd_prev = Uex(0);
 
-  // calculate predicted trajectory
+  /* calculate predicted trajectory */
   predicted_trajectory =
-    calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex);
+    calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt);
 
   // prepare diagnostic message
   diagnostic =
@@ -108,30 +115,6 @@ bool MPC::calculateMPC(
   return true;
 }
 
-Trajectory MPC::calcPredictedTrajectory(
-  const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix,
-  const VectorXd & x0_delayed, const VectorXd & Uex) const
-{
-  const VectorXd Xex = mpc_matrix.Aex * x0_delayed + mpc_matrix.Bex * Uex + mpc_matrix.Wex;
-  MPCTrajectory mpc_predicted_traj;
-  const auto & traj = mpc_resampled_ref_trajectory;
-  for (int i = 0; i < m_param.prediction_horizon; ++i) {
-    const int DIM_X = m_vehicle_model_ptr->getDimX();
-    const double lat_error = Xex(i * DIM_X);
-    const double yaw_error = Xex(i * DIM_X + 1);
-    const double x = traj.x.at(i) - std::sin(traj.yaw.at(i)) * lat_error;
-    const double y = traj.y.at(i) + std::cos(traj.yaw.at(i)) * lat_error;
-    const double z = traj.z.at(i);
-    const double yaw = traj.yaw.at(i) + yaw_error;
-    const double vx = traj.vx.at(i);
-    const double k = traj.k.at(i);
-    const double smooth_k = traj.smooth_k.at(i);
-    const double relative_time = traj.relative_time.at(i);
-    mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time);
-  }
-  return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj);
-}
-
 Float32MultiArrayStamped MPC::generateDiagData(
   const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
   const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
@@ -794,6 +777,35 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(
   return steer_rate_limits;
 }
 
+Trajectory MPC::calculatePredictedTrajectory(
+  const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+  const MPCTrajectory & reference_trajectory, const double dt) const
+{
+  const auto predicted_mpc_trajectory =
+    m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate(
+      mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
+      dt);
+
+  // do not over the reference trajectory
+  const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory);
+  const auto clipped_trajectory =
+    MPCUtils::clipTrajectoryByLength(predicted_mpc_trajectory, predicted_length);
+
+  const auto predicted_trajectory = MPCUtils::convertToAutowareTrajectory(clipped_trajectory);
+
+  // Publish trajectory in relative coordinate for debug purpose.
+  if (m_debug_publish_predicted_trajectory) {
+    const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate(
+      mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
+      dt);
+    const auto frenet_clipped = MPCUtils::convertToAutowareTrajectory(
+      MPCUtils::clipTrajectoryByLength(frenet, predicted_length));
+    m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped);
+  }
+
+  return predicted_trajectory;
+}
+
 bool MPC::isValid(const MPCMatrix & m) const
 {
   if (