[发明专利]一种AUV地形匹配导航的鲁棒BSLAM方法

说明书

本发明属于水下潜航器领域，公开了一种AUV地形匹配导航的鲁棒BSLAM方法，步骤(1)：输入测深和惯导数据；步骤(2)：数据预处理，得到滤波后的测深数据；步骤(3)：位姿图构建：计算弱数据关联，构建新子图，通过地形地位进行闭环检测后，再进行无效闭环检测；步骤(4)：后端图优化：利用聚类后的闭环数据计算一致性函数，找出所有类中一致性函数的最小值，然后融合惯导的关键状态的数据并输出修改后的BSLAM轨迹。本发明通过对测深数据和惯导数据的处理和闭环检测，实现了对数据的全局优化，且提高了定位与建图的实时性和一致性，且鲁棒性好，能够减弱测量数据的噪声对导航精度的影响。

技术领域

本发明属于水下潜航器领域，尤其涉及一种AUV地形匹配导航的鲁棒BSLAM方法。

背景技术

水下机器人主要分为两大类：一类是有缆水下机器人，习惯称为遥控潜器，简称ROV；另一类是无缆水下机器人，习惯称为自主式水下潜器，简称AUV。自主式水下机器人综合了人工智能和其他先进计算技术的任务控制器，集成了深潜器、传感器、环境效应、计算机软件、能量储存、转换与推进、新材料与新工艺、以及水下智能武器等高科技，军事上用于反潜战、水雷战、侦察与监视和后勤支援等领域，具有活动范围大、机动性好、安全、智能化等优点，成为完成各种水下任务的重要工具。例如，在民用领域，可用于铺设管线、海底考察、数据收集、钻井支援、海底施工，水下设备维护与维修等；在军用领域则可用于侦察、布雷、扫雷、援潜和救生等。由于无缆水下机器人具有活动范围不受电缆限制，隐蔽性好等优点，所以从60年代中期起，工业界和军方开始对无缆水下机器人发生兴趣。

AUV在水下活动时，一般采用惯性导航系统，惯性导航系统属于推算导航方式，即从一已知点的位置根据连续测得的运动体航向角和速度推算出其下一点的位置，因而可连续测出运动体的当前位置。惯性导航系统中的陀螺仪用来形成一个导航坐标系，使加速度计的测量轴稳定在该坐标系中，并给出航向和姿态角；加速度计用来测量运动体的加速度，经过对时间的一次积分得到速度，速度再经过对时间的一次积分即可得到位移。但长时间运作后，产生的误差也会累计。目前用BSLAM系统修正惯导累积误差。

公开号为CN107403163A的专利公开了一种基于深度学习的激光SLAM闭环检测方法，将SLAM闭环检测的问题转化为SALM数据样本的检索问题，并创新性地构建了一种深度Hash网络对激光点云样本进行Hash编码，进而在Hash编码的基础上进行样本相似度计算，实现相似样本的快速检索和SLAM闭环检测。但水下并不能使用激光传感器，且测深数据含有大量噪声，会产生无效闭环，因此不适用与水下导航。

公开号为CN7132521A的专利公开了一种BSLAM中地形匹配结果正确性判别方法，输入地形匹配数据和里程计数据，通过不断迭代计算一致性函数实现对地形匹配有效性的判别。在迭代中引入了自检和多窗口方法，其目的分别在于避免陷入局部最优和同时保证全局一致性和局部一致性。但该鲁棒滤波方法可靠性差，且抗干扰能力差。

发明内容

本发明的目的在于公开鲁棒性好、误差控制精度高的一种AUV地形匹配导航的鲁棒BSLAM方法。

本发明的目的是这样实现的：

一种AUV地形匹配导航的鲁棒BSLAM方法，包含如下步骤：

步骤(1)：输入测深和惯导数据：当AUV在海底长时间航行，惯导数据漂移过大时，初始化BSLAM，启动多波束声纳，收集海底地形的测深和惯导数据，并不断地输入到BSLAM程序中；

步骤(2)：数据预处理：利用基于Alpha-Shapes模型的多波束测深数据单ping滤波方法，对多波束声纳的测深数据中的野值进行有效剔除，得到滤波后的测深数据；

步骤(3)：位姿图构建：利用高斯过程回归估计地形深度，形成弱数据关联；AUV收集到的测深和导航数据超过阀值后，将数据储存在当前子图，并构建新子图；新子图构建后，通过地形定位进行闭环检测后，再进行无效闭环检测：