1. 四旋翼的动力学模型
这里不加推导地给出四旋翼的动力学模型:
m
[
x
¨
y
¨
z
¨
]
=
[
∑
i
=
1
4
F
i
(
cos
φ
sin
θ
cos
ψ
+
sin
φ
sin
ψ
)
∑
i
=
1
4
F
i
(
cos
φ
sin
θ
sin
ψ
−
sin
φ
cos
ψ
)
∑
i
=
1
4
F
i
cos
φ
cos
θ
−
m
g
]
(1)
m \left[ \begin{matrix} \ddot x \\ \ddot y \\ \ddot z \end{matrix} \right] = \left[ \begin{matrix} \sum_{i=1}^4 F_i \left( \cos \varphi \sin \theta \cos \psi + \sin \varphi \sin \psi \right) \\ \sum_{i=1}^4 F_i \left( \cos \varphi \sin \theta \sin \psi - \sin \varphi \cos \psi \right) \\ \sum_{i=1}^4 F_i \cos \varphi \cos \theta - mg \end{matrix} \right] \tag{1}
m
x¨y¨z¨
=
∑i=14Fi(cosφsinθcosψ+sinφsinψ)∑i=14Fi(cosφsinθsinψ−sinφcosψ)∑i=14Ficosφcosθ−mg
(1)
[
J
x
p
˙
J
y
q
˙
J
z
r
˙
]
=
[
l
(
F
2
−
F
2
)
−
(
J
z
−
J
y
)
q
r
l
(
F
1
−
F
3
)
−
(
J
x
−
J
z
)
p
r
M
D
1
−
M
D
2
+
M
D
3
−
M
D
4
−
(
J
y
−
J
x
)
p
q
]
(2)
\left[ \begin{matrix} J_x \dot p \\ J_y \dot q \\ J_z \dot r \end{matrix} \right] = \left[ \begin{matrix} l \left( F_2 - F_2 \right) - \left( J_z - J_y \right) qr \\ l \left( F_1 - F_3 \right) - \left( J_x - J_z \right) pr \\ M_{D1} - M_{D2} + M_{D3} - M_{D4} - \left( J_y - J_x \right) pq \end{matrix} \right] \tag{2}
Jxp˙Jyq˙Jzr˙
=
l(F2−F2)−(Jz−Jy)qrl(F1−F3)−(Jx−Jz)prMD1−MD2+MD3−MD4−(Jy−Jx)pq
(2)
[
φ
˙
θ
˙
ψ
˙
]
=
[
1
sin
φ
tan
θ
cos
φ
tan
θ
0
cos
φ
−
sin
φ
0
sin
φ
cos
θ
cos
φ
cos
θ
]
[
p
q
r
]
\left[ \begin{matrix} \dot \varphi \\ \dot \theta \\ \dot \psi \end{matrix} \right] = \left[ \begin{matrix} 1 & \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0 & \cos \varphi & - \sin \varphi \\ 0 & \frac{\sin \varphi}{\cos \theta} & \frac{\cos \varphi}{\cos \theta} \end{matrix} \right] \left[ \begin{matrix} p \\ q \\ r \end{matrix} \right]
φ˙θ˙ψ˙
=
100sinφtanθcosφcosθsinφcosφtanθ−sinφcosθcosφ
pqr
其中
φ
,
θ
,
ψ
\varphi, \theta, \psi
φ,θ,ψ均为机体欧拉角,
p
,
q
,
r
p,q,r
p,q,r为欧拉角的角速度。(关于更加详细的四旋翼建模过程可以参考笔者博客Backstepping反步法控制四旋翼无人机(一))
2. 模型预测控制(MPC)基本方程
模型预测控制(Model Predictive Control, MPC)本质上是最优控制的一种,尝试在约束条件下达到最优的系统表现。它通过预测模型在未来一段时间内的表现来进行优化控制,通过不断迭代,最终使系统逐渐优化。
一般地,对于一个系统
x
(
k
+
1
)
=
A
X
(
k
)
+
B
U
(
k
)
A
∈
R
n
×
n
,
B
∈
R
n
×
p
x \left( k+1 \right) = AX(k)+ BU(k) \\ A \in R^{n \times n}, \quad B \in R^{n \times p}
x(k+1)=AX(k)+BU(k)A∈Rn×n,B∈Rn×p建立向量
X
k
=
[
x
(
k
∣
k
)
x
(
k
+
1
∣
k
)
⋮
x
(
k
+
N
−
1
∣
k
)
]
,
U
k
=
[
u
(
k
∣
k
)
u
(
k
+
1
∣
k
)
⋮
u
(
k
+
N
−
1
∣
k
)
]
X_k = \left[ \begin{matrix} x(k \vert k) \\ x(k+1 \vert k) \\ \vdots \\ x(k+N-1 \vert k) \end{matrix} \right], \quad U_k = \left[ \begin{matrix} u(k \vert k) \\ u(k+1 \vert k) \\ \vdots \\ u(k+N-1 \vert k) \end{matrix} \right]
Xk=
x(k∣k)x(k+1∣k)⋮x(k+N−1∣k)
,Uk=
u(k∣k)u(k+1∣k)⋮u(k+N−1∣k)
同时假设输出即为
y
=
x
y=x
y=x,那么误差即为
E
=
y
−
x
=
0
E = y-x = 0
E=y−x=0另外,性能指标具有如下形式
J
=
∑
i
=
0
N
−
1
[
x
(
k
+
i
∣
k
)
T
Q
x
(
k
+
i
∣
k
)
+
u
(
k
+
i
∣
k
)
t
R
u
(
k
+
i
∣
k
)
]
+
x
(
k
+
N
)
T
F
x
(
k
+
N
)
J = \sum_{i=0}^{N-1} \left[ x(k+i \vert k)^T Q x(k+i \vert k) + u(k+i \vert k)^t R u(k+i \vert k)\right] + x(k+N)^TFx(k+N)
J=i=0∑N−1[x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)tRu(k+i∣k)]+x(k+N)TFx(k+N)其中
Q
,
R
,
F
Q,R,F
Q,R,F分别为状态、控制量、静态误差的权重。通过调整三者的相对大小,可以实现对三者的不同权重调整,系统表现也会有所不同。
设
Q
ˉ
=
[
Q
Q
⋱
Q
F
]
,
R
ˉ
=
[
R
R
⋱
R
]
\bar Q = \left[ \begin{matrix} Q & \quad & \quad & \quad & \quad \\ \quad & Q & \quad & \quad & \quad \\ \quad & \quad & \ddots & \quad & \quad \\ \quad & \quad & \quad & Q & \quad \\ \quad & \quad & \quad & \quad & F \end{matrix} \right], \quad \bar R = \left[ \begin{matrix} R & \quad & \quad & \quad \\ \quad & R & \quad & \quad \\ \quad & \quad & \ddots & \quad \\ \quad & \quad & \quad & R \end{matrix} \right]
Qˉ=
QQ⋱QF
,Rˉ=
RR⋱R
M
=
[
I
A
A
2
⋮
A
N
]
,
C
=
[
0
0
⋯
0
B
0
⋯
0
A
B
B
⋯
0
⋮
⋮
⋱
⋮
A
N
−
1
B
A
N
−
2
B
⋯
B
]
M = \left[ \begin{matrix} I \\ A \\ A^2 \\ \vdots \\ A^N \end{matrix} \right], \quad C = \left[ \begin{matrix} 0 & 0 & \cdots & 0 \\ B & 0 & \cdots & 0 \\ AB & B & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ A^{N-1}B & A^{N-2}B & \cdots & B \end{matrix} \right]
M=
IAA2⋮AN
,C=
0BAB⋮AN−1B00B⋮AN−2B⋯⋯⋯⋱⋯000⋮B
G
=
M
T
Q
ˉ
,
E
=
M
T
Q
ˉ
C
,
H
=
C
T
Q
ˉ
C
+
R
ˉ
G = M^T \bar Q, \quad E = M^T \bar Q C,\quad H = C^T \bar Q C + \bar R
G=MTQˉ,E=MTQˉC,H=CTQˉC+Rˉ那么性能指标可以写为
J
=
x
k
T
G
x
k
+
2
x
k
T
E
U
k
+
U
k
T
H
U
k
(3)
J = x_k^T G x_k + 2 x_k^T E U_k +U_k^THU_k \tag{3}
J=xkTGxk+2xkTEUk+UkTHUk(3)该式即为二次规划的一般形式。二次规划的一般形式为
min
(
z
T
Q
z
+
c
T
z
)
(4)
\min \left( z^T Q z + c^T z \right) \tag{4}
min(zTQz+cTz)(4)(3)式中的
U
U
U即对应(4)式中的
z
z
z。
3. 非线性模型预测控制
不难看出,以上给出的方法仅仅适用于线性系统。当系统为非线性时, A , B A,B A,B矩阵将很难写出,进而 M , C , G , E , H M,C,G,E,H M,C,G,E,H矩阵无法计算。这部分针对非线性系统给出了模型预测控制的算法。
很容易看出,非线性模型预测控制的难点就在于如何获取矩阵 A , B A,B A,B。不妨设系统的期望值为 x r x_r xr,利用泰勒展开将系统的非线性表达式在期望值附近展开,随后利用拉格朗日方程得到矩阵 A , B A,B A,B即可。
具体计算公式如下:
X
˙
=
f
i
(
x
i
,
u
j
)
\dot X = f_i \left(x_i, u_j \right)
X˙=fi(xi,uj)
A
=
[
∂
f
1
∂
x
1
∂
f
1
∂
x
2
⋯
∂
f
1
∂
x
n
∂
f
2
∂
x
1
∂
f
2
∂
x
2
⋯
∂
f
2
∂
x
n
⋮
⋮
⋱
⋮
∂
f
n
∂
x
1
∂
f
n
∂
x
2
⋯
∂
f
n
∂
x
n
]
x
i
=
x
r
A = \left[ \begin{matrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \cdots & \frac{\partial f_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial x_1} & \frac{\partial f_n}{\partial x_2} & \cdots & \frac{\partial f_n}{\partial x_n} \end{matrix} \right] _{x_i = x_r}
A=
∂x1∂f1∂x1∂f2⋮∂x1∂fn∂x2∂f1∂x2∂f2⋮∂x2∂fn⋯⋯⋱⋯∂xn∂f1∂xn∂f2⋮∂xn∂fn
xi=xr
B
=
[
∂
f
1
∂
u
1
∂
f
1
∂
u
2
⋯
∂
f
1
∂
u
p
∂
f
2
∂
u
1
∂
f
2
∂
u
2
⋯
∂
f
2
∂
u
p
⋮
⋮
⋱
⋮
∂
f
n
∂
u
1
∂
f
n
∂
u
2
⋯
∂
f
n
∂
u
p
]
u
i
=
u
r
B = \left[ \begin{matrix} \frac{\partial f_1}{\partial u_1} & \frac{\partial f_1}{\partial u_2} & \cdots & \frac{\partial f_1}{\partial u_p} \\ \frac{\partial f_2}{\partial u_1} & \frac{\partial f_2}{\partial u_2} & \cdots & \frac{\partial f_2}{\partial u_p} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial u_1} & \frac{\partial f_n}{\partial u_2} & \cdots & \frac{\partial f_n}{\partial u_p} \end{matrix} \right] _{u_i = u_r}
B=
∂u1∂f1∂u1∂f2⋮∂u1∂fn∂u2∂f1∂u2∂f2⋮∂u2∂fn⋯⋯⋱⋯∂up∂f1∂up∂f2⋮∂up∂fn
ui=ur值得注意的是,
A
,
B
A,B
A,B中的
x
i
,
u
i
x_i, u_i
xi,ui应当代入期望值
x
r
,
u
r
x_r, u_r
xr,ur。
下面以四旋翼为例进行计算。
设状态量为
x
1
=
φ
x
2
=
x
˙
1
=
φ
˙
x
3
=
θ
x
4
=
x
˙
3
=
θ
˙
x
5
=
ψ
x
6
=
x
˙
5
=
ψ
˙
x
7
=
x
x
8
=
x
˙
7
=
x
˙
x
9
=
y
x
10
=
x
˙
9
=
y
˙
x
11
=
z
x
12
=
x
˙
11
=
z
˙
\begin{aligned} x_1 &= \varphi \\ x_2 &= \dot x_1 = \dot \varphi \\ x_3 &= \theta \\ x_4 &= \dot x_3 = \dot \theta \\ x_5 &= \psi \\ x_6 &= \dot x_5 = \dot \psi \\ x_7 &= x \\ x_8 &= \dot x_7 = \dot x \\ x_9 &= y \\ x_{10} &= \dot x_9 = \dot y \\ x_{11} &= z \\ x_{12} &= \dot x_{11} = \dot z \end{aligned}
x1x2x3x4x5x6x7x8x9x10x11x12=φ=x˙1=φ˙=θ=x˙3=θ˙=ψ=x˙5=ψ˙=x=x˙7=x˙=y=x˙9=y˙=z=x˙11=z˙对(1)(2)求导有
X
˙
=
f
i
+
g
U
\dot X = f_i + g U
X˙=fi+gU
A
=
[
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
J
y
−
J
z
J
x
x
6
0
J
y
−
J
z
J
x
x
4
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
J
z
−
J
x
J
y
x
6
0
0
0
J
z
−
J
x
J
y
x
2
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
J
x
−
J
y
J
z
x
4
0
J
x
−
J
y
J
z
x
2
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
cos
x
1
sin
x
5
−
sin
x
1
sin
x
3
cos
x
5
m
U
1
0
cos
x
1
cos
x
3
cos
x
5
m
U
1
0
sin
x
1
cos
x
5
−
cos
x
1
sin
x
3
sin
x
5
m
U
1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
−
sin
x
1
sin
x
3
sin
x
5
−
cos
x
1
cos
x
5
m
U
1
0
cos
x
1
cos
x
3
sin
x
5
m
U
1
0
cos
x
1
sin
x
3
cos
x
5
+
sin
x
1
sin
x
5
m
U
1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
−
sin
x
1
cos
x
3
m
U
1
0
−
cos
x
1
sin
x
3
m
U
1
0
0
0
0
0
0
0
0
0
]
A = \left[ \begin{matrix} 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \frac{J_y - J_z}{J_x}x_6 & 0 & \frac{J_y - J_z}{J_x}x_4 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & \frac{J_z - J_x}{J_y}x_6 & 0 & 0 & 0 & \frac{J_z - J_x}{J_y}x_2 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & \frac{J_x - J_y}{J_z}x_4 & 0 & \frac{J_x - J_y}{J_z}x_2 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ \frac{\cos x_1 \sin x_5 - \sin x_1 \sin x_3 \cos x_5}{m}U_1 & 0 & \frac{\cos x_1 \cos x_3 \cos x_5}{m}U_1 & 0 & \frac{\sin x_1 \cos x_5 - \cos x_1 \sin x_3 \sin x_5}{m}U_1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ \frac{-\sin x_1 \sin x_3 \sin x_5 - \cos x_1 \cos x_5}{m}U_1 & 0 & \frac{\cos x_1 \cos x_3 \sin x_5}{m}U_1 & 0 & \frac{\cos x_1 \sin x_3 \cos x_5 + \sin x_1 \sin x_5}{m}U_1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \frac{-\sin x_1 \cos x_3}{m}U_1 & 0 & \frac{- \cos x_1 \sin x_3}{m}U_1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{matrix} \right]
A=
0000000mcosx1sinx5−sinx1sinx3cosx5U10m−sinx1sinx3sinx5−cosx1cosx5U10m−sinx1cosx3U1100JyJz−Jxx60JzJx−Jyx40000000000000mcosx1cosx3cosx5U10mcosx1cosx3sinx5U10m−cosx1sinx3U10JxJy−Jzx6100JzJx−Jyx20000000000000msinx1cosx5−cosx1sinx3sinx5U10mcosx1sinx3cosx5+sinx1sinx5U1000JxJy−Jzx40JyJz−Jxx210000000000000000000000000100000000000000000000000001000000000000000000000000010
B
=
[
0
0
0
0
0
l
J
x
0
0
0
0
0
0
0
0
l
J
y
0
0
0
0
0
0
0
0
1
J
z
0
0
0
0
cos
x
1
sin
x
3
cos
x
5
+
sin
x
1
sin
x
5
m
0
0
0
0
0
0
0
cos
x
1
sin
x
3
sin
x
5
−
sin
x
1
cos
x
5
m
0
0
0
0
0
0
0
cos
x
1
cos
x
3
m
0
0
0
]
B = \left[ \begin{matrix} 0 & 0 & 0 & 0 \\ 0 & \frac{l}{J_x} & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & \frac{l}{J_y} & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \frac{1}{J_z} \\ 0 & 0 & 0 & 0 \\ \frac{\cos x_1 \sin x_3 \cos x_5 + \sin x_1 \sin x_5}{m} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \frac{\cos x_1 \sin x_3 \sin x_5 - \sin x_1 \cos x_5}{m} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \frac{\cos x_1 \cos x_3}{m} & 0 & 0 & 0 \end{matrix} \right]
B=
0000000mcosx1sinx3cosx5+sinx1sinx50mcosx1sinx3sinx5−sinx1cosx50mcosx1cosx30Jxl0000000000000Jyl0000000000000Jz1000000
可见
n
=
12
,
p
=
4
n=12, p=4
n=12,p=4。
代入期望值
x
1
r
=
φ
r
=
0.02
x
2
r
=
φ
˙
r
=
0
x
3
r
=
θ
r
=
0.01
x
4
r
=
θ
˙
r
=
0
x
5
r
=
ψ
r
=
0.1
x
6
r
=
ψ
˙
r
=
0
x
7
r
=
x
r
=
5
x
8
r
=
x
˙
r
=
0
x
9
r
=
y
r
=
3
x
10
r
=
y
˙
r
=
0
x
11
r
=
z
r
=
−
2
x
12
r
=
z
˙
r
=
2
\begin{aligned} x_{1r} &= \varphi_r = 0.02 \\ x_{2r} &= \dot \varphi_r = 0 \\ x_{3r} &= \theta_r = 0.01 \\ x_{4r} &= \dot \theta_r = 0 \\ x_{5r} &= \psi_r = 0.1 \\ x_{6r} &= \dot \psi_r = 0 \\ x_{7r} &= x_r = 5 \\ x_{8r} &= \dot x_r = 0 \\ x_{9r} &= y_r = 3 \\ x_{10r} &= \dot y_r = 0 \\ x_{11r} &= z_r = -2 \\ x_{12r} &= \dot z_r = 2 \end{aligned}
x1rx2rx3rx4rx5rx6rx7rx8rx9rx10rx11rx12r=φr=0.02=φ˙r=0=θr=0.01=θ˙r=0=ψr=0.1=ψ˙r=0=xr=5=x˙r=0=yr=3=y˙r=0=zr=−2=z˙r=2以及其他常量
m
=
1
g
=
9.8
l
=
0.3
J
x
=
2.4
×
1
0
−
2
J
y
=
1.4
×
1
0
−
2
J
z
=
1.4
×
1
0
−
2
\begin{aligned} m &= 1 \\ g &= 9.8 \\ l &= 0.3 \\ J_x &= 2.4 \times 10^{-2} \\ J_y &= 1.4 \times 10^{-2} \\ J_z &= 1.4 \times 10^{-2} \\ \end{aligned}
mglJxJyJz=1=9.8=0.3=2.4×10−2=1.4×10−2=1.4×10−2共迭代
k
=
20
k=20
k=20步,迭代步长
N
=
5
N = 5
N=5。
则计算得到
A
=
[
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0.0996
0
0.9948
0
0.0189
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
−
0.9948
0
0.0998
0
0.0119
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
−
0.02
0
−
0.01
0
0
0
0
0
0
0
0
0
]
A = \left[ \begin{matrix} 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0.0996 & 0 & 0.9948 & 0 & 0.0189 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ -0.9948 & 0 & 0.0998 & 0 & 0.0119 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ -0.02 & 0 & -0.01 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{matrix} \right]
A=
00000000.09960−0.99480−0.0210000000000000000000.994800.09980−0.0100100000000000000000.018900.011900000010000000000000000000000000100000000000000000000000001000000000000000000000000010
B
=
[
0
0
0
0
0
12.5
0
0
0
0
0
0
0
0
21.4286
0
0
0
0
0
0
0
0
71.4286
0
0
0
0
0.0119
0
0
0
0
0
0
0
0.0119
0
0
0
0
0
0
0
0.9998
0
0
0
]
B = \left[ \begin{matrix} 0 & 0 & 0 & 0 \\ 0 & 12.5 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 21.4286 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 71.4286 \\ 0 & 0 & 0 & 0 \\ 0.0119 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0.0119 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0.9998 & 0 & 0 & 0 \end{matrix} \right]
B=
00000000.011900.011900.9998012.5000000000000021.4286000000000000071.4286000000
得到状态变量如下图所示。
注意,图中表示的是6个状态量的误差值。可以看出,6个状态量很快地达到了稳定态,证明了模型预测控制的有效性。