Autoware.Auto
mpc.hpp
Go to the documentation of this file.
1 // Copyright 2018-2021 The Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef TRAJECTORY_FOLLOWER__MPC_HPP_
16 #define TRAJECTORY_FOLLOWER__MPC_HPP_
17 
18 #include <deque>
19 #include <memory>
20 #include <string>
21 #include <vector>
22 
23 #include "tf2_ros/buffer.h"
24 #include "tf2_ros/transform_listener.h"
25 
36 
37 #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
38 #include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
39 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
40 #include "autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp"
41 #include "common/types.hpp"
42 #include "geometry/common_2d.hpp"
43 #include "geometry_msgs/msg/pose.hpp"
47 #include "rclcpp/rclcpp.hpp"
48 #include "rclcpp_components/register_node_macro.hpp"
49 
50 namespace autoware
51 {
52 namespace motion
53 {
54 namespace control
55 {
56 namespace trajectory_follower
57 {
60 struct MPCParam
61 {
76 // for weight matrix Q
93 // for weight matrix R
116 };
120 struct MPCData
121 {
122  int64_t nearest_idx;
124  geometry_msgs::msg::Pose nearest_pose;
129 };
133 struct MPCMatrix
134 {
135  Eigen::MatrixXd Aex;
136  Eigen::MatrixXd Bex;
137  Eigen::MatrixXd Wex;
138  Eigen::MatrixXd Cex;
139  Eigen::MatrixXd Qex;
140  Eigen::MatrixXd R1ex;
141  Eigen::MatrixXd R2ex;
142  Eigen::MatrixXd Urefex;
143  Eigen::MatrixXd Yrefex;
144 };
149 class TRAJECTORY_FOLLOWER_PUBLIC MPC
150 {
151 private:
153  rclcpp::Logger m_logger = rclcpp::get_logger("mpc_logger");
155  rclcpp::Clock::SharedPtr m_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
156 
158  std::string m_vehicle_model_type;
160  std::shared_ptr<trajectory_follower::VehicleModelInterface> m_vehicle_model_ptr;
162  std::shared_ptr<trajectory_follower::QPSolverInterface> m_qpsolver_ptr;
164  trajectory_follower::Butterworth2dFilter m_lpf_steering_cmd;
166  trajectory_follower::Butterworth2dFilter m_lpf_lateral_error;
169 
171  float64_t m_raw_steer_cmd_pprev = 0.0;
173  float64_t m_lateral_error_prev = 0.0;
175  float64_t m_yaw_error_prev = 0.0;
177  std::shared_ptr<float64_t> m_steer_prediction_prev;
179  rclcpp::Time m_time_prev = rclcpp::Time(0, 0, RCL_ROS_TIME);
181  float64_t m_sign_vx = 0.0;
183  std::vector<autoware_auto_control_msgs::msg::AckermannLateralCommand> m_ctrl_cmd_vec;
184 
188  bool8_t getData(
191  const geometry_msgs::msg::Pose & current_pose,
192  MPCData * data);
196  float64_t calcSteerPrediction();
200  float64_t getSteerCmdSum(
201  const rclcpp::Time & t_start, const rclcpp::Time & t_end,
202  const float64_t time_constant) const;
206  void storeSteerCmd(const float64_t steer);
211  Eigen::VectorXd getInitialState(const MPCData & data);
218  bool8_t updateStateForDelayCompensation(
219  const trajectory_follower::MPCTrajectory & traj, const float64_t & start_time,
220  Eigen::VectorXd * x);
225  MPCMatrix generateMPCMatrix(const trajectory_follower::MPCTrajectory & reference_trajectory);
232  bool8_t executeOptimization(
233  const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, Eigen::VectorXd * Uex);
237  bool8_t resampleMPCTrajectoryByTime(
238  float64_t start_time, const trajectory_follower::MPCTrajectory & input,
243  trajectory_follower::MPCTrajectory applyVelocityDynamicsFilter(
244  const trajectory_follower::MPCTrajectory & trajectory,
245  const geometry_msgs::msg::Pose & current_pose, const float64_t v0) const;
249  float64_t getPredictionTime() const;
253  void addSteerWeightR(Eigen::MatrixXd * R) const;
257  void addSteerWeightF(Eigen::MatrixXd * f) const;
261  bool8_t isValid(const MPCMatrix & m) const;
265  inline bool8_t isLowCurvature(const float64_t curvature)
266  {
267  return std::fabs(curvature) < m_param.low_curvature_thresh_curvature;
268  }
272  inline float64_t getWeightLatError(const float64_t curvature)
273  {
274  return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_error :
275  m_param.weight_lat_error;
276  }
280  inline float64_t getWeightHeadingError(const float64_t curvature)
281  {
282  return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error :
283  m_param.weight_heading_error;
284  }
288  inline float64_t getWeightHeadingErrorSqVel(const float64_t curvature)
289  {
290  return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error_squared_vel :
291  m_param.weight_heading_error_squared_vel;
292  }
296  inline float64_t getWeightSteerInput(const float64_t curvature)
297  {
298  return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input :
299  m_param.weight_steering_input;
300  }
304  inline float64_t getWeightSteerInputSqVel(const float64_t curvature)
305  {
306  return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input_squared_vel :
307  m_param.weight_steering_input_squared_vel;
308  }
312  inline float64_t getWeightLatJerk(const float64_t curvature)
313  {
314  return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_jerk :
315  m_param.weight_lat_jerk;
316  }
320  inline float64_t getWeightSteerRate(const float64_t curvature)
321  {
322  return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_rate :
323  m_param.weight_steer_rate;
324  }
328  inline float64_t getWeightSteerAcc(const float64_t curvature)
329  {
330  return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_acc :
331  m_param.weight_steer_acc;
332  }
333 
334 public:
340  std::deque<float64_t> m_input_buffer;
342  float64_t m_raw_steer_cmd_prev = 0.0;
343  /* parameters for control*/
354  /* parameters for path smoothing */
357 
361  MPC() = default;
371  bool8_t calculateMPC(
373  const float64_t current_velocity,
374  const geometry_msgs::msg::Pose & current_pose,
375  autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd,
377  autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic
378  );
382  void setReferenceTrajectory(
383  const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
384  const float64_t traj_resample_dist,
385  const bool8_t enable_path_smoothing,
386  const int64_t path_filter_moving_ave_num,
387  const bool8_t enable_yaw_recalculation,
388  const int64_t curvature_smoothing_num,
389  const geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr);
393  inline void setVehicleModel(
394  std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr,
395  const std::string & vehicle_model_type)
396  {
397  m_vehicle_model_ptr = vehicle_model_ptr;
398  m_vehicle_model_type = vehicle_model_type;
399  }
403  inline void setQPSolver(std::shared_ptr<trajectory_follower::QPSolverInterface> qpsolver_ptr)
404  {
405  m_qpsolver_ptr = qpsolver_ptr;
406  }
411  const float64_t steering_lpf_cutoff_hz,
412  const float64_t error_deriv_lpf_cutoff_hz)
413  {
414  m_lpf_steering_cmd.initialize(m_ctrl_period, steering_lpf_cutoff_hz);
415  m_lpf_lateral_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz);
416  m_lpf_yaw_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz);
417  }
421  inline bool8_t hasVehicleModel() const
422  {
423  return m_vehicle_model_ptr != nullptr;
424  }
428  inline bool8_t hasQPSolver() const
429  {
430  return m_qpsolver_ptr != nullptr;
431  }
435  inline void setLogger(rclcpp::Logger logger)
436  {
437  m_logger = logger;
438  }
442  inline void setClock(rclcpp::Clock::SharedPtr clock)
443  {
444  m_clock = clock;
445  }
446 }; // class MPC
447 } // namespace trajectory_follower
448 } // namespace control
449 } // namespace motion
450 } // namespace autoware
451 
452 #endif // TRAJECTORY_FOLLOWER__MPC_HPP_
qp_solver_unconstr_fast.hpp
autoware::motion::control::trajectory_follower::Butterworth2dFilter
2nd-order Butterworth Filter reference : S. Butterworth, "On the Theory of Filter Amplifier",...
Definition: lowpass_filter.hpp:83
autoware::motion::control::trajectory_follower::MPCData
Definition: mpc.hpp:120
autoware::motion::control::trajectory_follower::MPCMatrix::Urefex
Eigen::MatrixXd Urefex
Definition: mpc.hpp:142
mpc_utils.hpp
autoware::motion::control::trajectory_follower::MPCParam::weight_steer_acc
float64_t weight_steer_acc
steering error weight in matrix R in low curvature point
Definition: mpc.hpp:103
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_weight_steering_input_squared_vel
float64_t low_curvature_weight_steering_input_squared_vel
lateral jerk weight in matrix R in low curvature point
Definition: mpc.hpp:107
output
Definition: output_type_trait.hpp:32
autoware::motion::control::trajectory_follower::MPC::hasQPSolver
bool8_t hasQPSolver() const
return true if the QP solver of this MPC is set
Definition: mpc.hpp:428
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_weight_steer_rate
float64_t low_curvature_weight_steer_rate
steering angle acceleration weight in matrix R in low curvature
Definition: mpc.hpp:111
autoware::motion::control::trajectory_follower::MPC::m_steer_rate_lim
float64_t m_steer_rate_lim
control frequency [s]
Definition: mpc.hpp:351
visibility_control.hpp
autoware::motion::control::trajectory_follower::MPCParam::weight_lat_jerk
float64_t weight_lat_jerk
steering rate weight
Definition: mpc.hpp:99
autoware::motion::control::trajectory_follower::MPCParam::zero_ff_steer_deg
float64_t zero_ff_steer_deg
delay time for steering input to be compensated
Definition: mpc.hpp:67
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_weight_lat_jerk
float64_t low_curvature_weight_lat_jerk
steering rate weight in matrix R in low curvature point
Definition: mpc.hpp:109
autoware::motion::control::trajectory_follower::MPC::initializeLowPassFilters
void initializeLowPassFilters(const float64_t steering_lpf_cutoff_hz, const float64_t error_deriv_lpf_cutoff_hz)
initialize low pass filters
Definition: mpc.hpp:410
autoware::motion::control::trajectory_follower::MPC::m_use_steer_prediction
bool8_t m_use_steer_prediction
<
Definition: mpc.hpp:356
types.hpp
This file includes common type definition.
autoware::motion::control::trajectory_follower::MPCData::nearest_idx
int64_t nearest_idx
Definition: mpc.hpp:122
autoware::motion::control::trajectory_follower::MPCMatrix
Definition: mpc.hpp:133
autoware::motion::control::trajectory_follower::MPCData::lateral_err
float64_t lateral_err
Definition: mpc.hpp:127
qp_solver_osqp.hpp
common_2d.hpp
This file includes common functionality for 2D geometry, such as dot products.
angle_utils.hpp
autoware::motion::control::trajectory_follower::MPCData::nearest_time
float64_t nearest_time
Definition: mpc.hpp:123
autoware::motion::control::trajectory_follower::MPC::m_steer_lim
float64_t m_steer_lim
steering rate limit [rad/s]
Definition: mpc.hpp:349
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_weight_heading_error_squared_vel
float64_t low_curvature_weight_heading_error_squared_vel
Definition: mpc.hpp:92
autoware::motion::control::trajectory_follower::MPC::setQPSolver
void setQPSolver(std::shared_ptr< trajectory_follower::QPSolverInterface > qpsolver_ptr)
set the QP solver of this MPC
Definition: mpc.hpp:403
autoware::motion::control::trajectory_follower::MPCParam::weight_heading_error
float64_t weight_heading_error
heading error * velocity weight
Definition: mpc.hpp:80
vehicle_model_bicycle_dynamics.hpp
f
DifferentialEquation f
Definition: kinematic_bicycle.snippet.hpp:44
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_thresh_curvature
float64_t low_curvature_thresh_curvature
Definition: mpc.hpp:115
autoware::motion::control::trajectory_follower::MPCParam::prediction_horizon
int64_t prediction_horizon
<
Definition: mpc.hpp:63
interpolate.hpp
autoware::motion::control::trajectory_follower::MPCData::steer
float64_t steer
Definition: mpc.hpp:125
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware::motion::control::trajectory_follower::MPC::m_ctrl_period
float64_t m_ctrl_period
Definition: mpc.hpp:353
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::motion::control::trajectory_follower::MPCData::nearest_pose
geometry_msgs::msg::Pose nearest_pose
Definition: mpc.hpp:124
m
OnlineData m
Definition: linear_tire.snippet.hpp:36
osqp_interface.hpp
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_weight_lat_error
float64_t low_curvature_weight_lat_error
heading error weight in matrix Q in low curvature point
Definition: mpc.hpp:88
autoware::motion::control::trajectory_follower::MPC::m_admissible_position_error
float64_t m_admissible_position_error
<
Definition: mpc.hpp:345
autoware::motion::control::trajectory_follower::MPCParam::input_delay
float64_t input_delay
for trajectory velocity calculation
Definition: mpc.hpp:69
autoware::motion::control::trajectory_follower::MPC::hasVehicleModel
bool8_t hasVehicleModel() const
return true if the vehicle model of this MPC is set
Definition: mpc.hpp:421
autoware::motion::control::trajectory_follower::MPCMatrix::Bex
Eigen::MatrixXd Bex
Definition: mpc.hpp:136
autoware::motion::control::trajectory_follower::MPCParam::weight_lat_error
float64_t weight_lat_error
<
Definition: mpc.hpp:78
autoware::motion::control::trajectory_follower::MPC::setVehicleModel
void setVehicleModel(std::shared_ptr< trajectory_follower::VehicleModelInterface > vehicle_model_ptr, const std::string &vehicle_model_type)
set the vehicle model of this MPC
Definition: mpc.hpp:393
autoware::motion::control::trajectory_follower::MPC::m_input_buffer
std::deque< float64_t > m_input_buffer
mpc raw output in previous period
Definition: mpc.hpp:340
autoware::motion::control::trajectory_follower::MPC::m_ref_traj
trajectory_follower::MPCTrajectory m_ref_traj
<
Definition: mpc.hpp:336
autoware::motion::control::trajectory_follower::MPC
calculate control command to follow reference waypoints
Definition: mpc.hpp:149
x
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
autoware::motion::control::trajectory_follower::Butterworth2dFilter::initialize
void initialize(const float64_t &dt, const float64_t &f_cutoff_hz)
constructor
Definition: lowpass_filter.cpp:34
motion_common.hpp
lowpass_filter.hpp
mpc_trajectory.hpp
autoware::motion::control::trajectory_follower::MPCMatrix::R2ex
Eigen::MatrixXd R2ex
Definition: mpc.hpp:141
autoware::motion::control::trajectory_follower::MPCMatrix::Cex
Eigen::MatrixXd Cex
Definition: mpc.hpp:138
autoware::motion::control::trajectory_follower::MPC::setClock
void setClock(rclcpp::Clock::SharedPtr clock)
set the RCLCPP clock to use for time keeping
Definition: mpc.hpp:442
autoware::motion::control::trajectory_follower
Definition: debug_values.hpp:29
autoware::motion::control::trajectory_follower::MPC::m_admissible_yaw_error_rad
float64_t m_admissible_yaw_error_rad
steering command limit [rad]
Definition: mpc.hpp:347
autoware::motion::control::trajectory_follower::MPC::setLogger
void setLogger(rclcpp::Logger logger)
set the RCLCPP logger to use for logging
Definition: mpc.hpp:435
autoware::motion::control::trajectory_follower::MPCMatrix::Yrefex
Eigen::MatrixXd Yrefex
Definition: mpc.hpp:143
motion
Definition: controller_base.hpp:31
autoware::motion::control::trajectory_follower::MPCParam
Definition: mpc.hpp:60
autoware::motion::control::trajectory_follower::MPCParam::weight_steering_input_squared_vel
float64_t weight_steering_input_squared_vel
lateral jerk weight
Definition: mpc.hpp:97
sys_info_node.data
dictionary data
Definition: sys_info_node.py:31
vehicle_model_bicycle_kinematics_no_delay.hpp
autoware::motion::control::trajectory_follower::MPCMatrix::Wex
Eigen::MatrixXd Wex
Definition: mpc.hpp:137
motion::motion_common::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_common.hpp:41
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_weight_steering_input
float64_t low_curvature_weight_steering_input
steering error * velocity weight in matrix R in low curvature point
Definition: mpc.hpp:105
autoware::motion::control::trajectory_follower::MPCMatrix::Aex
Eigen::MatrixXd Aex
Definition: mpc.hpp:135
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_weight_heading_error
float64_t low_curvature_weight_heading_error
heading error * velocity weight in matrix Q in low curvature point
Definition: mpc.hpp:90
autoware::motion::control::trajectory_follower::MPCParam::prediction_dt
float64_t prediction_dt
threshold that feed-forward angle becomes zero
Definition: mpc.hpp:65
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42
autoware::motion::control::trajectory_follower::MPCData::predicted_steer
float64_t predicted_steer
Definition: mpc.hpp:126
autoware::motion::control::trajectory_follower::MPCParam::velocity_time_constant
float64_t velocity_time_constant
time constant for steer model
Definition: mpc.hpp:73
autoware::motion::control::trajectory_follower::MPCMatrix::R1ex
Eigen::MatrixXd R1ex
Definition: mpc.hpp:140
autoware::motion::control::trajectory_follower::MPCParam::low_curvature_weight_steer_acc
float64_t low_curvature_weight_steer_acc
threshold of curvature to use "low curvature" parameter
Definition: mpc.hpp:113
autoware::motion::control::trajectory_follower::MPCParam::weight_terminal_lat_error
float64_t weight_terminal_lat_error
terminal heading error weight
Definition: mpc.hpp:84
autoware::motion::control::trajectory_follower::MPCParam::acceleration_limit
float64_t acceleration_limit
for trajectory velocity calculation
Definition: mpc.hpp:71
autoware::motion::control::trajectory_follower::MPCParam::weight_steering_input
float64_t weight_steering_input
<
Definition: mpc.hpp:95
autoware::motion::control::trajectory_follower::MPCParam::steer_tau
float64_t steer_tau
Definition: mpc.hpp:75
autoware::motion::control::trajectory_follower::MPCParam::weight_terminal_heading_error
float64_t weight_terminal_heading_error
lateral error weight in matrix Q in low curvature point
Definition: mpc.hpp:86
autoware::motion::control::trajectory_follower::MPCParam::weight_heading_error_squared_vel
float64_t weight_heading_error_squared_vel
terminal lateral error weight
Definition: mpc.hpp:82
autoware::motion::control::trajectory_follower::MPC::m_param
MPCParam m_param
mpc_output buffer for delay time compensation
Definition: mpc.hpp:338
autoware::motion::control::trajectory_follower::MPCParam::weight_steer_rate
float64_t weight_steer_rate
steering angle acceleration weight
Definition: mpc.hpp:101
autoware::motion::control::trajectory_follower::MPCData::yaw_err
float64_t yaw_err
Definition: mpc.hpp:128
vehicle_model_bicycle_kinematics.hpp
autoware::motion::control::trajectory_follower::MPCMatrix::Qex
Eigen::MatrixXd Qex
Definition: mpc.hpp:139
autoware::motion::control::trajectory_follower::MPCTrajectory
calculate control command to follow reference waypoints
Definition: mpc_trajectory.hpp:37