(线性系统理论大作业)

题目

  水下潜器模型，可能是潜艇或者鱼雷等对象。一个主推进螺旋桨，前后两对水平陀翼，后面一对垂直陀翼。
  潜器前进过程中，通过调节助推进螺旋桨推力，以及三对陀翼的角度变化，对潜器的五个自由度，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\\ 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)(1)\\ A_y=& \frac{1}{76.661}(S_3-3.117A_x-58.221A_z)(2)\\ A_z=& \frac{1}{210.827}(S_2-58.221A_y)(3)\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$为输出，写成矩阵形式便于用计算机计算
$$\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_3& 0& c_2/k_3\\ 0& c_2/k_2& 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& 0& k_3\\ 0& k_2& 0\end{array}\right]\left[\begin{array}{c}{S}_1\\ S_2\\ S_3\end{array}\right]$$
解得XZ方向
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{ccc}165.987525236596& 3.81203748021033& -1.82515701785262\\ -8.5400764866803& -202.803573702547& 97.099875777814\\ 2.3583876502109& 266.832288053883& -26.8146483498798\end{array}\right]\left[\begin{array}{c}{S}_1\\ S_2\\ S_3\end{array}\right]$$
XY方向
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{ccc}-82.5449726643127& 10.3388761594846& 0.028182041202784\\ 272.84595872968& -0.118585842531209& -0.0931535356151053\\ -16.1243364604298& 0.00700804964573864& 20.6665050804407\end{array}\right]\left[\begin{array}{c}{S}_1\\ S_2\\ S_3\end{array}\right]$$

仿真

下面使用 simucpp 仿真。

代码

拆除代数环的矩阵方程代码

#include <iostream>
#include "zhnmat.hpp"
using namespace std;
using namespace zhnmat;
typedef std::vector<double>  vecdble;
constexpr double c1 = -3.117;
// constexpr double c2 = -58.221;
// constexpr double k1 = 165.827;
// constexpr double k2 = 210.827;
// constexpr double k3 = 76.661;
constexpr double c2 = -1.221;
constexpr double k1 = 271.827;
constexpr double k2 = 10.303;
constexpr double k3 = 20.661;
int main() {
    // Mat kA(3, 3, vecdble{
    //     0, c1/k1, 0,
    //     c1/k3, 0, c2/k3,
    //     0, c2/k2, 0,
    // });
    // Mat kS(3, 3, vecdble{
    //     k1, 0, 0,
    //     0, 0, k3,
    //     0, k2, 0,
    // });
    Mat kA(3, 3, vecdble{
        0, c1/k2, 0,
        c1/k1, 0, c2/k1,
        0, c2/k3, 0
    });
    Mat kS(3, 3, vecdble{
        0, k2, 0,
        k1, 0, 0,
        0, 0, k3,
    });
    Mat K = (eye(3) - kA).inv() * kS;
    cout << K << endl;
}