Autoware的MPC源码解析（五）mpc_follower解析：calculateMPC()函数解析

版权声明：本文为博主原创文章，未经博主允许不得转载。
源码地址：https://gitlab.com/autowarefoundation/autoware.ai/autoware/wikis/Source-Build
如有错误或建议，欢迎提出。

本文主要对核心函数calculateMPC()进行解析。

  const int N = mpc_param_.prediction_horizon;				//预测时域
  const double DT = mpc_param_.prediction_sampling_time;	//周期
  //根据选择的车辆模型，获取状态量X,输出量Y,控制量U的维度
  const int DIM_X = vehicle_model_ptr_->getDimX();
  const int DIM_U = vehicle_model_ptr_->getDimU();
  const int DIM_Y = vehicle_model_ptr_->getDimY();
  //获得车辆当前航向角度
  const double current_yaw = tf2::getYaw(vehicle_status_.pose.orientation);

计算出离路径最近的点以及其序号，并且计算出横向误差以及航向角误差

/* calculate nearest point on reference trajectory (used as initial state) */
  unsigned int nearest_index = 0;
  double yaw_err, dist_err, nearest_traj_time;
  geometry_msgs::Pose nearest_pose;
  if (!MPCUtils::calcNearestPoseInterp(ref_traj_, vehicle_status_.pose, nearest_pose, nearest_index, dist_err, yaw_err, nearest_traj_time))
  {
    ROS_WARN("[MPC] calculateMPC: error in calculating nearest pose. stop mpc.");
    return false;
  };

检查误差是否过大

  /* check if lateral error is not too large */
  if (dist_err > admisible_position_error_ || std::fabs(yaw_err) > amathutils::deg2rad(admisible_yaw_error_deg_ ))
  {
    ROS_WARN("[MPC] error is over limit, stop mpc. (pos: error = %f[m], limit: %f[m], yaw: error = %f[deg], limit %f[deg])",
             dist_err, admisible_position_error_, amathutils::rad2deg(yaw_err), admisible_yaw_error_deg_);
    return false;
  }

设置mpc初始时间并检查路径长度

/* set mpc initial time */
  const double mpc_start_time = nearest_traj_time;

  /* check trajectory length */
  const double mpc_end_time = mpc_start_time + (N - 1) * DT;
  if (mpc_end_time > ref_traj_.relative_time.back())
  {
    ROS_WARN("[MPC] path is too short for prediction. path end: %f[s], mpc end time: %f[s]", ref_traj_.relative_time.back(), mpc_end_time);
    return false;
  }

将笛卡尔坐标系下的横向误差转换成frenet坐标系下的横向误差

/* convert tracking x,y error to lat error */
  const double err_x = vehicle_status_.pose.position.x - nearest_pose.position.x;
  const double err_y = vehicle_status_.pose.position.y - nearest_pose.position.y;
  const double sp_yaw = tf2::getYaw(nearest_pose.orientation);
  const double err_lat = -sin(sp_yaw) * err_x + cos(sp_yaw) * err_y;//frenet坐标系下的误差

根据选择的车辆模型对状态变量赋初始值$x_0$

/* get steering angle */
  const double steer = vehicle_status_.tire_angle_rad;

/* define initial state for error dynamics 赋值状态变量X0*/
  Eigen::VectorXd x0 = Eigen::VectorXd::Zero(DIM_X);
  if (vehicle_model_type_ == "kinematics")
  {
    x0 << err_lat, yaw_err, steer;
  }
  else if (vehicle_model_type_ == "kinematics_no_delay")
  {
    x0 << err_lat, yaw_err;
  }
  else if (vehicle_model_type_ == "dynamics")
  {
    double dot_err_lat = (err_lat - lateral_error_prev_) / ctrl_period_;
    double dot_err_yaw = (yaw_err - yaw_error_prev_) / ctrl_period_;
    DEBUG_INFO("[MPC] (before lpf) dot_err_lat = %f, dot_err_yaw = %f", dot_err_lat, dot_err_yaw);
    lateral_error_prev_ = err_lat;
    yaw_error_prev_ = yaw_err;
    dot_err_lat = lpf_lateral_error_.filter(dot_err_lat);//滤波器处理
    dot_err_yaw = lpf_yaw_error_.filter(dot_err_yaw);
    DEBUG_INFO("[MPC] (after lpf) dot_err_lat = %f, dot_err_yaw = %f", dot_err_lat, dot_err_yaw);
    x0 << err_lat, dot_err_lat, yaw_err, dot_err_yaw;
  }
  else
  {
    ROS_ERROR("vehicle_model_type is undefined");
    return false;
  }
  DEBUG_INFO("[MPC] selfpose.x = %f, y = %f, yaw = %f", vehicle_status_.pose.position.x, vehicle_status_.pose.position.y, current_yaw);
  DEBUG_INFO("[MPC] nearpose.x = %f, y = %f, yaw = %f", nearest_pose.position.x, nearest_pose.position.y, tf2::getYaw(nearest_pose.orientation));
  DEBUG_INFO("[MPC] nearest_index = %d, nearest_traj_time = %f", nearest_index, nearest_traj_time);
  DEBUG_INFO("[MPC] lat error = %f, yaw error = %f, steer = %f, sp_yaw = %f, my_yaw = %f", err_lat, yaw_err, steer, sp_yaw, current_yaw);

优化目标建立如下：
设车辆的状态方程如下所示，其中矩阵A、B、C的值与选择的车辆的模型有关：
$$x(i+1)=Ax(i)+Bu(i)+C$$
则预测方程如下所示的

  [x1]        [A  ]           [B  0  0      ...     0]     [u0]       [C] 
  [x2]        [A^2]           [AB B  0      ...     0]     [u1]       [AC+C]
  [x3]    =   [A^3]   * x0  + [A^2B AB B    ...     0]  *  [u2]    +  [A^2C+AC+C]
  [...]       [...]           [..................... ]     [...]      [...]
  [xk]        [A^k]           [A^(K-1)B A^(K-2)B .. B]     [u(k-1)]   [A^(K-1)C+A^(K-2)C+...+C]
               ||                      ||                    ||           ||      
  Xec    =     Aex    * x0    +        Bex         *        Uex    +      Wex         (1)

建立cost function如下所示:

J = Xex’ * Qex * Xex + (Uex - Uref)’ * Rex * (Uex - Urefex) （2）

将（1）式带入（2）式可得：

J =Uex’*(Bex’ * Qex * Bex + Rex ) * Uex +2 * ((Aex * x0+Wex)’ * Qex * Bex - Urefex’ * Rex ) * Uex +
(Aex * x0 + Wex)’ * Qex * (Aex * x0 + Wex) + Urefex’ * Rex * Urefex ；

其中，后两项在程序每一次的循环中都是定值，所以化简目标函数如下：

J =0.5 * Uex’*(Bex’ * Qex * Bex + Rex ) * Uex + ((Aex * x0+Wex)’ * Qex * Bex - Urefex’ * Rex ) * Uex；

建立QP优化问题，其中
cost function:

1/2 * Uex’ * H * Uex + f’ * Uex

则：
H = Bex’ * Qex * Bex + Rex );
f = ((Aex * x0+Wex)’ * Qex * Bex - Urefex’ * Rex )’;

constraint matrix :

lb < U < ub, lbA < A*U < ubA

Autoware源码如下所示：
生成mpc矩阵：

/// generate mpc matrix  ///
  /*
   * predict equation: Xec = Aex * x0 + Bex * Uex + Wex
   * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * Rex * (Uex - Urefex)
   * Qex = diag([Q,Q,...]), Rex = diag([R,R,...])
   */
  Eigen::MatrixXd Aex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_X);
  Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_U * N);
  Eigen::MatrixXd Wex = Eigen::MatrixXd::Zero(DIM_X * N, 1);
  Eigen::MatrixXd Cex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_X * N);
  Eigen::MatrixXd Qex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_Y * N);
  Eigen::MatrixXd Rex = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N);
  Eigen::MatrixXd Urefex = Eigen::MatrixXd::Zero(DIM_U * N, 1);

  /* weight matrix depends on the vehicle model 设置权重矩阵*/
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y);
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_U, DIM_U);
  Eigen::MatrixXd Q_adaptive = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y);
  Eigen::MatrixXd R_adaptive = Eigen::MatrixXd::Zero(DIM_U, DIM_U);
  Q(0, 0) = mpc_param_.weight_lat_error;
  Q(1, 1) = mpc_param_.weight_heading_error;
  R(0, 0) = mpc_param_.weight_steering_input;

  Eigen::MatrixXd Ad(DIM_X, DIM_X);
  Eigen::MatrixXd Bd(DIM_X, DIM_U);
  Eigen::MatrixXd Wd(DIM_X, 1);
  Eigen::MatrixXd Cd(DIM_Y, DIM_X);
  Eigen::MatrixXd Uref(DIM_U, 1);

按照MPC的预测时域重置路径点

/* resample ref_traj with mpc sampling time */
  std::vector mpc_time_v;
  for (int i = 0; i < N; ++i)
  {
    mpc_time_v.push_back(mpc_start_time + i * DT);
  }
  MPCTrajectory mpc_resampled_ref_traj;
  if (!MPCUtils::interp1dMPCTraj(ref_traj_.relative_time, ref_traj_, mpc_time_v, mpc_resampled_ref_traj))
  {
    ROS_WARN("[MPC] calculateMPC: mpc resample error, stop mpc calculation. check code!");
    return false;
  }

设置权重矩阵Q，R以及求出预测时域内的参考转向角矩阵Urefex

/* predict dynamics for N times */
  for (int i = 0; i < N; ++i)//求出每个预测时刻下的参数矩阵
  {
    const double ref_k = mpc_resampled_ref_traj.k[i];
    const double ref_vx = mpc_resampled_ref_traj.vx[i];
    const double ref_vx_squared = ref_vx * ref_vx;

    /* get discrete state matrix A, B, C, W */
    vehicle_model_ptr_->setVelocity(ref_vx);
    vehicle_model_ptr_->setCurvature(ref_k);
    vehicle_model_ptr_->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT);

    Q_adaptive = Q;
    R_adaptive = R;
    if (i == N - 1)//设置路径终点处的权重
    {
      Q_adaptive(0, 0) = mpc_param_.weight_terminal_lat_error;
      Q_adaptive(1, 1) = mpc_param_.weight_terminal_heading_error;
    }
    Q_adaptive(1, 1) += ref_vx_squared * mpc_param_.weight_heading_error_squared_vel_coeff;
    R_adaptive(0, 0) += ref_vx_squared * mpc_param_.weight_steering_input_squared_vel_coeff;
    //航向角误差的权重和转向角度的权重是两部分的和：基本的权重以及和参考速度有关的补偿权重
    //每一个预测时域内的权重也不相同
    int idx_x_i = i * DIM_X;
    int idx_x_i_prev = (i - 1) * DIM_X;
    int idx_u_i = i * DIM_U;
    int idx_y_i = i * DIM_Y;
    if (i == 0)
    {
      Aex.block(0, 0, DIM_X, DIM_X) = Ad;
      Bex.block(0, 0, DIM_X, DIM_U) = Bd;
      Wex.block(0, 0, DIM_X, 1) = Wd;
    }
    else
    {
      Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X);
      for (int j = 0; j < i; ++j)
      {
        int idx_u_j = j * DIM_U;
        Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) = Ad * Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U);
      }
      Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd;
    }
    Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd;//加入对角线元素
    Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd;//赋值对角线元素
    Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive;//赋值对角线元素
    Rex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive;//赋值对角线元素

    /* get reference input (feed-forward) 根据每个预测点的曲率求前馈转向角，并且为了减少曲率噪声，档前馈角度小于2度时为0*/
    vehicle_model_ptr_->calculateReferenceInput(Uref);
    if (std::fabs(Uref(0, 0)) < amathutils::deg2rad(mpc_param_.zero_ff_steer_deg))
    {
      Uref(0, 0) = 0.0; // ignore curvature noise
    }

    Urefex.block(i * DIM_U, 0, DIM_U, 1) = Uref;
  }
  
  /* add lateral jerk : weight for (v * {u(i) - u(i-1)} )^2 */
  for (int i = 0; i < N - 1; ++i)
  {
    const double v = mpc_resampled_ref_traj.vx[i];
    const double lateral_jerk_weight = v * v * mpc_param_.weight_lat_jerk;
    Rex(i, i) += lateral_jerk_weight;
    Rex(i + 1, i) -= lateral_jerk_weight;
    Rex(i, i + 1) -= lateral_jerk_weight;
    Rex(i + 1, i + 1) += lateral_jerk_weight;
  }

QP优化

/// optimization ///
  /*
   * solve quadratic optimization.
   * cost function: 1/2 * Uex' * H * Uex + f' * Uex
   */
  const Eigen::MatrixXd CB = Cex * Bex;
  const Eigen::MatrixXd QCB = Qex * CB;
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N);
  H.triangularView() = CB.transpose() * QCB; // NOTE: This calculation is very heavy. searching for a good way...
  H.triangularView() += Rex;
  H.triangularView() = H.transpose();
  Eigen::MatrixXd f = (Cex * (Aex * x0 + Wex)).transpose() * QCB - Urefex.transpose() * Rex;
  /*
  * predict equation: Xec = Aex * x0 + Bex * Uex + Wex
  * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * Rex * (Uex - Urefex) 
  *
  * constraint matrix : lb < U < ub, lbA < A*U < ubA */
  const double u_lim = amathutils::deg2rad(steer_lim_deg_);
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N);
  Eigen::MatrixXd lbA = Eigen::MatrixXd::Zero(DIM_U * N, 1);
  Eigen::MatrixXd ubA = Eigen::MatrixXd::Zero(DIM_U * N, 1);
  Eigen::VectorXd lb = Eigen::VectorXd::Constant(DIM_U * N, -u_lim); // min steering angle
  Eigen::VectorXd ub = Eigen::VectorXd::Constant(DIM_U * N, u_lim);  // max steering angle

  auto start = std::chrono::system_clock::now();
  Eigen::VectorXd Uex;
  if (!qpsolver_ptr_->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex))
  {
    ROS_WARN("[MPC] qp solver error");
    return false;
  }
  double elapsed = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count() * 1.0e-6;
  DEBUG_INFO("[MPC] calculateMPC: qp solver calculation time = %f [ms]", elapsed);

  if (Uex.array().isNaN().any())
  {
    ROS_WARN("[MPC] calculateMPC: model Uex includes NaN, stop MPC. ");
    return false;
  }

  /* saturation */
  const double u_sat = std::max(std::min(Uex(0), u_lim), -u_lim);

  /* filtering 滤波*/
  const double u_filtered = lpf_steering_cmd_.filter(u_sat);

  /* set steering command */
  steer_cmd = u_filtered;
  steer_vel_cmd = (Uex(1) - Uex(0)) / DT;

  /* Velocity control: for simplicity, now we calculate steer and speed separately */
  vel_cmd = ref_traj_.vx[0];
  acc_cmd = (ref_traj_.vx[1] - ref_traj_.vx[0]) / DT;

  steer_cmd_prev_ = steer_cmd;

  DEBUG_INFO("[MPC] calculateMPC: mpc steer command raw = %f, filtered = %f, steer_vel_cmd = %f", Uex(0, 0), u_filtered, steer_vel_cmd);
  DEBUG_INFO("[MPC] calculateMPC: mpc vel command = %f, acc_cmd = %f", vel_cmd, acc_cmd);

至此，Autoware的mpc控制代码基本上解析完毕。如有建议或者错误，欢迎各位读者留言提出。抱拳抱拳抱拳。