(线性系统理论大作业)
(待完成。。。)
水下潜器模型，可能是潜艇或者鱼雷等对象。一个主推进螺旋桨，前后两对水平陀翼，后面一对垂直陀翼。
潜器前进过程中，通过调节助推进螺旋桨推力，以及三对陀翼的角度变化，对潜器的五个自由度，X轴和Z轴方向的速度，以及垂直、滚动和俯仰方向角速度，进行控制，实现潜器的各种机动以及在运动过程中的姿态平稳。
以大地坐标为静止坐标系，以潜器坐标为动坐标系，用动量定理以及动量矩定理可以得到潜器的动力学模型如下：
M
V
˙
+
F
I
=
F
F
+
F
G
+
F
B
+
F
C
M \dot{V}+F_{I}=F_{F}+F_{G}+F_{B}+F_{C}
MV˙+FI=FF+FG+FB+FC
公式中，
V
=
[
V
x
,
V
y
,
ω
y
,
ω
z
]
T
V=\left[V_{x}, V_{y}, \omega_{y}, \omega_{z}\right]^{T}
V=[Vx,Vy,ωy,ωz]T为速度向量,
M
M
M为由载体质量、附加质量、转动惯量和惯性积组成的载体惯性矩阵,
F
I
F_{I}
FI 为离心力和惯性流体力,
F
F
F_{F}
FF 为非惯性流体力,
F
G
F_{G}
FG 和
F
B
F_{B}
FB 分别为载体的重力和浮力,
F
C
F_{C}
FC 为陀翼以及推进器对载体所施加的控制力。
XZ方向
设右侧的3个加法器输出分别为
S
1
S_1
S1，
S
2
S_2
S2，
S
3
S_3
S3。
S
1
=
−
10.1
V
x
−
37.8
V
z
+
37.5
E
y
+
F
S
2
=
−
1047.5
V
z
−
569.9
W
y
−
189.97
E
a
−
379.943
E
e
S
3
=
−
210.9
V
z
−
239.4
W
y
+
0
E
y
+
171
E
a
−
228
E
e
\begin{aligned} S_1 =& -10.1V_x -37.8V_z +37.5E_y +F \\ S_2 =& -1047.5V_z -569.9W_y -189.97E_a -379.943E_e \\ S_3 =& -210.9V_z -239.4W_y +0E_y +171E_a -228E_e \\ \end{aligned}
S1=S2=S3=−10.1Vx−37.8Vz+37.5Ey+F−1047.5Vz−569.9Wy−189.97Ea−379.943Ee−210.9Vz−239.4Wy+0Ey+171Ea−228Ee
图中4个积分器的输出分别为
V
x
V_x
Vx，
V
z
V_z
Vz，
W
y
W_y
Wy，
E
y
E_y
Ey，另外定义中间变量
A
x
A_x
Ax，
A
y
A_y
Ay，
A
z
A_z
Az，满足
E
˙
y
=
W
y
W
˙
y
=
A
y
V
˙
x
=
A
x
V
˙
z
=
A
z
A
x
=
1
165.827
(
S
1
−
3.117
A
y
)
A
y
=
1
76.661
(
S
3
−
3.117
A
x
−
58.221
A
z
)
A
z
=
1
210.827
(
S
2
−
58.221
A
y
)
\begin{aligned} \dot{E}_y =& W_y \\ \dot{W}_y =& A_y \\ \dot{V}_x =& A_x \\ \dot{V}_z =& A_z \\ A_x =& \frac{1}{165.827}(S_1 -3.117A_y) \\ A_y =& \frac{1}{76.661}(S_3 -3.117A_x -58.221A_z) \\ A_z =& \frac{1}{210.827}(S_2 -58.221A_y) \\ \end{aligned}
E˙y=W˙y=V˙x=V˙z=Ax=Ay=Az=WyAyAxAz165.8271(S1−3.117Ay)76.6611(S3−3.117Ax−58.221Az)210.8271(S2−58.221Ay)
XY方向
同样右侧的3个加法器为
S
1
=
−
165.4
V
y
+
47.4
W
z
+
37.5
E
x
+
33.893
E
u
+
33.893
E
l
S
2
=
−
421.2
W
x
−
30.5
E
x
+
7.676
E
u
−
7.676
E
l
S
3
=
−
26.5
V
y
−
44.3
W
z
+
0.1
E
x
−
23.788
E
u
−
23.788
E
l
\begin{aligned} S_1 =& -165.4V_y +47.4W_z +37.5E_x +33.893E_u +33.893E_l \\ S_2 =& -421.2W_x -30.5E_x +7.676E_u -7.676E_l \\ S_3 =& -26.5V_y -44.3W_z +0.1E_x -23.788E_u -23.788E_l \\ \end{aligned}
S1=S2=S3=−165.4Vy+47.4Wz+37.5Ex+33.893Eu+33.893El−421.2Wx−30.5Ex+7.676Eu−7.676El−26.5Vy−44.3Wz+0.1Ex−23.788Eu−23.788El
图中5个积分器的输出分别为
V
y
V_y
Vy，
W
x
W_x
Wx，
E
x
E_x
Ex，
W
z
W_z
Wz，
E
z
E_z
Ez，另外定义中间变量
A
x
A_x
Ax，
A
y
A_y
Ay，
A
z
A_z
Az，满足
V
˙
y
=
A
y
E
˙
x
=
W
x
W
˙
x
=
A
x
E
˙
z
=
W
z
W
˙
z
=
A
z
A
x
=
1
10.303
(
S
2
−
3.117
A
y
)
A
y
=
1
271.827
(
S
1
−
3.117
A
x
−
1.221
A
z
)
A
z
=
1
20.661
(
S
3
−
1.221
A
y
)
\begin{aligned} \dot{V}_y =& A_y \\ \dot{E}_x =& W_x \\ \dot{W}_x =& A_x \\ \dot{E}_z =& W_z \\ \dot{W}_z =& A_z \\ A_x =& \frac{1}{10.303}(S_2 -3.117A_y) \\ A_y =& \frac{1}{271.827}(S_1 -3.117A_x -1.221A_z) \\ A_z =& \frac{1}{20.661}(S_3 -1.221A_y) \\ \end{aligned}
V˙y=E˙x=W˙x=E˙z=W˙z=Ax=Ay=Az=AyWxAxWzAz10.3031(S2−3.117Ay)271.8271(S1−3.117Ax−1.221Az)20.6611(S3−1.221Ay)
两个方向的加法器的输入均为积分器或外部输入，但几个中间状态无法确定哪些是自变量哪些是因变量，形成代数环。设
S
1
S_1
S1、
S
2
S_2
S2、
S
3
S_3
S3为输入，
A
x
A_x
Ax、
A
y
A_y
Ay、
A
z
A_z
Az为输出，写成矩阵形式便于用计算机计算。
XZ方向
[
A
x
A
y
A
z
]
=
[
0
c
1
k
1
0
c
1
k
2
0
c
2
k
2
0
c
2
k
3
0
]
[
A
x
A
y
A
z
]
+
[
k
1
0
0
0
k
2
0
0
0
k
3
]
[
S
1
S
2
S
3
]
\left[\begin{matrix} A_x \\ A_y \\ A_z \end{matrix}\right] =\left[\begin{matrix} 0 & c_1k_1 & 0 \\ c_1k_2 & 0 & c_2k_2 \\ 0 & c_2k_3 & 0 \end{matrix}\right] \left[\begin{matrix} A_x \\ A_y \\ A_z \end{matrix}\right] +\left[\begin{matrix} k_1 & 0 & 0 \\ 0 & k_2 & 0 \\ 0 & 0 & k_3 \\ \end{matrix}\right] \left[\begin{matrix} S_1 \\ S_2 \\ S_3 \end{matrix}\right]
AxAyAz
=
0c1k20c1k10c2k30c2k20
AxAyAz
+
k1000k2000k3
S1S2S3
其中
c
1
=
−
3.117
,
c
2
=
−
58.221
,
k
1
=
1
/
165.827
,
k
2
=
1
/
76.61
,
k
3
=
1
/
201.827
c_1=-3.117,c_2=-58.221,k_1=1/165.827,k_2=1/76.61,k_3=1/201.827
c1=−3.117,c2=−58.221,k1=1/165.827,k2=1/76.61,k3=1/201.827。
重新整理可解出输入与输出的关系
[
A
x
A
y
A
z
]
=
[
R
s
a
]
[
S
1
S
2
S
3
]
\left[\begin{matrix} A_x \\ A_y \\ A_z \end{matrix}\right] =\left[R_{sa}\right] \left[\begin{matrix} S_1 \\ S_2 \\ S_3 \end{matrix}\right]
AxAyAz
=[Rsa]
S1S2S3
重新写成状态空间表达式
[
S
1
S
2
S
3
]
=
[
+
37.5
0
−
10.1
−
37.8
0
−
569.9
0
−
1047.5
0
−
239.4
0
−
210.9
]
[
E
y
W
y
V
x
V
z
]
+
[
0
0
1
−
189.97
−
379.943
0
171
−
228
0
]
[
E
a
E
e
F
]
s
⃗
=
R
x
s
x
⃗
+
R
f
s
f
⃗
\left[\begin{matrix} S_1 \\ S_2 \\ S_3 \end{matrix}\right] =\left[\begin{matrix} +37.5 & 0 & -10.1 & -37.8 \\ 0 & -569.9 & 0 & -1047.5 \\ 0 & -239.4 & 0 & -210.9 \\ \end{matrix}\right] \left[\begin{matrix} E_y \\ W_y \\ V_x \\ V_z \end{matrix}\right] +\left[\begin{matrix} 0 & 0 & 1 \\ -189.97 & -379.943 & 0 \\ 171 & -228 & 0 \\ \end{matrix}\right] \left[\begin{matrix} E_a \\ E_e \\ F \end{matrix}\right] \\ \vec{s} = R_{xs}\vec{x} + R_{fs}\vec{f}
S1S2S3
=
+37.5000−569.9−239.4−10.100−37.8−1047.5−210.9
EyWyVxVz
+
0−189.971710−379.943−228100
EaEeF
s
=Rxsx
+Rfsf
上面两行公式中，下面一行用字母表示上面一行的矩阵和向量（读者应该能看懂命名方式）。
[
E
˙
y
W
˙
y
V
˙
x
V
˙
z
]
=
[
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
]
[
E
y
W
y
V
x
V
z
]
+
[
0
0
0
0
1
0
1
0
0
0
0
1
]
[
A
x
A
y
A
z
]
x
⃗
˙
=
R
x
x
x
⃗
+
R
a
x
a
⃗
\left[\begin{matrix} \dot{E}_y \\ \dot{W}_y \\ \dot{V}_x \\ \dot{V}_z \end{matrix}\right] =\left[\begin{matrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \end{matrix}\right] \left[\begin{matrix} E_y \\ W_y \\ V_x \\ V_z \end{matrix}\right] +\left[\begin{matrix} 0 & 0 & 0 \\ 0 & 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix}\right] \left[\begin{matrix} A_x \\ A_y \\ A_z \end{matrix}\right] \\ \dot{\vec{x}} = R_{xx}\vec{x} + R_{ax}\vec{a}
E˙yW˙yV˙xV˙z
=
0000100000000000
EyWyVxVz
+
001001000001
AxAyAz
x
˙=Rxxx
+Raxa
整理可得
[
E
˙
y
W
˙
y
V
˙
x
V
˙
z
]
=
[
0
1
0
0
−
0.0116462
−
8.32373
0.0031367
−
16.3331
0.226358
0.156459
−
0.0609658
0.0790592
0.00321615
1.16311
−
0.000866216
3.51012
]
[
E
y
W
y
V
x
V
z
]
+
[
0
0
0
−
3.91896
−
5.23722
−
0.000310564
0.0736635
0.0984425
0.00603622
1.89333
0.364831
8.57639
e
−
05
]
[
E
a
E
e
F
]
\left[\begin{matrix} \dot{E}_y \\ \dot{W}_y \\ \dot{V}_x \\ \dot{V}_z \end{matrix}\right] =\left[\begin{matrix} 0 & 1 & 0 & 0 \\ -0.0116462 & -8.32373 & 0.0031367 & -16.3331 \\ 0.226358 & 0.156459 & -0.0609658 & 0.0790592 \\ 0.00321615 & 1.16311 & -0.000866216 & 3.51012 \end{matrix}\right] \left[\begin{matrix} E_y \\ W_y \\ V_x \\ V_z \end{matrix}\right] +\left[\begin{matrix} 0 & 0 & 0 \\ -3.91896 & -5.23722 & -0.000310564 \\ 0.0736635 & 0.0984425 & 0.00603622 \\ 1.89333 & 0.364831 & 8.57639e-05 \end{matrix}\right] \left[\begin{matrix} E_a \\ E_e \\ F \end{matrix}\right]
E˙yW˙yV˙xV˙z
=
0−0.01164620.2263580.003216151−8.323730.1564591.1631100.0031367−0.0609658−0.0008662160−16.33310.07905923.51012
EyWyVxVz
+
0−3.918960.07366351.893330−5.237220.09844250.3648310−0.0003105640.006036228.57639e−05
EaEeF
XY方向
同样的方法计算XY方向
[
V
˙
y
E
˙
x
W
˙
x
E
˙
z
W
˙
z
]
=
[
A
]
[
V
y
E
x
W
x
E
z
W
z
]
+
[
B
]
[
E
u
E
l
]
\left[\begin{matrix} \dot{V}_y \\ \dot{E}_x \\ \dot{W}_x \\ \dot{E}_z \\ \dot{W}_z \end{matrix}\right] =\left[\begin{matrix} A \end{matrix}\right] \left[\begin{matrix} V_y \\ E_x \\ W_x \\ E_z \\ W_z \end{matrix}\right] +\left[\begin{matrix} B \end{matrix}\right] \left[\begin{matrix} E_u \\ E_l \end{matrix}\right]
V˙yE˙xW˙xE˙zW˙z
=[A]
VyExWxEzWz
+[B][EuEl]
下面使用 simucpp 仿真。
仿真的完整代码见 https://github.com/xd15zhn/submarine
