Apollo代碼學習(六)—模型預測控制(MPC)

Apollo代碼學習—模型預測控制

• 前言
• 模型預測控制
• 預測模型
• 線性化
• 單車模型
• 滾動優化
• 反饋矯正
• 總結

前言

非專業選手，此篇博文內容基於書本和網絡資源整理，可能理解的較爲狹隘，起點較低，就事論事。如發現有紕漏，請指正，很是感謝！！！

查看Apollo中關於MPC_controller的代碼能夠發現，它的主體集成了橫縱向控制，在計算控制命令時，計算了橫縱向偏差：

ComputeLongitudinalErrors(&trajectory_analyzer_, debug);

ComputeLateralErrors(com.x(), com.y(),
                       VehicleStateProvider::instance()->heading(),
                       VehicleStateProvider::instance()->linear_velocity(),
                       VehicleStateProvider::instance()->angular_velocity(),
                       trajectory_analyzer_, debug);

下文將從MPC原理入手，結合橫縱向控制進行分析。

模型預測控制

能夠先結合官方的教學視頻對MPC進行一個簡單的瞭解：模型預測控制。

根據維基百科，模型預測控制是一種先進的過程控制方法，在知足必定約束條件的前提下，被用來實現過程控制，它的實現依賴於過程的動態模型（一般爲線性模型）。在控制時域（一段有限時間）內，它主要針對當前時刻進行優化，但也考慮將來時刻，求取當前時刻的最優控制解，而後反覆優化，從而實現整個時域的優化求解。

也就是說，模型預測控制其實是一種時間相關的，利用系統當前狀態和當前的控制量，來實現對系統將來狀態的控制。而系統將來的狀態是不定的，所以在控制過程當中要不斷根據系統狀態對將來的控制量做出調整。並且相較於經典的的PID控制，它具備優化和預測的能力，也就是說，模型預測控制是一種致力於將更長時間跨度、甚至於無窮時間的最優化控制問題，分解爲若干個更短期跨度，或者有限時間跨度的最優化控制問題，而且在必定程度上仍然追求最優解¹。

能夠經過下圖對模型預測控制進行一個簡單的理解：

圖1 模型預測控制原理 (圖片來源：無人駕駛車輛模型預測控制)

圖1中，k軸爲當前狀態，左側爲過去狀態，右側爲未來狀態。可結合無人駕駛車輛模型預測控制²第3.1.2節來理解此圖。

模型預測控制在實現上有三個要素：

1. 預測模型，是模型預測控制的基礎，用來預測系統將來的輸出；
2. 滾動優化，一種在線優化，用於優化短期內的控制輸入，以儘量減少預測模型輸出與參考值的差距；
3. 反饋矯正，在新的採樣時刻，基於被控對象的實際輸出，對預測模型的輸出進行矯正，而後進行新的優化，以防模型失配或外界干擾致使的控制輸出與指望差距過大。

下面從三要素入手對模型預測控制進行分析。

預測模型

不管是運動學模型，仍是動力學模型，所搭建的均爲非線性系統，而線性模型預測控制較非線性模型預測控制有更好的實時性，且更易於分析和計算。對於無人車來講，實時性顯然很重要，所以，須要將非線性系統轉化爲線性系統，而非線性系統的線性化的方法大致可分爲精確線性化和近似線性化，多采用近似的線性化方法。

線性化

此部份內容主要參考《無人駕駛車輛模型預測控制》²3.3.2節，本人對對線性化、離散化等問題也理解的不深，不理解的能夠自行查找一些其餘的文章。

非線性系統線性化的方法有不少種，大體分爲：

圖2 線性化方法

下面以泰勒展開的方法爲例，結合《無人駕駛車輛模型預測控制》²3.3.2節非線性系統線性化方法，展現一種存在參考系統的線性化方法。存在參考系統的線性化方法假設參考系統已經在規劃軌跡上徹底跑通，得出每一個時刻上相應的狀態量 $X_r$和控制量 $U_r$，而後經過處理參考系統與當前系統的誤差，利用模型預測控制器來跟蹤指望路徑。

假設車輛參考系統在任意時刻的狀態和控制量知足以下方程：
$\dot{X}_r=f(X_r, U_r) \tag{1}$
在任意點 $(X_r, U_r)$處泰勒展開，只保留一階項，忽略高階項，有：
$\dot{X}=f(X_r, U_r)+\frac{\partial f}{\partial X}(X - X_r)+\frac{\partial f}{\partial U}(U - U_r) \tag{2}$
公式2與公式1相減可得：
$\dot{\widetilde{X}}=A(t)\widetilde{X}+B(t)\widetilde{U} \tag{3}$
其中， $\dot{\widetilde{X}}=\dot{X}-\dot{X}_r$， $\widetilde{X}=X-X_r$， $\widetilde{U}=U-U_r$， $A(t)$爲 $f(X, U)$對 $X$的雅克比矩陣， $B(t)$爲 $f(X, U)$對 $U$的雅克比矩陣。
此時，經過雅克比矩陣，將非線性系統近似轉化爲一個連續的線性系統，但並不適於模型預測控制器的設計，而後對其離散化處理可得，
$\dot{\widetilde{X}}(k)=\frac{\widetilde{X}(k+1)-\widetilde{X}(k)}{T}=A(k)\widetilde{X}(k)+B(k)\widetilde{U}(k) \tag{4}$
獲得線性化時變模型：
$\widetilde{X}(k+1)=\widetilde{A}(k)\widetilde{X}(k)+\widetilde{B}(k)\widetilde{U}(k) \tag{5}$
其中： $\widetilde{A}(t)=I+TA(t) ，\widetilde{B}(t)=TB(t)，I$爲單位矩陣。

至此，獲得一個非線性系統在任意一參考點線性化後的線性系統，該系統是設計模型預測控制算法的基礎。

以運動學模型中的式1.15爲例，低速狀況下的車輛運動學模型可表達爲：
$\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot\psi \end{bmatrix}= \begin{bmatrix} \cos\psi \\ \sin\psi \\ \tan{\delta_f}/l \end{bmatrix}v \tag{6}$
其狀態變量和控制變量分別爲：
$X= \begin{bmatrix} x \\ y \\ \psi \end{bmatrix}， U= \begin{bmatrix} v\\ \delta_f \end{bmatrix}$
對應的雅克比矩陣：
$A= \begin{bmatrix} 0 & 0 & -v\sin{\psi} \\ 0 & 0 & v\cos{\psi} \\ 0 & 0 & 0 \end{bmatrix}， B= \begin{bmatrix} \cos{\psi} & 0\\ \sin{\psi} & 0\\ \frac{\tan{\delta_f}}{l} & \frac{v}{l\cos^2{\delta_f}} \end{bmatrix} \tag{7}$
其線性時變模型爲：
$\dot{\widetilde{X}}=\widetilde{A}\widetilde{X}+\widetilde{B}\widetilde{U}$
其中，
$\widetilde{X}= \begin{bmatrix} x-x_0 \\ y-y_0 \\ \psi-\psi_0 \end{bmatrix}， \widetilde{U}= \begin{bmatrix} v-v_0 \\ \delta_f-\delta_{f0} \end{bmatrix}，\\ \widetilde{A}=I+TA= \begin{bmatrix} 1 & 0 & -v\sin{\psi}T \\ 0 & 1 & v\cos{\psi}T \\ 0 & 0 & 1 \end{bmatrix}， \widetilde{B}=TB= \begin{bmatrix} \cos{\psi}T & 0\\ \sin{\psi}T & 0\\ \frac{\tan{\delta_f}T}{l} & \frac{vT}{l\cos^2{\delta_f}} \end{bmatrix} \tag{8}$

單車模型

預測模型仍以單車模型爲主，結合運動學模型和動力學模型對車輛運動狀態進行分析。

圖3 單車模型

根據化代碼可知，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();

$matrix\_state\_= \begin{bmatrix} lateral\_error\\ lateral\_error\_rate\\ heading\_error\\ heading\_error\_rate\\ station\_error\\ speed\_error\\ \end{bmatrix}$
它們的計算請參考上一篇Apollo代碼學習(五)—橫縱向控制：
$\begin{cases} matrix\_state\_(0, 0)=dy*\cos{\varphi}-dx*\sin{\varphi}\\ matrix\_state\_(1, 0)=V_x*\sin{\Delta\varphi} =V_x*\sin{e_2}\\ matrix\_state\_(2, 0)=\varphi-\varphi_{des}\\ matrix\_state\_(3, 0)=\dot{\varphi}-\dot{\varphi}_{des}\\ matrix\_state\_(4, 0)=-(dx*\cos{\varphi_{des}} + dy*\sin{\varphi_{des}})\\ matrix\_state\_(5, 0)=V_{des} - V*\cos{\Delta\varphi}/k\\ \end{cases}$
控制變量有2個：

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

並結合代碼去分析 $control\_matrix$內包含的變量：

// 解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]
// 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_;
// control中第一個矩陣中的第二個值用於計算加速度
debug->set_acceleration_cmd_closeloop(control[0](1, 0));
double acceleration_cmd = control[0](1, 0) + debug->acceleration_reference();

可得
$control\_matrix= \begin{bmatrix} \delta_f \\ a \end{bmatrix}$
其中， $\delta_f$爲前輪轉角， $a$爲加速度補償。
結合方向盤控制的動力學模型：
$\frac{d}{dt} \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{af}+2C_{ar}}{mV_x} & \frac{2C_{af}+2C_{ar}}{m} & \frac{-2C_{af}\ell_f+2C_{ar}\ell_r}{mV_x}\\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_zV_x} & \frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_z} & -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \end{bmatrix} \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \end{bmatrix} \\ +\begin{bmatrix} 0 \\ \frac{2C_{\alpha f}}{m} \\ 0 \\ \frac{2C_{\alpha f}\ell_f}{I_z} \end{bmatrix}\delta_f+ \begin{bmatrix} 0 \\ -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{mV_x} -V_x\\ 0 \\ -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \end{bmatrix}\dot{\varphi} \tag{9}$
和mpc_controller.cc、mpc_slover.cc中的代碼註釋，

// 初始化矩陣
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();
  matrix_a_(1, 1) = matrix_a_coeff_(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_) *
               (matrix_i - ts_ * 0.5 * matrix_a_).inverse();

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

// 求解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)

可得MPC控制模型：
$\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} & \frac{C_f+C_r}{m} & \frac{C_r\ell_r-C_f\ell_f}{mV} & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & \frac{C_r\ell_r-C_f\ell_f}{I_zV} & \frac{C_f\ell_f-C_r\ell_r}{I_z} & -\frac{C_r\ell_r^2+C_f\ell_f^2}{I_zV} & 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\ell_f}{I_z} & 0\\ 0 & 0\\ 0 & -1 \end{bmatrix} \begin{bmatrix} \delta_f \\ a \end{bmatrix}+ \begin{bmatrix} 0\\ \frac{C_r\ell_r-C_f\ell_f}{mV}-V\\ 0\\ -\frac{C_r\ell_r^2+C_f\ell_f^2}{I_zV}\\ 0\\ 1 \end{bmatrix} \dot{\varphi} \tag{10}$

其中， $C_f，C_r$分別爲前/後輪轉彎剛度， $\ell_f，\ell_r$分別爲前懸/後懸長度， $m$爲車身質量， $V$爲車速， $I_z$爲車輛繞 $z$軸轉動的轉動慣量， $\dot{\varphi}$爲轉向速度。
對應的狀態矩陣、控制矩陣、擾動矩陣等以下：
$A=\begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0\\ 0 & -\frac{C_f+C_r}{mV} & \frac{C_f+C_r}{m} & \frac{C_r\ell_r-C_f\ell_f}{mV} & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & \frac{C_r\ell_r-C_f\ell_f}{I_zV} & \frac{C_f\ell_f-C_r\ell_r}{I_z} & -\frac{C_r\ell_r^2+C_f\ell_f^2}{I_zV} & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}，\\ B=\begin{bmatrix} 0 & 0\\ \frac{C_f}{m} & 0\\ 0 & 0\\ \frac{C_f\ell_f}{I_z} & 0\\ 0 & 0\\ 0 & -1 \end{bmatrix}， C=\begin{bmatrix} 0\\ \frac{C_r\ell_r-C_f\ell_f}{mV}-V\\ 0\\ -\frac{C_r\ell_r^2+C_f\ell_f^2}{I_zV}\\ 0\\ 1 \end{bmatrix} \tag{11}$
對應的線性化係數應爲（歐拉映射離散法）：
$\widetilde{A}=I+TA，\widetilde{B}=TB，\widetilde{C}=TC$
但代碼中實際線性化係數爲（雙線性變換離散法）：

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

  matrix_cd_ = matrix_c_ * debug->heading_error_rate() * ts_;

$\widetilde{A}=(I+TA/2)(I-TA/2)^{-1}，\widetilde{B}=TB，\widetilde{C}=TC*heading\_error\_rate \tag{12}$
對於 $\widetilde{C}$的形式不一樣，我的認爲是因爲 $heading\_error\_rate$是計算橫向偏差以後纔有的，沒法直接獲取，在矩陣更新的時候纔將 $heading\_error\_rate$做爲係數更新到矩陣中，也就是公式10中的 $\dot{\varphi}$，其實對於 $\widetilde{C}$來講，還是 $\widetilde{C}=TC$的形式。 $\widetilde{A}$的形式不同，是由於此處採用了雙線性變換離散法。感謝博友LLCCCJJ的友情提示。 關於連續系統離散化的方法可參考：連續系統離散化方法。

最終獲得形如公式13的離散線性模型：
$x(k+1)=Ax(k)+Bu(k)+C\tag{13}$
系統的輸出方程爲： $y(k)=Dx(k)$。
則在預測時域內有狀態變量方程和輸出變量方程以下：
$x(k)=A^kx(0)+\sum_{i=0}^{k-1}A^iBu(k-1-i)+\sum_{i=0}^{k-1}A^iC \tag{14}$
$y(k)=DA^kx(0)+\sum_{i=0}^{k-1}DA^iBu(k-1-i)+\sum_{i=0}^{k-1}DA^iC \tag{15}$
假設
$\xi(k|t)= \begin{bmatrix} x(k|t)\\ u(k-1|t) \end{bmatrix} \tag{16}$
可獲得新的狀態空間表達式：
$\begin{matrix} \xi(k+1|t)=\widetilde{A}\xi(k|t)+\widetilde{B}\Delta u(k|t)+\widetilde{C} \\ \eta(k|t)=\widetilde{D}\xi(k|t) \end{matrix}\tag{17}$
其中，
$\begin{matrix} \widetilde{A}= \begin{bmatrix} A & B \\ 0 & I \end{bmatrix}， \widetilde{B}= \begin{bmatrix} B \\ I \end{bmatrix}, \widetilde{C}= \begin{bmatrix} C \\ 0 \end{bmatrix}, \widetilde{D}=[D\ \ \ 0] \end{matrix}\tag{18}$
可將公式1六、公式18代入公式17，驗證公式18的由來。
則在預測時域內的狀態變量和輸出變量可用以下算式計算：
$\begin{matrix} \xi(k+N_p|t)=\widetilde{A}_t^{N_p}\xi(t|t)+\widetilde{A}_t^{N_p-1}\widetilde{B}_t\Delta u(t|t)+\widetilde{A}_t^{N_p-1}\widetilde{C}_t+\cdots +\widetilde{B}_t\Delta u(t+N_p-1|t)+\widetilde{C}_t \\ \eta(k+N_p|t)=\widetilde{D}_t\widetilde{A}_t^{N_p}\xi(t|t)+\widetilde{D}_t\widetilde{A}_t^{N_p-1}\widetilde{B}_t\Delta u(t|t)+\widetilde{D}_t\widetilde{A}_t^{N_p-1}\widetilde{C}_t+\cdots +\widetilde{D}_t\widetilde{B}_t\Delta u(t+N_p-1|t)+\widetilde{D}_t\widetilde{C}_t \end{matrix} \tag{19}$
將系統將來時刻的狀態和輸出以矩陣形式表達爲：
$\begin{matrix} X(t)=\Psi_t\xi(t|t)+\Phi_t\Delta U(t|t)+\Upsilon_t \\ Y(t)=\widetilde{D}_tX(t) \end{matrix} \tag{20}$
其中，
$X(t)=\begin{bmatrix} \xi(t+1|t)\\ \xi(t+2|t)\\ \xi(t+3|t)\\ \vdots\\ \xi(t+N_p|t)\\ \end{bmatrix}, Y(t)=\begin{bmatrix} \eta(t+1|t)\\ \eta(t+2|t)\\ \eta(t+3|t)\\ \vdots\\ \eta(t+N_p|t)\\ \end{bmatrix}, \Delta U=\begin{bmatrix} \Delta u(t|t)\\ \Delta u(t+1|t)\\ \Delta u(t+2|t)\\ \vdots\\ \Delta u(t+N_p-1|t)\\ \end{bmatrix}, \Upsilon_t=\begin{bmatrix} \widetilde{C}\\ \widetilde{A}\widetilde{C} + \widetilde{C}\\ \widetilde{A}^2\widetilde{C} + \widetilde{A}\widetilde{C} + \widetilde{C}\\ \vdots\\ \sum_{i=0}^{N_p-1}\widetilde{A}^i\widetilde{C} \end{bmatrix},\\ \Psi_t=\begin{bmatrix} \widetilde{A}_t\\ \widetilde{A}_t^2\\ \widetilde{A}_t^3\\ \vdots\\ \widetilde{A}_t^{N_p}\\ \end{bmatrix}, \Phi_t=\begin{bmatrix} \widetilde{B}_t & 0 & 0 & \cdots & 0\\ \widetilde{A}_t\widetilde{B}_t & \widetilde{B}_t & 0 & \cdots & 0\\ \vdots & \vdots & \cdots & \cdots & \cdots \\ \widetilde{A}_t^{N_p-1}\widetilde{B}_t & \widetilde{A}_t^{N_p-2}\widetilde{B}_t & \cdots & \cdots & \widetilde{B}_t\\ \end{bmatrix} \tag{21}$
經過公式20咱們能夠看出，預測時域內的狀態和輸出量均可以經過從系統當前的狀態量 $\xi(t|t)$和控制增量 $\Delta U(t|t)$求得，這也便是模型預測控制算法中「預測」功能的實現。
結合mpc_slover.cc，對MPC的模型進行分析：

unsigned int horizon = reference.size();  // horizon =10

  // Update augment reference matrix_t
  // matrix_t爲60*1的矩陣，存儲的參考軌跡信息
  Matrix matrix_t = Matrix::Zero(matrix_b.rows() * horizon, 1);
  ...

  // Update augment control matrix_v
  // matrix_v爲20*1的矩陣，存儲控制信息
  Matrix matrix_v = Matrix::Zero((*control)[0].rows() * horizon, 1);
  ...

  // matrix_a_power爲含有10個矩陣的容器，每一個矩陣爲6*6，matrix_a爲6*6矩陣
  std::vector<Matrix> matrix_a_power(horizon);
  ...

  // matrix_k爲60*20的矩陣，matrix_b爲6*2矩陣
  Matrix matrix_k =
      Matrix::Zero(matrix_b.rows() * horizon, matrix_b.cols() * horizon);
  ...

令 $Ref=matrix\_t, Ctr=matrix\_v, A=matrix\_a, AP=matrix\_a\_power, B=matrix\_b, K=matrix\_k$，根據代碼有：
$Ref=\begin{bmatrix} ref_1\\ ref_2\\ \vdots\\ ref_{10} \end{bmatrix}, Ctr=\begin{bmatrix} ctr_1\\ ctr_2\\ \vdots\\ ctr_{10} \end{bmatrix}, PA=\begin{bmatrix} A\\ A^2\\ \vdots\\ A^{10} \end{bmatrix},\\ K=\begin{bmatrix} AB & 0 & \cdots & 0 \\ A^2B & AB & \cdots & 0 \\ \vdots & \vdots & \cdots & \vdots \\ A^{10}B & A^9B & \cdots & AB \end{bmatrix}\tag{22}$
其中， $ref_i$爲參考軌跡序列， $ctr_i$爲預測控制序列，預測時域爲10個採樣週期。

滾動優化

MPC的線性優化問題能夠結合無人駕駛車輛模型預測控制第3.3.1節及如下文章：Model predictive control of a mobile robot using linearization進行學習。

滾動優化的目的是爲了求最優控制解，是一種在線優化，它每一時刻都會針對當前偏差從新計算控制量，經過使某一或某些性能指標達到最優來實現控制做用。所以，滾動優化可能不會獲得全局最優解，可是卻能對每一時刻的狀態進行最及時的響應，達到局部最優。

所以，須要設計合適的優化目標，以得到儘量最優的控制序列，目標函數的通常形式可表示爲狀態和控制輸入的二次函數：
$J(k)=\sum_{j=1}^N(\widetilde{x}(k+j|k)^TQ\widetilde{x}(k+j|k)+\widetilde{u}(k+j-1|k)^TR\widetilde{u}(k+j-1|k)) \tag{23}$

其中， $N$爲預測時域， $Q, R$爲權重矩陣，且知足 $Q\ge0, R>0$的正定矩陣。形如 $a(m|n)$表示，在 $n$時刻下預測的 $m$時刻的 $a$值。第一項反映了系統對參考軌跡的跟蹤能力，第二項反映了對控制量變化的約束。此外，上式（公式23）在書寫過程當中，累加和函數 $\sum$後幾乎都不加"()"，但我看起來實在難受，因此自行將 $x,u$一塊兒括起來。
優化求解的問題能夠理解爲在每個採樣點 $k$，尋找一組最優控制序列 $\widetilde{u}_t^*$和最優開銷 $J^*(k)$，其中:
$\widetilde{u}_t^*=\arg\min_{\widetilde{u}}{J(k)}$
在MPC控制規律中，將控制序列中的第一個參數做爲控制量，輸入給被控系統。

對於車輛控制來講，就是找到一組合適的控制量，如 $u=[\delta_f, a]^T$，其中， $\delta_f$爲前輪偏角， $a$爲剎車/油門係數，使車輛沿參考軌跡行駛，且偏差最小、控制最平穩、溫馨度最高等。所以在設計目標函數的時候能夠考慮如下幾點：

1. 看過Apollo官方控制模塊視頻的人，可能知道在設計代價函數時候，通常設計爲二次型的樣式，爲的是避免在預測時域內，偏差忽正忽負，致使偏差相互抵消；此外，對於實在不理解爲何代價函數要採用平方形式的同窗，也能夠參考損失函數爲何用平方形式（二）。
2. 可考慮的代價有：
   a. 距離偏差(Cross Track Error, CTE)，指實際軌跡點與參考軌跡點間的距離，偏差項可表示爲： $(z_i-z_{ref,i})^2$；
   b. 速度偏差，指實際速度與指望速度的差，偏差項可表示爲： $(v_i-v_{ref,i})^2$；
   c. 剎車/油門調節量，目的是爲了保證剎車/油門變化的平穩性，偏差項可表示爲： $(a_{i+1}-a_i)^2$；
   d. 航向偏差等…
3. 約束條件
   a. 最大前輪轉角
   b. 最大剎車/油門調節量
   c. 最大方向盤轉角
   d. 最大車速等

所以公式23中的第一項可表示爲：
$\sum_{j=1}^N\widetilde{x}(k+j|k)^TQ\widetilde{x}(k+j|k)=\sum_{j=1}^N(w_1(z_i-z_{ref,i})^2+w_2(v_i-v_{ref,i})^2+w_3(a_{i+1}-a_i)^2+...) \tag{24}$
其中， $w_i$爲該項偏差的權重，對應權重矩陣Q中的某一項。

對於上述的目標函數（公式23），是有多個變量，且要服從於這些變量的線性約束的二次函數，所以能夠經過適當處理將其轉換爲二次規劃問題。

基於公式20，將控制量變爲控制增量，以知足系統對控制增量的要求，則目標函數能夠改寫爲：
$J(\xi(t), u(t-1), \Delta U(t))=\sum_{i=1}^N||\eta(t+i|t)-\eta_{ref}(t+i|t)||_Q^2+\sum_{i=1}^{N-1}||\Delta U(t+i|t)||_R^2\tag{25}$

在此基礎上知足一些約束條件，通常以下：

$\begin{cases} 控制量約束:u_{min}(t+k) \le u(t+k) \le u_{max}(t+k) \\ 控制增量約束:\Delta u_{min}(t+k) \le \Delta u(t+k) \le \Delta u_{max}(t+k)\\ 輸出約束:y_{min}(t+k) \le y(t+k) \le y_{max}(t+k) \end{cases}\tag{26}$
其中， $k=0,1,2,3,...,N-1$。

將公式20代入公式23中，並將預測時域內輸出量誤差表示爲：
$E(t)=\Psi_t\xi(t|t)-Y_{ref}(t) \tag{27}$
其中， $Y_{ref}=[\eta_{ref}(t+1|t),...,\eta_{ref}(t+N|t)]^T$。
通過必定的矩陣計算，可將目標函數調整爲與控制增量相關的函數：
$J(\xi(t), u(t-1), \Delta U(t))=\Delta U(t)H_t\Delta U(t)^T+G_t\Delta U(t)^T+P_t \tag{28}$
其中， $H_t=\Phi^TQ_e\Phi +R_e，G_t=2E(t)^TQ_e\Phi，P_t=E(t)^TQ_eE(t)，P_t$爲常量， $Q_e,R_e$爲權重矩陣 $Q,R$的擴充矩陣。
結合mpc_slover.cc中的代碼，

// Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr,
  // vector of matrix A power
  Matrix matrix_m = Matrix::Zero(matrix_b.rows() * horizon, 1);      // 60*1
  Matrix matrix_qq = Matrix::Zero(matrix_k.rows(), matrix_k.rows()); // 60*60
  Matrix matrix_rr = Matrix::Zero(matrix_k.cols(), matrix_k.cols()); // 20*20
  Matrix matrix_ll = Matrix::Zero(horizon * matrix_lower.rows(), 1); //20*1
  Matrix matrix_uu = Matrix::Zero(horizon * matrix_upper.rows(), 1); //20*1
  ...
  // Update matrix_m1, matrix_m2, convert MPC problem to QP problem done
  Matrix matrix_m1 = matrix_k.transpose() * matrix_qq * matrix_k + matrix_rr;
  Matrix matrix_m2 = matrix_k.transpose() * matrix_qq * (matrix_m - matrix_t);
  ...
  // TODO(QiL) : change qp solver to box constraint or substitute QPOASES
  // Method 1: QPOASES
  Matrix matrix_inequality_constrain_ll =
      Matrix::Identity(matrix_ll.rows(), matrix_ll.rows());
  ...
  // 求解
  std::unique_ptr<QpSolver> qp_solver(new ActiveSetQpSolver(
      matrix_m1, matrix_m2, matrix_inequality_constrain,
      matrix_inequality_boundary, matrix_equality_constrain,
      matrix_equality_boundary));
  auto result = qp_solver->Solve();

令 $S=matrix\_initial\_state, M=matrix\_m, M1=matrix\_m1, M2=matrix\_m2, Q=matrix\_q, R=matrix\_r, QQ=matrix\_qq, RR=matrix\_rr, LL=matrix\_ll, UU=matrix\_uu$，根據代碼有：