MPC(Model Predictive Control)總結（一）

原創

HamRadioDXer666

2020-02-21 16:30

參考資料：

Apollo代碼學習(六)—模型預測控制(MPC)
Apollo代碼學習(五)—橫縱向控制
Autoware 13.01相關組件
Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking"
使用pure pursuit實現無人車軌跡追蹤
優達學城課程練習https://github.com/mvirgo/MPC-Project
MATLAB課程（老師好看）

實驗demo：

因爲年底了需要讓小車泡泡。所以先以解析實際用到的開始，apollo可能有點來不及實現，接受程度相對不高，就以autoware爲模板吧。

autoware 之前一直以幾何預描點的形式解算，19年底加入了一些mpc模塊，代碼規範比較有參考意義

ros下運行節點圖

路點格式轉換（mpc_waypoints_converter ）

如上圖描述，訂閱三個節點進行路點格式的轉換，/closest_waypoint、base_waypoints、final_waypoints，這些話題是由上層的局部路徑規劃openplanner發佈的，一些接口格式需要進行一些轉換，僅此而已

    pub_waypoints_ = nh_.advertise<autoware_msgs::Lane>("/mpc_waypoints", 1);
    sub_closest_waypoint_ = nh_.subscribe("/closest_waypoint", 1, &MPCWaypointsConverter::callbackClosestWaypoints, this);
    sub_base_waypoints_ = nh_.subscribe("/base_waypoints", 1, &MPCWaypointsConverter::callbackBaseWaypoints, this);
    sub_final_waypoints_ = nh_.subscribe("/final_waypoints", 1, &MPCWaypointsConverter::callbackFinalWaypoints, this);


    autoware_msgs::Lane mpc_waypoints;
    mpc_waypoints.header = final_waypoints.header;
    mpc_waypoints.increment = final_waypoints.increment;
    mpc_waypoints.lane_id = final_waypoints.lane_id;
    mpc_waypoints.lane_index = final_waypoints.lane_index;
    mpc_waypoints.cost = final_waypoints.cost;
    mpc_waypoints.closest_object_distance = final_waypoints.closest_object_distance;
    mpc_waypoints.closest_object_velocity = final_waypoints.closest_object_velocity;
    mpc_waypoints.is_blocked = final_waypoints.is_blocked;
 
---

//在base_waypoints中找到最近的點索引（由於延遲，主題/closest_waypoints與/final_waypoints不一致）
int closest_idx = -1;
    for (int i = 0; i < (int)base_waypoints_.waypoints.size(); ++i) {
      const double d = sq_dist(final_waypoints.waypoints[1].pose.pose.position, base_waypoints_.waypoints[i].pose.pose.position);
      if (d < 0.01) {
        closest_idx = i;
        break;
      }
    }
    if (closest_idx == -1) {
      ROS_ERROR("cannot find closest base_waypoints' waypoint to final_waypoints.waypoint[1] !!");
    }

int base_start = std::max(closest_idx - back_waypoints_num_, 0);

//找到最近跑過的點，以當前點開始，往後倒back_waypoints_num個點作爲back優化點。

for (int i = base_start; i < closest_idx; ++i)

{

//往後倒back_waypoints_num個點作爲back優化點，將這幾個back點加入到mpc的局部路徑點

mpc_waypoints.waypoints.push_back(base_waypoints_.waypoints.at(i));

mpc_waypoints.waypoints.back().twist = final_waypoints.waypoints[1].twist;

}

同樣也需要將往前走的幾個點加入到mpc路點容器

int final_end = std::min(front_waypoints_num_ + 1, (int)final_waypoints.waypoints.size());
    for (int i = 1; i < final_end; ++i)
    {
      mpc_waypoints.waypoints.push_back(final_waypoints.waypoints.at(i));
    }

發佈mpc用到的局部路點

pub_waypoints_.publish(mpc_waypoints);
  }

主節點：/mpc_follower

主要節點在mpc_follower/mpc_follower_core.cpp裏面，先從頭文件查看有哪些主要函數

得到參考路徑
回調接收測試車當前Pose和Status
發佈了兩種commands，分別是通用的twist和autoware自定義的cmd
MPC解算
RVIZ可視化和twist估計以便debug

  /**
   * @brief set current_waypoints_ with receved message
   */
  void callbackRefPath(const autoware_msgs::Lane::ConstPtr &);

  /**
   * @brief set vehicle_status_.pose with receved message 
   */
  void callbackPose(const geometry_msgs::PoseStamped::ConstPtr &);

  /**
   * @brief set vehicle_status_.twist and vehicle_status_.tire_angle_rad with receved message
   */
  void callbackVehicleStatus(const autoware_msgs::VehicleStatus &msg);

  /**
   * @brief 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);

  /**
   * @brief publish control command as autoware_msgs/ControlCommand type
   * @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
   */
  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
   */
  bool calculateMPC(double &vel_cmd, double &acc_cmd, double &steer_cmd, double &steer_vel_cmd);

  /* debug */
  bool show_debug_info_;      //!< @brief flag to display debug info

  ros::Publisher pub_debug_filtered_traj_;        //!< @brief publisher for debug info
  ros::Publisher pub_debug_predicted_traj_;       //!< @brief publisher for debug info
  ros::Publisher pub_debug_values_;               //!< @brief publisher for debug info
  ros::Publisher pub_debug_mpc_calc_time_;        //!< @brief publisher for debug info

  ros::Subscriber sub_estimate_twist_;         //!< @brief subscriber for /estimate_twist for debug
  geometry_msgs::TwistStamped estimate_twist_; //!< @brief received /estimate_twist for debug

  /**
   * @brief convert MPCTraj to visualizaton marker for visualization
   */
  void convertTrajToMarker(const MPCTrajectory &traj, visualization_msgs::Marker &markers,
                           std::string ns, double r, double g, double b, double z);

  /**
   * @brief callback for estimate twist for debug
   */
  void callbackEstimateTwist(const geometry_msgs::TwistStamped &msg) { estimate_twist_ = msg; }
};

這也符合mpc的基本流程