一种多机器人协同水下地形匹配导航系统及方法与流程

本发明涉及一种定位与导航，尤其涉及一种多机器人协同水下地形匹配导航系统及方法，属于水下定位与导航技术。

背景技术：

水下机器人在商业领域和国防工业领域受到了越来越多的重视，由于在水下环境中，高频信号衰减快，所以GPS等常规定位方式并不适用，由于定位与导航是水下机器人能否安全返航的重要技术因素，所以地形匹配导航成为了可选的选项之一。

多机器人协同编队运行由于其可以更方便地完成多样化、复杂化的任务，具有更高的鲁棒性等优点近年来受到了更多的关注。而如何利用多个机器人的传感器传输回的数据进行定位和导航也成为了各国家研究机构研究的新热点，其中多水下机器人协同地形匹配导航技术上还属于空白状态。

技术实现要素：

本发明的目的是为了提供一种多机器人协同水下地形匹配导航系统及方法，包括多机器人利用多波束声纳收集深度信息，汇总处理数据得出待匹配地形和与先验地图进行匹配等技术环节，最终得到每个机器人的精确位置。

本发明的目的是这样实现的：一种多机器人协同水下地形匹配导航系统，包括负责采集数据的机器人1…机器人I…机器人N和主机器人共N+1个机器人；

机器人1…机器人N均携带有多波束声纳，可互相进行水声通信，能将多波束声纳数据在声速修正和潮位修正后得到探测地型深度数据，能通过水声通信确定相对位置以及互相传输数据，能通过主机器人传输来的机器人位置信息确定其潜器状态；

主机器人可与其他机器人进行水声通信，存储先验地图，能将收到的机器人相对位置信息结合探测地型数据处理为待匹配地形，再结合先验地图进行匹配操作，得到各机器人位置，最终返回给各机器人。

本发明还包括这样一些结构特征：

1.一种多机器人协同水下地形匹配导航方法，

步骤一：N个机器人编队运行，由各个机器人将多波束声纳数据进行声速修正、潮位修正后得到待验地图深度数据，并将待验地图深度数据通过水声通信传递给主机器人；

步骤二：将各个机器人间的相对位置信息通过水声通信传递给主机器人；

步骤三：主机器人结合得到的相对位置信息以及待验地图深度数据进行汇总整理，经过计算得到待匹配地形；

步骤四：主机器人提取先验地图中的地形数据，利用极大似然估计来找出待匹配地形在先验地图中的最佳匹配点(x_match,y_match)，x、y分别对应大地坐标系中的经度值、纬度值；

步骤五：主机器人通过最佳匹配点(x_match,y_match)与各个机器人的相对位置信息，计算出每一个机器人对应的位置信息(x₂,y₂)，(x₃,y₃)…(x_n,y_n)后，再返回给各个机器人，完成多机器人协同地形匹配定位。

2.步骤三中的汇总整理包括：

第一：结合得到的待验地图深度数据和相对位置信息得到这些水深数据点的位置与数据信息；

第二：将水深数据点的位置与数据信息逐个读取，结合各个机器人的相对位置信息，判断该水深数据点是否已写入待匹配地形：若未写入，则进行第三步，若已写入，则进行第四步；

第三：将未写入的水深数据点写入待匹配地形；

第四：保存已写入的水深数据点对应的所有数据，取平均值作为数据写入待匹配地形；

第五：所有数据点写入完毕，整理数据，得到待匹配地形。

与现有技术相比，本发明的有益效果是：本发明通过处理编队运行的多个水下机器人的多波束声纳的数据，结合机器人之间的相对位置信息，得到探测到的地形，与已知的先验地形进行匹配，匹配完成后，再通过机器人的相对位置信息计算得到每个机器人的精确位置。在不依赖于GPS、初始位置等信息的情况下进行的地形匹配定位与导航任务。

也即，本发明利用概率相关的方法确定与之匹配的地形，进而结合机器人相对位置信息确定水下机器人所在的精确位置。本发明无需GPS、声学基阵的辅助，依靠自身传感器信号与相互通信，进行计算给出精确位置，完成导航。本发明可应用于多水下机器人协同地形匹配导航，且具有较高的鲁棒性

附图说明

图1为本发明的流程图；

图2为汇总处理探测地形流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

结合图1和图2，本发明的多机器人协同地形匹配系统，系统中包括负责采集数据的机器人1…机器人I…机器人N，和主机器人共N+1个机器人。机器人1…机器人N携带有多波束声纳等传感器，可互相进行水声通信，功能上可将多波束声纳数据在声速修正和潮位修正后得到探测地型深度数据，可通过水声通信确定相对位置以及互相传输数据，可通过主机器人传输来的机器人位置信息确定其潜器状态。主机器人可与其他机器人进行水声通信，存储有先验地图，功能上可以将收到的机器人相对位置信息结合探测地型数据处理为待匹配地形，再结合先验地图进行匹配操作，得到各机器人位置，最终返回给各机器人。

本发明的一种多机器人协同水下地形匹配导航方法，包括：

(1)多个机器人编队运行，由各机器人将多波束声纳数据进行声速修正、潮位修正后得到待验地图深度数据，通过水声通信传递给主机器人。

(2)同时将机器人间的相对位置信息通过水声通信传递给主机器人。

(3)主机器人结合步骤(2)中的相对位置信息以及步骤(1)中收到的每个机器人收集的深度信息，进行汇总整理，经过计算得到待匹配地形。

(4)主机器人提取先验地图中的地形数据，利用极大似然估计来找出待匹配地形在先验地图中的最佳匹配点(x_match,y_match)(在本发明中均使用大地坐标系，x、y分别对应经度、纬度值)。

(5)主机器人通过最佳匹配点(x_match,y_match)与机器人的相对位置信息，计算出每一个机器人的位置信息(x₂,y₂)，(x₃,y₃)…(x_n,y_n)。返回给各个机器人，完成多机器人协同地形匹配定位。

在步骤(3)中，汇总整理为利用以下方法实现：将多波束声纳数据通过多波束声纳的参数计算的到待匹配区域的水深数据，逐个读取水深数据。此时，结合机器人的相对位置信息，判断该点是否已写入待匹配地形，若未写入，则将该点写入待匹配地形，若已写入，则计算该点上所有数据的平均值作为水深数据，写入待匹配地形，依次读取完毕后完成待匹配地形的构建。具体说是：

首先结合接收到的各个机器人给出的待匹配地形深度数据，与机器人的相对位置信息得到这些水深数据点的位置与数据信息。

其次，逐个读取水深数据点的位置与数据信息，判断该点是否已经写入待匹地形，若没有则将该点写入，若同一位置已经存在数据点，则将这个数据视为修正数据，将该点对应的所有数据取平均值，写入对应的位置中。

最后，当所有数据点写入完毕，整理数据，得到待匹配地形。

更为简单的说，本发明的流程是：

1、多个机器人编队运行，由各机器人将多波束声纳数据进行声速修正、潮位修正后得到待验地图深度数据，通过水声通信传递给主机器人。

2、同时将机器人间的相对位置信息通过水声通信传递给主机器人。

3、主机器人将待验地图深度数据汇总并逐个读取，结合各机器人的相对位置信息，判断该点是否已写入待匹配地形，若未写入，则进行步骤4，若已写入，则进行步骤5。

4、将该点写入待匹配地形。

5、保存该点对应的所有数据，取平均值作为数据进行步骤4。

6、当所有深度数据读取完毕时，完成待匹配地形的构建。

7、提取先验地图中的地形数据，利用极大似然估计来找出待匹配地形在先验地图中的最佳匹配点(x_match,y_match)。

8、将最佳匹配点(x_match,y_match)结合机器人的相对位置信息，计算出每一个机器人的位置信息(x₂,y₂)，(x₃,y₃)…(x_n,y_n)。传输给每一个机器人，完成多机器人协同地形匹配定位。