[发明专利]水下机器人自主定位与节点地图构建方法

说明书

本发明涉及水下机器人导航技术领域，旨在提供一种水下机器人自主定位与节点地图构建方法。本发明提出一种新型的节点定位与水下机器人自主定位方法，即在移动节点只能获得自身与随机节点的位置关系时，对所有节点进行定位，获得节点地图并同时对AUV进行定位。该方法能够确定较大范围内随机布放的节点位置，获得节点地图；并保证AUV的位置坐标误差在一个较小的范围内。本发明只需获得AUV在每个最优估计位置上与未知节点的距离即可，无需获得二者之间的角度关系；相较于传统方法，只利用声呐进行测距，大大降低测量成本。能满足在大范围海域内的工作需求，可保证自身定位误差，以及保证节点地图的节点定位平均误差小。

技术领域

本发明属于水下机器人导航技术领域，设计一种利用水下机器人(AutonomousUnderwater Vehicle，AUV；以下简称AUV)对无线传感器网络节点进行定位并同时确定自身位置的方法。

背景技术

随着海洋的开发，无线传感器网络技术被广泛的应用于海洋生态状况监控、海洋地质信息采集以及航行器定位等领域中。由于布设成本所限，无法对网络中每个节点进行事先预设以精确布置，但实际使用中有要求所有节点位置已知，因此解决无线传感器网络节点定位问题是无线传感器网络应用的关键。

为了解决此问题，研究人员提出包括凸优化、MDS-MAP、DV-Hop等多种传统的无线传感器网络节点定位方法，利用信标间的通信关系构建整体无线传感器网络节点位置关系。这种方法在通信质量高的环境下具有较高的准确度，但由于海洋中的特殊环境导致各种通信干扰，使得节点定位问题面临两难情况：1)各节点间在保证足够的通信质量下需求较高的通信成本；2)由于无线传感器网络节点数量多，无法满足较高的通信成本。这些直接导致这些传统的无线传感器网络节点定位方法无法直接用于海洋中。

目前为了解决上述问题，研究人员提出引入即时定位与地图构建(simultaneouslocalization and mapping，SLAM；以下简称SLAM)方法对节点进行定位。即引入移动节点(如AUV)，采用SLAM方法对移动节点进行即时定位，利用移动节点获取随机节点相对于自身的距离和角度，对无线传感器网络节点进行定位，得到节点地图，再利用地图对移动节点进行后续定位。这种方法从理论上基本是可行的，但是在实际使用情况中移动节点只能获得自身与随机节点的位置关系而无法获得角度关系，直接导致无法确定随机节点位置，导致方法整体不可行。

发明内容

本发明要解决的技术问题是，克服现有技术中的不足，提供一种水下机器人自主定位与节点地图构建方法。

为解决技术问题，本发明的解决方案是：

提供一种水下机器人自主定位与节点地图构建方法，包括以下步骤：

(1)通过水下机器人AUV搭载的传感器获取其自身角度、距离参数，用于计算移动误差小，并作出下一时刻的位置预测：

上式中的递归公式是利用贝叶斯滤波的方式得到机器人在t时刻的位姿的后验概率；式中是t时刻AUV的预测位置，X_t-1是t-1时刻的AUV的真实位置，P(·)表示随机变量的概率密度函数，P(·|·)表示条件概率密度函数(后文P(·|·)均表示条件概率，不再赘述)；

利用粒子滤波器对后验概率进行近似估计，在t时刻时每个粒子位置的后验概率分布满足：

上式中，为第j(j＝1,2,…,m)个粒子在t时刻的位置，U_t-1是t-1时刻的AUV的控制量；

(2)在状态预测方程中加入观测值Z_t-1，利用观测量修正AUV的预测值得到更为精确的概率分布并作为提议分布位置：