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

题目

  水下潜器模型，可能是潜艇或者鱼雷等对象。一个主推进螺旋桨，前后两对水平陀翼，后面一对垂直陀翼。
  潜器前进过程中，通过调节助推进螺旋桨推力，以及三对陀翼的角度变化，对潜器的五个自由度，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方向
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{ccc}0& c_1{k}_1& 0\\ c_1{k}_2& 0& c_2{k}_2\\ 0& c_2{k}_3& 0\end{array}\right]\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]+\left[\begin{array}{ccc}{k}_1& 0& 0\\ 0& k_2& 0\\ 0& 0& k_3\end{array}\right]\left[\begin{array}{c}{S}_1\\ S_2\\ S_3\end{array}\right]$$
其中$c_1=-3.117,c_2=-58.221,k_1=1/165.827,k_2=1/76.61,k_3=1/201.827$。
重新整理可解出输入与输出的关系
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[R_{sa}\right]\left[\begin{array}{c}{S}_1\\ S_2\\ S_3\end{array}\right]$$
重新写成状态空间表达式
$$\begin{gathered} \left[\begin{array}{c}{S}_1\\ S_2\\ S_3\end{array}\right]=\left[\begin{array}{cccc}+37.5& 0& -10.1& -37.8\\ 0& -569.9& 0& -1047.5\\ 0& -239.4& 0& -210.9\end{array}\right]\left[\begin{array}{c}{E}_y\\ W_y\\ V_x\\ V_z\end{array}\right]+\left[\begin{array}{ccc}0& 0& 1\\ -189.97& -379.943& 0\\ 171& -228& 0\end{array}\right]\left[\begin{array}{c}{E}_a\\ E_e\\ F\end{array}\right] \\ \vec{s}=R_{xs}\vec{x}+R_{fs}\vec{f} \end{gathered}$$
上面两行公式中，下面一行用字母表示上面一行的矩阵和向量（读者应该能看懂命名方式）。
$$\begin{gathered} \left[\begin{array}{c}{\dot{E}}_y\\ {\dot{W}}_y\\ {\dot{V}}_x\\ {\dot{V}}_z\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{E}_y\\ W_y\\ V_x\\ V_z\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ 0& 1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right] \\ \dot{\vec{x}}=R_{xx}\vec{x}+R_{ax}\vec{a} \end{gathered}$$
整理可得
$$\left[\begin{array}{c}{\dot{E}}_y\\ {\dot{W}}_y\\ {\dot{V}}_x\\ {\dot{V}}_z\end{array}\right]=\left[\begin{array}{cccc}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{array}\right]\left[\begin{array}{c}{E}_y\\ W_y\\ V_x\\ V_z\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ -3.91896& -5.23722& -0.000310564\\ 0.0736635& 0.0984425& 0.00603622\\ 1.89333& 0.364831& 8.57639e-05\end{array}\right]\left[\begin{array}{c}{E}_a\\ E_e\\ F\end{array}\right]$$
XY方向
同样的方法计算XY方向
$$\left[\begin{array}{c}{\dot{V}}_y\\ {\dot{E}}_x\\ {\dot{W}}_x\\ {\dot{E}}_z\\ {\dot{W}}_z\end{array}\right]=\left[\begin{array}{c}A\end{array}\right]\left[\begin{array}{c}{V}_y\\ E_x\\ W_x\\ E_z\\ W_z\end{array}\right]+\left[\begin{array}{c}B\end{array}\right]\left[\begin{array}{c}{E}_u\\ E_l\end{array}\right]$$

仿真

下面使用 simucpp 仿真。

代码

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