[发明专利]基于粒子滤波与图优化的海底地形同步定位与建图方法

说明书

本发明属于海底地形同步定位与建图技术领域，具体涉及一种基于粒子滤波与图优化的海底地形同步定位与建图方法。本发明设计了一种使用位姿图结构对粒子自身储存轨迹进行更新，并使用位姿图在每一时刻结束时生成此时刻输出地图的粒子滤波BSLAM算法。本发明只需要在每一时刻输入里程计数据和由多波束声获得的地形测深，在没有先验地形图的情况下实现AUV的同步定位与建图。本发明所提出的图优化混合粒子滤波BSLAM算法相较于基于子地图匹配的粒子滤波BSLAM算法，在大累积误差下仍能实时准确的提供定位与建图结果。

技术领域

本发明属于海底地形同步定位与建图技术领域，具体涉及一种基于粒子滤波与图优化的海底地形同步定位与建图方法。

背景技术

测深信息同步定位与建图(BathymetricSimultaneousLocalizationandMapping,BSLAM)以多波束声呐为传感器收集海底地形信息，能够实现智能水下机器人在不依赖于外界提供信息的情况下，独立长时间的准确定位与建图，粒子滤波是实现这一方法的重要手段之一。但因为粒子滤波只能求解当前时刻的最优状态，在大累积误差的情况下会产生性能下降的问题。除此之外，因为基于子地图匹配的粒子滤波BSLAM中每一个粒子都会储存一副地图，选择这些粒子中最优地图存在问题。现有方法中使用粒子储存地图重复区域的最小均方误差作为选择的标准，但是存在着计算复杂度高，结果不稳定，误差较大的问题。因为BSLAM输出的地图需要为其它功能(如路径规划)提供信息，因此，需要一种在大累积误差下，可以准确实时生成地图的BSLAM方法。本发明在子地图匹配粒子滤波BSLAM的基础上提出了一种基于图优化理论的粒子地图更新方法与最优地图生成算法，能实现智能水下机器人在航位推算累积误差较大的情况下，能实时的提供准确的建图结果。

目前针对使用图优化理论进行地图更新与最终地图生成的粒子滤波BSLAM没有相关方法，较为相似的是：公开日为2020年1月24日，公开号为CN110726415A，发明名称为“一种自适应的水下多波束同步定位与建图方法”，该方法虽然可以使用在大累积误差下实时准确的进行定位与建图，但是无法处理粒子在重采样后聚类，由粒子集均值所生成轨迹不是最优轨迹的情况，而且需要在重采样后加入更新均值地图部分，算法较为复杂。除此之外，这种方法存在设定参数较多的问题。

发明内容

本发明的目的在于提供在没有先验地形图的情况下实现AUV的同步定位与建图的一种基于粒子滤波与图优化的海底地形同步定位与建图方法。

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

步骤1：设定粒子数N、粒子闭环检测半径R、粒子匹配子地图M_p的测线数目N_p、粒子轨迹子地图M_h的测线数目N_h、算法总步数T；初始化粒子集P_(1:N)；对于任一粒子P_(n)，其内部储存粒子当前状态粒子自身历史轨迹粒子权值w_n＝1/N、粒子储存的闭环集合L_n；n为粒子序数；t为时间步长；

t₀＝1时刻粒子集P_(1:N)的状态和自身历史轨迹根据GPS获得的先验状态设定，将t₀＝1时刻以及对应的粒子位姿加入粒子集P_(1:N)中各粒子储存的闭环集合L_n，初始化t＝2；

步骤2：获取当前t时刻输入的对海底的深度测量值z_(t)、里程计更新值v_(t)以及里程计的轨迹DR_(t)，更新粒子集P_(1:N)中各粒子的当前状态

其中，ω为里程计的噪声；