# 模型預測控制（Model predictive control、MPC）

## 最佳控制(optimal control)
動機: 在給定的限制下達到最佳的表現

例如: 在車輛控制下，

假設有兩個軌跡(紅線和綠線)，哪個最好，在此範例紅色會比綠色好，原因是較舒服，軌跡規劃內會有cost function當作標準。
但當有障礙物(紫色區塊→限制條件)，出現則綠線為最優的方案。

## Single input Single output (SISO)
Cost function(objective function): 目標或是代價函數
$$\min_{u} J$$
設計一個控制系$u$讓J有最小值，$J$為
$$J=\int_{0}^{t} w\times e^2 + v \times u^2 dt$$
$w$和$v$是調整兩項變數之間的權重，看我們希望哪個比較重要
假設
$$
\begin{align}
w>>v: 誤差比較重要\\
v>>w: 輸入比較重要
\end{align}
$$

## Multiple input Multiple output (MIMO)

狀態空間方程式
$$
\begin{align}
\frac{dX}{dt}=AX+Bu\\
Y=CX
\end{align}
$$
$X$: 系統狀態變量
$Y$: 系統輸出

$$J=\int_{0}^{\infty} (E^TQE) + (u^T R u)dt$$

example:

$$
\frac{d}{dt} \begin{bmatrix}x_1\\x_2\end{bmatrix} = A\begin{bmatrix}x_1\\x_2\end{bmatrix} + B \begin{bmatrix}u_1\\u_2\end{bmatrix}
$$
$$
\begin{bmatrix}y_1\\y_2\end{bmatrix} = \begin{bmatrix}x_1\\x_2\end{bmatrix} (假設C=1)
$$
$$
R(參考)=\begin{bmatrix}r_1\\r_2\end{bmatrix} = \begin{bmatrix}0\\0\end{bmatrix}
$$
$$
E=\begin{bmatrix}e_1\\e_2\end{bmatrix} = \begin{bmatrix}y_1-r_1\\y_2-r_2\end{bmatrix}= \begin{bmatrix}x_1\\x_2\end{bmatrix}
$$
$$
\begin{align}
E^TQE = \begin{bmatrix}x_1\\x_2\end{bmatrix}^T\begin{bmatrix}q_1&0\\0&q_2\end{bmatrix}\begin{bmatrix}x_1\\x_2\end{bmatrix} = q_1x_1^2+q_2x_2^2\\
u^TRu=\begin{bmatrix}u_1\\u_2\end{bmatrix}^T\begin{bmatrix}r_1&0\\0&r_2\end{bmatrix}\begin{bmatrix}u_1\\u_2\end{bmatrix} = r_1u_1^2+r_2u_2^2
\end{align}
$$
$Q$和$R$:調節矩陣 ($q_1、q_2、r_1、r_2$權重係數)

## MPC 模型預測

通過模型來預測系統在某一未來時間段內的表現來進行優化控制。
多用於數位控制，所以大多採用離散型態狀空間表達形式
$$
X_{k+1} = AX_k + B U_k
$$

分為三個步驟
在k時間點
step 1. 估計/量測當前的系統狀態
step 2. 基於$U_k, U_{k+1}, ..., U_{k+n}$來進行最佳化
$$J=\sum_{k}^{k+n} (E_k^TQE_k) + (U_k^T R U_k) + (E_n^T+FE_n)$$
$E_n^T+FE_n$ : Terminal cost function

step 3. 只進行$U_k$動作

Receding horizon control(滾動最佳化控制): 下一次預測和執行則是整個window往後移，繼續最佳化

# 二次規劃(Quadratic Programming)
一般型態:
$$\min Z^TQZ + C^TZ$$

系統
$$
X_{k+1} = A X_k + B U_k
$$
$X$和$U$都是多維度($d\times1$)。

在k時間點
$$
輸入: U_k=
\begin{bmatrix}u(k|k)\\u(k+1|k)\\...\\u(k+n-1|k)\end{bmatrix}
$$
$$
狀態: X_k=
\begin{bmatrix}x(k|k)\\x(k+1|k)\\x(k+2|k)\\...\\x(k+n|k)\end{bmatrix}
$$
假設
輸出:$y=x$參考: $R=0$ → 誤差: $E=y-R=x-0=x$

目標函數 Cost function:
$$J = x(k+n|k)^TFx(k+n|k) + \sum_{i=0}^{n-1} [x(k+i|k)^TQx(k+i|k) + u(k+i|k)^TRu(k+i|k)] $$

$x(k+i|k)^TQx(k+i|k)$: 誤差加權和
$u(k+i|k)^TRu(k+i|k)$: 輸入加權和
$x(k+n|k)^TFx(k+n|k)$: 終端誤差

**主要是找輸入$U$**，所以要想辦法把$x$取去掉，所以我們看一下$x(k+i|k)$之間的關聯。

$$
\begin{align}
&x(k|k) = x_k (初始條件)\\
&x(k+1|k) = Ax(k|k)+Bu(k|k) = Ax_k + B u(k|k)\\
&x(k+2|k) = Ax(k+1|k)+Bu(k+1|k) = A^2x_k + AB u(k|k)+Bu(k+1|k)\\
&...\\
&x(k+n|k) = A^nx_k + A^{n-1}B u(k|k)+...+Bu(k+n-1|k)\\
\end{align}
$$

從上面我們整理出，狀態$X_k$為

$$
X_k=\begin{bmatrix}I\\A\\A^2\\...\\A^n\end{bmatrix} x_k + \\
\begin{bmatrix}0&0&...&0\\
B&0&...&0\\
AB&B&...&0\\
...\\
A^{n-1}B&A^{n-2}B&...&B\end{bmatrix}\begin{bmatrix}u(k|k)\\u(k+1|k)\\...\\u(k+n-1|k)\end{bmatrix}\\
$$
$$
X_k=Mx_k+CU_k
$$
$$
J = x(k+n|k)^TFx(k+n|k) + \sum_{i=0}^{n-1} [x(k+i|k)^TQx(k+i|k) + u(k+i|k)^TRu(k+i|k)]\\
$$

$$
\begin{align}
&x(k+n|k)^TFx(k+n|k) + \sum_{i=0}^{n-1} [x(k+i|k)^TQx(k+i|k) \\
&=\begin{bmatrix}x(k|k)\\x(k+1|k)\\...\\x(k+n|k)\end{bmatrix}\begin{bmatrix}Q&0&...&0\\
0&Q&0&...&0\\
0&0&Q&...&0\\
&&&...&\\
0&0&0&...&F\end{bmatrix}\begin{bmatrix}x(k|k)\\x(k+1|k)\\...\\x(k+n|k)\end{bmatrix}\\
&=X_k^T\bar{Q}X_k
\end{align}
$$

$$
\begin{align}
&J=X_k^T\bar{Q}X_k+U_k^T\bar{R}U_k\\
&=(x_k^TM^T+U_k^TC^T)\bar{Q}(Mx_
k+CU_k)+U_k^T\bar{R}U_k\\
&=x_k^TM^T\bar{Q}Mx_k+2x_k^TM\bar{Q}CU_k + U_k^TC^T\bar{Q}CU_k + U_k^T\bar{R}U_k
\end{align}
$$
令
$M^T\bar{Q}M=G$
$M\bar{Q}C=E$
$C^T\bar{Q}C+R=H$
$$
J=x_k^TGx_k+2x_k^TEU_k + U_k^THU_k
$$
因此為對於$U_k$的二次式，可以套用最佳化演算法求解→程式碼。

# 範例
$$
x(k+1) = Ax(k)+Bu(k)
$$
$x(k+1)\in R^{n\times1}$、$u(k)\in R^{p\times1}$、$A\in R^{n\times n}$、$B\in R^{n\times p}$
$$
\begin{bmatrix}x_1(k+1)\\x_2(k+1)\end{bmatrix}=\begin{bmatrix}0&0.1\\0&2\end{bmatrix}\begin{bmatrix}x_1(k)\\x_2(k)\end{bmatrix}+\begin{bmatrix}0\\0.5\end{bmatrix}u(k)
$$

$$
X(k) = Mx(k)+CU(k)
$$
$A=\begin{bmatrix}0&0.1\\0&2\end{bmatrix}$
$B=\begin{bmatrix}0\\0.5\end{bmatrix}$
假設$n=3$
$$
X(k) =\begin{bmatrix}x(k|k)\\x(k+1|k)\\x(k+2|k)\\x(k+3|k)\end{bmatrix},
U(k) =\begin{bmatrix}u(k|k)\\u(k+1|k)\\u(k+2|k)\end{bmatrix}
$$
$$
M = \begin{bmatrix} I \\ A\\A^2\\A^3\end{bmatrix}, A^2= \begin{bmatrix} 0&0.1 \\ 0 & 2\end{bmatrix}\begin{bmatrix} 0&0.1 \\ 0 & 2\end{bmatrix}=\begin{bmatrix} 1&0.3 \\ 0 & 4\end{bmatrix}, A^3=\begin{bmatrix} 0&0.7 \\ 0 & 8\end{bmatrix}
$$
$AB=\begin{bmatrix} 1&0.1\\0&2\end{bmatrix}\begin{bmatrix} 0\\0.5\end{bmatrix}=\begin{bmatrix} 0.05\\1\end{bmatrix}$
$A^2B=\begin{bmatrix} 1&0.3 \\ 0 & 4\end{bmatrix}\begin{bmatrix} 0\\0.5\end{bmatrix}=\begin{bmatrix} 0.15\\2\end{bmatrix}$

$$
C=\begin{bmatrix}
0&0&0\\
0&0&0\\
0&0&0\\
B&0&0\\
AB&B&0\\
A^2B&AB&B
\end{bmatrix}=\begin{bmatrix}
0&0&0\\
0&0&0\\
0&0&0\\
0.5&0&0\\
0.05&0.5&0\\
1&0&0\\
0.01&0.05&0\\
2&1&0.5
\end{bmatrix}
$$
目標函數
$$
J = x(k)^TGx(k)+U(k)^THU(k)+2x(k)^TEU(k)
$$

$$
\begin{align}
&G=M^T \bar{Q}M\\
&E = C^T \bar{Q}M\\
&H = C^T \bar{Q}C+\bar{R}
\end{align}
$$

$$
\bar{Q}=\begin{bmatrix} Q&...& \\ \vdots&Q&\vdots \\...&...&F\end{bmatrix};\bar{R}=\begin{bmatrix} R&...& \\\vdots &\ddots &\vdots \\...&...&R\end{bmatrix}
$$

已經將目標函數轉成二次式，可以套用最佳化演算法求解→程式碼

# 程式範例

程式實作，首先我們要
1. 設定矩陣$A, B, Q, R, F$，
狀態矩陣用的
$A$: 狀態的權重矩陣
$B$: 輸入的權重矩陣
損失函數用的
$Q$: 誤差加權的權重矩陣
$R$: 輸入的權重矩陣
$R$: 終端狀態的的權重
2. 設定$k$的最大值
3. 給定初始狀態$x_0$
4. 初始化$X_k$和$U_k$矩陣，設定預設區間$n$
5. 根據上述公式推算係數矩陣$G, E, H$
6. call最佳化方法解二次式，對$X_k$進行更新。

###### tags: `Learning`