feat(autoware_mpc_lateral_controller): create ADT model as vehicle model for mpc

control/autoware_mpc_lateral_controller/CMakeLists.txt

@@ -18,6 +18,7 @@ ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED
   src/mpc_utils.cpp
   src/qp_solver/qp_solver_osqp.cpp
   src/qp_solver/qp_solver_unconstraint_fast.cpp
+  src/vehicle_model/vehicle_model_adt_kinematics.cpp
   src/vehicle_model/vehicle_model_bicycle_dynamics.cpp
   src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
   src/vehicle_model/vehicle_model_bicycle_kinematics.cpp

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_adt_kinematics.hpp

@@ -0,0 +1,122 @@
+// Copyright 2024 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ *    Representation
+ * k      : curvature
+ * e      : lateral error
+ * th     : heading angle error
+ * steer  : steering angle
+ * steer_d: desired steering angle (input)
+ * v      : velocity
+ * Wf     : front wheelbase length
+ * Wr     : rear wheelbase length
+ * tau    : time constant for steering dynamics
+ *
+ *    State & Input
+ * x = [e, th, steer]^T
+ * u = steer_d
+ *
+ *    Nonlinear model
+ * dx1/dt = v * sin(x2)
+ * dx2/dt = v * tan(x3) / W
+ * dx3/dt = -(x3 - u) / tau
+ *
+ *    Linearized model around reference point (v = v_r, th = th_r, steer = steer_r)
+ *         [0,  vr,       0]       [    0]       [                    0]
+ * dx/dt = [0,   0,       B] * x + [    0] * u + [-vr*k + A - B*steer_r]
+ *         [0,   0,  -1/tau]       [1/tau]       [                    0]
+ *
+ * where A = vr * sin(steer_r) / (Wf + Wr * cos(steer_r))
+ *       B = A_(steer_r) = vr * (Wf * cos(steer_r) + Wr) / (Wf + Wr * cos(steer_r))^2
+ *
+ */
+
+#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_ADT_KINEMATICS_HPP_
+#define AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_ADT_KINEMATICS_HPP_
+
+#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <string>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/**
+ * Vehicle model class of articulated dump truck kinematics
+ * @brief calculate model-related values
+ */
+class KinematicsAdtModel : public VehicleModelInterface
+{
+public:
+  /**
+   * @brief constructor with parameter initialization
+   * @param [in] front_wheelbase front wheelbase length [m]
+   * @param [in] rear_wheelbase rear wheelbase length [m]
+   * @param [in] steer_lim steering angle limit [rad]
+   * @param [in] steer_tau steering time constant for 1d-model [s]
+   */
+  KinematicsAdtModel(
+    const double front_wheelbase, const double rear_wheelbase, const double steer_lim,
+    const double steer_tau);
+
+  /**
+   * @brief destructor
+   */
+  ~KinematicsAdtModel() = default;
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk
+   * @param [out] a_d coefficient matrix
+   * @param [out] b_d coefficient matrix
+   * @param [out] c_d coefficient matrix
+   * @param [out] w_d coefficient matrix
+   * @param [in] dt Discretization time [s]
+   */
+  void calculateDiscreteMatrix(
+    Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+    const double dt) override;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] u_ref input
+   */
+  void calculateReferenceInput(Eigen::MatrixXd & u_ref) override;
+
+  std::string modelName() override { return "adt_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_front_wheelbase;  //!< @brief front wheelbase length [m]
+  double m_rear_wheelbase;   //!< @brief rear wheelbase length [m]
+  double m_steer_lim;        //!< @brief steering angle limit [rad]
+  double m_steer_tau;        //!< @brief steering time constant for 1d-model [s]
+
+  const double m_wheelbase_diff;
+  const double m_wheelbase_squared_diff;
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_ADT_KINEMATICS_HPP_

control/autoware_mpc_lateral_controller/src/mpc.cpp

@@ -337,6 +337,8 @@ VectorXd MPC::getInitialState(const MPCData & data)
     x0 << lat_err, dlat, yaw_err, dyaw;
     RCLCPP_DEBUG(m_logger, "(before lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
     RCLCPP_DEBUG(m_logger, "(after lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
+  } else if (vehicle_model == "adt_kinematics") {
+    x0 << lat_err, yaw_err, steer;
   } else {
     RCLCPP_ERROR(m_logger, "vehicle_model_type is undefined");
   }
@@ -711,7 +713,8 @@ double MPC::calcDesiredSteeringRate(
   const MPCMatrix & mpc_matrix, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered,
   const float current_steer, const double predict_dt) const
 {
-  if (m_vehicle_model_ptr->modelName() != "kinematics") {
+  std::string model = m_vehicle_model_ptr->modelName();
+  if (model != "kinematics" && model != "adt_kinematics") {
     // not supported yet. Use old implementation.
     return (u_filtered - current_steer) / predict_dt;
   }

control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -17,6 +17,7 @@
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
 #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
+#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_adt_kinematics.hpp"
 #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
 #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
 #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
@@ -189,6 +190,16 @@ std::shared_ptr<VehicleModelInterface> MpcLateralController::createVehicleModel(
     return vehicle_model_ptr;
   }
 
+  if (vehicle_model_type == "adt_kinematics") {
+    const double rear_wheelbase_ratio =
+      node.declare_parameter<double>("vehicle.rear_wheelbase_ratio", 0.747);
+    const double front_wheelbase = wheelbase * (1.0 - rear_wheelbase_ratio);
+    const double rear_wheelbase = wheelbase * rear_wheelbase_ratio;
+    vehicle_model_ptr =
+      std::make_shared<KinematicsAdtModel>(front_wheelbase, rear_wheelbase, steer_lim, steer_tau);
+    return vehicle_model_ptr;
+  }
+
   RCLCPP_ERROR(logger_, "vehicle_model_type is undefined");
   return vehicle_model_ptr;
 }

control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_adt_kinematics.cpp

@@ -0,0 +1,166 @@
+// Copyright 2024 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_adt_kinematics.hpp"
+
+#include <cmath>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+KinematicsAdtModel::KinematicsAdtModel(
+  const double front_wheelbase, const double rear_wheelbase, const double steer_lim,
+  const double steer_tau)
+: VehicleModelInterface(
+    /* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2, front_wheelbase + rear_wheelbase),
+  m_wheelbase_diff(front_wheelbase - rear_wheelbase),
+  m_wheelbase_squared_diff(rear_wheelbase * rear_wheelbase - front_wheelbase * front_wheelbase)
+{
+  m_front_wheelbase = front_wheelbase;
+  m_rear_wheelbase = rear_wheelbase;
+  m_steer_lim = steer_lim;
+  m_steer_tau = steer_tau;
+}
+
+void KinematicsAdtModel::calculateDiscreteMatrix(
+  Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+  const double dt)
+{
+  auto sign = [](double x) { return (x > 0.0) - (x < 0.0); };
+
+  /* Linearize delta around delta_r (reference delta) */
+  const double curvature_squared = m_curvature * m_curvature;
+  double delta_r = 2.0 * std::atan(
+                           (1.0 - std::sqrt(m_wheelbase_squared_diff * curvature_squared + 1.0)) /
+                           (m_wheelbase_diff * m_curvature));
+  if (std::abs(delta_r) >= m_steer_lim) {
+    delta_r = m_steer_lim * static_cast<double>(sign(delta_r));
+  }
+
+  double velocity = m_velocity;
+  if (std::abs(m_velocity) < 1e-04) {
+    velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1);
+  }
+
+  const double front_wheelbase_c_rear_wheelbase_inv =
+    1.0 / (m_front_wheelbase + m_rear_wheelbase * cos(delta_r));
+
+  const double turning_velocity = velocity * sin(delta_r) * front_wheelbase_c_rear_wheelbase_inv;
+  const double pd_turning_velocity =
+    velocity * (m_front_wheelbase * cos(delta_r) + m_rear_wheelbase) *
+    front_wheelbase_c_rear_wheelbase_inv * front_wheelbase_c_rear_wheelbase_inv;
+
+  a_d << 0.0, velocity, 0.0, 0.0, 0.0, pd_turning_velocity, 0.0, 0.0, -1.0 / m_steer_tau;
+
+  b_d << 0.0, 0.0, 1.0 / m_steer_tau;
+
+  c_d << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0;
+
+  w_d << 0.0, -velocity * m_curvature + turning_velocity - pd_turning_velocity * delta_r, 0.0;
+
+  // bilinear discretization for ZOH system
+  // no discretization is needed for Cd
+  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x);
+  const Eigen::MatrixXd i_dt2a_inv = (I - dt * 0.5 * a_d).inverse();
+  a_d = i_dt2a_inv * (I + dt * 0.5 * a_d);
+  b_d = i_dt2a_inv * b_d * dt;
+  w_d = i_dt2a_inv * w_d * dt;
+}
+
+void KinematicsAdtModel::calculateReferenceInput(Eigen::MatrixXd & u_ref)
+{
+  u_ref(0, 0) = std::atan(m_wheelbase * m_curvature);
+}
+
+MPCTrajectory KinematicsAdtModel::calculatePredictedTrajectoryInWorldCoordinate(
+  [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d,
+  [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d,
+  const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+  const MPCTrajectory & reference_trajectory, const double dt) const
+{
+  // Calculate predicted state in world coordinate since there is modeling errors in Frenet
+  // Relative coordinate x = [lat_err, yaw_err, steer]
+  // World coordinate x = [x, y, yaw, steer]
+
+  const auto & t = reference_trajectory;
+
+  // create initial state in the world coordinate
+  Eigen::VectorXd state_w = [&]() {
+    Eigen::VectorXd state = Eigen::VectorXd::Zero(4);
+    const auto lateral_error_0 = x0(0);
+    const auto yaw_error_0 = x0(1);
+    state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0;  // world-x
+    state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0;  // world-y
+    state(2, 0) = t.yaw.at(0) + yaw_error_0;                            // world-yaw
+    state(3, 0) = x0(2);                                                // steering
+    return state;
+  }();
+
+  // update state in the world coordinate
+  const auto updateState = [&](
+                             const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input,
+                             const double dt, const double velocity) {
+    const auto yaw = state_w(2);
+    const auto steer = state_w(3);
+    const auto desired_steer = input(0);
+
+    Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4);
+    dstate(0) = velocity * std::cos(yaw);
+    dstate(1) = velocity * std::sin(yaw);
+    dstate(2) =
+      velocity * std::sin(steer) / (m_front_wheelbase + m_rear_wheelbase * std::cos(steer));
+    dstate(3) = -(steer - desired_steer) / m_steer_tau;
+
+    // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation
+    // in Eigen.
+    const Eigen::VectorXd next_state = state_w + dstate * dt;
+    return next_state;
+  };
+
+  MPCTrajectory mpc_predicted_trajectory;
+  const auto DIM_U = getDimU();
+
+  for (size_t i = 0; i < reference_trajectory.size(); ++i) {
+    state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i));
+    mpc_predicted_trajectory.push_back(
+      state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i),
+      t.relative_time.at(i));
+  }
+  return mpc_predicted_trajectory;
+}
+
+MPCTrajectory KinematicsAdtModel::calculatePredictedTrajectoryInFrenetCoordinate(
+  const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d,
+  [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d,
+  const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+  const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const
+{
+  // Relative coordinate x = [lat_err, yaw_err, steer]
+
+  Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d;
+  MPCTrajectory mpc_predicted_trajectory;
+  const auto DIM_X = getDimX();
+  const auto & t = reference_trajectory;
+
+  for (size_t i = 0; i < reference_trajectory.size(); ++i) {
+    const auto lateral_error = Xex(i * DIM_X);  // model dependent
+    const auto yaw_error = Xex(i * DIM_X + 1);  // model dependent
+    const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error;
+    const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error;
+    const auto yaw = t.yaw.at(i) + yaw_error;
+    mpc_predicted_trajectory.push_back(
+      x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i));
+  }
+  return mpc_predicted_trajectory;
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp

@@ -70,11 +70,10 @@ double SimModelDelayArticulateAccGeared::getAx()
 double SimModelDelayArticulateAccGeared::getWz()
 {
   const double steer = state_(IDX::STEER);
-  const double front_wheelbase_lon = front_wheelbase_ * std::cos(steer);
-  const double pseudo_wheelbase = front_wheelbase_lon + rear_wheelbase_;
+  const double c_steer = std::cos(steer);
 
-  return state_(IDX::VX) * std::sin(steer) / pseudo_wheelbase -
-         state_steer_rate_ * front_wheelbase_lon / pseudo_wheelbase;
+  return (state_(IDX::VX) * std::sin(steer) - state_steer_rate_ * front_wheelbase_ * c_steer) /
+         (front_wheelbase_ + rear_wheelbase_ * c_steer);
 }
 double SimModelDelayArticulateAccGeared::getSteer()
 {
@@ -143,14 +142,13 @@ Eigen::VectorXd SimModelDelayArticulateAccGeared::calcModel(
   double state_steer_rate_ =
     sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_);
 
-  const double front_wheelbase_lon = front_wheelbase_ * std::cos(steer);
-  const double pseudo_wheelbase = front_wheelbase_lon + rear_wheelbase_;
+  const double c_steer = std::cos(steer);
 
   Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
   d_state(IDX::X) = vel * cos(yaw);
   d_state(IDX::Y) = vel * sin(yaw);
-  d_state(IDX::YAW) = vel * std::sin(steer) / pseudo_wheelbase -
-                      state_steer_rate_ * front_wheelbase_lon / pseudo_wheelbase;
+  d_state(IDX::YAW) = (vel * std::sin(steer) - state_steer_rate_ * front_wheelbase_ * c_steer) /
+                      (front_wheelbase_ + rear_wheelbase_ * c_steer);
   d_state(IDX::VX) = acc;
   d_state(IDX::STEER) = state_steer_rate_;
   d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_;