[发明专利]一种自适应的水下多波束同步定位与建图方法

说明书

本发明属于海底地形同步定位与建图领域，具体涉及一种自适应的水下多波束同步定位与建图方法。本发明采用反距离加权方法实现在不提取环境特征的情况下进行数据关联并通过库尔贝克‑莱布勒散度采样实现粒子数实时调整的基于粒子滤波的AUV测深数据同步定位与建图技术。本发明通过粒子分地图与均值轨迹分地图匹配实现不需要提取特征情况下的数据关联，通过库尔贝克—莱布勒散度实时控制粒子数，同时采用地形丰富度对粒子权值进行修正，提高了算法的鲁棒性。本发明不需要在海底地形中提取特征，可以实时控制粒子数，同时能够保证建图精度和实时性，适用性好，计算开销小，可以保证实时性。

技术领域

本发明属于海底地形同步定位与建图领域，具体涉及一种自适应的水下多波束同步定位与建图方法。

背景技术

同步定位与建图(Simultaneous Localization and Mapping,SLAM)提供了一种AUV在没有先验地图的情况下实现全海域航行的方法。粒子滤波是进行水下多波束声纳SLAM的重要手段之一。传统粒子滤波方法由于固定了粒子数目。计算开销较大。除此之外，海底地形趋于平缓，典型的粒子滤波SLAM框架如FastSLAM需要识别海洋环境中尺度独立的特征，导致适用性较差。因此，需要一种可以在不需要对海底特征识别和建模的情况下，能够实时控制粒子数的粒子滤波SLAM方法。

目前针对自适应的水下多波束同步定位与建图没有相关方法，较为相似的是使用其它传感器和算法来实现水下SLAM的方法。

公开日为2019年1月11日，公开号为CN109186610A，发明名称为“一种AUV地形匹配导航的鲁棒BSLAM方法”的专利申请，该方法以多波束声纳为传感器，使用图优化算法进行水下SLAM，计算开销较大。

公开日为2019年3月29日，公开号为CN109541606A，发明名称为“单水声信标测距辅助的水下同步定位与构图方法”的专利申请，该方法以单水声信标测距辅助来进行水下SLAM，需要提前布置水声信标，适用性较差。

发明内容

本发明的目的在于提供在不需要对海底特征识别和建模的情况下，能够实时控制粒子数，同时保证建图精度和实时性的一种自适应的水下多波束同步定位与建图方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤一：初始粒子数M₍₀₎、粒子集P₍₀₎和粒子权值w₍₀₎；根据GPS数据初始化粒子集均值轨迹mean₍₀₎；设定算法总步数T和检测半径R，设定当前算法步数t＝1；

步骤二：若t＝1，根据里程计对t时刻粒子集P_(t)进行运动更新，得到t时刻粒子均值轨迹mean_(t)，并令t＝t+1后返回步骤二；若1tT，构建t-1时刻临时粒子集P_(t-1)，初始化粒子标号i＝0，初始化t时刻粒子数M_(t)＝0，初始化空直方图与非空位数k＝1；若tT，则判定完成建图，结束计算；

步骤三：令M_(t)＝M_(t)+1，i＝i+1，判断M_(t)与t-1时刻的粒子集数目M_(t-1)的大小关系，若M_(t)≤M_(t-1)，则按下式对第i号粒子进行运动更新：

其中，为第i号粒子于t时刻的位姿；u_t为t时刻控制；ε_t-1为t-1时刻误差；若M_(t)＞M_(t-1)，则从P_(t-1)中随机抽取一粒子作为第i号粒子进行运动更新；