(线性系统理论大作业)
(待完成。。。)
题目
水下潜器模型，可能是潜艇或者鱼雷等对象。一个主推进螺旋桨，前后两对水平陀翼，后面一对垂直陀翼。
潜器前进过程中，通过调节助推进螺旋桨推力，以及三对陀翼的角度变化，对潜器的五个自由度，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面与XY面运动方程。设计解耦控制器，实现系统的解耦控制。
- 在保证解耦的前提下，对系统进行极点配置，提高系统控制性能，以及抑止由于水流波动带来的干扰。
- 对模型中变量的说明如下：
W x W_x Wx， W y W_y Wy， W z W_z Wz分别表示绕三个轴的角速度；
V x V_x Vx， V y V_y Vy， V z V_z Vz分别表示三个轴向的速度；
E x E_x Ex， E y E_y Ey， E z E_z Ez分别表示绕三个轴转动的角度；
XZ面模型输入为前后水平舵转动角度 E a E_a Ea和 E e E_e Ee，以及螺旋桨推力 F F F；
XY面模型输入为上下垂直舵转动角度 E u E_u Eu和 E l E_l El。 - 控制的目的在于：
a. 保证潜器的行进平稳，速度变化是不引起艇身的滚动，俯仰和垂直转动时保持姿态和速度；
b. 抑止水流带来的对潜器运动状态的干扰。
模型文件解析
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
S1=−10.1Vx−37.8Vz+37.5Ey+FS2=−1047.5Vz−569.9Wy−189.97Ea−379.943EeS3=−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
)
˙Ey=Wy˙Wy=Ay˙Vx=Ax˙Vz=AzAx=1165.827(S1−3.117Ay)Ay=176.661(S3−3.117Ax−58.221Az)Az=1210.827(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
S1=−165.4Vy+47.4Wz+37.5Ex+33.893Eu+33.893ElS2=−421.2Wx−30.5Ex+7.676Eu−7.676ElS3=−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
)
˙Vy=Ay˙Ex=Wx˙Wx=Ax˙Ez=Wz˙Wz=AzAx=110.303(S2−3.117Ay)Ay=1271.827(S1−3.117Ax−1.221Az)Az=120.661(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[AxAyAz
其中
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[AxAyAz
重新写成状态空间表达式
[
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[S1S2S3
上面两行公式中，下面一行用字母表示上面一行的矩阵和向量（读者应该能看懂命名方式）。
[
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[˙Ey˙Wy˙Vx˙Vz
整理可得
[
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[˙Ey˙Wy˙Vx˙Vz
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[˙Vy˙Ex˙Wx˙Ez˙Wz
仿真
下面使用 simucpp 仿真。
代码
仿真的完整代码见 https://github.com/xd15zhn/submarine
评论记录：
回复评论：