水下潜航器的建模与控制

(线性系统理论大作业)
(待完成。。。)

题目

  水下潜器模型，可能是潜艇或者鱼雷等对象。一个主推进螺旋桨，前后两对水平陀翼，后面一对垂直陀翼。
  潜器前进过程中，通过调节助推进螺旋桨推力，以及三对陀翼的角度变化，对潜器的五个自由度，X轴和Z轴方向的速度，以及垂直、滚动和俯仰方向角速度，进行控制，实现潜器的各种机动以及在运动过程中的姿态平稳。
以大地坐标为静止坐标系，以潜器坐标为动坐标系，用动量定理以及动量矩定理可以得到潜器的动力学模型如下：
$$M\dot{V}+F_I=F_F+F_G+F_B+F_C$$
公式中，$V={\left[V_x,V_y,{\omega }_y,{\omega }_z\right]}^T$为速度向量,
$M$为由载体质量、附加质量、转动惯量和惯性积组成的载体惯性矩阵,
$F_I$ 为离心力和惯性流体力,
$F_F$ 为非惯性流体力,
$F_G$ 和 $F_B$ 分别为载体的重力和浮力,
$F_C$ 为陀翼以及推进器对载体所施加的控制力。

1. 考虑到潜器横向水平面与纵向垂直面运动间耦合微弱，因而分别建立XZ面与XY面运动方程。设计解耦控制器，实现系统的解耦控制。
2. 在保证解耦的前提下，对系统进行极点配置，提高系统控制性能，以及抑止由于水流波动带来的干扰。
3. 对模型中变量的说明如下：
   $W_x$，$W_y$，$W_z$分别表示绕三个轴的角速度；
   $V_x$，$V_y$，$V_z$分别表示三个轴向的速度；
   $E_x$，$E_y$，$E_z$分别表示绕三个轴转动的角度；
   XZ面模型输入为前后水平舵转动角度$E_a$和$E_e$，以及螺旋桨推力$F$；
   XY面模型输入为上下垂直舵转动角度$E_u$和$E_l$。
4. 控制的目的在于：
   a. 保证潜器的行进平稳，速度变化是不引起艇身的滚动，俯仰和垂直转动时保持姿态和速度；
   b. 抑止水流带来的对潜器运动状态的干扰。

模型文件解析

XZ方向

设右侧的3个加法器输出分别为$S_1$，$S_2$，$S_3$。
$$\begin{array}{rl}{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{array}$$
图中4个积分器的输出分别为$V_x$，$V_z$，$W_y$，$E_y$，另外定义中间变量$A_x$，$A_y$，$A_z$，满足
$$\begin{array}{rl}{\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{array}$$

XY方向

同样右侧的3个加法器为
$$\begin{array}{rl}{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{array}$$
图中5个积分器的输出分别为$V_y$，$W_x$，$E_x$，$W_z$，$E_z$，另外定义中间变量$A_x$，$A_y$，$A_z$，满足
$$\begin{array}{rl}{\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{array}$$

公式重新整理

两个方向的加法器的输入均为积分器或外部输入，但几个中间状态无法确定哪些是自变量哪些是因变量，形成代数环。设$S_1$、$S_2$、$S_3$为输入，$A_x$、$A_y$、$A_z$为输出，写成矩阵形式便于用计算机计算。
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] ​Ax​Ay​Az​​ ​= ​0c1​k2​0​c1​k1​0c2​k3​​0c2​k2​0​ ​ ​Ax​Ay​Az​​ ​+ ​k1​00​0k2​0​00k3​​ ​ ​S1​S2​S3​​ ​
其中$c_1=-3.117,c_2=-58.221,k_1=1/165.827,k_2=1/76.61,k_3=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] ​Ax​Ay​Az​​ ​=[Rsa​] ​S1​S2​S3​​ ​
重新写成状态空间表达式
[ 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} ​S1​S2​S3​​ ​= ​+37.500​0−569.9−239.4​−10.100​−37.8−1047.5−210.9​ ​ ​Ey​Wy​Vx​Vz​​ ​+ ​0−189.97171​0−379.943−228​100​ ​ ​Ea​Ee​F​ ​s =Rxs​x +Rfs​f ​
上面两行公式中，下面一行用字母表示上面一行的矩阵和向量（读者应该能看懂命名方式）。
[ 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˙y​W˙y​V˙x​V˙z​​ ​= ​0000​1000​0000​0000​ ​ ​Ey​Wy​Vx​Vz​​ ​+ ​0010​0100​0001​ ​ ​Ax​Ay​Az​​ ​x ˙=Rxx​x +Rax​a
整理可得
[ E ˙ y W ˙ y V ˙ x V ˙ z ] = [ A ] [ E y W y V x V z ] + [ B ] [ 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} A \end{matrix}\right] \left[\begin{matrix} E_y \\ W_y \\ V_x \\ V_z \end{matrix}\right] +\left[\begin{matrix} B \end{matrix}\right] \left[\begin{matrix} E_a \\ E_e \\ F \end{matrix}\right] ​E˙y​W˙y​V˙x​V˙z​​ ​=[A​] ​Ey​Wy​Vx​Vz​​ ​+[B​] ​Ea​Ee​F​ ​
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˙y​E˙x​W˙x​E˙z​W˙z​​ ​=[A​] ​Vy​Ex​Wx​Ez​Wz​​ ​+[B​][Eu​El​​]

仿真

下面使用 simucpp 仿真。

代码

仿真的完整代码见 https://github.com/xd15zhn/submarine