Apollo学习笔记（11）MPC

关于MPC控制算法，在控制部分，已经有详细的MPC算法的详细解释和说明，这里就不再多说了，可以自行查阅。

这里主要针对Apollo中，现有的部分进行一下相关的说明。

MPC使用的模型

Apollo中使用的是车辆动力学模型，具体内容请参考这里。
横纵向的综合动力学模型，其控制的状态量就是在单横向的基础上，加上了 station_error，speed_error两个量。
$matrix_-state = \begin{bmatrix} lateral_-error \\ lateral_-error_-rate \\ heading_-error \\ heading_-error_-rate \\ station_-error \\ speed_-error \\ \end{bmatrix}$matrix−​state=⎣⎢⎢⎢⎢⎢⎢⎡​lateral−​errorlateral−​error−​rateheading−​errorheading−​error−​ratestation−​errorspeed−​error​⎦⎥⎥⎥⎥⎥⎥⎤​
具体这六个状态量，在纵向控制和横向控制的代码里面均有详细的说明，可以自行查阅下。

控制变量有2个：
$matrix_-control=\begin{bmatrix} \delta_f \\ a \\ \end{bmatrix}$matrix−​control=[δf​a​]
其中，$\delta_f$δf​为前轮转角，$a$a为车辆加速度。
加上之前的，动力学模型，结合Apollo中的相关代码可知：
$\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{2C_{af}+2C_{ar}}{mV} && \frac{2C_{af}+2C_{ar}}{m} && - \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{mV} && 0 && 0 \\ 0 && 0 && 0 && 1 && 0 && 0 \\ 0 && -\frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{I_{z}V} && \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{I_{z}} && -\frac{2C_{af}\ell_{f}^{2}+2C_{ar}\ell_{r}^{2}}{I_{z}V} && 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{2C_{af}}{m} && 0 \\ 0 && 0 \\ \frac{2C_{af}\ell_{f}}{I_{z}} && 0 \\ 0 && 0 \\ 0 && -1 \\ \end{bmatrix} \begin{bmatrix} \delta_f \\ a \\ \end{bmatrix}+ \begin{bmatrix} 0 \\ -V- \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{mV} \\ 0 \\ -\frac{2C_{af}\ell_{f}^{2}+2C_{ar}\ell_{r}^{2}}{I_{z}V} \\ 0 \\ 1 \\ \end{bmatrix}\dot{\varphi} \tag{1}$dtd​⎣⎢⎢⎢⎢⎢⎢⎡​lateral−​errorlateral−​error−​rateheading−​errorheading−​error−​ratestation−​errorspeed−​error​⎦⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎢⎡​000000​​1−mV2Caf​+2Car​​0−Iz​V2Caf​ℓf​−2Car​ℓr​​00​​0m2Caf​+2Car​​0Iz​2Caf​ℓf​−2Car​ℓr​​00​​0−mV2Caf​ℓf​−2Car​ℓr​​1−Iz​V2Caf​ℓf2​+2Car​ℓr2​​00​​000000​​000010​⎦⎥⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎡​lateral−​errorlateral−​error−​rateheading−​errorheading−​error−​ratestation−​errorspeed−​error​⎦⎥⎥⎥⎥⎥⎥⎤​+⎣⎢⎢⎢⎢⎢⎢⎡​0m2Caf​​0Iz​2Caf​ℓf​​00​​00000−1​⎦⎥⎥⎥⎥⎥⎥⎤​[δf​a​]+⎣⎢⎢⎢⎢⎢⎢⎢⎡​0−V−mV2Caf​ℓf​−2Car​ℓr​​0−Iz​V2Caf​ℓf2​+2Car​ℓr2​​01​⎦⎥⎥⎥⎥⎥⎥⎥⎤​φ˙​(1)
式中，$C_f,C_r$Cf​,Cr​分别为前、后轮的侧偏刚度，$\ell_f,\ell_r$ℓf​,ℓr​分别为前、后悬长度，$m$m为车身质量，$V$V为车身速度，$I_z$Iz​为车辆绕z轴的转动惯量，$\dot{\varphi}$φ˙​为车辆$yaw$yaw角度的变化率，也就是横摆角速度。
Apollo中使用的车身坐标系为，车辆右侧为x轴正向，车头前向为y轴正向，z轴垂直向上。
将式(1)化为线性形式
$x(k+1)=Ax(k)+Bu(k)+C \tag{2}$x(k+1)=Ax(k)+Bu(k)+C(2)
式中，
$A=\begin{bmatrix} 0 && 1 && 0 && 0 && 0 && 0 \\ 0 && -\frac{2C_{af}+2C_{ar}}{mV} && \frac{2C_{af}+2C_{ar}}{m} && - \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{mV} && 0 && 0 \\ 0 && 0 && 0 && 1 && 0 && 0 \\ 0 && -\frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{I_{z}V} && \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{I_{z}} && -\frac{2C_{af}\ell_{f}^{2}+2C_{ar}\ell_{r}^{2}}{I_{z}V} && 0 && 0 \\ 0 && 0 && 0 && 0 && 0 && 1 \\ 0 && 0 && 0 && 0 && 0 && 0 \\ \end{bmatrix}，$A=⎣⎢⎢⎢⎢⎢⎢⎢⎡​000000​​1−mV2Caf​+2Car​​0−Iz​V2Caf​ℓf​−2Car​ℓr​​00​​0m2Caf​+2Car​​0Iz​2Caf​ℓf​−2Car​ℓr​​00​​0−mV2Caf​ℓf​−2Car​ℓr​​1−Iz​V2Caf​ℓf2​+2Car​ℓr2​​00​​000000​​000010​⎦⎥⎥⎥⎥⎥⎥⎥⎤​，
$B=\begin{bmatrix} 0 && 0 \\ \frac{2C_{af}}{m} && 0 \\ 0 && 0 \\ \frac{2C_{af}\ell_{f}}{I_{z}} && 0 \\ 0 && 0 \\ 0 && -1 \\ \end{bmatrix}，$B=⎣⎢⎢⎢⎢⎢⎢⎡​0m2Caf​​0Iz​2Caf​ℓf​​00​​00000−1​⎦⎥⎥⎥⎥⎥⎥⎤​，
$C=\begin{bmatrix} 0 \\ -V- \frac{2C_{af}\ell_{f}-2C_{ar}\ell_{r}}{mV} \\ 0 \\ -\frac{2C_{af}\ell_{f}^{2}+2C_{ar}\ell_{r}^{2}}{I_{z}V} \\ 0 \\ 1 \\ \end{bmatrix}$C=⎣⎢⎢⎢⎢⎢⎢⎢⎡​0−V−mV2Caf​ℓf​−2Car​ℓr​​0−Iz​V2Caf​ℓf2​+2Car​ℓr2​​01​⎦⎥⎥⎥⎥⎥⎥⎥⎤​

代码中线性化系数（双线性变换离散法）：

matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() * (matrix_i + ts_ * 0.5 * matrix_a_);
matrix_bd_ = matrix_b_ * ts_;

$\widetilde{A}=(I-t_sA/2)^{-1}(I+t_sA/2) \\ \widetilde{B}=Bt_s \\ \widetilde{C}=t_sC\dot{\varphi} \tag{3}$A=(I−ts​A/2)−1(I+ts​A/2)B=Bts​C=ts​Cφ˙​(3)
另一种离散化方法连续系统离散化方法，我没有仔细看，后期再说吧。
模型预测控制模型的部分大体上就这些。

增益系数的插值

由于车辆运行时速不是按照一个固定值，所以在不同的车速情况下，会选择不同的系数，Apollo中主要对四个参数进行了插值补偿：

1. 侧向误差
2. 朝向误差
3. 方向盘前馈
4. 方向盘控制矩阵权重

见代码

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

个人也很认同这种思路，但是Apollo中control_conf.pb.txt文件中的增益系数，设置的感觉有点不太好，还是需要自己根据实际工程情况去进行相关标定的。

求解器

目前用的比较多的求解器，

• qpOASES
• OSQP
• ceres
• Ipopt

之前用过Ipopt，感觉挺好使的，按照给定的接口就可以很快的求解，Apollo中现在使用的是OSQP，代码如下：

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

在求解MPC时，Apollo封装好了OSQP的类，这里直接调用就行，具体的用法

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

我们需要做的就是计算好对应的参数，把值付给求解器即可，

• matrix_ad_ – 离散化后的系数A
• matrix_bd_ – 离散化后的系数B
• matrix_q_updated_ – 权重q矩阵
• matrix_r_updated_ – 权重r矩阵
• matrix_state_ – 当前时刻的状态矩阵
• lower_bound – 控制量下限
• upper_bound – 控制量上限
• lower_state_bound – 状态量下限
• upper_state_bound – 状态量上限
• reference_state – 参考状态量
• mpc_max_iteration_ – 求解器最大求解迭代次数
• horizon_ – 预测步长
• mpc_eps_ – 预测周期

这里对reference_state为零做一下说明，我们选取的模型是误差模型，所以当然希望最终的误差为零。
具体的OSQP网上资料有很多，搜一下就好。