【运动控制】Apollo6.0的mpc_controller解析

mpc_controller解析

知乎：【运动控制】Apollo6.0的mpc_controller解析

1 Init

1.1 输入

输入为控制配置表control_conf,判断是否加载成功。

if (!LoadControlConf(control_conf)) {
    AERROR << "failed to load control conf";
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
                  "failed to load control_conf");
  }

1.2 动力学模型初始化

矩阵初始化依据车辆动力学模型，参考《Vehicle Dynamics and Control》的2.5 Dynamic Model in Terms of Error with Respect to Road(P37)。
d d t [ l a t e r a l _ e r r o r l a t e r a l _ e r r o r _ r a t e h e a d i n g _ e r r o r h e a d i n g _ e r r o r _ r a t e s t a t i o n _ e r r o r s p e e d _ e r r o r ] = [ 0 1 0 0 0 0 0 − C f + C r m V x C f + C r m C r l r − C f l f m V x 0 0 0 0 0 1 0 0 0 C r l r − C f l f I z V x C f l f − C r l r I z − C r l r 2 + C f l f 2 I z V x 0 0 0 0 0 0 0 1 0 0 0 0 0 0 ] [ l a t e r a l _ e r r o r l a t e r a l _ e r r o r _ r a t e h e a d i n g _ e r r o r h e a d i n g _ e r r o r _ r a t e s t a t i o n _ e r r o r s p e e d _ e r r o r ] + [ 0 0 C f m 0 0 0 C f l f I z 0 0 0 0 − 1 ] [ δ f Δ a x ] + [ 0 C r l r − C f l f m V x − V x 0 − C r l r 2 + C f l f 2 I z V x 0 0 ] Ψ ˙ d e s \begin{aligned} \frac{d}{dt} \begin{bmatrix} {lateral\_error} \\ {lateral\_error\_rate} \\ {heading\_error} \\ {heading\_error\_rate} \\ {station\_error} \\ {speed\_error} \end{bmatrix} &= \begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0\\ 0 & -\frac{C_f+C_r}{mV_x} & \frac{C_f+C_r}{m} & \frac{C_r l_r-C_f l_f}{mV_x} & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & \frac{C_r l_r-C_f l_f}{I_z V_x} & \frac{C_f l_f-C_r l_r}{I_z} & -\frac{C_r l_r^2+C_f l_f^2}{I_z V_x} & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} {lateral\_error} \\ {lateral\_error\_rate} \\ {heading\_error} \\ {heading\_error\_rate} \\ {station\_error} \\ {speed\_error} \end{bmatrix} \\ &+ \begin{bmatrix} 0 & 0\\ \frac{C_f}{m} & 0\\ 0 & 0\\ \frac{C_f l_f}{I_z} & 0\\ 0 & 0\\ 0 & -1 \end{bmatrix} \begin{bmatrix} \delta_f \\ \Delta a_x \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{C_r l_r-C_f l_f}{mV_x} - V_x \\ 0 \\ -\frac{C_r l_r^2+C_f l_f^2}{I_z V_x} \\ 0 \\ 0 \end{bmatrix} \dot{\Psi}_{des} \end{aligned} dtd​⎣⎢⎢⎢⎢⎢⎢⎡​lateral_errorlateral_error_rateheading_errorheading_error_ratestation_errorspeed_error​⎦⎥⎥⎥⎥⎥⎥⎤​​=⎣⎢⎢⎢⎢⎢⎢⎢⎡​000000​1−mVx​Cf​+Cr​​0Iz​Vx​Cr​lr​−Cf​lf​​00​0mCf​+Cr​​0Iz​Cf​lf​−Cr​lr​​00​0mVx​Cr​lr​−Cf​lf​​1−Iz​Vx​Cr​lr2​+Cf​lf2​​00​000000​000010​⎦⎥⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎡​lateral_errorlateral_error_rateheading_errorheading_error_ratestation_errorspeed_error​⎦⎥⎥⎥⎥⎥⎥⎤​+⎣⎢⎢⎢⎢⎢⎢⎡​0mCf​​0Iz​Cf​lf​​00​00000−1​⎦⎥⎥⎥⎥⎥⎥⎤​[δf​Δax​​]+⎣⎢⎢⎢⎢⎢⎢⎢⎡​0mVx​Cr​lr​−Cf​lf​​−Vx​0−Iz​Vx​Cr​lr2​+Cf​lf2​​00​⎦⎥⎥⎥⎥⎥⎥⎥⎤​Ψ˙des​​
上式可以简化成下式
d d t x = A x + B u + C Ψ ˙ d e s \begin{aligned} \frac{d}{dt}x = Ax + Bu + C\dot{\Psi}_{des} \end{aligned} dtd​x=Ax+Bu+CΨ˙des​​

  // 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);
  // 20210915, yanghq
  // matrix_c_(5, 0) = 1.0;
  matrix_cd_ = Matrix::Zero(basic_state_size_, 1);

1.3 前馈矩阵初始化

参考以下公式
u f e e d f o r w a r d = − k x u_{feedforward} = - kx ufeedforward​=−kx

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

1.3 QP问题Q和R初始化

参考以下公式
min ⁡ x k , u k J = min ⁡ x k , u k [ ∑ 0 N ( x k − x r ) T Q ( x k − x r ) + ∑ 0 N − 1 u k T R u k ] \begin{aligned} \underset{x_k,u_k}{\min} J = \underset{x_k,u_k}{\min}\left [ \sum_{0}^{N}(x_k-x_r)^T Q(x_k-x_r) + \sum_{0}^{N-1}u_k^T R u_k \right ] \\ \end{aligned} xk​,uk​min​J=xk​,uk​min​[0∑N​(xk​−xr​)TQ(xk​−xr​)+0∑N−1​ukT​Ruk​]​

  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 =
        absl::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_;

1.4 滤波器初始化

InitializeFilters(control_conf);

低通滤波器，采用的是巴特沃斯双极点低通滤波器，具体可参阅LpfCoefficients解析。

void MPCController::InitializeFilters(const ControlConf *control_conf) {
  // Low pass filter
  std::vector<double> den(3, 0.0);
  std::vector<double> num(3, 0.0);
  common::LpfCoefficients(
      ts_, control_conf->mpc_controller_conf().cutoff_freq(), &den, &num);
  digital_filter_.set_coefficients(den, num);
  lateral_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
      control_conf->mpc_controller_conf().mean_filter_window_size()));
  heading_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
      control_conf->mpc_controller_conf().mean_filter_window_size()));
}

1.5 加载MPC增益调度器

LoadMPCGainScheduler(control_conf->mpc_controller_conf());

加载增益调度表，构造一维线性差值器Interpolation1D。

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();
  ADEBUG << "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";
}

1.6 Log初始化参数

LogInitParameters();

在log里打印质量、绕z轴转动惯量、质心与前轴距离、质心与后轴距离。

void MPCController::LogInitParameters() {
  ADEBUG << name_ << " begin.";
  ADEBUG << "[MPCController parameters]"
         << " mass_: " << mass_ << ","
         << " iz_: " << iz_ << ","
         << " lf_: " << lf_ << ","
         << " lr_: " << lr_;
}

1.7 返回

 ADEBUG << "[MPCController] init done!";
  return Status::OK();

2 ComputeControlCommand

2.1 输入

定位、底盘、规划发送轨迹、控制命令

const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
 const planning::ADCTrajectory *planning_published_trajectory,
ControlCommand *cmd

2.2 过程

2.2.1 拷贝轨迹和创建debug

拷贝轨迹

trajectory_analyzer_ =
      std::move(TrajectoryAnalyzer(planning_published_trajectory));

创建debug

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

2.2.2 计算纵向误差

ComputeLongitudinalErrors(&trajectory_analyzer_, debug);

参数初始化

  double s_matched = 0.0;
  double s_dot_matched = 0.0;
  double d_matched = 0.0;
  double d_dot_matched = 0.0;

查询位置最近点matched_point

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

在轨迹坐标系下，计算s_matched, s_dot_matched, d_matched, d_dot_matched

  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);

查询时间最近点reference_point

  const double current_control_time = Clock::NowInSeconds();

  TrajectoryPoint reference_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTimInterpolation1De(
          current_control_time);

计算速度linear_v、加速度linear_a、航向角误差heading_error、纵向速度lon_speed、纵向加速度lon_acceleration、横向误差系数one_minus_kappa_lat_error

  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更新位置参考station_reference、位置反馈station_feedback、位置误差station_error、速度参考speed_reference、速度反馈speed_feedback、速度误差speed_error、加速度参考acceleration_reference、加速度反馈acceleration_feedback、加速度误差acceleration_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);

debug更新加加速度参考jerk_reference、纵向加加速度反馈lon_jerk、加加速度误差jerk_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和加速度反馈previous_acceleration_

  previous_acceleration_reference_ = debug->acceleration_reference();
  previous_acceleration_ = debug->acceleration_feedback();

2.2.3 更新状态量、矩阵和前馈

  // Update state
UpdateState(debug);
UpdateMatrix(debug);
FeedforwardUpdate(debug);

UpdateState函数，更新横向误差lateral_error、横向误差变化率lateral_error_rate、航向角误差heading_error、航向角误差变化率heading_error_rate、位置误差station_error。对应的向量如下
[ l a t e r a l _ e r r o r l a t e r a l _ e r r o r _ r a t e h e a d i n g _ e r r o r h e a d i n g _ e r r o r _ r a t e s t a t i o n _ e r r o r s p e e d _ e r r o r ] \begin{aligned} \begin{bmatrix} {lateral\_error} \\ {lateral\_error\_rate} \\ {heading\_error} \\ {heading\_error\_rate} \\ {station\_error} \\ {speed\_error} \end{bmatrix} \end{aligned} ⎣⎢⎢⎢⎢⎢⎢⎡​lateral_errorlateral_error_rateheading_errorheading_error_ratestation_errorspeed_error​⎦⎥⎥⎥⎥⎥⎥⎤​​

  // 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();

UpdateMatrix函数，更新matrix_a_、matrix_ad_、matrix_c_、matrix_cd_

  matrix_a_(1, 1) = matrix_a_(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函数，计算kv和steer_angle_feedforwardterm_，参考公式(Vehicl Dynamics and Control, P57)如下
K v = l r m 2 C α f ( l f + l r ) − l f m 2 C α r ( l f + l r ) δ f f = L R + K v a y − k 3 [ l r R − l f 2 C α r m V x 2 R L ] \begin{aligned} K_v &= \frac{l_rm}{2C_{\alpha f}(l_f+l_r)} - \frac{l_fm}{2C_{\alpha r}(l_f+l_r)} \\ \delta_{ff} &= \frac{L}{R} + K_v a_y - k_3\left [ \frac{l_r}{R} - \frac{l_f}{2C_{\alpha r}} \frac{{mV_x}^{2} }{RL} \right ] \end{aligned} Kv​δff​​=2Cαf​(lf​+lr​)lr​m​−2Cαr​(lf​+lr​)lf​m​=RL​+Kv​ay​−k3​[Rlr​​−2Cαr​lf​​RLmVx​2​]​

计算转角前馈的公式有些不同，如下
κ = R − 1 a y = v 2 R = v 2 κ δ f f _ 1 = L κ + K v v 2 κ \begin{aligned} \kappa &= R^{-1} \\ a_y &= \frac{v^2}{R} = v^2 \kappa \\ \delta_{ff\_1} &= L \kappa + K_v v^2 \kappa \end{aligned} κay​δff_1​​=R−1=Rv2​=v2κ=Lκ+Kv​v2κ​

  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());

问题： K v K_v Kv​的计算公式里多除了2，因为Cf和Cr赋值时已经是2倍了。

2.2.4 高速转向增益

gain_scheduler参数调整q矩阵的(0, 0)和(2,2)（横向偏差和航向角偏差），前馈增益，r矩阵的（0，0）(输出转角)。

  // 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_;
  }

2.2.5 debug更新q和r

  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));

2.2.6 矩阵和参数初始化

controls_为控制时域长度（代码里为2），horizon_为预测时域长度（代码里为10）

control_gain和addition_gain为控制增益矩阵，对应于无约束QP问题，无约束QP问题相当于

  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_;

  const double max = std::numeric_limits<double>::max();
  Matrix lower_state_bound(basic_state_size_, 1);
  Matrix upper_state_bound(basic_state_size_, 1);

  // lateral_error, lateral_error_rate, heading_error, heading_error_rate
  // station_error, station_error_rate
  lower_state_bound << -1.0 * max, -1.0 * max, -1.0 * M_PI, -1.0 * max,
      -1.0 * max, -1.0 * max;
  upper_state_bound << max, max, M_PI, max, max, max;

  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 unconstrained_control_diff = 0.0;
  double control_gain_truncation_ratio = 0.0;
  double unconstrained_control = 0.0;
  const double v = VehicleStateProvider::Instance()->linear_velocity();

2.2.7 优化求解(osqp或linear)

对于车辆误差动力学方程，有
x k + 1 = A x k + B u k + C \begin{aligned} x_{k+1} = Ax_{k} + Bu_{k} + C \end{aligned} xk+1​=Axk​+Buk​+C​
状态变量x(k)，输入量u(k)，如下
x ( k ) = [ e l ( k ) e l ˙ ( k ) e ψ ( k ) e ψ ˙ ( k ) e s ( k ) e s ˙ ( k ) ] ， u ( k ) = [ δ ( k ) a ( k ) ] x(k) = \begin{bmatrix} e_{l}(k) \\ \dot{e_{l}}(k) \\ e_{\psi }(k) \\ \dot{e_{\psi }}(k) \\ e_{s}(k) \\ \dot{e_{s}}(k) \\ \end{bmatrix} ， u(k) = \begin{bmatrix} \delta_{}(k) \\ a_{}(k) \\ \end{bmatrix} x(k)=⎣⎢⎢⎢⎢⎢⎢⎡​el​(k)el​˙​(k)eψ​(k)eψ​˙​(k)es​(k)es​˙​(k)​⎦⎥⎥⎥⎥⎥⎥⎤​，u(k)=[δ​(k)a​(k)​]
状态量的约束条件为 x m i n x_{min} xmin​和 x m a x x_{max} xmax​，输入量的约束条件为 u m i n u_{min} umin​和 u m a x u_{max} umax​。

k k k时刻的状态代价矩阵为 Q Q Q，输入代价矩阵为 R R R。

优化目标函数如下
min ⁡ x k , u k J = min ⁡ x k , u k [ ∑ 0 N ( x k − x r ) T Q ( x k − x r ) + ∑ 0 N − 1 u k T R u k ] \begin{aligned} \underset{x_k,u_k}{\min} J = \underset{x_k,u_k}{\min}\left [ \sum_{0}^{N}(x_k-x_r)^T Q(x_k-x_r) + \sum_{0}^{N-1}u_k^T R u_k \right ] \\ \end{aligned} xk​,uk​min​J=xk​,uk​min​[0∑N​(xk​−xr​)TQ(xk​−xr​)+0∑N−1​ukT​Ruk​]​

x k + 1 = A x k + B u k x m i n ≤ x k ≤ x m a x u m i n ≤ u k ≤ u m a x \begin{aligned} x_{k+1} = Ax_{k} + Bu_{k} \\ x_{min} \le x_k \le x_{max} \\ u_{min} \le u_k \le u_{max} \\ \end{aligned} xk+1​=Axk​+Buk​xmin​≤xk​≤xmax​umin​≤uk​≤umax​​

式中， N N N为预测时域horizon。

  std::vector<double> control_cmd(controls_, 0);
  if (FLAGS_use_osqp_solver) {
    apollo::common::math::MpcOsqp mpc_osqp(
        matrix_ad_, matrix_bd_, matrix_q_updated_, matrix_r_updated_,
        matrix_state_, lower_bound, upper_bound, lower_state_bound,
        upper_state_bound, reference_state, mpc_max_iteration_, horizon_,
        mpc_eps_);
    if (!mpc_osqp.Solve(&control_cmd)) {
      AERROR << "MPC OSQP solver failed";
    } else {
      ADEBUG << "MPC OSQP problem solved! ";
      control[0](0, 0) = control_cmd.at(0);
      control[0](1, 0) = control_cmd.at(1);
    }
  } else {
    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";
    } else {
      ADEBUG << "MPC problem solved! ";
    }
  }

2.2.7.1 osqp

osqp二次规划的标准形式如下
min ⁡ x 1 2 x T P x + q T x l ≤ A c x ≤ u \underset{x}{\min} \frac{1}{2}x^TPx + q^Tx \\ l \le A_c x \le u xmin​21​xTPx+qTxl≤Ac​x≤u
上述方程的决策变量 x x x，由状态变量和输入构成，维度为horizon+1+control，如下
x = [ x ( k ) x ( k + 1 ) ⋮ x ( k + N ) u ( k ) ⋮ u ( k + N − 1 ) ] x = \begin{bmatrix} x(k) \\ x(k+1) \\ \vdots \\ x(k+N) \\ u(k) \\ \vdots \\ u(k+N-1) \end{bmatrix} x=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​x(k)x(k+1)⋮x(k+N)u(k)⋮u(k+N−1)​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​
式中， N N N为预测步长。

Hessian矩阵 P P P的形式如下(CalculateKernel)
P = d i a g ( Q , Q , . . . , Q , R , . . . , R ) P = diag(Q,Q,...,Q,R,...,R) P=diag(Q,Q,...,Q,R,...,R)
Gradient向量q的形式如下(CalculateGradient)

问题：向量q计算时， x r x_r xr​基本为零。osqp的形式只有 1 2 x T P x \frac{1}{2}x^TPx 21​xTPx起作用，基本就退化为 ∑ 0 N ( x k − x r ) T Q ( x k − x r ) + ∑ 0 N − 1 u k T R u k \sum_{0}^{N}(x_k-x_r)^T Q(x_k-x_r) + \sum_{0}^{N-1}u_k^T R u_k ∑0N​(xk​−xr​)TQ(xk​−xr​)+∑0N−1​ukT​Ruk​。

对于每个部分的误差，实际上是 e r r o r = A x k + B u k − x k + 1 error = Ax_{k} + Bu_{k} - x_{k+1} error=Axk​+Buk​−xk+1​，但 A x k + B u k − x k + 1 = − C Ax_{k} + Bu_{k} - x_{k+1} = -C Axk​+Buk​−xk+1​=−C，这个误差不会趋近于零。

q = [ − Q x r − Q x r ⋮ − Q x r 0 ⋮ 0 ] q = \begin{bmatrix} -Qx_r \\ -Qx_r \\ \vdots \\ -Qx_r \\ 0 \\ \vdots \\ 0 \end{bmatrix} q=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​−Qxr​−Qxr​⋮−Qxr​0⋮0​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​

Equality Constraint矩阵A的形式如下(CalculateEqualityConstraint)
A c = [ E x E u I E x I E u ] A_c = \begin{bmatrix} E_x & E_u \\ IE_x & IE_u \end{bmatrix} Ac​=[Ex​IEx​​Eu​IEu​​]

E x = [ − I 0 0 … 0 A − I 0 … 0 0 A − I … 0 ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 … − I ] , E u = [ 0 0 … 0 B 0 … 0 0 B … 0 ⋮ ⋮ ⋱ ⋮ 0 0 … B ] E_x = \begin{bmatrix} -I & 0 & 0 & \dots & 0\\ A & -I & 0 & \dots & 0\\ 0 & A & -I & \dots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & \dots & -I \end{bmatrix}, E_u = \begin{bmatrix} 0 & 0 & \dots & 0\\ B & 0 & \dots & 0\\ 0 & B & \dots & 0\\ \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & \dots & B \end{bmatrix} Ex​=⎣⎢⎢⎢⎢⎢⎡​−IA0⋮0​0−IA⋮0​00−I⋮0​………⋱…​000⋮−I​⎦⎥⎥⎥⎥⎥⎤​,Eu​=⎣⎢⎢⎢⎢⎢⎡​0B0⋮0​00B⋮0​………⋱…​000⋮B​⎦⎥⎥⎥⎥⎥⎤​

I E x = [ I 0 0 … 0 0 I 0 … 0 0 0 I … 0 ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 … I 0 0 0 … 0 0 0 0 … 0 ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 … 0 ] , I E u = [ 0 0 … 0 0 0 … 0 0 0 … 0 ⋮ ⋮ ⋱ ⋮ 0 0 … 0 I 0 … 0 0 I … 0 ⋮ ⋮ ⋱ ⋮ 0 0 … I ] IE_x = \begin{bmatrix} I & 0 & 0 & \dots & 0\\ 0 & I & 0 & \dots & 0\\ 0 & 0 & I & \dots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & \dots & I \\ 0 & 0 & 0 & \dots & 0\\ 0 & 0 & 0 & \dots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & \dots & 0 \end{bmatrix}, IE_u = \begin{bmatrix} 0 & 0 & \dots & 0\\ 0 & 0 & \dots & 0\\ 0 & 0 & \dots & 0\\ \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & \dots & 0 \\ I & 0 & \dots & 0\\ 0 & I & \dots & 0\\ \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & \dots & I \end{bmatrix} IEx​=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​I00⋮000⋮0​0I0⋮000⋮0​00I⋮000⋮0​………⋱………⋱…​000⋮I00⋮0​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​,IEu​=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​000⋮0I0⋮0​000⋮00I⋮0​………⋱………⋱…​000⋮000⋮I​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​

Constraint向量 l l l和 u u u的形式如下(CalculateConstraintVectors)
l = [ − x 0 0 ⋮ 0 x m i n ⋮ x m i n u m i n ⋮ u m i n ] , u = [ − x 0 0 ⋮ 0 x m a x ⋮ x m a x u m a x ⋮ u m a x ] l = \begin{bmatrix} -x_0 \\ 0 \\ \vdots \\ 0 \\ x_{min} \\ \vdots \\ x_{min} \\ u_{min} \\ \vdots \\ u_{min} \\ \end{bmatrix}, u = \begin{bmatrix} -x_0 \\ 0 \\ \vdots \\ 0 \\ x_{max} \\ \vdots \\ x_{max} \\ u_{max} \\ \vdots \\ u_{max} \\ \end{bmatrix} l=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​−x0​0⋮0xmin​⋮xmin​umin​⋮umin​​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​,u=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​−x0​0⋮0xmax​⋮xmax​umax​⋮umax​​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​
对于MpcOsqp对象，matrix_a为系统动力学矩阵，matrix_b为控制矩阵，matrix_q为状态量的代价矩阵，matrix_r为控制量的代价矩阵，matrix_initial_x为初始状态量，matrix_u_lower为控制下限，matrix_u_upper为控制上限，matrix_x_lower为状态量下限，matrix_x_upper为状态量上限，matrix_x_ref为参考状态量，max_iter为最大迭代次数，horizon为预测时域，eps_abs为容忍度。

state_dim为状态量维度，control_dim为控制量维度,num_param为未知。。。

MpcOsqp::MpcOsqp(const Eigen::MatrixXd &matrix_a,
                 const Eigen::MatrixXd &matrix_b,
                 const Eigen::MatrixXd &matrix_q,
                 const Eigen::MatrixXd &matrix_r,
                 const Eigen::MatrixXd &matrix_initial_x,
                 const Eigen::MatrixXd &matrix_u_lower,
                 const Eigen::MatrixXd &matrix_u_upper,
                 const Eigen::MatrixXd &matrix_x_lower,
                 const Eigen::MatrixXd &matrix_x_upper,
                 const Eigen::MatrixXd &matrix_x_ref, const int max_iter,
                 const int horizon, const double eps_abs)
    : matrix_a_(matrix_a),
      matrix_b_(matrix_b),
      matrix_q_(matrix_q),
      matrix_r_(matrix_r),
      matrix_initial_x_(matrix_initial_x),
      matrix_u_lower_(matrix_u_lower),
      matrix_u_upper_(matrix_u_upper),
      matrix_x_lower_(matrix_x_lower),
      matrix_x_upper_(matrix_x_upper),
      matrix_x_ref_(matrix_x_ref),
      max_iteration_(max_iter),
      horizon_(horizon),
      eps_abs_(eps_abs) {
  state_dim_ = matrix_b.rows();
  control_dim_ = matrix_b.cols();
  ADEBUG << "state_dim" << state_dim_;
  ADEBUG << "control_dim_" << control_dim_;
  num_param_ = state_dim_ * (horizon_ + 1) + control_dim_ * horizon_;
}

2.2.7.2 linear

待补充

2.2.8 转角和加速度反馈

输出参数为前轮转角steer_angle_feedback和加速度增量acc_feedback，如下
U = [ δ f Δ a x ] U = \begin{bmatrix} \delta_{f} \\ \Delta a_x \\ \end{bmatrix} U=[δf​Δax​​]

  steer_angle_feedback = Wheel2SteerPct(control[0](0, 0));
  acc_feedback = control[0](1, 0);

2.2.9 前馈补偿

控制时序control_gain，参考时序addition_gain，参考公式如下
δ f f = L R + K v a y − k 3 [ l r R − l f 2 C α r m V x 2 R l ] \begin{aligned} \delta_{ff} &= \frac{L}{R} + K_v a_y - k_3\left [ \frac{l_r}{R} - \frac{l_f}{2C_{\alpha r}} \frac{{mV_x}^{2} }{Rl} \right ] \end{aligned} δff​​=RL​+Kv​ay​−k3​[Rlr​​−2Cαr​lf​​RlmVx​2​]​

代码里用的公式进行了一些改动，如下
δ f f _ 2 = − k 3 κ [ l r − l f 2 C α r m V x 2 L ] \begin{aligned} \delta_{ff\_2} &=- k_3 \kappa \left [ l_r - \frac{l_f}{2C_{\alpha r}} \frac{{mV_x}^{2} }{L} \right ] \end{aligned} δff_2​​=−k3​κ[lr​−2Cαr​lf​​LmVx​2​]​

但实际计算的公式多了一项，如下
δ f f _ 2 = − k 3 κ [ l r − l f 2 C α r m V x 2 L ] − v κ ⋅ k a d d i t i o n \begin{aligned} \delta_{ff\_2} &=- k_3 \kappa \left [ l_r - \frac{l_f}{2C_{\alpha r}} \frac{{mV_x}^{2} }{L} \right ] - v \kappa \cdot k_{addition} \end{aligned} δff_2​​=−k3​κ[lr​−2Cαr​lf​​LmVx​2​]−vκ⋅kaddition​​

这里的 k 3 k_3 k3​是求解无约束规划问题的黎卡提方程得到的，addition_gain不知道是什么。

为什么 k 3 k_3 k3​用的是黎卡提方程的解？

  for (int i = 0; i < basic_state_size_; ++i) {
    unconstrained_control += control_gain[0](0, i) * matrix_state_(i, 0);
  }
  unconstrained_control += addition_gain[0](0, 0) * v * debug->curvature();
  if (enable_mpc_feedforward_compensation_) {
    unconstrained_control_diff =
        Wheel2SteerPct(control[0](0, 0) - unconstrained_control);
    if (fabs(unconstrained_control_diff) <= unconstrained_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) / unconstrained_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;
  }

2.2.10 限制和输出转角

steer_angle由三部分组成，分别是转角反馈、转角前馈1和转角前馈2。

  // 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);

2.2.11 限制和输出加速度

acceleration_cmd由两部分组成，分别是加速度反馈acc_feedback和加速度参考acceleration_reference。

FLAGS_steer_angle_rate默认为100。

  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);

2.2.12 debug更新计算数据

  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_unconstrained_control_diff(unconstrained_control_diff);
  debug->set_steer_angle_feedback(steer_angle_feedback);
  debug->set_steering_position(chassis->steering_percentage());

2.2.13 输出挡位

若速度小于停车最大平均车速或挡位处于规划挡位或档位处于空挡，则设置挡位为规划挡位；否则设置挡位为底盘所处挡位。

  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());
  }

2.2.14 debug更新chassis

  ProcessLogs(debug, chassis);

2.3 返回

  return Status::OK();

3 结语

Mpc的模块解析写的比较仓促，有一个地方仍然没有弄清楚（addition_gain），欢迎大家批评指正。