CN111928851A - 基于tma技术的多自主水下机器人集群协同导航方法 - Google Patents

Abstract

本发明涉及水下组合导航技术领域，尤其涉及一种自主水下机器人集群的协同组合导航方法，本发明包括采用拓扑式协同导航结构，在子集群内指定多个领航者作为子集群的导航节点，其它跟随者作为扩展节点，扩展节点在线搜索有效导航节点集合，实时获得有效导航节点的导航定位状态，通过测距仪对有效导航节点进行测距，利用扩展节点在不同时刻对不同导航节点的测距，结合扩展节点的移动矢量径和分时测距技术(简称TMA技术)，构建多源分时测距矩阵，在线计算扩展节点导航精度上界，依托导航精度上界迭代估计扩展节点的导航位置，从而实现集群内所有自主水下机器人的协同导航。

Description

基于TMA技术的多自主水下机器人集群协同导航方法

技术领域

本发明涉及水下组合导航技术领域，尤其涉及一种自主水下机器人(简称AUV)集群的协同组合导航方法。

背景技术

在海洋工程、海洋科学调查过程中，自主水下机器人发挥越来越重要的作用。自主水下机器人经过几十年的发展已经取得了长足的进步并且已经有较为成熟的产品用于海洋工程和深海资源勘查，但是随着海洋任务需求的增加,研究多功能自主水下机器人面临着成本高、冗余性差、任务范围小、工作效率低等局限性，所以需要构建多台自主水下机器人组成的水下集群，发挥集群系统的空间分布和功能分布优势，提高集群的深海资源勘查效率，实现从海洋地形勘察、光学精细探测、水文采样等多维度调查深海海底资源和深海科学目标取样。而协同导航技术是多自主水下机器人集群协同控制的关键技术，它的作用是为集群内自主水下机器人提供高精度的导航位置。因为多功能集群内的自主水下机器人搭载不同的任务载荷，配置不同成本和精度的导航传感器，所以研究多水下机器人协同导航技术，尤其是异构集群协同导航技术，保证集群内不同自主水下机器人导航能力的一致性，克服集群内自主水下机器人导航系统长时间航行的累积误差，发挥集群协同优势，进而提高集群的协同导航精度。多自主水下机器人集群协同导航从导航结构上被划分为并行式协同导航、主从式协同导航。(1)并行式协同导航，集群内所有自主水下机器人具有同等地位，导航设备精度一致，并要求自主水下机器人与多个相邻自主水下机器人通信，对水声通信要求较高，而有限的水声通信频带限制了自主水下机器人集群的规模；(2)主从式协同导航模式放宽了集群的通信限制，它将集群划分为若干小集群，每个子集群指定一台自主水下机器人为领航者，子集群内其它自主水下机器人是跟随者，跟随者与领航者通信并测距，进而获得自身的位置估计，在海洋工程应用中，1台领航者自主水下机器人带领2到3台跟随者自主水下机器人。如果子集群领航者发生故障，那么跟随者将无法进行协同导航，所以子集群领航者的可靠性限制了子集群的可靠性。因为并行式协同导航的集群规模有限，主从式协同导航的系统冗余性不足，所以需要研究拓扑式协同导航技术，满足集群规模扩展和提高集群冗余性的任务需求。拓扑式协同导航在子集群内指定多个领航者作为子集群的导航节点，其它跟随者作为扩展节点，扩展节点在线搜索有效导航节点集合，实时获得有效导航节点的导航定位状态，通过测距仪对有效导航节点进行测距，利用扩展节点在不同时刻对不同导航节点的测距，结合扩展节点的分时测距技术(简称TMA技术)，构建多源分时测距矩阵，在线计算扩展节点导航精度上界，依托导航精度上界迭代估计扩展节点的导航位置，从而实现集群内自主水下机器人的协同导航。

发明内容

本发明涉及水下组合导航技术领域，尤其涉及一种自主水下机器人(简称AUV)集群的协同组合导航方法，针对多自主水下机器人集群的并行式协同导航存在制约集群规模和主从式协同导航的系统冗余性不足的缺点，采用拓扑式协同导航结构，提出适用于拓扑式网络结构的集群协同导航方法，将集群内的自主水下机器人划分为导航节点和扩展节点，利用扩展节点与导航节点间的测距构建量测矩阵，在线估计扩展节点导航精度的边界，实时计算扩展节点的位置估计，从而实现集群内所有自主水下机器人的协同导航。

本发明为实现上述目的所采用的技术方案是：

基于TMA技术的多自主水下机器人集群协同导航方法，包括以下步骤：

1)构建有效导航节点集合；

2)分别构建有效导航节点集合内，每个扩展节点对应的TMA量测集合；

3)分别计算有效导航节点集合内，每个扩展节点的精度上界；

4)根据步骤1)～步骤3)，计算有效导航节点集合内某一个扩展节点的位置估计；

5)循环步骤4)，计算有效导航节点集合内，所有扩展节点的位置估计。

所述步骤1)具体为：依次判断集群内所有导航节点是否满足节点约束条件，满足约束条件的导航节点即有效导航节点，所有的有效导航节点构成有效导航节点集合B_k。

所述节点约束条件为：

其中，X₁、X₂…X_i分别表示导航节点1、2…、i分别在时刻t的二维空间位置，c是导航节点的矢量相关性判据，为设定值。

所述步骤2)具体为：

利用扩展节点k在不同时刻的移动矢量径构建扩展节点k的量测向量和量测矩阵，量测向量和量测矩阵构成量测集合；定义Y_k表示扩展节点k的量测向量，定义H_k表示扩展节点k的量测矩阵，量测向量Y_k和量测矩阵H_k是以下方程组解的代数表达式：

其中，b_k[j]表示集合B_k内第j个元素，它的值是导航节点编号，b_k[j]的小脚标k对应扩展节点k，n_k表示集合B_k中有效导航节点的数目，

表示扩展节点k在接收导航节点b_k[j]测距时的空间位置；

表示扩展节点k在接收导航节点b_k[n_k]测距时的空间位置，

表示扩展节点k接收到导航节点b_k[j]的测距的时刻；

表示扩展节点k接收到导航节点b_k[n_k]的测距的时刻；

是扩展节点k从时刻

到时刻

的移动矢量径；R(t)是自主水下机器人在t时刻的航向角、纵倾角、横滚角组成的姿态矩阵，u(t)是自主水下机器人在t时刻的速度矢量矩阵；

是扩展节点k与导航节点b_k[j]的空间距离估计；

是扩展节点k与导航节点b_k[j]的空间距离，Y_k,j是扩展节点k与导航节点b_k[n_k]构建的量测矢量，Y_k是由n_k个Y_k,j测量矢量组成的；H_k,j是扩展节点k与导航节点b_k[n_k]构建的量测行向量，H_k是由n_k个H_k,j测量行向量组成的。

所述步骤3)具体为：

将有效导航节点集合内不同导航节点的精度特征和当前位置状态进行数据融合，得到扩展节点k在当前时刻位置的精度上界:

其中，dlg(.)表示对角矩阵算子，F_N(.)表示北向位移投影函数，F_E(.)表示东向位移投影函数；定义

表示导航节点b_k[j]的空间位置，

表示扩展节点k在接收导航节点b_k[j]测距时的空间位置，

为中间变量；

表示扩展节点k与导航节点b_k[j]的空间距离量测的协方差作为精度特征。

所述步骤4)具体为：

选取有效导航节点集合内某一扩展节点对应的TMA量测集合，计算该扩展节点的位置估计修正量；

循环迭代计算该扩展节点的空间位置，直至该扩展节点的修正量范数小于精度上界范数；

保留迭代结果即该扩展节点的位置估计。

本发明具有以下有益效果及优点：

1、相对于传统自主水下机器人集群的协同导航方法，本方法使用基于TMA技术的自主水下机器人集群协同导航技术，扩展了集群协同导航的规模，提高了集群协同导航系统的冗余性。

2、针对传统自主水下机器人集群协同导航的水下信道利用率低和计算成本高的问题，本方法将有效导航点搜寻策略和TMA分时测距模型相结合，提高了自主水下机器人集群协同导航计算的实时性和系统可靠性。

3、应用范围广，本发明不但可以应用于自主水下机器人集群的协同导航，还可以用于水下滑翔机集群、自主水下机器人/无人水面艇的混合集群的协同导航。

附图说明

图1a是本发明的组成示意图一；

图1b是本发明的组成示意图二；

图2是本发明的流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明包括采用拓扑式协同导航结构，在子集群内指定多个领航者作为子集群的导航节点，其它跟随者作为扩展节点。首先，扩展节点在线搜索有效导航节点集合，实时获得有效导航节点的导航定位状态，通过测距仪对有效导航节点进行测距；其次，利用扩展节点在不同时刻对不同导航节点的测距，结合扩展节点的移动矢量径和分时测距技术(简称TMA技术)，构建多源分时测距矩阵；最后，在线计算扩展节点导航精度上界，依托导航精度上界迭代估计扩展节点的导航位置，从而实现集群内所有自主水下机器人的协同导航。拓扑式协同导航的关键技术问题是高效搜寻不同扩展节点对应的有效导航节点集合，它的作用是减少不必要的水声测距和水声通信，为自主水下机器人集群规模的进一步扩展而预留更多的水声信道容量；拓扑式协同导航的另一关键技术问题是高效融合多源TMA测距，因为自主水下机器人是实时嵌入式系统，数据融合的计算成本和效率对控制系统的实时性存在一定程度的影响，所以高效融合多源TMA测距也是十分必要的，以上两个关键问题的求解即是本方法的创新点。本方法能够有效地解决自主树下机器人集群的导航问题，采用拓扑式网络结构，扩展集群协同导航的规模，提高集群协同导航的冗余度。引入有效导航节点集合概念，提高了水下信道的利用率，采用TMA分时技术构建量测矩阵，在线估计导航精度边界，实现了高精度导航位置的在线估计，降低导航计算的时间成本，满足自主水下机器人协同导航的实时性需求，具有较强的工程应用价值；本方法移植方便，扩展性强，也适用于水下滑翔机集群、自主水下机器人/无人水面艇的混合集群等的组合导航应用领域。

整个系统组成包括6台或6台以上的自主水下机器人、自主水下机器人搭载的多普勒计程仪、深水罗经、深度计、测距仪、水声通信机等设备。其中自主水下机器人是搭载多普勒计程仪、深水罗经、深度计、测距仪的运载具，自主水下机器人依据在集群协同导航的功能分为导航节点式自主水下机器人(简称为导航节点)和扩展节点式自主水下机器人(简称为扩展节点)，导航节点式自主水下机器人的作用是接收扩展节点式自主水下机器人的定位询问信号，同时回复定位应答信号和广播自身定位信息到扩展节点式自主水下机器人，提供扩展节点式自主水下机器人与导航节点式自主水下机器人间的距离量测，即它们是集群协同导航的移动定位参考源。扩展节点是接收扩展节点式自主水下机器人的定位询问信号，同时回复定位应答信号和广播自身定位信息到扩展节点式自主水下机器人，即他们是集群协同的被定位水下机器人。多普勒计程仪、深水罗经、深度计、水声通信机是自主水下机器人的内部导航传感器，其中多普勒计程仪的作用是测量自主水下机器人相对于海底的航行速度；深水罗经的作用是测量自主水下机器人的航向角、纵倾角、横滚角等姿态角；深度计的作用是测量自主水下机器人相对于海平面的深度。测距仪是导航外部传感器，它的作用是测量导航节点式自主水下机器人与扩展节点式自主水下机器人的距离。水声通信机是自主水下机器人集群间信息交互的设备，导航节点通过水声通信机向扩展节点告知导航节点的当前位置，扩展节点通过水声通信机向导航节点告知彼此的定位询问频率、定位应答频率，系统组成如图1a～图1b所示。

整个系统按照如下流程工作：

为了描述方便，首先对一些变量进行定义。定义X_i表示集群导航节点i在时刻t_i的空间位置，X_i＝(x_i,y_i)^T，其中x_i,，y_i,分别表示北向位置，东向位置，同理，定义X₁、X₂…X_n分别表示导航节点1、2…、n分别在时刻t的二维空间位置，n表示导航节点的索引。定义S_K表示集群内扩展节点的数目，s_k表示集群扩展节点的索引。

如图2所示为本发明的流程图。

第一步，构建有效导航节点集合。

根据导航节点的矢量线性相关性，构建任意扩展节点k的有效导航集合B_k，构建方法如表1所示：

表1构建扩展节点的有效导航节点集合

其中定义M_N表示集群内导航节点的数目，i表示集群内导航节点的索引；c是导航节点的矢量相关性判据，工程上一般取值为0.9。B_k表示扩展节点k相应的有效导航节点集合，·表示向量内积算法运算，*表示标量乘法运算。

第二步，构建扩展节点对应的TMA量测集合。

利用扩展节点k在不同时刻的移动矢量径构建扩展节点k的量测向量和量测矩阵，量测向量和量测矩阵构成量测集合。定义Y_k表示扩展节点k的量测向量，定义H_k表示扩展节点k的量测矩阵，量测向量Y_k和量测矩阵H_k是以下方程组解的代数表达式：

其中B_k表示扩展节点k相应的有效导航节点集合，b_k[j]表示集合B_k内第j个元素，它的值是导航节点编号，b_k[j]的小脚标k对应扩展节点k，n_k表示集合B_k中有效导航节点的数目。

表示扩展节点k在接收导航节点b_k[j]测距时的空间位置，它是中间变量；

表示扩展节点k在接收导航节点b_k[n_k]测距时的空间位置，简称为当前时刻的空间位置，它的初始值由扩展节点的深水罗经和多普勒计程仪联合提供，它的最终取值是通过后续步骤迭代求取，它是未知量，它在本步骤中以符号变量形式存在。

表示扩展节点k接收到导航节点b_k[j]的测距的时刻；

表示扩展节点k接收到导航节点b_k[n_k]的测距的时刻；

是扩展节点k从时刻

到时刻

的移动矢量径；R(t)是自主水下机器人在t时刻的航向角、纵倾角、横滚角组成的姿态矩阵，它是由自主水下机器人搭载的深水罗经测量获得；u(t)是自主水下机器人在t时刻的速度矢量矩阵，它是由自主水下机器人搭载的多普勒计程仪测量获得；

是扩展节点k与导航节点b_k[j]的空间距离估计，它是中间变量，||·||是范数算子；

是扩展节点k与导航节点b_k[j]的空间距离的测量，它由搭载在扩展节点k上的测距仪测量获得；Y_k,j是扩展节点k与导航节点b_k[n_k]构建的量测矢量，它是中间变量；Y_k是由n_k个Y_k,j测量矢量组成的，它是本方程组的待求解变量；H_k,j是扩展节点k与导航节点b_k[n_k]构建的量测行向量，它是中间变量；H_k是由n_k个H_k,j测量行向量组成的，它也是本方程组的待求解变量。很明显，方程组(1)解的计算方法是从公式(1.1)到公式(1.6)依次代入求解。

第三步，计算扩展节点的精度估计。

数据融合不同导航节点的精度特征和当前位置状态，估计扩展节点k在当前时刻位置精度的上界。定义

表示扩展节点k当前时刻基于导航节点集合b_k[n_k]的位置精度的上界，它的计算方法如下:

其中dlg(.)表示对角矩阵算子，F_N(.)表示北向位移投影函数，F_E(.)表示东向位移投影函数；定义

表示导航节点b_k[j]的空间位置，

它是已知量，由导航节点通过水声通信机告知扩展节点；

表示扩展节点k在接收导航节点b_k[j]测距时的空间位置，

它是中间变量；

表示扩展节点k与导航节点b_k[j]的空间距离量测的协方差，它是已知量，它是测距仪的性能参数，由设备厂家在设备出厂时进行标定。

第四步，计算扩展节点的位置估计

使用迭代法求解扩展节点的位置估计，迭代结束判据是修正量范数小于精度上界的范数，扩展节点k当前时刻的位置估计

的计算方法如下：

表2计算扩展节点的位置估计

其中ΔX_k表示扩展节点k位置估计的修正量，它是中间变量；

表示扩展节点k在接收导航节点b_k[j]测距时的空间位置，它是中间变量；

表示扩展节点k当前时刻的位置估计，它是结果输出变量。

第五步，计算所有扩展节点的位置估计

重复步骤四，计算所有扩展节点的位置估计，该步骤只是简单的循环函数调用，所以该步骤不作为权利述求。

表3计算所有扩展节点的位置估计

Claims (6)

1.基于TMA技术的多自主水下机器人集群协同导航方法，其特征在于，包括以下步骤：

1)构建有效导航节点集合；

2)分别构建有效导航节点集合内，每个扩展节点对应的TMA量测集合；

3)分别计算有效导航节点集合内，每个扩展节点的精度上界；

4)根据步骤1)～步骤3)，计算有效导航节点集合内某一个扩展节点的位置估计；

5)循环步骤4)，计算有效导航节点集合内，所有扩展节点的位置估计。

2.根据权利要求1所述的基于TMA技术的多自主水下机器人集群协同导航方法，其特征在于，所述步骤1)具体为：依次判断集群内所有导航节点是否满足节点约束条件，满足约束条件的导航节点即有效导航节点，所有的有效导航节点构成有效导航节点集合B_k。

3.根据权利要求2所述的基于TMA技术的多自主水下机器人集群协同导航方法，其特征在于，所述节点约束条件为：

其中，X₁、X₂…X_i分别表示导航节点1、2…、i分别在时刻t的二维空间位置，c是导航节点的矢量相关性判据，为设定值。

4.根据权利要求1所述的基于TMA技术的多自主水下机器人集群协同导航方法，其特征在于，所述步骤2)具体为：

利用扩展节点k在不同时刻的移动矢量径构建扩展节点k的量测向量和量测矩阵，量测向量和量测矩阵构成量测集合；定义Y_k表示扩展节点k的量测向量，定义H_k表示扩展节点k的量测矩阵，量测向量Y_k和量测矩阵H_k是以下方程组解的代数表达式：

其中，b_k[j]表示集合B_k内第j个元素，它的值是导航节点编号，b_k[j]的小脚标k对应扩展节点k，n_k表示集合B_k中有效导航节点的数目，

表示扩展节点k在接收导航节点b_k[j]测距时的空间位置；

表示扩展节点k在接收导航节点b_k[n_k]测距时的空间位置，

表示扩展节点k接收到导航节点b_k[j]的测距的时刻；

表示扩展节点k接收到导航节点b_k[n_k]的测距的时刻；

是扩展节点k从时刻

到时刻

的移动矢量径；R(t)是自主水下机器人在t时刻的航向角、纵倾角、横滚角组成的姿态矩阵，u(t)是自主水下机器人在t时刻的速度矢量矩阵；

是扩展节点k与导航节点b_k[j]的空间距离估计；

是扩展节点k与导航节点b_k[j]的空间距离，Y_k,j是扩展节点k与导航节点b_k[n_k]构建的量测矢量，Y_k是由n_k个Y_k,j测量矢量组成的；H_k,j是扩展节点k与导航节点b_k[n_k]构建的量测行向量，H_k是由n_k个H_k,j测量行向量组成的。

5.根据权利要求1所述的基于TMA技术的多自主水下机器人集群协同导航方法，其特征在于，所述步骤3)具体为：

将有效导航节点集合内不同导航节点的精度特征和当前位置状态进行数据融合，得到扩展节点k在当前时刻位置的精度上界:

其中，dlg(.)表示对角矩阵算子，F_N(.)表示北向位移投影函数，F_E(.)表示东向位移投影函数；定义

表示导航节点b_k[j]的空间位置，

表示扩展节点k在接收导航节点b_k[j]测距时的空间位置，

为中间变量；

表示扩展节点k与导航节点b_k[j]的空间距离量测的协方差作为精度特征。

6.根据权利要求1所述的基于TMA技术的多自主水下机器人集群协同导航方法，其特征在于，所述步骤4)具体为：

选取有效导航节点集合内某一扩展节点对应的TMA量测集合，计算该扩展节点的位置估计修正量；

循环迭代计算该扩展节点的空间位置，直至该扩展节点的修正量范数小于精度上界范数；

保留迭代结果即该扩展节点的位置估计。

Images