关于MPC控制算法，在控制部分，已经有详细的MPC算法的详细解释和说明，这里就不再多说了，可以自行查阅。
这里主要针对Apollo中，现有的部分进行一下相关的说明。
Apollo中使用的是车辆动力学模型，具体内容请参考这里。 横纵向的综合动力学模型，其控制的状态量就是在单横向的基础上，加上了 station_error，speed_error两个量。 m a t r i x − s t a t e = [ 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 ] 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个： m a t r i x − c o n t r o l = [ δ f a ] matrix_-control=\begin{bmatrix} \delta_f \\ a \\ \end{bmatrix} matrix−control=[δfa] 其中， δ f \delta_f δf为前轮转角， a a a为车辆加速度。 加上之前的，动力学模型，结合Apollo中的相关代码可知： 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 − 2 C a f + 2 C a r m V 2 C a f + 2 C a r m − 2 C a f ℓ f − 2 C a r ℓ r m V 0 0 0 0 0 1 0 0 0 − 2 C a f ℓ f − 2 C a r ℓ r I z V 2 C a f ℓ f − 2 C a r ℓ r I z − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V 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 2 C a f m 0 0 0 2 C a f ℓ f I z 0 0 0 0 − 1 ] [ δ f a ] + [ 0 − V − 2 C a f ℓ f − 2 C a r ℓ r m V 0 − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V 0 1 ] φ ˙ (1) \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⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎡0000001−mV2Caf+2Car0−IzV2Cafℓf−2Carℓr000m2Caf+2Car0Iz2Cafℓf−2Carℓr000−mV2Cafℓf−2Carℓr1−IzV2Cafℓf2+2Carℓr200000000000010⎦⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡lateral−errorlateral−error−rateheading−errorheading−error−ratestation−errorspeed−error⎦⎥⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎡0m2Caf0Iz2Cafℓf0000000−1⎦⎥⎥⎥⎥⎥⎥⎤[δfa]+⎣⎢⎢⎢⎢⎢⎢⎢⎡0−V−mV2Cafℓf−2Carℓr0−IzV2Cafℓf2+2Carℓr201⎦⎥⎥⎥⎥⎥⎥⎥⎤φ˙(1) 式中， C f , C r C_f,C_r Cf,Cr分别为前、后轮的侧偏刚度， ℓ f , ℓ r \ell_f,\ell_r ℓf,ℓr分别为前、后悬长度， m m m为车身质量， V V V为车身速度， I z I_z Iz为车辆绕z轴的转动惯量， φ ˙ \dot{\varphi} φ˙为车辆 y a w yaw yaw角度的变化率，也就是横摆角速度。 Apollo中使用的车身坐标系为，车辆右侧为x轴正向，车头前向为y轴正向，z轴垂直向上。 将式(1)化为线性形式 x ( k + 1 ) = A x ( k ) + B u ( k ) + C (2) x(k+1)=Ax(k)+Bu(k)+C \tag{2} x(k+1)=Ax(k)+Bu(k)+C(2) 式中， A = [ 0 1 0 0 0 0 0 − 2 C a f + 2 C a r m V 2 C a f + 2 C a r m − 2 C a f ℓ f − 2 C a r ℓ r m V 0 0 0 0 0 1 0 0 0 − 2 C a f ℓ f − 2 C a r ℓ r I z V 2 C a f ℓ f − 2 C a r ℓ r I z − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V 0 0 0 0 0 0 0 1 0 0 0 0 0 0 ] ， 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=⎣⎢⎢⎢⎢⎢⎢⎢⎡0000001−mV2Caf+2Car0−IzV2Cafℓf−2Carℓr000m2Caf+2Car0Iz2Cafℓf−2Carℓr000−mV2Cafℓf−2Carℓr1−IzV2Cafℓf2+2Carℓr200000000000010⎦⎥⎥⎥⎥⎥⎥⎥⎤， B = [ 0 0 2 C a f m 0 0 0 2 C a f ℓ f I z 0 0 0 0 − 1 ] ， 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=⎣⎢⎢⎢⎢⎢⎢⎡0m2Caf0Iz2Cafℓf0000000−1⎦⎥⎥⎥⎥⎥⎥⎤， C = [ 0 − V − 2 C a f ℓ f − 2 C a r ℓ r m V 0 − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V 0 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ℓr0−IzV2Cafℓf2+2Carℓr201⎦⎥⎥⎥⎥⎥⎥⎥⎤
代码中线性化系数（双线性变换离散法）：
matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() * (matrix_i + ts_ * 0.5 * matrix_a_); matrix_bd_ = matrix_b_ * ts_;A ~ = ( I − t s A / 2 ) − 1 ( I + t s A / 2 ) B ~ = B t s C ~ = t s C φ ˙ (3) \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−tsA/2)−1(I+tsA/2)B =BtsC =tsCφ˙(3) 另一种离散化方法连续系统离散化方法，我没有仔细看，后期再说吧。 模型预测控制模型的部分大体上就这些。
由于车辆运行时速不是按照一个固定值，所以在不同的车速情况下，会选择不同的系数，Apollo中主要对四个参数进行了插值补偿：
侧向误差朝向误差方向盘前馈方向盘控制矩阵权重见代码
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文件中的增益系数，设置的感觉有点不太好，还是需要自己根据实际工程情况去进行相关标定的。
目前用的比较多的求解器，
qpOASESOSQPceresIpopt之前用过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_ – 离散化后的系数Amatrix_bd_ – 离散化后的系数Bmatrix_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网上资料有很多，搜一下就好。
