forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 34
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(autoware_mpc_lateral_controller): create ADT model as vehicle mo…
…del for mpc (#1552) * fix sim model Signed-off-by: Autumn60 <[email protected]> * create adt_kinematics model as copy of bicycle_kinematics model Signed-off-by: Autumn60 <[email protected]> * fix model name Signed-off-by: Autumn60 <[email protected]> * implement adt kinematics foc mpc vehicle_model Signed-off-by: Autumn60 <[email protected]> * update description Signed-off-by: Autumn60 <[email protected]> * fix formula Signed-off-by: Autumn60 <[email protected]> --------- Signed-off-by: Autumn60 <[email protected]>
- Loading branch information
Showing
6 changed files
with
310 additions
and
9 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
122 changes: 122 additions & 0 deletions
122
...er/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_adt_kinematics.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
166 changes: 166 additions & 0 deletions
166
control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_adt_kinematics.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters