基于动力学的MPC控制器设计盲点解析

自动驾驶小卡

已于 2025-03-04 15:08:29 修改

阅读量1.4k

点赞数 28

分类专栏： Apollo 横向控制LQR&MPC算法文章标签：算法 MPC C++

于 2024-12-31 18:06:23 首次发布

本文链接：https://blog.csdn.net/zxy_ZXY123/article/details/144854080

版权

Apollo 横向控制LQR&MPC算法专栏收录该内容

3 篇文章

订阅专栏

文章目录

Apollo MPC控制器的设计架构

误差模型和离散化

状态变量和控制变量

1、Apollo MPC控制器中状态变量主要有如下6个

  matrix_state_ = Matrix::Zero(basic_state_size_, 1);
  
  // 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();

状态变量计算：

控制变量有两个：

其中 $\delta _{_{f}}$ 为前轮转角， $a$ 为加速度补偿

  Eigen::MatrixXd control_matrix(controls_, 1);
  control_matrix << 0, 0;

在代码中控制量的计算和输出：

// 解mpc，输出一个vector，control内有10个control_matrix
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)
// 已知，mpc仅取第一组解为当前时刻最优控制解，即control[0]
// steer_single_direction_max_degree_ --- the maximum turn of steer
// control中第一个矩阵中的第一个值用于计算方向盘转角
double steer_angle_feedback = control[0](0, 0) * 180 / M_PI *
                                steer_transmission_ratio_ /
                                steer_single_direction_max_degree_ * 100;

double steer_angle = steer_angle_feedback + steer_angle_feedforwardterm_updated_ +
                     steer_angle_ff_compensation + steer_angle_feedback_augment;

// control中第一个矩阵中的第二个值control[0](1, 0)，用于计算加速度 = 加速度补偿 + 期望加速度 + 坡度加速度补偿
debug->set_acceleration_cmd_closeloop(control[0](1, 0));
double acceleration_cmd = control[0](1, 0) + debug->acceleration_reference() + 
                          control_conf_.enable_slope_offset() * debug->slope_offset_compensation();

动力学误差模型

代码实现：

注意：关于矩阵C离散化更正：

//矩阵C离散化
matrix_cd_ = matrix_c_ * debug->ref_heading_rate() * ts_;

// 初始化矩阵
Status MPCController::Init(const ControlConf *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_);
  ...
  // TODO(QiL): expand the model to accomendate more combined states.

  matrix_a_coeff_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  ...

  matrix_b_ = Matrix::Zero(basic_state_size_, controls_);
  matrix_bd_ = Matrix::Zero(basic_state_size_, controls_);
  ...
  matrix_bd_ = matrix_b_ * ts_;

  matrix_c_ = Matrix::Zero(basic_state_size_, 1);
  ...
  matrix_cd_ = Matrix::Zero(basic_state_size_, 1);
  ...
  }

// 更新矩阵
void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {
  const double v = VehicleStateProvider::instance()->linear_velocity();
    //状态矩阵A离散化之前的补全6x6
    matrix_a_(1,1) = matrix_a_coeff_(1,1) / v;
    matrxi_a_(1,3) = matrix_a_coeff_(1,3) / v;   
    matrxi_a_(3,1) = matrix_a_coeff_(3,1) / v;  
    matrxi_a_(3,3) = matrix_a_coeff_(3,3) / v;  

    //状态矩阵A离散化：方法一：双线性变换 方法二：中点欧拉法
    Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols()); //6x6单位矩阵
    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;
    //矩阵C离散化
    matrix_cd_ = matrix_c_ * debug->ref_heading_rate() * ts_; 
}

// 求解MPC
// discrete linear predictive control solver, with control format
// x(i + 1) = A * x(i) + B * u (i) + C
bool SolveLinearMPC(const Matrix &matrix_a, const Matrix &matrix_b,
                    const Matrix &matrix_c, const Matrix &matrix_q,
                    const Matrix &matrix_r, const Matrix &matrix_lower,
                    const Matrix &matrix_upper,
                    const Matrix &matrix_initial_state,
                    const std::vector<Matrix> &reference, const double eps,
                    const int max_iter, std::vector<Matrix> *control)

apollo使用的是双线性变换离散法的方法：

    //状态矩阵A离散化：方法一：双线性变换 方法二：中点欧拉法
    Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols()); //6x6单位矩阵
    matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *
                 (matrix_i + ts_ * 0.5 * matrix_a_);
               
  matrix_bd_ = matrix_b_ * ts_;
 
  //matrix_cd_ = matrix_c_ * debug->heading_error_rate() * ts_; //错误

   注意：关于矩阵C离散化更正：

    //矩阵C离散化
    matrix_cd_ = matrix_c_ * debug->ref_heading_rate() * ts_;

离散化后正确写法：

$\widetilde{A} = (I - T*A/2)^{-1}(I + T*A/2)$

$\widetilde{B} = B * T$

$\widetilde{C} = T * C$ * ref_heading_rate

补充：

Apollo采用的中点欧拉法和向前欧拉法的结合：

预测模型推导

为什么不用上述推导出的离散化的状态空间方程来推导我们的预测模型，以此来设计MPC控制器？

主要考虑在设计目标函数的时候，想将我们的控制增量设计到我们的目标函数中去，而非直接使用控制量，以控制增量为代价，能有效改善控制的平滑性。

此处我们需要注意的是：

控制步长为Nc,因此当预测步长为k+Nc+1时，此时预测状态y(k+Nc+1)的最后一项也只写到k+Nc-1。因为到k+Nc时刻及以后的控制量有两种处理策略：第一种就是控制步长Nc之后的预测控制量保持不变；第二种就是Nc之后的预测控制量全部置零。我们此处达到控制步长之后的控制量全置零；
最终推导出的系统的预测模型Y, 其可以根据系统当前的状态量，以及施加一个未来一段时间Nc序列的控制增量，我们就可以知道系统未来(Np步)的表现是什么样子的（系统输出），即y(k+1)~y(k+Np)；

目标函数和约束设计

设计目标：

希望我们的路径跟踪误差在预测时域内尽可能趋于0，也即希望目标函数的第一项的几个状态量尽可能趋于0，这样我们的车辆才能更好的跟随规划路径；
其次，我们希望目标函数的第二项控制增量的代价越小越好，也即希望前后两帧的控制量变化越小越好，这样控制的效果越平滑，对应前轮转角和加速度变化越平滑；
目标函数最后一项引入松弛因子(在程序中是一个具体的数值)，其主要作用就是改善优化问题的可行解，提高求解效率；

在MPC控制规律中，将控制序列中的第一个参数作为控制量，输入给被控系统。

对于车辆控制来说，就是找到一组合适的控制量，如 $u = [\delta _{f},a]$ , 其中 $\delta _{_{f}}$ 为前轮转角， $a$ 为刹车/油门系数，使车辆沿参考轨迹行驶，且误差最小、控制最平稳、舒适度最高等。因此在设计目标函数的时候可以考虑以下几点：

1、设计代价函数时候，一般设计为二次型的样式，为的是避免在预测时域内，误差忽正忽负，导致误差相互抵消；

2、考虑的代价主要有如下：

横向误差：指实际轨迹点与参考轨迹点间的距离；
航向误差：车辆当前航向与参考轨迹点航向的偏差；
速度误差：车辆当前速度与规划轨期望车速的偏差；
刹车/油门调节量：目的是为了保证刹车/油门变化的平稳性；

3、约束条件:

最大前轮转角
最大前轮转角变化量
最大刹车/油门调节量
最大方向盘转角
最大车速
最大加速度等

此处关于控制增量的约束，比如说，我们的前轮转角打30°需要2s,则1s需要打15°，然而我们的控制周期T=0.05s，则控制周期我们需要下发的转角最大为0.75°，也即我们下发转角的最大速率不超过0.75°/T。

关于软约束，也即代价函数，使其值越小越好。

优化求解

滚动优化求解的目的是为了求最优控制解，是一种在线优化，它每一时刻都会针对当前误差重新计算控制量，通过使某一或某些性能指标达到最优来实现控制作用。因此，滚动优化可能不会得到全局最优解，但是却能对每一时刻的状态进行最及时的响应，达到局部最优。

第一步：目标函数如何转化为标准二次型

注意：

目标函数化简后的每一项其实都是一个具体的数，第三项表示具体数的转置等于其自身。因此合并第二项和第三项，二者实质相等。其次化成二次型后，G属于已知项，且Q也是已知项，故二次型的最后一项其实不影响最终的优化求解最小值问题，因此放在最后一项，且求解过程可以忽略。

第二步：约束的转化

将 $u(k)^{*}$ 作为此时的控制量输入给系统，直到下一个控制时刻，系统根据新的状态信息预测下一时段内的输出，然后通过优化得到一组新的控制序列。如此反复，直至完成整个控制过程。

OSQP求解器

带参构造函数MpcOsqp 求解

输入矩阵Ad,Bd,Q,R,初始状态阵X0,控制量上下边界，状态量上下边界，参考状态(0矩阵)，最大迭代次数mpc_max_iteration_，预测时域周期数horizon_，求解精度mpc_eps_，用输入参数去初始化类对象的数据成员

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

OSQP求解结果输出：

 std::vector<double> control_cmd(controls_, 0);

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

  //第一个元素前轮反馈转角
  steer_angle_feedback = Wheel2SteerPct(control[0](0, 0));

  //第二个元素加速度补偿
  acc_feedback = control[0](1, 0);


 // 最终计算下发的控制车轮转角 = mpc计算反馈转角 + 前馈的前两部分 + 前馈的最后一部分改进 + Lead controller 计算补偿转角
  double steer_angle = 
        steer_angle_feedback + steer_angle_feedforwardterm_updated_ + 
        steer_angle_ff_compensation + steer_angle_feedback_augment;

  //是否对转角加限制
  if (FLAGS_set_steer_limit) {
        const double steer_limit = std::atan(max_lat_acc_ * wheelbase_ /
                                         (vehicle_state->linear_velocity() *
                                          vehicle_state->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计算加速度进行处理，其计算的是加速度的反馈量
  debug->set_acceleration_cmd_closeloop(acc_feedback);

  //对pitch值进行处理，以便于计算坡度补偿加速度
  double vehicle_pitch = 0.0;
  if (use_pitch_angle_filter_) {
    vehicle_pitch =
        digital_filter_pitch_angle_.Filter(injector_->vehicle_state()->pitch());
  } else {
    vehicle_pitch = injector_->vehicle_state()->pitch();
  }  

  if (std::isnan(vehicle_pitch)) {
    AINFO << "pitch angle is nan.";
    vehicle_pitch = 0;
  }
  debug->set_vehicle_pitch(vehicle_pitch);

  //计算坡度补偿加速度
  double slope_offset_compensation = GRA_ACC * std::sin(vehicle_pitch);
  if (std::isnan(slope_offset_compensation)) {
    slope_offset_compensation = 0;
  }
  debug->set_slope_offset_compensation(slope_offset_compensation);

  //计算总的加速度输出: = 反馈加速度 + 期望加速度 + 坡度补偿加速度
  double acceleration_cmd = acc_feedback + debug->acceleration_reference() + 
    control_conf_.enable_slope_offset() * debug->slope_offset_compensation;

参考文献

本文针对运动学MPC控制器进行回顾总结，详细可以参考下述文献。

1、【基础】自动驾驶控制算法第五讲连续方程的离散化与离散LQR原理_哔哩哔哩_bilibili

2、基于MPC轨迹跟踪～理论篇上_哔哩哔哩_bilibili

3、Apollo代码学习(六)—模型预测控制(MPC)_apollo mpc-CSDN博客