百度Apollo5.0控制模块代码学习（七）MPC控制

1 流程图

2代码分析

以下内容对代码进行分析：

以下代码是加载MPC控制的相关配置参数；

bool MPCController::LoadControlConf(const ControlConf *control_conf) {
  if (!control_conf) {
    AERROR << "[MPCController] control_conf = nullptr";
    return false;
  }
  vehicle_param_ = VehicleConfigHelper::Instance()->GetConfig().vehicle_param();

  ts_ = control_conf->mpc_controller_conf().ts();
  CHECK_GT(ts_, 0.0) << "[MPCController] Invalid control update interval.";
  cf_ = control_conf->mpc_controller_conf().cf();
  cr_ = control_conf->mpc_controller_conf().cr();
  wheelbase_ = vehicle_param_.wheel_base();
  steer_ratio_ = vehicle_param_.steer_ratio();
  steer_single_direction_max_degree_ =
      vehicle_param_.max_steer_angle() * 180 / M_PI;
  max_lat_acc_ = control_conf->mpc_controller_conf().max_lateral_acceleration();
  wheel_single_direction_max_degree_ =
      steer_single_direction_max_degree_ / steer_ratio_ / 180 * M_PI;
  max_acceleration_ = vehicle_param_.max_acceleration();
  max_deceleration_ = vehicle_param_.max_deceleration();

  const double mass_fl = control_conf->mpc_controller_conf().mass_fl();
  const double mass_fr = control_conf->mpc_controller_conf().mass_fr();
  const double mass_rl = control_conf->mpc_controller_conf().mass_rl();
  const double mass_rr = control_conf->mpc_controller_conf().mass_rr();
  const double mass_front = mass_fl + mass_fr;
  const double mass_rear = mass_rl + mass_rr;
  mass_ = mass_front + mass_rear;

  lf_ = wheelbase_ * (1.0 - mass_front / mass_);
  lr_ = wheelbase_ * (1.0 - mass_rear / mass_);
  iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;

  mpc_eps_ = control_conf->mpc_controller_conf().eps();
  mpc_max_iteration_ = control_conf->mpc_controller_conf().max_iteration();
  throttle_lowerbound_ =
      std::max(vehicle_param_.throttle_deadzone(),
               control_conf->mpc_controller_conf().throttle_minimum_action());
  brake_lowerbound_ =
      std::max(vehicle_param_.brake_deadzone(),
               control_conf->mpc_controller_conf().brake_minimum_action());

  minimum_speed_protection_ = control_conf->minimum_speed_protection();
  max_acceleration_when_stopped_ =
      control_conf->max_acceleration_when_stopped();
  max_abs_speed_when_stopped_ = vehicle_param_.max_abs_speed_when_stopped();
  standstill_acceleration_ =
      control_conf->mpc_controller_conf().standstill_acceleration();

  enable_mpc_feedforward_compensation_ =
      control_conf->mpc_controller_conf().enable_mpc_feedforward_compensation();

  unconstraint_control_diff_limit_ =
      control_conf->mpc_controller_conf().unconstraint_control_diff_limit();

  LoadControlCalibrationTable(control_conf->mpc_controller_conf());
  AINFO << "MPC conf loaded";
  return true;
}

以下代码是对MPC控制器进行初始化，包括矩阵A，矩阵B，矩阵C，矩阵K，矩阵R，矩阵Q，加载MPC增益调度序列，初始化滤波器，

Status MPCController::Init(const ControlConf *control_conf) {
  if (!LoadControlConf(control_conf)) {
    AERROR << "failed to load control conf";
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
                  "failed to load control_conf");
  }
  // Matrix init operations.
  matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  matrix_a_(0, 1) = 1.0;
  matrix_a_(1, 2) = (cf_ + cr_) / mass_;
  matrix_a_(2, 3) = 1.0;
  matrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;
  matrix_a_(4, 5) = 1.0;
  matrix_a_(5, 5) = 0.0;
  // TODO(QiL): expand the model to accommodate more combined states.

  matrix_a_coeff_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  matrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;
  matrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;
  matrix_a_coeff_(2, 3) = 1.0;
  matrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;
  matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;

  matrix_b_ = Matrix::Zero(basic_state_size_, controls_);
  matrix_bd_ = Matrix::Zero(basic_state_size_, controls_);
  matrix_b_(1, 0) = cf_ / mass_;
  matrix_b_(3, 0) = lf_ * cf_ / iz_;
  matrix_b_(4, 1) = 0.0;
  matrix_b_(5, 1) = -1.0;
  matrix_bd_ = matrix_b_ * ts_;

  matrix_c_ = Matrix::Zero(basic_state_size_, 1);
  matrix_c_(5, 0) = 1.0;
  matrix_cd_ = Matrix::Zero(basic_state_size_, 1);

  matrix_state_ = Matrix::Zero(basic_state_size_, 1);
  matrix_k_ = Matrix::Zero(1, basic_state_size_);

  matrix_r_ = Matrix::Identity(controls_, controls_);

  matrix_q_ = Matrix::Zero(basic_state_size_, basic_state_size_);

  int r_param_size = control_conf->mpc_controller_conf().matrix_r_size();
  for (int i = 0; i < r_param_size; ++i) {
    matrix_r_(i, i) = control_conf->mpc_controller_conf().matrix_r(i);
  }

  int q_param_size = control_conf->mpc_controller_conf().matrix_q_size();
  if (basic_state_size_ != q_param_size) {
    const auto error_msg = common::util::StrCat(
        "MPC controller error: matrix_q size: ", q_param_size,
        " in parameter file not equal to basic_state_size_: ",
        basic_state_size_);
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
  }
  for (int i = 0; i < q_param_size; ++i) {
    matrix_q_(i, i) = control_conf->mpc_controller_conf().matrix_q(i);
  }

  // Update matrix_q_updated_ and matrix_r_updated_
  matrix_r_updated_ = matrix_r_;
  matrix_q_updated_ = matrix_q_;

  InitializeFilters(control_conf);
  LoadMPCGainScheduler(control_conf->mpc_controller_conf());
  LogInitParameters();
  AINFO << "[MPCController] init done!";
  return Status::OK();
}

下列代码就是加载MPC增益序列，具体有以下序列：
1，横向偏差增益序列；
2，航向偏差增益序列；
3，前馈项增益序列；
4，转向加权增益序列；

void MPCController::LoadMPCGainScheduler(
    const MPCControllerConf &mpc_controller_conf) {
  const auto &lat_err_gain_scheduler =
      mpc_controller_conf.lat_err_gain_scheduler();
  const auto &heading_err_gain_scheduler =
      mpc_controller_conf.heading_err_gain_scheduler();
  const auto &feedforwardterm_gain_scheduler =
      mpc_controller_conf.feedforwardterm_gain_scheduler();
  const auto &steer_weight_gain_scheduler =
      mpc_controller_conf.steer_weight_gain_scheduler();
  AINFO << "MPC control gain scheduler loaded";
  Interpolation1D::DataType xy1, xy2, xy3, xy4;
  for (const auto &scheduler : lat_err_gain_scheduler.scheduler()) {
    xy1.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  }
  for (const auto &scheduler : heading_err_gain_scheduler.scheduler()) {
    xy2.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  }
  for (const auto &scheduler : feedforwardterm_gain_scheduler.scheduler()) {
    xy3.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  }
  for (const auto &scheduler : steer_weight_gain_scheduler.scheduler()) {
    xy4.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  }

  lat_err_interpolation_.reset(new Interpolation1D);
  CHECK(lat_err_interpolation_->Init(xy1))
      << "Fail to load lateral error gain scheduler for MPC controller";

  heading_err_interpolation_.reset(new Interpolation1D);
  CHECK(heading_err_interpolation_->Init(xy2))
      << "Fail to load heading error gain scheduler for MPC controller";

  feedforwardterm_interpolation_.reset(new Interpolation1D);
  CHECK(feedforwardterm_interpolation_->Init(xy2))
      << "Fail to load feed forward term gain scheduler for MPC controller";

  steer_weight_interpolation_.reset(new Interpolation1D);
  CHECK(steer_weight_interpolation_->Init(xy2))
      << "Fail to load steer weight gain scheduler for MPC controller";
}

以下代码为计算控制命令，
输入：当前车辆状态，目标轨迹
输出：方向盘转向角度，油门，制动

Status MPCController::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis,
    const planning::ADCTrajectory *planning_published_trajectory,
    ControlCommand *cmd) {
  trajectory_analyzer_ =
      std::move(TrajectoryAnalyzer(planning_published_trajectory));

  SimpleMPCDebug *debug = cmd->mutable_debug()->mutable_simple_mpc_debug();
  debug->Clear();

计算纵向偏差，实现如下所示：

  ComputeLongitudinalErrors(&trajectory_analyzer_, debug);

计算纵向偏差。
1，首先求得一个matched_point，求取的具体方法可参考纵向控制。
2，然后 求得车辆当前位置到matched_point点的横向偏差，横向车速，纵向偏差，纵向车速。
3，求得reference_point，该点是时间距离当前车辆时间点最近的规划点。
4，计算航向角偏差，即当前车辆航向角和matched_point点的航向角差值。
5，计算车辆到matched_point的纵向车速偏差和纵向加速度偏差以及到reference_point的曲率变化；
6，设置各种偏差值，后面会用到；

void MPCController::ComputeLongitudinalErrors(
    const TrajectoryAnalyzer *trajectory_analyzer, SimpleMPCDebug *debug) {
  // the decomposed vehicle motion onto Frenet frame
  // s: longitudinal accumulated distance along reference trajectory
  // s_dot: longitudinal velocity along reference trajectory
  // d: lateral distance w.r.t. reference trajectory
  // d_dot: lateral distance change rate, i.e. dd/dt
  double s_matched = 0.0;
  double s_dot_matched = 0.0;
  double d_matched = 0.0;
  double d_dot_matched = 0.0;

  const auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(
      VehicleStateProvider::Instance()->x(),
      VehicleStateProvider::Instance()->y());

  trajectory_analyzer->ToTrajectoryFrame(
      VehicleStateProvider::Instance()->x(),
      VehicleStateProvider::Instance()->y(),
      VehicleStateProvider::Instance()->heading(),
      VehicleStateProvider::Instance()->linear_velocity(), matched_point,
      &s_matched, &s_dot_matched, &d_matched, &d_dot_matched);

  const double current_control_time = Clock::NowInSeconds();

  TrajectoryPoint reference_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(
          current_control_time);

  ADEBUG << "matched point:" << matched_point.DebugString();
  ADEBUG << "reference point:" << reference_point.DebugString();

  const double linear_v = VehicleStateProvider::Instance()->linear_velocity();
  const double linear_a =
      VehicleStateProvider::Instance()->linear_acceleration();
  double heading_error = common::math::NormalizeAngle(
      VehicleStateProvider::Instance()->heading() - matched_point.theta());
  double lon_speed = linear_v * std::cos(heading_error);
  double lon_acceleration = linear_a * std::cos(heading_error);
  double one_minus_kappa_lat_error = 1 - reference_point.path_point().kappa() *
                                             linear_v * std::sin(heading_error);

  debug->set_station_reference(reference_point.path_point().s());
  debug->set_station_feedback(s_matched);
  debug->set_station_error(reference_point.path_point().s() - s_matched);
  debug->set_speed_reference(reference_point.v());
  debug->set_speed_feedback(lon_speed);
  debug->set_speed_error(reference_point.v() - s_dot_matched);
  debug->set_acceleration_reference(reference_point.a());
  debug->set_acceleration_feedback(lon_acceleration);
  debug->set_acceleration_error(reference_point.a() -
                                lon_acceleration / one_minus_kappa_lat_error);
  double jerk_reference =
      (debug->acceleration_reference() - previous_acceleration_reference_) /
      ts_;
  double lon_jerk =
      (debug->acceleration_feedback() - previous_acceleration_) / ts_;
  debug->set_jerk_reference(jerk_reference);
  debug->set_jerk_feedback(lon_jerk);
  debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
  previous_acceleration_reference_ = debug->acceleration_reference();
  previous_acceleration_ = debug->acceleration_feedback();
}

更新状态变量；

  // Update state
  UpdateState(debug);

更新状态变量的具体代码如下：
首先计算车辆横向偏差，然后设置系统的状态参数如下：
1，横向偏差：当前车辆位置到距离车辆最近轨迹点（追踪点）的横向偏差。
2，横向偏差变化率：线车速与（当前车辆航向角-最近轨迹点航向角）的正弦。
3，航向角偏差：当前车辆航向角-最近轨迹点航向角。
4，航向角偏差变化率：当前车辆角速度-追踪点曲率*追踪点线车速（即追踪点的角速度）。
5，位置偏差：当前车辆到纵向偏差计算中的reference_point走过的距离-当前车辆到纵向matched_point的纵向距离。
6，速度偏差：reference_point的车速-纵向matched_point中的纵向车速。

void MPCController::UpdateState(SimpleMPCDebug *debug) {
  const auto &com = VehicleStateProvider::Instance()->ComputeCOMPosition(lr_);
  ComputeLateralErrors(com.x(), com.y(),
                       VehicleStateProvider::Instance()->heading(),
                       VehicleStateProvider::Instance()->linear_velocity(),
                       VehicleStateProvider::Instance()->angular_velocity(),
                       VehicleStateProvider::Instance()->linear_acceleration(),
                       trajectory_analyzer_, debug);

  // State matrix update;
  matrix_state_(0, 0) = debug->lateral_error();
  matrix_state_(1, 0) = debug->lateral_error_rate();
  matrix_state_(2, 0) = debug->heading_error();
  matrix_state_(3, 0) = debug->heading_error_rate();
  matrix_state_(4, 0) = debug->station_error();
  matrix_state_(5, 0) = debug->speed_error();
}

横向偏差计算的具体实现如下：
1，首先计算距离车辆最近的轨迹点matched_point，这个点和纵向控制中的matched_point不一样；
2，计算车辆当前位置到横向matched_point的横向偏差，航向角偏差，横向车速偏差，横向加速度偏差。

void MPCController::ComputeLateralErrors(
    const double x, const double y, const double theta, const double linear_v,
    const double angular_v, const double linear_a,
    const TrajectoryAnalyzer &trajectory_analyzer, SimpleMPCDebug *debug) {
  const auto matched_point =
      trajectory_analyzer.QueryNearestPointByPosition(x, y);

  const double dx = x - matched_point.path_point().x();
  const double dy = y - matched_point.path_point().y();

  const double cos_matched_theta = std::cos(matched_point.path_point().theta());
  const double sin_matched_theta = std::sin(matched_point.path_point().theta());
  // d_error = cos_matched_theta * dy - sin_matched_theta * dx;
  debug->set_lateral_error(cos_matched_theta * dy - sin_matched_theta * dx);

  // matched_theta = matched_point.path_point().theta();
  debug->set_ref_heading(matched_point.path_point().theta());
  const double delta_theta =
      common::math::NormalizeAngle(theta - debug->ref_heading());
  debug->set_heading_error(delta_theta);

  const double sin_delta_theta = std::sin(delta_theta);
  // d_error_dot = chassis_v * sin_delta_theta;
  double lateral_error_dot = linear_v * sin_delta_theta;
  double lateral_error_dot_dot = linear_a * sin_delta_theta;
  if (FLAGS_reverse_heading_control) {
    if (VehicleStateProvider::Instance()->gear() ==
        canbus::Chassis::GEAR_REVERSE) {
      lateral_error_dot = -lateral_error_dot;
      lateral_error_dot_dot = -lateral_error_dot_dot;
    }
  }

  debug->set_lateral_error_rate(lateral_error_dot);
  debug->set_lateral_acceleration(lateral_error_dot_dot);
  debug->set_lateral_jerk(
      (debug->lateral_acceleration() - previous_lateral_acceleration_) / ts_);
  previous_lateral_acceleration_ = debug->lateral_acceleration();

  // matched_kappa = matched_point.path_point().kappa();
  debug->set_curvature(matched_point.path_point().kappa());
  // theta_error = delta_theta;
  debug->set_heading_error(delta_theta);
  // theta_error_dot = angular_v - matched_point.path_point().kappa() *
  // matched_point.v();
  debug->set_heading_rate(angular_v);
  debug->set_ref_heading_rate(debug->curvature() * matched_point.v());
  debug->set_heading_error_rate(debug->heading_rate() -
                                debug->ref_heading_rate());

  debug->set_heading_acceleration(
      (debug->heading_rate() - previous_heading_rate_) / ts_);
  debug->set_ref_heading_acceleration(
      (debug->ref_heading_rate() - previous_ref_heading_rate_) / ts_);
  debug->set_heading_error_acceleration(debug->heading_acceleration() -
                                        debug->ref_heading_acceleration());
  previous_heading_rate_ = debug->heading_rate();
  previous_ref_heading_rate_ = debug->ref_heading_rate();

  debug->set_heading_jerk(
      (debug->heading_acceleration() - previous_heading_acceleration_) / ts_);
  debug->set_ref_heading_jerk(
      (debug->ref_heading_acceleration() - previous_ref_heading_acceleration_) /
      ts_);
  debug->set_heading_error_jerk(debug->heading_jerk() -
                                debug->ref_heading_jerk());
  previous_heading_acceleration_ = debug->heading_acceleration();
  previous_ref_heading_acceleration_ = debug->ref_heading_acceleration();
}

更新系数矩阵

  UpdateMatrix(debug);

更新系数矩阵的具体代码实现如下：
更新矩阵A，并采用双线性变换离散法进行离散化处理；
更新矩阵C，并将其离散化处理；

void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {
  const double v = std::max(VehicleStateProvider::Instance()->linear_velocity(),
                            minimum_speed_protection_);
  matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
  matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
  matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;
  matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;

  Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());
  matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *
               (matrix_i + ts_ * 0.5 * matrix_a_);

  matrix_c_(1, 0) = (lr_ * cr_ - lf_ * cf_) / mass_ / v - v;
  matrix_c_(3, 0) = -(lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_ / v;
  matrix_cd_ = matrix_c_ * debug->heading_error_rate() * ts_;
}

计算前馈控制量,

 FeedforwardUpdate(debug);

具体代码实现如下:首先计算前轮转角，然后将前轮转角转换成方向盘转角转动百分比。

void MPCController::FeedforwardUpdate(SimpleMPCDebug *debug) {
  const double v = VehicleStateProvider::Instance()->linear_velocity();
  const double kv =
      lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;
  steer_angle_feedforwardterm_ = Wheel2SteerPct(
      wheelbase_ * debug->curvature() + kv * v * v * debug->curvature());
}

如果需要增加对于高速转向的增益序列，则需要将Q，R矩阵的相应参数乘以车速。
并更新Q，R矩阵。

  // Add gain scheduler for higher speed steering
  if (FLAGS_enable_gain_scheduler) {
    matrix_q_updated_(0, 0) =
        matrix_q_(0, 0) *
        lat_err_interpolation_->Interpolate(
            VehicleStateProvider::Instance()->linear_velocity());
    matrix_q_updated_(2, 2) =
        matrix_q_(2, 2) *
        heading_err_interpolation_->Interpolate(
            VehicleStateProvider::Instance()->linear_velocity());
    steer_angle_feedforwardterm_updated_ =
        steer_angle_feedforwardterm_ *
        feedforwardterm_interpolation_->Interpolate(
            VehicleStateProvider::Instance()->linear_velocity());
    matrix_r_updated_(0, 0) =
        matrix_r_(0, 0) *
        steer_weight_interpolation_->Interpolate(
            VehicleStateProvider::Instance()->linear_velocity());
  } else {
    matrix_q_updated_ = matrix_q_;
    matrix_r_updated_ = matrix_r_;
    steer_angle_feedforwardterm_updated_ = steer_angle_feedforwardterm_;
  }

  debug->add_matrix_q_updated(matrix_q_updated_(0, 0));
  debug->add_matrix_q_updated(matrix_q_updated_(1, 1));
  debug->add_matrix_q_updated(matrix_q_updated_(2, 2));
  debug->add_matrix_q_updated(matrix_q_updated_(3, 3));

  debug->add_matrix_r_updated(matrix_r_updated_(0, 0));
  debug->add_matrix_r_updated(matrix_r_updated_(1, 1));

设置以下矩阵：
预测时序，参考时序等。

  Matrix control_matrix = Matrix::Zero(controls_, 1);
  std::vector<Matrix> control(horizon_, control_matrix);

  Matrix control_gain_matrix = Matrix::Zero(controls_, basic_state_size_);
  std::vector<Matrix> control_gain(horizon_, control_gain_matrix);

  Matrix addition_gain_matrix = Matrix::Zero(controls_, 1);
  std::vector<Matrix> addition_gain(horizon_, addition_gain_matrix);

  Matrix reference_state = Matrix::Zero(basic_state_size_, 1);
  std::vector<Matrix> reference(horizon_, reference_state);

  Matrix lower_bound(controls_, 1);
  lower_bound << -wheel_single_direction_max_degree_, max_deceleration_;

  Matrix upper_bound(controls_, 1);
  upper_bound << wheel_single_direction_max_degree_, max_acceleration_;

之后为解MPC控制方程。
MPC控制器解法和论文《Model Predictive Control of a Mobile Robot Using Linearization》(Walter Fetter Lages etc.)一致。在这里就不展开说明了，后续会详细分析。

  double mpc_start_timestamp = Clock::NowInSeconds();
  double steer_angle_feedback = 0.0;
  double acc_feedback = 0.0;
  double steer_angle_ff_compensation = 0.0;
  double unconstraint_control_diff = 0.0;
  double control_gain_truncation_ratio = 0.0;
  double unconstraint_control = 0.0;
  const double v = VehicleStateProvider::Instance()->linear_velocity();
  if (!common::math::SolveLinearMPC(matrix_ad_, matrix_bd_, matrix_cd_,
                                    matrix_q_updated_, matrix_r_updated_,
                                    lower_bound, upper_bound, matrix_state_,
                                    reference, mpc_eps_, mpc_max_iteration_,
                                    &control, &control_gain, &addition_gain)) {
    AERROR << "MPC solver failed";
  }

MPC算法求得两个最优值：车轮转角，加速度。
之将车轮转角转换成方向盘转角。
求出控制量的约束值。

 else {
    ADEBUG << "MPC problem solved! ";
    steer_angle_feedback = Wheel2SteerPct(control[0](0, 0));
    acc_feedback = control[0](1, 0);
    for (int i = 0; i < basic_state_size_; ++i) {
      unconstraint_control += control_gain[0](0, i) * matrix_state_(i, 0);
    }
    unconstraint_control += addition_gain[0](0, 0) * v * debug->curvature();

如果允许对MPC控制进行前馈补偿，则不受约束的控制量差值=正常控制量-约束控制量，如果该差值小于差值限制值，则计算方向盘转角补偿量。否则补偿量等于0。

    if (enable_mpc_feedforward_compensation_) {
      unconstraint_control_diff =
          Wheel2SteerPct(control[0](0, 0) - unconstraint_control);
      if (fabs(unconstraint_control_diff) <= unconstraint_control_diff_limit_) {
        steer_angle_ff_compensation =
            Wheel2SteerPct(debug->curvature() *
                           (control_gain[0](0, 2) *
                                (lr_ - lf_ / cr_ * mass_ * v * v / wheelbase_) -
                            addition_gain[0](0, 0) * v));
      } else {
        control_gain_truncation_ratio = control[0](0, 0) / unconstraint_control;
        steer_angle_ff_compensation =
            Wheel2SteerPct(debug->curvature() *
                           (control_gain[0](0, 2) *
                                (lr_ - lf_ / cr_ * mass_ * v * v / wheelbase_) -
                            addition_gain[0](0, 0) * v) *
                           control_gain_truncation_ratio);
      }
    } else {
      steer_angle_ff_compensation = 0.0;
    }
  }

  double mpc_end_timestamp = Clock::NowInSeconds();

  ADEBUG << "MPC core algorithm: calculation time is: "
         << (mpc_end_timestamp - mpc_start_timestamp) * 1000 << " ms.";

方向盘转角=MPC计算的方向盘转角+大车速下的方向盘转角增加量+方向盘转角补偿量。

  // TODO(QiL): evaluate whether need to add spline smoothing after the result
  double steer_angle = steer_angle_feedback +
                       steer_angle_feedforwardterm_updated_ +
                       steer_angle_ff_compensation;

如果允许设置方向盘转角限制值，则限制值为最大纵向加速度*轴距/速度平方转换成方向盘转角。
之后对其进行滤波处理。

  if (FLAGS_set_steer_limit) {
    const double steer_limit =
        std::atan(max_lat_acc_ * wheelbase_ /
                  (VehicleStateProvider::Instance()->linear_velocity() *
                   VehicleStateProvider::Instance()->linear_velocity())) *
        steer_ratio_ * 180 / M_PI / steer_single_direction_max_degree_ * 100;

    // Clamp the steer angle with steer limitations at current speed
    double steer_angle_limited =
        common::math::Clamp(steer_angle, -steer_limit, steer_limit);
    steer_angle_limited = digital_filter_.Filter(steer_angle_limited);
    steer_angle = steer_angle_limited;
    debug->set_steer_angle_limited(steer_angle_limited);
  }
  steer_angle = digital_filter_.Filter(steer_angle);
  // Clamp the steer angle to -100.0 to 100.0
  steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);

设置方向盘转动角度。

  cmd->set_steering_target(steer_angle);

加速命令=MPC求出的加速反馈+reference_point点的加速度；

  debug->set_acceleration_cmd_closeloop(acc_feedback);

  double acceleration_cmd = acc_feedback + debug->acceleration_reference();
  // TODO(QiL): add pitch angle feed forward to accommodate for 3D control

如果满足停车条件，则加速度为最大停车加速度。

  if ((planning_published_trajectory->trajectory_type() ==
       apollo::planning::ADCTrajectory::NORMAL) &&
      (std::fabs(debug->acceleration_reference()) <=
           max_acceleration_when_stopped_ &&
       std::fabs(debug->speed_reference()) <= max_abs_speed_when_stopped_)) {
    acceleration_cmd =
        (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
            ? std::max(acceleration_cmd, -standstill_acceleration_)
            : std::min(acceleration_cmd, standstill_acceleration_);
    ADEBUG << "Stop location reached";
    debug->set_is_full_stop(true);
  }
  // TODO(Yu): study the necessity of path_remain and add it to MPC if needed

设置加速度命令。

  debug->set_acceleration_cmd(acceleration_cmd);

根据加速度命令，查找标定表得到标定值。

  double calibration_value = 0.0;
  if (FLAGS_use_preview_speed_for_table) {
    calibration_value = control_interpolation_->Interpolate(
        std::make_pair(debug->speed_reference(), acceleration_cmd));
  } else {
    calibration_value = control_interpolation_->Interpolate(std::make_pair(
        VehicleStateProvider::Instance()->linear_velocity(), acceleration_cmd));
  }

设置标定值。
根据标定值分别计算油门/刹车值。

  debug->set_calibration_value(calibration_value);

  double throttle_cmd = 0.0;
  double brake_cmd = 0.0;
  if (calibration_value >= 0) {
    throttle_cmd = std::max(calibration_value, throttle_lowerbound_);
    brake_cmd = 0.0;
  } else {
    throttle_cmd = 0.0;
    brake_cmd = std::max(-calibration_value, brake_lowerbound_);
  }

  cmd->set_steering_rate(FLAGS_steer_angle_rate);
  // if the car is driven by acceleration, disgard the cmd->throttle and brake
  cmd->set_throttle(throttle_cmd);
  cmd->set_brake(brake_cmd);
  cmd->set_acceleration(acceleration_cmd);

  debug->set_heading(VehicleStateProvider::Instance()->heading());
  debug->set_steering_position(chassis->steering_percentage());
  debug->set_steer_angle(steer_angle);
  debug->set_steer_angle_feedforward(steer_angle_feedforwardterm_updated_);
  debug->set_steer_angle_feedforward_compensation(steer_angle_ff_compensation);
  debug->set_steer_unconstraint_control_diff(unconstraint_control_diff);
  debug->set_steer_angle_feedback(steer_angle_feedback);
  debug->set_steering_position(chassis->steering_percentage());

  if (std::fabs(VehicleStateProvider::Instance()->linear_velocity()) <=
          vehicle_param_.max_abs_speed_when_stopped() ||
      chassis->gear_location() == planning_published_trajectory->gear() ||
      chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
    cmd->set_gear_location(planning_published_trajectory->gear());
  } else {
    cmd->set_gear_location(chassis->gear_location());
  }

  ProcessLogs(debug, chassis);
  return Status::OK();
}