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的基本流程