[发明专利]一种基于系统可观测度的多水下无人艇协同运动编队设计方法

权利要求书

1.一种基于系统可观测度的多水下无人艇协同运动编队设计方法，其特征在于，所述运动编队设计方法包括以下步骤：

步骤一、对协同系统进行可观测度计算；

步骤二、依据计算得到的可观测度算式设计协同运动编队运动方案，

在步骤一中，当主艇的数目为双主艇系统时，可观测度计算方法如下：

建立如下的系统状态方程与量测方程：

式中，V、分别为从艇在以主艇中心为原点建立的坐标系中的速度与航向角，f₁、f₂表示系统的状态方程在上述坐标系中沿x、y轴的连续分量形式，(x_k,y_k)、分别表示从艇、1号主艇、2号主艇的位置坐标，h₁、h₂分别表示从艇距1号主艇距离表达式、从艇距2号主艇距离表达式，

运用李导数弱可观测理论计算可得系统的可观测性矩阵为：

由线性代数知识可知对于m×n的矩阵A的秩有：rank(A)≤min(m,n)，因此该可观测性矩阵的秩最大为2，

可观测性矩阵可记为：

首先分析系统不可观测的情况，即可观测性矩阵不满秩，也就是rank(A)＝1，于是得到：

计算得到：

由上式得知当主从UUV在同一直线上运动时系统不可观测，

可将观测量取为主从UUV距离平方的一半，表示为：

计算得可观测阵的奇异值为：

式中，p₁、p₂分别为从艇到两主艇的距离；θ为从艇到两主艇之间的距离方向之间的夹角，

系统的可观测度为：

2.根据权利要求1所述的一种基于系统可观测度的多水下无人艇协同运动编队设计方法，其特征在于，在步骤一中，当主艇的数目为单主艇系统时，可观测度计算方法如下：

建立如下的系统状态方程与量测方程：

式中，(x_k,y_k)、分别为从艇位置坐标与主艇位置坐标，并有

采用李导数弱可观测理论对上述系统模型进行计算，得到系统的可观测性矩阵：

式中，L_f⁰h、L_f¹h表示量测方程h相对于状态方程f的零阶李导数、一阶李导数，

可观测性矩阵的行列式为：

若要系统模型是可观测的，则需系统可观测阵满秩，当矩阵Obs满秩时，根据数学理论知识可知，该矩阵的行列式不为0时，

首先分析系统不可观测状态时的情况，即由det(Obs)＝0得到：

从上式得知系统不可观测的情况如下：

第一种情况、主、从UUV保持相对静止运动；

第二种情况、当前时刻从UUV的位置与主UUV位置连线的夹角等于从UUV的航向角，

通过上述分析可知，当协同定位系统中主从UUV出现上述两种运动方式时，系统不可观测，这两种运动方式即为单主艇模式下主从式UUV系统的不可观运动形式，除此之外，其余的主从UUV运动方式都是可观运动形式，

应用矩阵条件数理论对系统的可观测性度做定量分析，

计算Obs·Obs^T：

式中，Obs^T为可观测性矩阵Obs的转置矩阵，

奇异值为：

σ₁＝1，

系统的可观测度为：

3.根据权利要求1所述的一种基于系统可观测度的多水下无人艇协同运动编队设计方法，其特征在于，在步骤二中，具体的，首先依据步骤一中得到的可观测度计算式进行协同系统的可观测度仿真分析，然后设计相应的协同运动编队方案。

4.根据权利要求3所述的一种基于系统可观测度的多水下无人艇协同运动编队设计方法，其特征在于，单主艇多UUV协同系统的编队协同运动方案为：主艇匀速前进运动，从艇围绕主艇做螺旋前进运动。

5.根据权利要求3所述的一种基于系统可观测度的多水下无人艇协同运动编队设计方法，其特征在于，双主艇多UUV协同系统的编队协同运动方案为：加入负反馈环节，在初始状态时，将从艇放在系统中可观测度较高位置，即从艇到两主艇的距离相等，到两艇距离的而方向的夹角为90°。