[发明专利]一种用于水下机器人导航的声呐地图构建与重定位方法

说明书

本发明公开了一种用于水下机器人导航的声呐地图构建方法，包括以下步骤：(1)水下机器人利用扩展卡尔曼滤波进行导航，同时采集海底形貌的声呐图像，从声呐图像中提取地图特征并标记位置，建立局部地图；当前局部地图中地图特征数量达到预设值后，关闭该幅局部地图，开启新的局部地图；(2)将建立的局部地图按照先后序列进行联结，得到全局地图；(3)将得到的全局地图进行优化。本发明还公开了基于全局地图进行水下机器人重定位的方法。本发明的声呐地图构建方法，将地图的更新分成两部分，全局地图的更新优化放在后台进行，局部地图的更新实时进行，用于及时修正航位，降低了地图更新的复杂度。

技术领域

本发明涉及水下导航技术领域，尤其涉及一种用于水下机器人导航的声呐地图构建与重定位方法。

背景技术

随着科技的进步，人们加速了对海洋的探索与开采，水下机器人的使用也因此越来越广泛。然而，由于水下无法获得GPS信号，水下机器人的导航会产生累积误差无法修正，最终使得导航算法发散。

在海洋中，主要是依靠声学定位系统进行导航定位，例如超短基线(USBL)、短基线(SBL)和长基线(LBL)等。

公开号为CN104457754A的中国专利文献公开了一种利用长基线进行潜器定位的导航算法，然而利用长基线进行定位的前提是需要提前在潜器航行的区域进行基线的布放，增加了实现的难度和成本。SBL和USBL的使用相比LBL难度虽然有所降低，但这两种定位系统的成本依旧高昂，难以推广应用。

公开号为CN104075715A的中国专利文献公开了一种结合海底地形和环境特征的同步定位与地图构建的导航方法，利用扩展卡尔曼滤波作为导航的算法，提高了导航的精度。但该方法引入了深度方向的信息，增加了导航系统的计算复杂度。

公开号为CN106123850A的中国专利文献公开了一种声呐地图的修正方法，但该方法采用单幅地图结合水下机器人的定位信息进行地图修正，当特征数量较多时，单幅地图无法实时进行更新。目前，并没有发现关于水下声呐地图序列联结方法的研究，也鲜有发现用于水下机器人在全局地图中快速重定位的方法。

发明内容

本发明提供一种用于水下机器人导航的声呐地图构建方法，将地图的更新分成两部分，全局地图的更新优化放在后台进行，局部地图的更新实时进行，用于及时修正航位，降低了地图更新的复杂度。

本发明提供了如下技术方案：

一种用于水下机器人导航的声呐地图构建方法，包括以下步骤：

(1)水下机器人利用扩展卡尔曼滤波进行导航，同时采集海底形貌的声呐图像，从声呐图像中提取地图特征并标记位置，建立局部地图；当前局部地图中地图特征数量达到预设值后，关闭该幅局部地图，开启新的局部地图；

(2)将建立的局部地图按照先后序列进行联结，得到全局地图；

(3)将得到的全局地图进行优化。

步骤(1)中，在i时刻初始化的局部地图中，以水下机器人i时刻的位置R_i作为参考基准，水下机器人的初始化定位为初始协方差矩阵当j时刻地图特征F₁，F₂，...，F_n数量达到预设值时，关闭该局部地图。

i时刻至j时刻得到的局部地图表示为：

其中，为地图特征状态变量，分别表示j时刻水下机器人的位置R_j、地图特征F₁、F₂、...、F_n的位置坐标；为对应的协方差矩阵。

局部地图中特征的提取从靠水下机器人的近端开始，从而状态变量中的地图特征按从近到远排列。