[发明专利]单水声信标测距辅助的水下同步定位与构图方法

说明书

本发明公开了一种单水声信标测距辅助的水下同步定位与构图方法：步骤1，确定航行器运动的状态方程，依据状态方程得到预测方程；步骤2，在实际航行过程中，航行器对环境特征进行观测，得到观测量，同时对提前布置好的单水声信标进行测距观测，得到增广的观测量；步骤3，依据步骤2得到的增广的观测量对新观测到的路标进行数据关联，不断对特征地图和状态量进行更新，从而得到实时的定位导航结果。本发明利用单水声信标测距信息对水下同步定位与构图方法进行约束，减少系统噪声的影响，提高定位精度。

技术领域

本发明涉及同步定位与构图技术，特别涉及一种单水声信标测距辅助基于扩展卡尔曼滤波的水下同步定位与构图方法。

背景技术

在当前的水下导航定位方法中，可供水下航行器使用的导航方式主要有非自主导航和自主导航两大类。非自主导航方式，如罗兰、欧米伽、GPS等，仅在接收机能够接收到导航信号时才能完成导航。罗兰、欧米伽相对于GPS导航精度要低。但是这些基于无线电的导航方式，由于电磁波在水中很快衰减的原因，在水下航行器上的使用受到很大限制。相对于电磁信号来说，声信号可以在水下传播距离较远，因此声换能器可以作为信标来引导载体的航行。目前采用的声学导航系统主要有长基线导航、短基线导航和超短基线导航三种形式。这三种形式都需事先在航行海域布放换能器或换能器阵，由于声学导航需要位置已知的信标，因此这种方式更加适用于科学研究及其他民用领域。地球物理导航方法目前主要有基于地磁的导航、基于重力场的导航和基于地形、地貌的导航三种方法，但进行这种导航时载体需要将测量到的数据与先验的测绘图或数据库进行匹配，不仅生成这些先验测绘图的成本与难度很大，而且随着空间维数的增加，寻找匹配峰值的计算复杂度呈指数级增长。正是由于这些导航方法的不足，才促使人们探索研究无需先验图的水下导航算法，这也正是同步定位与构图(Simultaneous Localization and Mapping,SLAM)算法力求解决的问题。

机器人在完全未知的环境中依靠自身携带的传感器来感知周围信息并且同时实现自身的定位和环境地图的创建是其真正实现自主导航的一个关键问题。SLAM技术的最初目标是使移动机器人在未知的位置和没有环境预知信息的条件下，利用自身携带的传感器来构建地图，同时利用这个地图来估计自身的姿态(位置和方向)。

近年来，众多关于解决室内、室外，甚至空中和水下的同步定位与构图技术应运而生，然而在水下应用中，传感器感知能力减弱，系统噪声增大且噪声统计特性无法预测，最重要的是水下可识别的特征十分贫乏，这使得水下SLAM问题成为最具挑战性的问题之一。

发明内容

本发明的目的是克服现有技术中的不足，提供一种单水声信标测距辅助的水下同步定位与构图方法，利用单水声信标测距信息对水下同步定位与构图方法进行约束，减少系统噪声的影响，提高定位精度。

本发明所采用的技术方案是：一种单水声信标测距辅助的水下同步定位与构图方法，包括以下步骤：

步骤1，确定航行器运动的状态方程，依据状态方程得到预测方程；

步骤2，在实际航行过程中，航行器对环境特征进行观测，得到观测量，同时对提前布置好的单水声信标进行测距观测，得到增广的观测量；

步骤3，依据步骤2得到的增广的观测量对新观测到的路标进行数据关联，不断对特征地图和状态量进行更新，从而得到实时的定位导航结果。

进一步的，步骤1具体包括以下步骤：

步骤1-1，对侧扫声呐图像进行处理，进行特征提取，步骤1-1中提取的特征为点特征；

步骤1-2，利用EKF-SLAM系统的模型进行预测，得到预测状态量和预测状态协方差：