(线性系统理论大作业)
(待完成。。。)
题目
水下潜器模型，可能是潜艇或者鱼雷等对象。一个主推进螺旋桨，前后两对水平陀翼，后面一对垂直陀翼。
潜器前进过程中，通过调节助推进螺旋桨推力，以及三对陀翼的角度变化，对潜器的五个自由度，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 \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 仿真。
代码
仿真的完整代码见 /xd15zhn/submarine
