欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-03-05 19:19:19
...

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} dtdlateral_errorlateral_error_rateheading_errorheading_error_ratestation_errorspeed_error=0000001mVxCf+Cr0IzVxCrlrCflf000mCf+Cr0IzCflfCrlr000mVxCrlrCflf1IzVxCrlr2+Cflf200000000000010lateral_errorlateral_error_rateheading_errorheading_error_ratestation_errorspeed_error+0mCf0IzCflf00000001[δfΔax]+0mVxCrlrCflfVx0IzVxCrlr2+Cflf200Ψ˙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} dtdx=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,ukminJ=xk,ukmin[0N(xkxr)TQ(xkxr)+0N1ukTRuk]

  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函数,计算kvsteer_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)lrm2Cαr(lf+lr)lfm=RL+Kvayk3[Rlr2CαrlfRLmVx2]

计算转角前馈的公式有些不同,如下
κ = 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=R1=Rv2=v2κ=Lκ+Kvv2κ

  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_gainaddition_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,ukminJ=xk,ukmin[0N(xkxr)TQ(xkxr)+0N1ukTRuk]

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+Bukxminxkxmaxuminukumax

式中, 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 xmin21xTPx+qTxlAcxu
上述方程的决策变量 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+N1)
式中, 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 21xTPx起作用,基本就退化为 ∑ 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(xkxr)TQ(xkxr)+0N1ukTRuk

对于每个部分的误差,实际上是 e r r o r = A x k + B u k − x k + 1 error = Ax_{k} + Bu_{k} - x_{k+1} error=Axk+Bukxk+1,但 A x k + B u k − x k + 1 = − C Ax_{k} + Bu_{k} - x_{k+1} = -C Axk+Bukxk+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=QxrQxrQxr00

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=[ExIExEuIEu]

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=IA000IA000I0000I,Eu=0B0000B0000B

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=I0000000I0000000I0000000I000,IEu=0000I0000000I0000000I

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=x000xminxminuminumin,u=x000xmaxxmaxumaxumax
对于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+Kvayk3[Rlr2CαrlfRlmVx2]

代码里用的公式进行了一些改动,如下
δ 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κ[lr2CαrlfLmVx2]

但实际计算的公式多了一项,如下
δ 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κ[lr2CαrlfLmVx2]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),欢迎大家批评指正。