MPC（1）线性时变模型预测控制
前段时间在做无人驾驶车辆模型预测控制这方面的开发，已经在Matlab/CarSim中验证过相关的算法可行性，现在对之前的工作做一个简单的记录。实际控制和理论方面还是有很大差距的，这里仅仅是参考而已。
这本书《无人驾驶模型预测控制》很有参考价值，以下几篇内容均是其中比较精华的部分摘取，有些会加上个人的一看见解，若有不当之处，劳烦指出。
线性时变模型预测控制算法是以线性时变模型做为预测模型，目前在模型预测领域应用最广的一种形式。相比非线性模型预测控制，其最大的优点就是计算较为简单，从而保证了良好的实时性。
预测方程
首先，采用以下离散线性化模型：
x ( k + 1 ) = A k , t x ( k ) + B ( k , t ) u ( k ) (1) x(k+1)=A_{k,t}x(k)+B(k,t)u(k) \tag{1} x(k+1)=Ak,tx(k)+B(k,t)u(k)(1)
式中，x为状态量，u为控制量。
令
ξ ( k ∣ t ) = [ x ( k ∣ t ) u ( k − 1 ∣ t ) ] (2) \xi(k\vert t)=\begin{bmatrix} x(k \vert t) \\ u(k-1 \vert t) \\ \end{bmatrix} \tag{2} ξ(k∣t)=[x(k∣t)u(k−1∣t)](2)
则可以得到一个新的状态空间表达式：
ξ ( k + 1 ∣ t ) = A ~ k , t ξ ( k ∣ t ) + B ~ k , t Δ u ( k ∣ t ) (3) \xi(k+1 \vert t) = \widetilde{A}_{k,t}\xi(k \vert t)+\widetilde{B}_{k,t}\Delta u(k \vert t) \tag{3} ξ(k+1∣t)=A k,tξ(k∣t)+B k,tΔu(k∣t)(3)
η ( k ∣ t ) = C ~ k , t ξ ( k ∣ t ) (4) \eta(k \vert t)=\widetilde{C}_{k,t}\xi(k \vert t) \tag{4} η(k∣t)=C k,tξ(k∣t)(4)
式中各个矩阵的定义如下，
A ~ k , t = [ A k , t B k , t 0 m ∗ n I m ] (5) \widetilde{A}_{k,t}=\begin{bmatrix} A_{k,t} & B_{k,t} \\ 0_{m*n} & I_m \\ \end{bmatrix} \tag{5} A k,t=[Ak,t0m∗nBk,tIm](5)
B ~ k , t = [ B k , t I m ] (6) \widetilde{B}_{k,t}=\begin{bmatrix} B_{k,t} \\ I_m \\ \end{bmatrix} \tag{6} B k,t=[Bk,tIm](6)
C ~ k , t = [ C k , t 0 ] (7) \widetilde{C}_{k,t}=[C_{k,t}\space0] \tag{7} C k,t=[Ck,t 0](7)
为了进一步简化计算，做出以下假设：
A ~ k , t = A ~ t , k = 1 , . . . , t + N − 1 (8) \widetilde{A}_{k,t}=\widetilde{A}_{t},k=1,...,t+N-1 \tag{8} A k,t=A t,k=1,...,t+N−1(8)
B ~ k , t = B ~ t , k = 1 , . . . , t + N − 1 (9) \widetilde{B}_{k,t}=\widetilde{B}_{t},k=1,...,t+N-1 \tag{9} B k,t=B t,k=1,...,t+N−1(9)
假设系统的预测时域Np，控制时域Nc，此时预测时域内的状态量和系统输出量可以按下下式计算：
ξ ( t + N p ∣ t ) = A ~ t N p ξ ( t ∣ t ) + A ~ t N p − 1 B ~ t Δ u ( t ∣ t ) + . . . + A ~ t N p − N c − 1 B ~ t Δ u ( t + N c ∣ t ) (10) \xi(t+N_p|t)= \widetilde{A}^{N_p}_{t}\xi(t|t)+\widetilde{A}^{N_p-1}_{t}\widetilde{B}_{t}\Delta u(t|t)+\\ ...+\widetilde{A}^{N_p-N_c-1}_{t}\widetilde{B}_{t}\Delta u(t+N_c|t) \tag{10} ξ(t+Np∣t)=A tNpξ(t∣t)+A tNp−1B tΔu(t∣t)+...+A tNp−Nc−1B tΔu(t+Nc∣t)(10)
η ( t + N p ∣ t ) = C ~ t , t A ~ t , t N p ξ ( t ∣ t ) + C ~ t A ~ t N p − 1 B ~ t Δ u ( t ∣ t ) + . . . + C ~ t A ~ t N p − N c − 1 B ~ t Δ u ( t + N c ∣ t ) (11) \eta(t+N_p|t)=\widetilde{C}_{t,t}\widetilde{A}^{N_p}_{t,t}\xi(t|t)+\widetilde{C}_{t}\widetilde{A}^{N_p-1}_{t}\widetilde{B}_{t}\Delta u(t|t)+ \\ ...+\widetilde{C}_{t}\widetilde{A}^{N_p-N_c-1}_{t}\widetilde{B}_{t}\Delta u(t+N_c|t) \tag{11} η(t+Np∣t)=C t,tA t,tNpξ(t∣t)+C tA tNp−1B tΔu(t∣t)+...+C tA tNp−Nc−1B tΔu(t+Nc∣t)(11)
将系统未来输出时刻的输出以矩阵的形式表达：
Y ( t ) = Ψ t ξ ( t ∣ t ) + Θ Δ U ( t ) (12) Y(t)=\Psi_t\xi(t|t)+\varTheta\Delta U(t) \tag{12} Y(t)=Ψtξ(t∣t)+ΘΔU(t)(12)
式中：
Y ( t ) = [ η ( t + 1 ∣ t ) η ( t + 2 ∣ t ) … η ( t + N c ∣ t ) … η ( t + N p ∣ t ) ] Y(t)=\begin{bmatrix} \eta(t+1|t) \\ \eta(t+2|t) \\ \dots \\ \eta(t+N_c|t) \\ \dots \\ \eta(t+N_p|t) \\ \end{bmatrix} Y(t)=⎣⎢⎢⎢⎢⎢⎢⎡η(t+1∣t)η(t+2∣t)…η(t+Nc∣t)…η(t+Np∣t)⎦⎥⎥⎥⎥⎥⎥⎤，
Ψ t = [ C ~ t , t ∗ A ~ t , t C ~ t , t ∗ A ~ t , t 2 … C ~ t , t ∗ A ~ t , t N c … C ~ t , t ∗ A ~ t , t N p ] \Psi_t=\begin{bmatrix} \tilde C_{t,t}*\tilde A_{t,t} \\ \tilde C_{t,t}*\tilde A_{t,t}^2 \\ \dots \\ \tilde C_{t,t}*\tilde A_{t,t}^{N_c} \\ \dots \\ \tilde C_{t,t}*\tilde A_{t,t}^{N_p} \\ \end{bmatrix} Ψt=⎣⎢⎢⎢⎢⎢⎢⎡C~t,t∗A~t,tC~t,t∗A~t,t2…C~t,t∗A~t,tNc…C~t,t∗A~t,tNp⎦⎥⎥⎥⎥⎥⎥⎤，
Δ U ( t ) = [ Δ u ( t ∣ t ) Δ u ( t + 1 ∣ t ) ⋯ Δ u ( t + N c ∣ t ) ] \Delta U(t)=\begin{bmatrix} \Delta u(t|t) \\ \Delta u(t+1|t) \\ \cdots \\ \Delta u(t+N_c|t) \\ \end{bmatrix} ΔU(t)=⎣⎢⎢⎡Δu(t∣t)Δu(t+1∣t)⋯Δu(t+Nc∣t)⎦⎥⎥⎤,
Θ t = [ C ~ t B ~ t 0 0 0 C ~ t A ~ t B ~ t C ~ t B ~ t 0 0 ⋯ ⋯ ⋱ ⋯ C ~ t A ~ t N c − 1 B ~ t C ~ t A ~ t N c − 2 B ~ t ⋯ C ~ t B ~ t C ~ t A ~ t N c B ~ t C ~ t A ~ t N c − 1 B ~ t ⋯ C ~ t A ~ t B ~ t ⋮ ⋮ ⋱ ⋮ C ~ t A ~ t N p − 1 B ~ t C ~ t A ~ t N p − 2 B ~ t ⋯ C ~ t A ~ t N p − N c − 1 B ~ t ] \varTheta_{t}= \begin{bmatrix} \tilde{C}_{t}\tilde{B}_{t} && 0 && 0 && 0 \\ \tilde{C}_{t}\tilde{A}_{t}\tilde{B}_{t} && \tilde{C}_{t}\tilde{B}_{t} && 0 && 0 \\ \cdots && \cdots && \ddots && \cdots \\ \tilde{C}_{t}\tilde{A}^{N_c - 1}_{t}\tilde{B}_{t} && \tilde{C}_{t}\tilde{A}^{N_c - 2}_{t}\tilde{B}_{t} && \cdots && \tilde{C}_{t}\tilde{B}_{t} \\ \tilde{C}_{t}\tilde{A}^{N_c}_{t}\tilde{B}_{t} && \tilde{C}_{t}\tilde{A}^{N_c - 1}_{t}\tilde{B}_{t} && \cdots && \tilde{C}_{t}\tilde{A}_{t}\tilde{B}_{t} \\ \vdots && \vdots && \ddots && \vdots \\ \tilde{C}_{t}\tilde{A}^{N_{p}-1}_{t}\tilde{B}_{t} && \tilde{C}_{t}\tilde{A}^{N_{p}-2}_{t}\tilde{B}_{t} && \cdots && \tilde{C}_{t}\tilde{A}^{N_{p} - N_{c} -1}_{t}\tilde{B}_{t} \end{bmatrix} Θt=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡C~tB~tC~tA~tB~t⋯C~tA~tNc−1B~tC~tA~tNcB~t⋮C~tA~tNp−1B~t0C~tB~t⋯C~tA~tNc−2B~tC~tA~tNc−1B~t⋮C~tA~tNp−2B~t00⋱⋯⋯⋱⋯00⋯C~tB~tC~tA~tB~t⋮C~tA~tNp−Nc−1B~t⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
由式（12）中可以清楚的看到，在预测时域内的状态量和输出量都可以通过当前的系统状态量和控制增量来计算出。
优化求解
在实际控制系统中，控制增量是未知的，此时只有通过设定合适的优化目标，根据此目标对预测方程进行求解出控制时域内的控制序列。不同的控制系统，可以根据控制器的情况设置不同的目标函数。一般，优化目标函数设置为如下形式：
J ( ξ ( t ) , u ( t − 1 ) , Δ U ( t ) ) = ∑ i = 1 N p ∥ η ( t + i ∣ t ) − η r e f ( t + i ∣ t ) ∥ Q 2 + ∑ i = 1 N c − 1 ∥ Δ u ( t + i ∣ t ) ∥ R 2 (13) J(\xi(t),u(t-1),\Delta U(t)) \space =\space \sum\limits_{i=1}^{N_p} \begin{Vmatrix} \eta(t+i|t) \space - \eta_{ref}(t+i|t) \end{Vmatrix}_Q^2 \space + \\ \space \sum\limits_{i=1}^{N_c-1} \begin{Vmatrix} \Delta u(t+i|t) \end{Vmatrix}_R^2 \space \tag{13} J(ξ(t),u(t−1),ΔU(t)) = i=1∑Np∥∥η(t+i∣t) −ηref(t+i∣t)∥∥Q2 + i=1∑Nc−1∥∥Δu(t+i∣t)∥∥R2 (13)
其中第一项为预测轨迹与参考轨迹的偏差，第二项为控制量变化量的平方差，Q和R为权重矩阵。同时，也会根据实际控制系统中，控制器所能达到的控制极限做出一定的约束，
控制量约束：
u m i n ( t + k ) ≤ u ( t + k ) ≤ u m a x ( t + k ) , k = 0 , 1 , … , N c − 1 (14) u_{min}(t+k) \space \leq \space u(t+k) \space \leq \space u_{max}(t+k) , k \space = \space 0,1,\dots,N_c-1 \tag{14} umin(t+k) ≤ u(t+k) ≤ umax(t+k),k = 0,1,…,Nc−1(14)
控制增量约束：
Δ u m i n ( t + k ) ≤ Δ u ( t + k ) ≤ Δ u m a x ( t + k ) , k = 0 , 1 , … , N c − 1 (15) \Delta u_{min}(t+k) \space \leq \space \Delta u(t+k) \space \leq \space \Delta u_{max}(t+k) ,k = 0,1,\dots,N_c-1 \tag{15} Δumin(t+k) ≤ Δu(t+k) ≤ Δumax(t+k),k=0,1,…,Nc−1(15)
输出约束：
y m i n ( t + k ) ≤ y ( t + k ) ≤ y m a x ( t + k ) , k = 0 , 1 , … , N c − 1 (16) y_{min}(t+k) \leq y(t+k) \leq y_{max}(t+k) ,k = 0,1,\dots,N_c-1 \tag{16} ymin(t+k)≤y(t+k)≤ymax(t+k),k=0,1,…,Nc−1(16)
结合14 - 16式，求解出带约束的优化目标，可以得到未来一段时间的控制序列。
滚动优化
通过以上步骤，解算出控制时域内的一系列控制输入增量：
Δ U t ∗ = [ Δ u t ∗ , Δ u t + 1 ∗ , … , Δ u t + N c − 1 ∗ ] T (17) \Delta U_t^* \space = \space [\Delta u_t^*,\Delta u_{t+1}^*,\dots,\Delta u_{t+N_c-1}^*]^T \tag{17} ΔUt∗ = [Δut∗,Δut+1∗,…,Δut+Nc−1∗]T(17)
取第一个控制增量做为实际控制输入增量：
u ( t ) = u ( t − 1 ) + Δ u t ∗ (18) u(t) \space = \space u(t-1) \space + \space \Delta u_t^* \tag{18} u(t) = u(t−1) + Δut∗(18)
系统按照此控制量直到下一时刻t+1。t+1时刻按照此时的参考轨迹，重复以上操作，重新得到优化后的新的控制增量。如此循环直到控制过程结束。