[发明专利]一种基于多约束模型的水下协同导航编队约束方法

权利要求书

1.一种基于多约束模型的水下协同导航编队约束方法，其特征在于：包括以下步骤：

步骤1、协同导航任务准备，根据协同导航精度需求，设定协同导航领航UUV的编队间距及编队队形；

步骤2、根据步骤1设定的领航UUV的编队间距及编队队形，构建基于领航UUV的导航几何因子数据库，并将数据库分发至各个单体；

步骤3、进行协同导航任务，输入步骤2得到的导航几何因子数据库，跟随UUV根据协同导航任务在区域进行航行，并计算当前几何精度因子，判断几何因子和几何因子门限、水声时延和直达声水声时延门限、水声时延和最大水声测距通信作用距离时延门限，若大于门限则进行步骤4，否则进行步骤5；

步骤4、跟随UUV自身规划路径，并改善几何因子和传播时延；

步骤5、跟随UUV继续自由航行，并返回步骤3。

2.根据权利要求1所述的一种基于多约束模型的水下协同导航编队约束方法，其特征在于：所述步骤1的具体实现方法为：根据协同导航精度需求，反求领航UUV编队间距，且领航UUV的编队约束为正多边形，同时利用球面交叉定位算法进行编队间距反求。

3.根据权利要求1所述的一种基于多约束模型的水下协同导航编队约束方法，其特征在于：所述步骤2的具体实现方法为：根据步骤1设定的领航UUV的编队间距及编队队形，利用球面交叉定位算法扫描编队周围区域内的几何因子，扫描的区域范围根据作业范围和精度要求确定，构建协同导航几何因子数据库。

4.根据权利要求3所述的一种基于多约束模型的水下协同导航编队约束方法，其特征在于：所述利用球面交叉定位算法扫描编队周围区域内的几何因子，扫描的区域范围根据作业范围和精度要求确定，构建协同导航几何因子数据库的具体实现方法为：

定义函数f:

用估计位置定义函数

接收机的实际位置为：

得到：

在收敛附近，比小很多，进行一阶泰勒级数展开，得到：

考虑到中间变量：

方程转换为：

估计值和实际伪距之间的最终关系式为：

以矩阵的形式表示：

经过数次跌迭代，当满足|Δx_r|+|Δy_r|＜ε时，迭代停止，最终HDOP表示成：

M＝(H^TH)^-1

扫描编队范围内的位置，得到几何精度因子数据库，其中，(x_r,y_r)为跟随UUV真实位置；为跟随UUV初始位置估计值；为真实的位置与估计位置之间的差，(x_i,y_i)为领航UUV的位置坐标。

5.根据权利要求1所述的一种基于多约束模型的水下协同导航编队约束方法，其特征在于：所述步骤3中几何因子门限由协同导航精度要求确定，直达声水声时延门限由领航UUV和跟随UUV的深度以及当前所处海洋环境下的水下声速由射线声学方法确定，最大水声测距通信作用距离时延门限由水声通信设备的通信范围确定。

6.根据权利要求1所述的一种基于多约束模型的水下协同导航编队约束方法，其特征在于：所述步骤3中判断几何因子和几何因子门限、水声时延和直达声水声时延门限、水声时延和最大水声测距通信作用距离时延门限三者为或关系，三者至少其一超过对应门限，则进行步骤4，否则进行步骤5。

7.根据权利要求1所述的一种基于多约束模型的水下协同导航编队约束方法，其特征在于：所述步骤4的具体实现方法为：给跟随UUV发送反馈信号，要求跟随UUV从新规划路径，并且使得几何因子和水声时延均不超过对应门限。