autoware軌跡跟蹤之MPC(二)

回顧上節主函數中的主要函數列表和參數：

計算併發布具有恆定控制週期的路徑跟隨控制命令

void timerCallback(const ros::TimerEvent &);

使用收到的消息設置當前航路點

void callbackRefPath(const autoware_msgs::Lane::ConstPtr &);

需要設置當前poseset vehicle_status_.pose with receved message

void callbackPose(const geometry_msgs::PoseStamped::ConstPtr &);

publish control command calculated by MPC

* @param [in] vel_cmd velocity command [m/s] for vehicle control

* @param [in] acc_cmd acceleration command [m/s2] for vehicle control

* @param [in] steer_cmd steering angle command [rad] for vehicle control

* @param [in] steer_vel_cmd steering angle speed [rad/s] for vehicle control

將解算出的最後四個值進行發佈

void publishControlCommands(const double &vel_cmd, const double &acc_cmd,const double &steer_cmd, const double &steer_vel_cmd);

兩種最後的發佈方式：

* @brief publish control command as geometry_msgs/TwistStamped type

* @param [in] vel_cmd velocity command [m/s] for vehicle control

* @param [in] omega_cmd angular velocity command [rad/s] for vehicle control

void publishTwist(const double &vel_cmd, const double &omega_cmd);

void publishCtrlCmd(const double &vel_cmd, const double &acc_cmd, const double &steer_cmd);

* @brief calculate control command by MPC algorithm

* @param [out] vel_cmd velocity command

* @param [out] acc_cmd acceleration command

* @param [out] steer_cmd steering command

* @param [out] steer_vel_cmd steering rotation speed command

這個是最主要的函數，佔到70%內容

bool calculateMPC(double &vel_cmd, double &acc_cmd, double &steer_cmd, double &steer_vel_cmd);

接下來看mpc_follower_core.cpp具體實現：

1.節點參數初始化

  pnh_.param("path_filter_moving_ave_num", path_filter_moving_ave_num_, int(35));
  pnh_.param("path_smoothing_times", path_smoothing_times_, int(1));
  pnh_.param("curvature_smoothing_num", curvature_smoothing_num_, int(35));

一些參數的實際意義在git連接上做過介紹：https://github.com/HamRadioDXer/zhwl_mpc_follower

2. vehicle model setup

目前有三種

kinematics
kinematics_no_delay
dynamics

車輛模型設置：

if (vehicle_model_type_ == "kinematics")
  {
    double steer_tau;
    pnh_.param("vehicle_model_steer_tau", steer_tau, double(0.1));

    vehicle_model_ptr_ = std::make_shared<KinematicsBicycleModel>(wheelbase_, amathutils::deg2rad(steer_lim_deg_), steer_tau);
    ROS_INFO("[MPC] set vehicle_model = kinematics");
  }

函數原型描述：

/**

* @brief constructor with parameter initialization

* @param [in] wheelbase wheelbase length [m]

* @param [in] steer_lim steering angle limit [rad]

* @param [in] steer_tau steering time constant for 1d-model

*/

軸距軸距長度[m]
steer_lim轉向角限制[rad]
steer_tau一維模型的轉向時間常數

KinematicsBicycleModel(const double &wheelbase, const double &steer_lim, const double &steer_tau);

數學描述（參考https://blog.csdn.net/cyj1871/article/details/100553264）

其他模型類似

3.QP 求解器設置

現有三種求解器

unconstraint_fast
unconstraint
qpoases_hotstart

三種求解器繼承自虛基類

4.輸出命令的低通濾波處理：

  /* initialize lowpass filter */
  double steering_lpf_cutoff_hz, error_deriv_lpf_curoff_hz;
  pnh_.param("steering_lpf_cutoff_hz", steering_lpf_cutoff_hz, double(3.0));
  pnh_.param("error_deriv_lpf_curoff_hz", error_deriv_lpf_curoff_hz, double(5.0));
  lpf_steering_cmd_.initialize(ctrl_period_, steering_lpf_cutoff_hz);
  lpf_lateral_error_.initialize(ctrl_period_, error_deriv_lpf_curoff_hz);
  lpf_yaw_error_.initialize(ctrl_period_, error_deriv_lpf_curoff_hz);