线性模型预测控制二次规划(Quadratic Programming)的不同构造形式
在之前的博客中我们提到了Apollo OSQP Solver构造二次规划问题的形式, 该种构造方式称为Non-Condensed Format,简单的来说, 我们在构造二次规划的优化问题的时候将输入量和状态量一起当做决策变量和, 之前的仅仅把状态变量当做决策变量的形式叫做Condensed Format. Apollo在构造二次规划问题的时候是没有考虑对控制的增量进行惩罚的, 如果我们希望输入更加平滑,需要对控制的变化率进行惩罚, 另外我们把终端状态约束单独提出来, 我们需要使用如下优化目标函数:
arg min x , u ( x N − x r e f ) T Q x N ( x N − x r e f ) ⏞ J x N + ∑ k = 0 N p − 1 ( x − x r e f ) T Q x ( x k − x r e f ) ⏞ J x + ∑ k = 0 N p − 1 ( u − u r e f ) T Q u ( u k − u r e f ) ⏞ J u + ∑ k = 0 N p − 1 Δ u k T Q Δ u Δ u k ⏞ J Δ u s u b j e c t t o u m i n ≤ u k ≤ u m a x x m i n ≤ x k ≤ x m a x Δ u m i n ≤ Δ u k ≤ Δ u m a x x 0 = x ˉ u − 1 = u ˉ \arg \min_{x,u} \overbrace{(x_N-x_{ref})^{T}Q_{x_N}(x_N-x_{ref})}^{J_{x_N}} + \overbrace{\sum_{k=0}^{N_{p}-1}(x - x_{ref})^{T}Q_{x}(x_{k} - x_{ref})}^{J_{x}} + \\ \overbrace{\sum_{k=0}^{N_{p}-1}(u - u_{ref})^{T}Q_{u}(u_{k} - u_{ref})}^{J_{u}} +\\ \overbrace{\sum_{k=0}^{N_{p} -1} \Delta u_k^{T}Q_{\Delta u} \Delta u_k}^{J_{\Delta u}} \\ subject~to \\ u_{min} \leq u_k \leq u_{max}\\ x_{min} \leq x_k \leq x_{max} \\ \Delta u_{min} \leq \Delta u_k \leq \Delta u_{max} \\ x_0 = \bar x\\ u_{-1} = \bar u argx,umin(xN−xref)TQxN(xN−xref) JxN+k=0∑Np−1(x−xref)TQx(xk−xref) Jx+k=0∑Np−1(u−uref)TQu(uk−uref) Ju+k=0∑Np−1ΔukTQΔuΔuk JΔusubject toumin≤uk≤umaxxmin≤xk≤xmaxΔumin≤Δuk≤Δumaxx0=xˉu−1=uˉ
其中 Δ u k = u k − u k − 1 \Delta u_k = u_k - u_{k-1} Δuk=uk−uk−1,
如果我们同时选取状态量和输入量作为优化问题的决策变量, 我们优化问题的决策变量为:
x = [ x 0 x 1 … x N p ] , u = [ u 0 u 1 … u N p − 1 ] x = \begin{bmatrix} x_0 & x_1 & \dots & x_{N_{p}} \end{bmatrix},~ u = \begin{bmatrix} u_0 & u_1 & \dots & u_{N_{p-1}} \end{bmatrix} x=[x0x1…xNp], u=[u0u1…uNp−1]
我们需要将上述优化问题转换为标准的二次规划形式
min 1 2 x T P x + q T x s u b j e c t t o l ≤ A x ≤ u \min \frac{1}{2} x^{T} P x + q^{T} x \\ subject~to ~l \leq Ax \leq u min21xTPx+qTxsubject to l≤Ax≤u
目标函数部分转化
下面我们需要将优化函数的下述三个部分转换成二次规划的形式
J x = 1 2 [ x 0 T x 1 T … x N p − 1 T ] [ Q x 0 0 … … 0 0 Q x 0 0 … 0 0 0 ⋱ ⋱ ⋱ 0 0 0 ⋱ ⋱ ⋱ 0 0 0 ⋱ ⋱ ⋱ 0 0 0 0 0 Q x 0 0 0 0 0 0 Q x ] [ x 0 x 1 ⋮ x N p − 1 ] + [ − x r e f T Q x − x r e f T Q x … − x r e f T Q x ] [ x 0 x 1 ⋮ x N p − 1 ] J_{x} = \dfrac{1}{2} \begin{bmatrix} x_{0}^{T}&x_{1}^{T}&\dots&x_{N_{p} - 1}^{T} \end{bmatrix} \begin{bmatrix} Q_{x} &0 &0 & \dots & \dots & 0\\ 0 & Q_{x} &0 &0 & \dots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &0 &0 & Q_{x} &0\\ 0 & 0 &0 &0 & 0&Q_{x}\\ \end{bmatrix} \begin{bmatrix} x_{0} \\ x_{1}\\ \vdots \\ x_{N_{p} - 1} \end{bmatrix} + \\ \begin{bmatrix} -x_{ref}^{T}Q_{x}&-x_{ref}^{T}Q_{x}&\dots&-x_{ref}^{T}Q_{x} \end{bmatrix} \begin{bmatrix} x_{0} \\ x_{1} \\ \vdots \\ x_{N_{p}-1} \end{bmatrix} Jx=21[x0Tx1T…xNp−1T]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡Qx0000000Qx0000000⋱⋱⋱00…0⋱⋱⋱00……⋱⋱⋱Qx0000000Qx⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡x0x1⋮xNp−1⎦⎥⎥⎥⎤+[−xrefTQx−xrefTQx…−xrefTQx]⎣⎢⎢⎢⎡x0x1⋮xNp−1⎦⎥⎥⎥⎤
J u = 1 2 [ u 0 T u 1 T … u N p − 1 T ] [ Q u 0 0 … … 0 0 Q u 0 0 … 0 0 0 ⋱ ⋱ ⋱ 0 0 0 ⋱ ⋱ ⋱ 0 0 0 ⋱ ⋱ ⋱ 0 0 0 0 0 Q u 0 0 0 0 0 0 Q u ] [ u 0 u 1 ⋮ u N p − 1 ] + [ − u r e f T Q u − u r e f T Q u … − u r e f T Q u ] [ u 0 u 1 ⋮ u N p − 1 ] J_{u} = \dfrac{1}{2} \begin{bmatrix} u_{0}^{T}&u_{1}^{T}&\dots&u_{N_{p} - 1}^{T} \end{bmatrix} \begin{bmatrix} Q_{u} &0 &0 & \dots & \dots & 0\\ 0 & Q_{u} &0 &0 & \dots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &0 &0 & Q_{u} &0\\ 0 & 0 &0 &0 & 0&Q_{u}\\ \end{bmatrix} \begin{bmatrix} u_{0} \\ u_{1}\\ \vdots \\ u_{N_{p} - 1} \end{bmatrix} + \\ \begin{bmatrix} -u_{ref}^{T}Q_{u}&-u_{ref}^{T}Q_{u}&\dots&-u_{ref}^{T}Q_{u} \end{bmatrix} \begin{bmatrix} u_{0} \\ u_{1} \\ \vdots \\ u_{N_{p}-1} \end{bmatrix} Ju=21[u0Tu1T…uNp−1T]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡Qu0000000Qu0000000⋱⋱⋱00…0⋱⋱⋱00……⋱⋱⋱Qu0000000Qu⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡u0u1⋮uNp−1⎦⎥⎥⎥⎤+[−urefTQu−urefTQu…−urefTQu]⎣⎢⎢⎢⎡u0u1⋮uNp−1⎦⎥⎥⎥⎤
J Δ u = 1 2 [ u 0 T u 1 T … u N p − 1 T ] [ 2 Q Δ u − Q Δ u 0 … … 0 − Q Δ u 2 Q Δ u − Q Δ u 0 … 0 0 − Q Δ u ⋱ ⋱ ⋱ 0 0 0 ⋱ ⋱ ⋱ 0 0 0 ⋱ ⋱ ⋱ 0 0 0 0 − Q Δ u 2 Q Δ u − Q Δ u 0 0 0 0 − Q Δ u Q Δ u ] [ u 0 u 1 ⋮ u N p − 1 ] + [ − u ˉ Q Δ u 0 … 0 ] [ u 0 u 1 ⋮ u N p − 1 ] J_{\Delta u} = \dfrac{1}{2} \begin{bmatrix} u_{0}^{T}&u_{1}^{T}&\dots&u_{N_{p} - 1}^{T} \end{bmatrix} \begin{bmatrix} 2Q_{\Delta u} & -Q_{\Delta u} &0 & \dots & \dots & 0\\ -Q_{\Delta u} & 2Q_{\Delta u} &-Q_{\Delta u} &0 & \dots & 0\\ 0 & -Q_{\Delta u} &\ddots & \ddots &\ddots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &0 &-Q_{\Delta u} & 2Q_{\Delta u} &-Q_{\Delta u}\\ 0 & 0 &0 &0 & -Q_{\Delta u} &Q_{\Delta u}\\ \end{bmatrix} \begin{bmatrix} u_{0} \\ u_{1}\\ \vdots \\ u_{N_{p} - 1} \end{bmatrix} + \\ \begin{bmatrix} -\bar{u}Q_{\Delta u}&0&\dots&0 \end{bmatrix} \begin{bmatrix} u_{0} \\ u_{1} \\ \vdots \\ u_{N_{p}-1} \end{bmatrix} JΔu=21[u0Tu1T…uNp−1T]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡2QΔu−QΔu00000−QΔu2QΔu−QΔu00000−QΔu⋱⋱⋱00…0⋱⋱⋱−QΔu0……⋱⋱⋱2QΔu−QΔu00000−QΔuQΔu⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡u0u1⋮uNp−1⎦⎥⎥⎥⎤+[−uˉQΔu0…0]⎣⎢⎢⎢⎡u0u1⋮uNp−1⎦⎥⎥⎥⎤
约束构建
根据系统的状态方程
[ x 0 x 1 ⋮ x N p − 1 x N p ] = [ 0 0 … 0 A d 0 … 0 0 A d … 0 ⋮ 0 ⋱ 0 0 0 … A d ] ⏞ = A [ x 0 x 1 ⋮ x N p − 1 x N p ] + [ 0 0 … 0 B d 0 … 0 0 B d … 0 ⋮ 0 ⋱ 0 0 0 … B d ] ⏞ = B [ u 0 u 1 ⋮ u N p − 2 u N p − 1 ] + [ x ˉ 0 ⋮ ⋮ 0 ] ⏞ C \begin{bmatrix} x_0 \\ x_1 \\ \vdots \\ x_{N_{p} - 1} \\ x_{N_{p}} \end{bmatrix} = \overbrace{ \begin{bmatrix} 0 &0 &\dots &0\\ A_d &0 &\dots & 0\\ 0 &A_d &\dots &0\\ \vdots &0 &\ddots & 0\\ 0 &0 &\dots &A_d \end{bmatrix} }^{=\mathcal{A}} \begin{bmatrix} x_0 \\ x_1 \\ \vdots \\ x_{N_{p} - 1} \\ x_{N_{p}} \end{bmatrix} + \overbrace{ \begin{bmatrix} 0 &0 &\dots &0\\ B_d &0 &\dots & 0\\ 0 &B_d &\dots &0\\ \vdots &0 &\ddots & 0\\ 0 &0 &\dots &B_d \end{bmatrix} }^{=\mathcal{B}} \begin{bmatrix} u_0 \\ u_1 \\ \vdots \\ u_{N_{p} - 2} \\ u_{N_{p} - 1} \end{bmatrix} + \overbrace { \begin{bmatrix} \bar{x} \\ 0 \\ \vdots \\ \vdots \\ 0 \end{bmatrix} }^{\mathcal{C}} ⎣⎢⎢⎢⎢⎢⎡x0x1⋮xNp−1xNp⎦⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡0Ad0⋮000Ad00………⋱…0000Ad⎦⎥⎥⎥⎥⎥⎤ =A⎣⎢⎢⎢⎢⎢⎡x0x1⋮xNp−1xNp⎦⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎡0Bd0⋮000Bd00………⋱…0000Bd⎦⎥⎥⎥⎥⎥⎤ =B⎣⎢⎢⎢⎢⎢⎡u0u1⋮uNp−2uNp−1⎦⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎡xˉ0⋮⋮0⎦⎥⎥⎥⎥⎥⎥⎤ C
我们可以根据上述方程我们可以得到如下系统等式约束
[ ( A − I ) B ] [ x u ] = C [(\mathcal{A} - I)~\mathcal{B}] \begin{bmatrix} x \\ u \end{bmatrix} = \mathcal{C} [(A−I) B][xu]=C
输入量和状态量本身也存在约束:
[ x m i n u m i n ] ≤ [ I 0 0 I ] [ x u ] ≤ [ x m a x u m a x ] \begin{bmatrix} x_{min} \\ u_{min} \end{bmatrix} \leq \begin{bmatrix} I &0\\ 0 & I \end{bmatrix} \begin{bmatrix} x\\u \end{bmatrix} \leq \begin{bmatrix} x_{max} \\ u_{max} \end{bmatrix} [xminumin]≤[I00I][xu]≤[xmaxumax]
当我们对输入变化率进行限制时, 我们可以得到输入量的以下约束
[ u − 1 + Δ u m i n Δ u m i n ⋮ Δ u m i n ] ≤ [ I 0 … … 0 0 − I I 0 … 0 0 0 − I I … 0 0 ⋮ 0 0 … 0 − I I ] [ u 0 u 1 ⋮ u N c − 1 ] ≤ [ u − 1 + Δ u m a x Δ u m a x ⋮ Δ u m a x ] \begin{bmatrix} u_{-1} +\Delta u_{min}\\ \Delta u_{min}\\ \vdots\\ \Delta u_{min}\\ \end{bmatrix} \leq \begin{bmatrix} I & 0 & \dots & \dots & 0 & 0\\ -I & I & 0 & \dots & 0 & 0\\ 0 & -I & I & \dots & 0 & 0\\ \vdots\\ 0 & 0 & \dots & 0 &-I & I\\ \end{bmatrix} \begin{bmatrix} u_0\\ u_1\\ \vdots\\ u_{N_{c}-1}\\ \end{bmatrix} \leq \begin{bmatrix} u_{-1} +\Delta u_{max}\\ \Delta u_{max}\\ \vdots\\ \Delta u_{max}\\ \end{bmatrix} ⎣⎢⎢⎢⎡u−1+ΔuminΔumin⋮Δumin⎦⎥⎥⎥⎤≤⎣⎢⎢⎢⎢⎢⎡I−I0⋮00I−I0…0I…………0000−I000I⎦⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡u0u1⋮uNc−1⎦⎥⎥⎥⎤≤⎣⎢⎢⎢⎡u−1+ΔumaxΔumax⋮Δumax⎦⎥⎥⎥⎤
当我们对状态变量 x x x进行限制的时候, 有可能会导优化问题无解, 因此我们添加slack variable
[ x m i n u m i n ] ≤ [ I 0 I 0 I 0 ] [ x u ϵ ] ≤ [ x m a x u m a x ] \begin{bmatrix} x_{min}\\u_{min} \end{bmatrix} \leq \begin{bmatrix} I &0 &I\\ 0 &I & 0 \end{bmatrix} \begin{bmatrix} x \\ u \\ \epsilon \end{bmatrix} \leq \begin{bmatrix} x_{max}\\u_{max} \end{bmatrix} [xminumin]≤[I00II0]⎣⎡xuϵ⎦⎤≤[xmaxumax]
我们最后优化问题的决策变量为:
x = [ x 0 x 1 ⋮ x N p − 1 x N p u 0 u 1 ⋮ u N p − 1 ] x = \begin{bmatrix} x_{0} \\ x_{1} \\ \vdots \\x_{N_{p} - 1}\\x_{N_{p}} \\ u_{0} \\ u_{1} \\ \vdots \\ u_{N_{p}-1} \end{bmatrix} x=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡x0x1⋮xNp−1xNpu0u1⋮uNp−1⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
P = [ Q x 0 0 ⋯ 0 0 0 ⋯ ⋯ 0 0 Q x 0 ⋯ 0 0 0 ⋯ ⋯ 0 0 0 Q x ⋯ 0 0 0 ⋯ ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ ⋮ ⋮ ⋱ ⋱ ⋮ 0 0 0 ⋯ Q x N 0 0 ⋯ ⋯ 0 0 0 0 ⋯ 0 Q u + 2 Q Δ u − Q Δ u ⋯ ⋯ 0 0 0 0 ⋯ 0 − Q Δ u Q u + 2 Q Δ u ⋯ ⋯ 0 0 0 0 ⋯ 0 0 0 ⋯ ⋯ 0 ⋮ ⋮ ⋱ ⋱ ⋮ ⋮ ⋮ ⋱ ⋱ ⋮ 0 0 0 ⋯ 0 0 0 − Q Δ u 2 Q Δ u − Q Δ u 0 0 0 ⋯ 0 0 0 0 − Q Δ u Q Δ u ] P = \left[ \begin{array}{ccccc|ccccc} Q_{x} & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots &\cdots& 0\\ 0 & Q_{x} & 0 & \cdots & 0 & 0 & 0 & \cdots &\cdots& 0 \\ 0 & 0 & Q_{x} & \cdots & 0 & 0 & 0 & \cdots &\cdots& 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \ddots&\vdots \\ 0 & 0 & 0 & \cdots & Q_{xN} & 0 & 0 & \cdots&\cdots & 0\\ \hline 0 & 0 & 0 & \cdots & 0 & Q_{u} + 2Q_{\Delta u} & -Q_{\Delta u} & \cdots &\cdots& 0\\ 0 & 0 & 0 & \cdots & 0 & -Q_{\Delta u} & Q_{u} + 2Q_{\Delta u} & \cdots&\cdots & 0\\ 0 & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots&\cdots & 0\\ \vdots & \vdots & \ddots & \ddots & \vdots&\vdots & \vdots & \ddots & \ddots & \vdots \\0&0&0&\cdots&0&0&0&-Q_{\Delta u}&2Q_{\Delta u}&-Q_{\Delta u}\\ 0 & 0 & 0 & \cdots & 0 & 0&0&0 &-Q_{\Delta u}&Q_{\Delta u}\end{array} \right] P=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡Qx00⋮0000⋮000Qx0⋮0000⋮0000Qx⋮0000⋱00⋯⋯⋯⋱⋯⋯⋯⋯⋱⋯⋯000⋮QxN000⋮00000⋮0Qu+2QΔu−QΔu0⋮00000⋮0−QΔuQu+2QΔu0⋮00⋯⋯⋯⋱⋯⋯⋯⋯⋱−QΔu0⋯⋯⋯⋱⋯⋯⋯⋯⋱2QΔu−QΔu000⋮0000⋮−QΔuQΔu⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
A = [ − I 0 0 ⋯ 0 0 0 ⋯ 0 A − I 0 ⋯ 0 B 0 ⋯ 0 0 A − I ⋯ 0 0 B ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 ⋯ − I 0 0 ⋯ B I 0 0 ⋯ 0 0 0 ⋯ 0 0 I 0 ⋯ 0 0 0 ⋯ 0 0 0 I ⋯ 0 0 0 ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 ⋯ I 0 0 ⋯ 0 0 0 0 ⋯ 0 2 I 0 ⋯ 0 0 0 0 ⋯ 0 − I 2 I ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 ⋯ 0 0 0 ⋯ 2 I ] A = \left[ \begin{array}{ccccc|cccc} -I & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ A & -I & 0 & \cdots & 0 & B & 0 & \cdots & 0 \\ 0 & A & -I & \cdots & 0 & 0 & B & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & -I & 0 & 0 & \cdots & B\\ \hline I & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 0 & I & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 0 & 0 & I & \cdots & 0 & 0 & 0 & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & I & 0 & 0 & \cdots & 0\\ 0 & 0 & 0 & \cdots & 0 & 2I & 0 & \cdots & 0\\ 0 & 0 & 0 & \cdots & 0 & -I & 2I & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 2I \end{array} \right] A=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−IA0⋮0I00⋮000⋮00−IA⋮00I0⋮000⋮000−I⋮000I⋮000⋮0⋯⋯⋯⋱⋯⋯⋯⋯⋱⋯⋯⋯⋱⋯000⋮−I000⋮I00⋮00B0⋮0000⋮02I−I⋮000B⋮0000⋮002I⋮0⋯⋯⋯⋱⋯⋯⋯⋯⋱⋯⋯⋯⋱⋯000⋮B000⋮000⋮2I⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
l = [ − x 0 0 ⋮ 0 x m i n ⋮ x m i n u m i n + u − 1 + Δ u m i n ⋮ Δ u m i n + u m i n ] u = [ − x 0 0 ⋮ 0 x m a x ⋮ x m a x u m a x + u − 1 + Δ u m a x ⋮ u m a x + Δ u m a x ] l = \begin{bmatrix} -x_0 \\ 0 \\ \vdots \\ 0 \\ x_{min}\\ \vdots\\ x_{min}\\ u_{min}+u_{-1} +\Delta u_{min}\\ \vdots\\ \Delta u_{min}+u_{min}\\ \end{bmatrix} \quad u = \begin{bmatrix} -x_0 \\ 0 \\ \vdots \\ 0 \\ x_{max}\\ \vdots\\ x_{max}\\ u_{max}+u_{-1}+\Delta u_{max}\\ \vdots\\ u_{max}+\Delta u_{max}\\ \end{bmatrix} l=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−x00⋮0xmin⋮xminumin+u−1+Δumin⋮Δumin+umin⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤u=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−x00⋮0xmax⋮xmaxumax+u−1+Δumax⋮umax+Δumax⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
q = [ − Q x r − Q x r ⋮ − Q x r − R u r ⋮ − R u r ] q = \begin{bmatrix} -Q x_r \\ -Q x_r \\ \vdots \\ -Q x_r \\ -R u_r\\ \vdots\\ -R u_r \end{bmatrix} q=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−Qxr−Qxr⋮−Qxr−Rur⋮−Rur⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
上述形式其实还不够一般和普试用, 我们在还没有在二次规划中增加Slack Variable 以及没有考虑control horizon和prediction horizon不相等的情况, 打公式太繁琐, 以后再去补上带上control horizon的形式和代码