CN103674029B - 一种基于水声通信的多艇协同导航编队构型的方法 - Google Patents

Abstract

本发明公开了一种基于水声通信的多艇协同导航编队构型的方法，该基于水声通信的多艇协同导航编队构型的方法包括以下步骤：通过根据协同导航系统模型建立系统状态方程、量测方程，利用非线性系统李导数可观测性理论对其进行可观测性分析，根据舰艇尾部螺旋桨产生的尾流建立水声通讯盲区模型，根据艇间安全距离确定安全区域模型，最终得到基于可观测性、尾流通讯盲区、艇间安全距离区域综合考虑情况下的无人艇编队构型设计方案。本发明实现了协同导航系统编队构型优化设计，适用于多艇协同导航情况下的编队构型设计。本发明忽略掉尾流的影响而得到的编队构型编排，对于协同导航系统编队构型设计有很高的实用价值。

Description

一种基于水声通信的多艇协同导航编队构型的方法

技术领域

本发明属于导航编队构型技术领域，尤其涉及一种基于水声通信的多艇协同导航编队构型的方法。

背景技术

协同作战包括多艘艇相互配合、相互协作，执行战斗任务等作战方式。多艇协同就是利用多艘成本相对较低、功能简单的艇完成一个复杂系统才能完成的任务。多艘艇组成的编队，只要有足够有效的协同策略，就能充分发挥综合资源优势，艇编队作战效能就能大于多艘艇各自为战的作战效能。多艇编队是指向目标行进过程中，保持某些队形，同时又要适应环境约束的控制技术，包含了协同定位、路径规划、通信以及编队构型等问题。编队构型设计是多艇协同导航的一项重要研究内容，编队设计的原则除了要满足特定的军事需求外，还要考虑编队构型对协同定位精度的影响、以及保证水声通信与测距的正常工作、防止艇间发生碰撞等因素的影响。艇数量增多会使队列拓扑结构复杂化，另外，在实际应用中，各艇通过外部传感器对周边其它艇进行定位，当艇自身处于某个特定物理位置时，由于周围环境的屏蔽，或是艇之间的互相遮挡，影响通信及定位。在传统长基线算法中，海底应答器可以是2个、3个或者以上(一般用到2个)，定位算法有球面交会法和双曲线、双曲面法。对于艇的协同导航来说，按照几何算法用2条领航艇的位置及领航艇与跟随艇间距离可以确定跟随艇位置的2个解，后面EKF算法融合位置信息就可以得到唯一解。如果采用3条领航艇，两两分别做双曲线几何算法，可确定唯一解。艇队形和艇间距关系研究艇间距离越大，通信设备产生误码或者中断的几率也越大，另外艇间距离越大，水声时延误差带来的测距误差越大。因此间的距离两艇之不能过大。但是任意两艇之间的距离不能小于安全距离，以防止艇间发生碰撞。利用艇的运动及它们之间的相对位置观测从而完成定位，那么艇编队在定位过程中的相对位置、运动轨迹必然会对合作定位精度产生影响。艇横向、纵向一字型排列会影响观测性和整体定位的精度，而彼此之间形成三角形构型，效果要好于并排或者纵向排列效果。综上所述，艇编队构型受到多种因素的影响，并且实际运行中，一定存在着某些环境因素(比如海流、暗礁等等)，会影响艇速度、间距和航向等约束因素的选择，进而影响艇的队形结构设计。编队规模可以结合实际的任务性质和环境约束进行适当的调整。

发明内容

本发明实施例的目的在于提供一种基于水声通信的多艇协同导航编队构型的方法，旨在解决多艘艇的编队在实际运行存在着环境因素，会影响艇速度、间距和航向约束因素的选择，进而影响艇的队形结构设计的问题。

本发明实施例是这样实现的，一种基于水声通信的多艇协同导航编队构型的方法，该基于水声通信的多艇协同导航编队构型的方法包括以下步骤：

步骤一，建立系统状态方程

在地理坐标系下，设t₀时刻跟随艇起始位置为O(0，0，0)，则t_k时刻跟随艇自身推算得到的状态量可以表示为：

$\left\{\begin{array}{c}{x}_k=x_{k-1}+\mathrm{\Δt}({\hat{v}}_k\cos {\widehat{\mathrm{\θ}}}_k+{\hat{w}}_k\sin {\widehat{\mathrm{\θ}}}_k)\\ y_k=y_{k-1}+\mathrm{\Δt}({\hat{v}}_k\sin {\widehat{\mathrm{\θ}}}_k-{\hat{w}}_k\cos {\widehat{\mathrm{\θ}}}_k)\\ z_k={\hat{z}}_k\\ {\mathrm{\θ}}_k={\widehat{\mathrm{\θ}}}_k\end{array}\right.$

式中，为DVL测量的跟随艇纵向、横向速度；为微机械惯性系统输出的跟随艇航向信息；为深度传感器输出的跟随艇垂向深度信息；Δt为采样时间间隔；

步骤二：建立系统的量测方程

为简化算法分析的复杂度，系统观测量取跟随艇相对领航艇距离平方的二分之一，观测方程表示如下：

$h\left(X\right)=\left(\begin{array}{c}{h}_1\\ h_2\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}{(x-x_1)}^2+\frac{1}{2}{(y-y_1)}^2\\ \frac{1}{2}{(x-x_2)}^2+\frac{1}{2}{(y-y_2)}^2\end{array}\right)$

式中，(x₁，y₁)、(x₂，y₂)分别为两个领航艇的位置坐标，h₁、h₂分别为跟随艇与两个领航艇之间的距离；

步骤三：可观测性分析

根据非线性系统李导数可观测性判别方法，非线性系统局部可观测当且仅当李导数矩阵G的梯度满秩；

首先，定义n维非线性系统量测方程的李导数

$\begin{array}{c}{L}_f^0\left(h\right)=h\\ L_f^1\left(h\right)=\frac{\∂h}{\∂x}\·f=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂h}{\∂x_i}\·f_i\\ .\\ .\\ .\\ L_f^{n-1}\left(h\right)=\frac{\∂}{\∂x}\left[L_f^{n-2}\left(h\right)\right]\·f\end{array}$

得到非线性系统的李导数矩阵G

根据上述非线性系统可观测性李导数判别方法，得到该系统的可观测性矩阵

$\mathrm{Obs}=\left(\begin{array}{c}{\mathrm{dL}}_f^0\left(h_1\right)\\ {\mathrm{dL}}_f^0\left(h_2\right)\\ {\mathrm{dL}}_f^1\left(h_1\right)\\ {\mathrm{dL}}_f^1\left(h_2\right)\end{array}\right)=\left(\begin{array}{cc}x-x_1& y-y_1\\ x-x_2& y-y_2\\ f_1& f_2\\ f_1& f_2\end{array}\right)$

通过系统可观测矩阵可以看出，对于二阶系统，要想系统可观测，当且仅当观测矩降秩为2，由于观测矩阵第3、4行相同，因此只针对矩阵前三行进行系统可观测性的讨论，将观测矩阵进一步变换成极坐标形式，表示如下：

$\mathrm{Obs}=\left(\begin{array}{cc}{\mathrm{\Δx}}_1& {\mathrm{\Δy}}_1\\ {\mathrm{\Δx}}_2& {\mathrm{\Δy}}_2\\ v_e& v_n\end{array}\right)=\left(\begin{array}{cc}{p}_1\sin \mathrm{\α}& p_1\cos \mathrm{\α}\\ p_2\sin \mathrm{\β}& p_2\cos \mathrm{\β}\\ v\sin \mathrm{\γ}& v\cos \mathrm{\γ}\end{array}\right)$

式中，p₁、p₂为跟随艇相对两个领航艇之间的距离，v为跟随艇的速度，α、β、γ分别为相对应的方位角；

从系统观测矩阵可以看出，系统可观测性与主跟随艇之间的距离向量p和跟随艇速度向量v有关，对于距离p₁、p₂可以始终满足为大于0常数，但是对于跟随艇来说，可以分为运动和静止两种状态，下面，分别就跟随艇不同的运动状态进行系统的可观测性分析；

当跟随艇运动(v≠0)时，系统的可观测性比较容易满足，从可观测矩阵可以看出，当且仅当α、β、γ相等或者相差180°时观测矩阵的秩为1，不满足系统可观测条件，也就是说，只有在跟随艇与领航艇航行轨迹在水平面上投影沿一条直线时系统不可观测，其余条件下均可观测，

当跟随艇静止(v=0)时，系统可观测矩阵表示为

$\mathrm{Obs}=\left(\begin{array}{cc}{p}_1\sin \mathrm{\α}& p_1\cos \mathrm{\α}\\ p_2\sin \mathrm{\β}& p_2\cos \mathrm{\β}\end{array}\right)$

可以看出，若矩阵奇异则有

tanα＝tanβ

当且仅当α=β+nπ(n=0,1，2…)即跟随艇相对不同领航艇的距离方向平行时系统不可观测，其它状态情况下均可观测，为了进一步分析系统可观测性好坏与距离向量的关系，下面引入条件数分析理论对系统的可观测度做进一步分析，对观测矩阵做进一步变换

$\mathrm{Obs}=p_2\left(\begin{array}{cc}\mathrm{\γ}\sin \mathrm{\α}& \mathrm{\γ}\cos \mathrm{\α}\\ \sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right)$

式中

$\mathrm{\γ}=\frac{p_2}{p_1}$

定义跟随艇与领航艇之间的距离向量夹角为θ=α-β，则观测矩降条件数C可以表示为

$C=\frac{\max \left\{{\mathrm{\σ}}_{\mathrm{1,2}}\right\}}{\min \left\{{\mathrm{\σ}}_{\mathrm{1,2}}\right\}}=\frac{{\mathrm{\γ}}^2+1+\sqrt{{\mathrm{\γ}}^4+2{\mathrm{\γ}}^2\cos \left(2\mathrm{\θ}\right)+1}}{2\mathrm{\γ}\left|\sin \left(\mathrm{\θ}\right)\right|}$

式中，σ_1，2为观测矩阵的两个奇异值

${\mathrm{\σ}}_{\mathrm{1,2}}=p_2\frac{\sqrt{2}}{2}\sqrt{{\mathrm{\γ}}^2+1\±\sqrt{{\mathrm{\γ}}^4+2{\mathrm{\γ}}^2\cos \left(2\mathrm{\θ}\right)+1}}$

选取条件数的倒数C^-1进行仿真分析，当系统满足条件

$\left\{\begin{array}{c}\mathrm{\γ}=1\\ \mathrm{\θ}=\±\frac{\mathrm{\π}}{2}\end{array}\right.$

时，C^-1最大，也就是系统的可观测性最好；

步骤四：基于可观测性分析的编队构型，在设计协同导航系统编队过程中要充分考虑到对系统可观测性的影响；

在两条领航艇的间距确定的情况下，在以两条领航艇连线的中点为圆心，距离的一半为半径的圆周上其可观测性最好，且跟随艇有两个最优位置，分别在两条领航艇连线中垂线与圆周交汇处；

步骤五：考虑舰艇尾流影响的编队构型，为了保证主、跟随艇间通信不受尾流的影响，仅考虑尾流的主要影响区域，称为主尾流区，对无人艇设定一个主尾流区：夹角为θ，腰长为l的等腰三角形，假设艇间的连线即为传播声线，下面以3条艇为例，对尾流影响下的编队构型进行分析；

参考水声通信试验中无人艇高速行驶的时候所形成尾流区域的大小，设尾流区θ＝60°,l＝30m的等边三角形，领航艇与跟随艇间的距离为d＝200m，根据艇间通信互不影响原则，即领航艇与跟随艇间连线不能穿过任何一个尾流区域；

对于3艇的编队构型，考虑到艇的数目较少，跟随艇的位置应该在阴影区的范围内，根据镜像对称关系，可以得出3艇的编队构型中，跟随艇可位于以两条领航艇为顶点的菱形区域内；

步骤六：考虑舰艇尾流和安全距离影响的编队构型，假设跟随艇与领航艇间的距离要保证在100m以上，可得出以下的跟随艇分布；

在考虑安全距离的条件下，可得到跟随艇分布图，分别以领航艇1和领航艇2为圆心，安全距离100m为半径得到两个圆，在这2个圆与图中阴影区相交的区域中分布的跟随艇不满足安全距离100m的要求，因此可以得到一个新的阴影区，跟随艇的位置应该在新的阴影区的范围内；

步骤七：考虑以上综合因素影响的无人艇编队构型，利用几何关系可以得到跟随艇的最优分布区域，由可观测性分析可知，在跟随艇运动时，跟随艇与两领航艇的距离向量夹角接近90°，同时相对不同领航艇距离越接近，系统的可观测性最好，当跟随艇分布在以两领航艇连线的中心点为圆心，距离的一半为半径的圆周上可观测性最好，结合尾流影响下的跟随艇分布区以及安全距离考虑，可以得出跟随艇的最优分布区域。

进一步，该基于水声通信的多艇协同导航编队构型的方法采用可观测性分析，尾流影响区域划定以及防碰撞安全距离圈划定综合考虑下的协同导航系统编队构型设计，忽略掉尾流的影响而得到的编队构型编排。

进一步，在步骤一中，考虑到跟随艇航向信息θ_k及深度信息z_k直接可观测，只对东北向位置状态x、y进行可观测性分析，系统的连续状态方程表示如下：

$\stackrel{\·}{X}=f(X,u)$

式中

$f=\left(\begin{array}{c}{f}_1\\ f_2\end{array}\right)=\left(\begin{array}{c}\hat{v}\cos \widehat{\mathrm{\θ}}+\hat{w}\sin \widehat{\mathrm{\θ}}\\ \hat{v}\sin \widehat{\mathrm{\θ}}-\hat{w}\cos \widehat{\mathrm{\θ}}\end{array}\right).$

本发明提供的基于水声通信的多艇协同导航编队构型的方法，通过根据协同导航系统模型建立系统状态方程、量测方程，利用非线性系统李导数可观测性理论对其进行可观测性分析，根据舰艇尾部螺旋桨产生的尾流建立水声通讯盲区模型，根据艇间安全距离确定安全区域模型，最终得到基于可观测性、尾流通讯盲区、艇间安全距离区域综合考虑情况下的无人艇编队构型设计方案，实现了协同导航系统编队构型优化设计，适用于多艇协同导航情况下的编队构型设计。本发明忽略掉尾流的影响而得到的编队构型编排，对于协同导航系统编队构型设计有很高的实用价值。

附图说明

图1是本发明实施例提供的基于水声通信的多艇协同导航编队构型的方法流程图；

图2是本发明实施例提供的条件数倒数与主从AUV相对距离向量关系示意图；

图3是本发明实施例提供的两条领航艇情况下的编队构型设计示意图；

图4是本发明实施例提供的三条领航艇情况下的编队构型设计示意图；

图5是本发明实施例提供的考虑尾流的编队示意图；

图6是本发明实施例提供的考虑尾流及安全距离的编队示意图；

图7是本发明实施例提供的考虑综合因素的编队示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图及具体实施例对本发明的应用原理作进一步描述。

如图1所示，本发明实施例的基于水声通信的多艇协同导航编队构型的方法包括以下步骤：

S101：根据协同导航系统模型建立系统状态方程、量测方程；

S102：利用非线性系统李导数可观测性理论对其进行可观测性分析；

S103：根据舰艇尾部螺旋桨产生的尾流建立水声通讯盲区模型；根据艇间安全距离确定安全区域模型；

S104：最终得到基于可观测性、尾流通讯盲区、艇间安全距离区域综合考虑情况下的无人艇编队构型设计方案。

结合本发明的具体实施例对本发明做进一步的说明：

本发明实施例具体由以下步骤实现：

步骤一：建立系统状态方程

在地理坐标系下，设t₀时刻跟随艇起始位置为O(0，o，0)，则t_k时刻跟随艇自身推算得到的状态量可以表示为：

$\left\{\begin{array}{c}{x}_k=x_{k-1}+\mathrm{\Δt}({\hat{v}}_k\cos {\widehat{\mathrm{\θ}}}_k+{\hat{w}}_k\sin {\widehat{\mathrm{\θ}}}_k)\\ y_k=y_{k-1}+\mathrm{\Δt}({\hat{v}}_k\sin {\widehat{\mathrm{\θ}}}_k-{\hat{w}}_k\cos {\widehat{\mathrm{\θ}}}_k)\\ z_k={\hat{z}}_k\\ {\mathrm{\θ}}_k={\widehat{\mathrm{\θ}}}_k\end{array}\right.---\left(1\right)$

式中，为DVL测量的跟随艇纵向、横向速度；为微机械惯性系统输出的跟随艇航向信息；为深度传感器输出的跟随艇垂向深度信息；Δt为采样时间间隔；

考虑到跟随艇航向信息θ_k及深度信息z_k直接可观测，接下来只讨论二维系统的可观测性问题，即只对东北向位置状态x、y进行可观测性分析，系统的连续状态方程表示如下：

$\stackrel{\·}{X}=f(X,u)---\left(2\right)$

式中

$f=\left(\begin{array}{c}{f}_1\\ f_2\end{array}\right)=\left(\begin{array}{c}\hat{v}\cos \widehat{\mathrm{\θ}}+\hat{w}\sin \widehat{\mathrm{\θ}}\\ \hat{v}\sin \widehat{\mathrm{\θ}}-\hat{w}\cos \widehat{\mathrm{\θ}}\end{array}\right)---\left(3\right);$

步骤二：建立系统的量测方程

为简化算法分析的复杂度，系统观测量取跟随艇相对领航艇距离平方的二分之一，观测方程表示如下：

$h\left(X\right)=\left(\begin{array}{c}{h}_1\\ h_2\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}{(x-x_1)}^2+\frac{1}{2}{(y-y_1)}^2\\ \frac{1}{2}{(x-x_2)}^2+\frac{1}{2}{(y-y_2)}^2\end{array}\right)---\left(4\right)$

式中，(x₁，y₁)、(x₂，y₂)分别为两个领航艇的位置坐标，h₁、h₂分别为跟随艇与两个领航艇之间的距离；

步骤三：可观测性分析

针对双领航者协同导航系统的非线性数学模型，为了避免模型线性化损失掉系统有价值的参考信息，接下来采用非线性系统李导数可观测性理论对其进行可观测性分析；

根据非线性系统李导数可观测性判别方法，非线性系统局部可观测当且仅当李导数矩阵G的梯度(即系统的可观测矩阵)满秩；

首先，定义n维非线性系统量测方程的李导数

$\begin{array}{c}{L}_f^0\left(h\right)=h\\ L_f^1\left(h\right)=\frac{\∂h}{\∂x}\·f=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂h}{\∂x_i}\·f_i\\ .\\ .\\ .\\ L_f^{n-1}\left(h\right)=\frac{\∂}{\∂x}\left[L_f^{n-2}\left(h\right)\right]\·f\end{array}---\left(5\right)$

得到非线性系统的李导数矩阵G

根据上述非线性系统可观测性李导数判别方法，得到该系统的可观测性矩阵

$\mathrm{Obs}=\left(\begin{array}{c}{\mathrm{dL}}_f^0\left(h_1\right)\\ {\mathrm{dL}}_f^0\left(h_2\right)\\ {\mathrm{dL}}_f^1\left(h_1\right)\\ {\mathrm{dL}}_f^1\left(h_2\right)\end{array}\right)=\left(\begin{array}{cc}x-x_1& y-y_1\\ x-x_2& y-y_2\\ f_1& f_2\\ f_1& f_2\end{array}\right)---\left(7\right)$

通过系统可观测矩阵可以看出，对于二阶系统，要想系统可观测，当且仅当观测矩阵秩为2，由于观测矩阵第3、4行相同，因此只针对矩阵前三行进行系统可观测性的讨论，将观测矩阵进一步变换成极坐标形式，表示如下：

$\mathrm{Obs}=\left(\begin{array}{cc}{\mathrm{\Δx}}_1& {\mathrm{\Δy}}_1\\ {\mathrm{\Δx}}_2& {\mathrm{\Δy}}_2\\ v_e& v_n\end{array}\right)=\left(\begin{array}{cc}{p}_1\sin \mathrm{\α}& p_1\cos \mathrm{\α}\\ p_2\sin \mathrm{\β}& p_2\cos \mathrm{\β}\\ v\sin \mathrm{\γ}& v\cos \mathrm{\γ}\end{array}\right)---\left(8\right)$

式中，p₁、p₂为跟随艇相对两个领航艇之间的距离，v为跟随艇的速度，α、β、γ分别为相对应的方位角；

从系统观测矩阵可以看出，系统可观测性与主跟随艇之间的距离向量p和跟随艇速度向量v有关，对于距离p₁、p₂可以始终满足为大于0常数，但是对于跟随艇来说，可以分为运动和静止两种状态，下面，分别就跟随艇不同的运动状态进行系统的可观测性分析；

当跟随艇运动(v≠0)时，系统的可观测性比较容易满足，从可观测矩阵可以看出，当且仅当α、β、γ相等或者相差180°时观测矩阵的秩为1，不满足系统可观测条件，也就是说，只有在跟随艇与领航艇航行轨迹在水平面上投影沿一条直线时系统不可观测，其余条件下均可观测；

当跟随艇静止(v=0)时，系统可观测矩阵表示为

$\mathrm{Obs}=\left(\begin{array}{cc}{p}_1\sin \mathrm{\α}& p_1\cos \mathrm{\α}\\ p_2\sin \mathrm{\β}& p_2\cos \mathrm{\β}\end{array}\right)---\left(9\right)$

可以看出，若矩阵奇异则有

tanα=tanβ (10)

也就是说，当且仅当α=β+nπ(n=0,1，2…)即跟随艇相对不同领航艇的距离方向平行时系统不可观测，其它状态情况下均可观测，为了进一步分析系统可观测性好坏与距离向量的关系，下面引入条件数分析理论对系统的可观测度做进一步分析，对观测矩阵做进一步变换

$\mathrm{Obs}=p_2\left(\begin{array}{cc}\mathrm{\γ}\sin \mathrm{\α}& \mathrm{\γ}\cos \mathrm{\α}\\ \sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right)---\left(11\right)$

式中

$\mathrm{\γ}=\frac{p_2}{p_1}---\left(12\right)$

定义跟随艇与领航艇之间的距离向量夹角为θ=α-β，则观测矩阵条件数C可以表示为：

$C=\frac{\max \left\{{\mathrm{\σ}}_{\mathrm{1,2}}\right\}}{\min \left\{{\mathrm{\σ}}_{\mathrm{1,2}}\right\}}=\frac{{\mathrm{\γ}}^2+1+\sqrt{{\mathrm{\γ}}^4+2{\mathrm{\γ}}^2\cos \left(2\mathrm{\θ}\right)+1}}{2\mathrm{\γ}\left|\sin \left(\mathrm{\θ}\right)\right|}---\left(13\right)$

式中，σ_1，2为观测矩阵的两个奇异值

${\mathrm{\σ}}_{\mathrm{1,2}}=p_2\frac{\sqrt{2}}{2}\sqrt{{\mathrm{\γ}}^2+1\±\sqrt{{\mathrm{\γ}}^4+2{\mathrm{\γ}}^2\cos \left(2\mathrm{\θ}\right)+1}}---\left(14\right)$

根据矩阵条件数理论，条件数小的矩阵称为“良性”矩阵，反之称为“病态”矩阵，系统可观测矩阵的条件数越大，系统的可观测程度越差；如果系统可观测矩阵的条件数无穷大，则系统不可观测；反之，系统可观测矩阵的条件数越接近1，系统的可观测性最好，为了分析问题的方便，根据式(13)我们选取条件数的倒数C^-1进行仿真分析，从图2中可以看出，当系统满足条件

$\left\{\begin{array}{c}\mathrm{\γ}=1\\ \mathrm{\θ}=\±\frac{\mathrm{\π}}{2}\end{array}\right.---\left(15\right)$

时，C^-1最大，也就是系统的可观测性最好；

步骤四：基于可观测性分析的编队构型设计

从协同导航系统可观测性条件可以看出，系统可观测性大小取决于跟随艇领航艇之间的相对距离以及相对方位，因此，在设计协同导航系统编队过程中要充分考虑到对系统可观测性的影响，下面将从这一点出发，对系统的编队构型进行设计；

在两条领航艇的间距确定的情况下(以200米为例)，在以两条领航艇连线的中点为圆心，距离的一半为半径的圆周上其可观测性最好，且跟随艇有两个最优位置，分别在两条领航艇连线中垂线与圆周交汇处，图3、图4中分别给出了两条领航艇和三条领航艇的情况；

步骤五：考虑舰艇尾流影响的编队构型设计

由于舰艇运动需要利用螺旋桨进行推进，则舰艇尾部会产生尾流，这样势必会对艇间的通信造成影响，下面从尾流对艇间通信的影响出发，对协同导航的编队问题进行研究；

为了保证主、跟随艇间通信不受尾流的影响，仅考虑尾流的主要影响区域，称为主尾流区，对艇设定一个主尾流区：夹角为θ，腰长为l的等腰三角形，假设艇间的连线即为传播声线，下面以3条艇为例，对尾流影响下的编队构型进行分析；

参考水声通信试验中艇高速行驶的时候所形成尾流区域的大小，设尾流区θ＝60°,l＝30m的等边三角形，领航艇与跟随艇间的距离为d=200m，根据艇间通信互不影响原则，即领航艇与跟随艇间连线不能穿过任何一个尾流区域；

对于3艇的编队构型，考虑到艇的数目较少，因而采用2条领航艇的方案，根据通信互不影响原则及三角几何关系，可得出下图所示的3艇编队示意图，由于主尾流区为边长为30m的等边三角形，为了使声线不通过主尾流区，考虑极限状态即艇间通信的声线与主尾流区的边界线及其延长线重合，则此时的跟随艇位于距两领航艇距离均为200m的等边三角形的顶点处(图中的位置1)，在图中当跟随艇处于阴影区外部(以位置2为例)时，可以看出跟随艇与领航艇2的声线通过主尾流区，这样违背了我们说规定的通信原则，所以跟随艇的位置应该在阴影区的范围内，根据镜像对称关系，可以得出3艇的编队构型中，跟随艇可位于以两条领航艇为顶点的菱形区域内；

步骤六：考虑舰艇尾流和安全距离影响的编队构型设计

在上述分析中我们是将舰艇看成了质点，可是在实际系统中舰艇自身有一定的长度，为了舰艇不会碰撞，需设置一定的安全距离，因此，在下述论述中假设跟随艇与领航艇间的距离要保证在100m以上，可得出以下的跟随艇分布；

在考虑安全距离的条件下，可得到跟随艇分布图7，分别以领航艇1和领航艇2为圆心，安全距离100m为半径得到两个圆，在这2个圆与图中阴影区相交的区域中分布的跟随艇不满足安全距离100m的要求，因此可以得到一个新的阴影区，跟随艇的位置应该在新的阴影区的范围内；

步骤七：考虑以上综合因素影响的艇编队构型设计

编队设计作为协同导航的一项重要研究内容，在协设计过程中，既要考虑可观测性的影响，又要保持艇间安全距离，还要考虑尾流区对通信的影响，因此，利用几何关系我们可以得到跟随艇的最优分布区域；

由可观测性分析可知，在跟随艇运动时，跟随艇与两领航艇的距离向量夹角接近90°，同时相对不同领航艇距离越接近，系统的可观测性最好，也就是说，当跟随艇分布在以两领航艇连线的中心点为圆心，距离的一半为半径的圆周上其可观测性最好，结合尾流影响下的跟随艇分布区以及安全距离考虑，可以得出图中的红色实线即为3艇编队下，跟随艇的最优分布区域；

本发明的优越性在于采用可观测性分析，尾流影响区域划定以及防碰撞安全距离圈划定综合考虑下的协同导航系统编队构型设计，而不是简单的将艇视为一个质点，忽略掉尾流的影响而得到的编队构型编排，对于协同导航系统编队构型设计有很高的实用价值。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于水声通信的多艇协同导航编队构型的方法，其特征在于，该基于水声通信的多艇协同导航编队构型的方法包括以下步骤：

步骤一，建立系统状态方程

在地理坐标系下，设t₀时刻跟随艇起始位置为O(0，0，0)，则t_k时刻跟随艇自身推算得到的状态量可以表示为：

$\left\{\begin{array}{c}{x}_k=x_{k-1}+\mathrm{\Δ}t({\hat{v}}_{k}cos{\widehat{\mathrm{\θ}}}_k+{\hat{w}}_{k}sin{\widehat{\mathrm{\θ}}}_k)\\ y_k=y_{k-1}+\mathrm{\Δ}t({\hat{v}}_{k}sin{\widehat{\mathrm{\θ}}}_k-{\hat{w}}_k\cos {\widehat{\mathrm{\θ}}}_k)\\ z_k={\hat{z}}_k\\ {\mathrm{\θ}}_k={\widehat{\mathrm{\θ}}}_k\end{array}\right.$

式中，为DVL测量的跟随艇纵向、横向速度；为微机械惯性系统输出的跟随艇航向信息；为深度传感器输出的跟随艇垂向深度信息；Δt为采样时间间隔；

步骤二：建立系统的量测方程

为简化算法分析的复杂度，系统观测量取跟随艇相对领航艇距离平方的二分之一，观测方程表示如下：

$h\left(X\right)=\left(\begin{array}{c}{h}_1\\ h_2\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}{(x-x_1)}^2+\frac{1}{2}{(y-y_1)}^2\\ \frac{1}{2}{(x-x_2)}^2+\frac{1}{2}{(y-y_2)}^2\end{array}\right)$

式中，(x₁，y₁)、(x₂，y₂)分别为两个领航艇的位置坐标，h₁、h₂分别为跟随艇与两个领航艇之间的距离；

步骤三：可观测性分析

根据非线性系统李导数可观测性判别方法，非线性系统局部可观测当且仅当李导数矩阵G的梯度满秩；

首先，定义n维非线性系统量测方程的李导数

$\begin{array}{c}{L}_f^0\left(h\right)=h\\ L_f^1\left(h\right)=\frac{\∂h}{\∂x}\·f=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂h}{\∂x_i}\·f_i\\ \·\\ \·\\ \·\\ L_f^{n-1}\left(h\right)=\frac{\∂}{\∂x}\[L_f^{n-2}\left(h\right)\]\·f\end{array}$

得到非线性系统的李导数矩阵G

根据上述非线性系统可观测性李导数判别方法，得到该系统的可观测性矩阵

$Obs=\left(\begin{array}{c}d{L}_f^0\left(h_1\right)\\ d{L}_f^0\left(h_2\right)\\ d{L}_f^1\left(h_1\right)\\ d{L}_f^1\left(h_2\right)\end{array}\right)=\left(\begin{array}{cc}x-x_1& y-y_1\\ x-x_2& y-y_2\\ f_1& f_2\\ f_1& f_2\end{array}\right)$

通过系统可观测矩阵可以看出，对于二阶系统，要想系统可观测，当且仅当观测矩阵秩为2，由于观测矩阵第3、4行相同，因此只针对矩阵前三行进行系统可观测性的讨论，将观测矩阵进一步变换成极坐标形式，表示如下：

$Obs=\left(\begin{array}{cc}{\mathrm{\Δx}}_1& {\mathrm{\Δy}}_1\\ {\mathrm{\Δx}}_2& {\mathrm{\Δy}}_2\\ v_e& v_n\end{array}\right)=\·\left(\begin{array}{cc}{p}_{1}sin\mathrm{\α}& p_{1}cos\mathrm{\α}\\ p_{2}sin\mathrm{\β}& p_{2}cos\mathrm{\β}\\ v\sin \mathrm{\γ}& vcos\mathrm{\γ}\end{array}\right)$

式中，p₁、p₂为跟随艇相对两个领航艇之间的距离，v为跟随艇的速度，α、β、γ分别为相对应的方位角；

从系统观测矩阵可以看出，系统可观测性与主跟随艇之间的距离向量p和跟随艇速度向量v有关，对于距离p₁、p₂可以始终满足为大于0常数，但是对于跟随艇来说，可以分为运动和静止两种状态，下面，分别就跟随艇不同的运动状态进行系统的可观测性分析；

当跟随艇运动(v≠0)时，系统的可观测性比较容易满足，从可观测矩阵可以看出，当且仅当α、β、γ相等或者相差180°时观测矩阵的秩为1，不满足系统可观测条件，也就是说，只有在跟随艇与领航艇航行轨迹在水平面上投影沿一条直线时系统不可观测，其余条件下均可观测，

当跟随艇静止(v＝0)时，系统可观测矩阵表示为

$Obs=\left(\begin{array}{cc}{p}_{1}sin\mathrm{\α}& p_{1}cos\mathrm{\α}\\ p_2\sin \mathrm{\β}& p_{2}cos\mathrm{\β}\end{array}\right)$

可以看出，若矩阵奇异则有

tanα＝tanβ

当且仅当α＝β+nπ(n＝0，1，2…)即跟随艇相对不同领航艇的距离方向平行时系统不可观测，其它状态情况下均可观测，为了进一步分析系统可观测性好坏与距离向量的关系，下面引入条件数分析理论对系统的可观测度做进一步分析，对观测矩阵做进一步变换

$Obs=p_2\left(\begin{array}{cc}\mathrm{\γ}sin\mathrm{\α}& \mathrm{\γ}cos\mathrm{\α}\\ \sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right)$

式中

$\mathrm{\γ}=\frac{p_2}{p_1}$

定义跟随艇与领航艇之间的距离向量夹角为θ＝α-β，则观测矩阵条件数C可以表示为

$C=\frac{max\left\{{\mathrm{\σ}}_{1,2}\right\}}{min\left\{{\mathrm{\σ}}_{1,2}\right\}}=\frac{{\mathrm{\γ}}^2+1+\sqrt{{\mathrm{\γ}}^4+2{\mathrm{\γ}}^{2}cos\left(2\mathrm{\θ}\right)+1}}{2\mathrm{\γ}\left|sin\left(\mathrm{\θ}\right)\right|}$

式中，σ_1，2为观测矩阵的两个奇异值

${\mathrm{\σ}}_{1,2}=p_2\frac{\sqrt{2}}{2}\sqrt{{\mathrm{\γ}}^2+1\±\sqrt{{\mathrm{\γ}}^4+2{\mathrm{\γ}}^2\cos \left(2\mathrm{\θ}\right)+1}}$

选取条件数的倒数C^-1进行仿真分析，当系统满足条件

$\left\{\begin{array}{c}\mathrm{\γ}=1\\ \mathrm{\θ}=\±\frac{\mathrm{\π}}{2}\end{array}\right.$

时，C^-1最大，也就是系统的可观测性最好；

步骤四：基于可观测性分析的编队构型，在设计协同导航系统编队过程中要充分考虑到对系统可观测性的影响；

在两条领航艇的间距确定的情况下，在以两条领航艇连线的中点为圆心，距离的一半为半径的圆周上其可观测性最好，且跟随艇有两个最优位置，分别在两条领航艇连线中垂线与圆周交汇处；

步骤五：考虑舰艇尾流影响的编队构型，为了保证主、跟随艇间通信不受尾流的影响，仅考虑尾流的主要影响区域，称为主尾流区，对无人艇设定一个主尾流区：夹角为θ，腰长为l的等腰三角形，假设艇间的连线即为传播声线，下面以3条艇为例，对尾流影响下的编队构型进行分析；

参考水声通信试验中无人艇高速行驶的时候所形成尾流区域的大小，设尾流区θ＝60°，l＝30m的等边三角形，领航艇与跟随艇间的距离为d＝200m，根据艇间通信互不影响原则，即领航艇与跟随艇间连线不能穿过任何一个尾流区域；

对于3艇的编队构型，考虑到艇的数目较少，跟随艇的位置应该在阴影区的范围内，根据镜像对称关系，可以得出3艇的编队构型中，跟随艇可位于以两条领航艇为顶点的菱形区域内；

步骤六：考虑舰艇尾流和安全距离影响的编队构型，假设跟随艇与领航艇间的距离要保证在100m以上，可得出以下的跟随艇分布；

在考虑安全距离的条件下，可得到跟随艇分布图，分别以领航艇1和领航艇2为圆心，安全距离100m为半径得到两个圆，在这2个圆与阴影区相交的区域中分布的跟随艇不满足安全距离100m的要求，因此可以得到一个新的阴影区，跟随艇的位置应该在新的阴影区的范围内；

步骤七：考虑以上综合因素影响的无人艇编队构型，利用几何关系可以得到跟随艇的最优分布区域，由可观测性分析可知，在跟随艇运动时，跟随艇与两领航艇的距离向量夹角接近90°，同时相对不同领航艇距离越接近，系统的可观测性最好，当跟随艇分布在以两领航艇连线的中心点为圆心，距离的一半为半径的圆周上可观测性最好，结合尾流影响下的跟随艇分布区以及安全距离考虑，可以得出跟随艇的最优分布区域。

2.根据权利要求l所述基于水声通信的多艇协同导航编队构型的方法，其特征在于，该基于水声通信的多艇协同导航编队构型的方法采用可观测性分析，尾流影响区域划定以及防碰撞安全距离圈划定综合考虑下的协同导航系统编队构型设计，忽略掉尾流的影响而得到的编队构型编排。

3.根据权利要求1所述基于水声通信的多艇协同导航编队构型的方法，其特征在于，在步骤一中，考虑到跟随艇航向信息θ_k及深度信息z_k直接可观测，只对东北向位置状态x、y进行可观测性分析，系统的连续状态方程表示如下：

$\stackrel{\·}{X}=f(X,u)$

式中

$f=\left(\begin{array}{c}{f}_1\\ f_2\end{array}\right)=\left(\begin{array}{c}\hat{v}cos\widehat{\mathrm{\θ}}+\hat{w}sin\widehat{\mathrm{\θ}}\\ \hat{v}sin\widehat{\mathrm{\θ}}-\hat{w}cos\widehat{\mathrm{\θ}}\end{array}\right).$