一种AUV地形匹配导航的鲁棒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收集到的测深和导航数据超过阀值后，将数据储存在当前子图，并构建新子图；新子图构建后，通过地形地位进行闭环检测后，再进行无效闭环检测：

步骤(3.1)：输入预处理后的数据：输入惯导数据和滤波后的测深数据；

步骤(3.2)：建立弱数据关联：

根据高斯过程回归和滤波后的测深数据z⁺，得到周围的估计深度z^-，建立弱数据关联l(xt；xt-1)：

上式中，是第k个采样点的高斯估计深度，是第k个采样点的测量深度，n是采样点的个数，是采样点的测量方差，是测量方差组成的对角矩阵；

步骤(3.3)：子图构建：测深数据和位姿数据达到一定阀值后，储存在当前子地图，并构建新子图；

步骤(3.4)：新子图构建完毕后，进行地形定位，计算新子图与历史子图的平均校正值，判别是否有闭环产生：

新子图与历史子图的平均校正值为：

上式中，(yt，k-hk(xt))²表示状态xt处k个测量点的校正值，n是采样点的个数，yt，k表示采样点k的测量深度，hk(xt)表示历史子图中对应点的深度；

若t(xt)小于一定阀值，则判断结果为有闭环产生，进入步骤(3.5)，否则返回步骤(3.1)；

步骤(3.5)：分割：在频域中，检验闭环的校正分布是否满足高斯分布，取n(μ,σ²)(μ＝±0.5,σ²＝0.4)；在空域中，提取两个匹配子图中校正值大于μ±σ的所有测量点作为区域2，剩下的区域作为区域1；

步骤(3.6)：融合地形信息：在坐标(i,j)处高程为h(i,j)的地图中，利用灰色理论融合多个物理量描述区域1和区域2的地形信息，计算区域i的灰色关联度ri：

首先计算每个区域的最佳序列：

x0＝[max(rt(i)),min(ht(i)),max(σt(i)),max(ft(i))](i＝1,2,...,k)；

灰色关系系数为：

上式中，分辨率ρ∈[0,1]，rt是地形粗糙度，ht是地形熵，σt是地形标准差，ft是fisher信息；

区域i的灰色关联度ri为：

上式中，w1，w2,，w3和w4是fisher信息ft的权重；

步骤(3.7)：判别闭环是否有效：利用人工神经网络识别无效闭环并保留有效闭环；输入新子图与历史子图的平均校正值，预处理信息和区域1、区域2的有效点数，输出值若小于阀值0.1，则该闭环为有效闭环，进入步骤(4)；否则是无效闭环，返回步骤(3.1)。

步骤(4)：后端图优化：利用聚类后的闭环数据计算一致性函数，找出所有类中一致性函数的最小值，然后融合惯导的关键状态的数据并输出修改后的bslam轨迹。

步骤(4.1)：输入闭环数据：初始化，令k＝0，输入n个闭环，初始化open表和close表，将所有闭环放入open表中，清空close表；

步骤(4.2)：聚类并计算一致性函数：

将所有闭环聚类，把所有的类在局部优化和全局优化中计算一致性函数：

上式中，是从惯导数据所得的auv状态，是从bslam所得的auv状态，j是位姿的数量；

步骤(4.3)：判断是否满足连续性条件：设定一个阀值，判断χ²是否小于阀值，若小于阀值进入步骤(4.7)，否则进入步骤(4.4)；

步骤(4.4)：令k＝k+1，依次计算open表中所有类的一致性函数和最小值对在open表中的类i,i＝1,2,...,n，依次将类从open表中取出并计算找到最小值并把最小值对应的类从open表中移至close表中；

步骤(4.5)：如果小于则进入步骤(4.3)，否则进入步骤(4.6)；

步骤(4.6)：对在close表中的类j,j＝1,2,...,m，依次将类从close表中取出并计算j＝1,2,...,m；如果中的最小值小于则将最小值对应的类从close表中移至open表中并执行步骤(4.6)，否则进入步骤(4.4)；

步骤(4.7)：融合惯导的关键状态的数据并输出修改后的bslam轨迹。

本发明的有益效果为：

本发明通过对测深数据和惯导数据的处理和闭环检测，实现了对数据的全局优化，保证了全局一致性，且提高了定位与建图的实时性和一致性，且鲁棒性好，能够减弱测量数据的噪声对导航精度的影响。

附图说明

图1是一种auv地形匹配导航的鲁棒bslam方法流程图；

图2是位姿图构建流程框图；

图3是鲁棒滤波流程框图。

具体实施方式

下面结合附图来进一步描述本发明：

如图1，一种auv地形匹配导航的鲁棒bslam方法，包含如下步骤：

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

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

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

步骤(3.1)：输入预处理后的数据：输入惯导数据和滤波后的测深数据；

步骤(3.2)：建立弱数据关联：

根据高斯过程回归和滤波后的测深数据z⁺，得到周围的估计深度z^-，建立弱数据关联l(xt；xt-1)：

上式中，是第k个采样点的高斯估计深度，是第k个采样点的测量深度，n是采样点的个数，是采样点的测量方差，是测量方差组成的对角矩阵；

步骤(3.3)：子图构建：测深数据和位姿数据达到一定阀值后，储存在当前子地图，并构建新子图；

步骤(3.4)：新子图构建完毕后，进行地形定位，计算新子图与历史子图的平均校正值，判别是否有闭环产生：

新子图与历史子图的平均校正值为：

上式中，(yt，k-hk(xt))²表示状态xt处k个测量点的校正值，n是采样点的个数，yt，k表示采样点k的测量深度，hk(xt)表示历史子图中对应点的深度；

若t(xt)小于一定阀值，则判断结果为有闭环产生，进入步骤(3.5)，否则返回步骤(3.1)；

步骤(3.5)：分割：在频域中，检验闭环的校正分布是否满足高斯分布，取n(μ,σ²)(μ＝±0.5,σ²＝0.4)；在空域中，提取两个匹配子图中校正值大于μ±σ的所有测量点作为区域2，剩下的区域作为区域1；

步骤(3.6)：融合地形信息：对于在坐标(i,j)处高程为h(i,j)的地图，常用的描述地形信息的量是地形粗糙度rt，地形熵ht，地形标准差σt和fisher信息ft。然而，单一的量并不能完整描述地形信息，因此，利用灰色理论来融合所有的量以描述区域1和区域2的地形信息：

首先计算每个区域的最佳序列：

x0＝[max(rt(i)),min(ht(i)),max(σt(i)),max(ft(i))](i＝1,2,...,k)；

灰色关系系数为：

上式中，分辨率ρ∈[0,1]，rt是地形粗糙度，ht是地形熵，σt是地形标准差，ft是fisher信息；

区域i的灰色关联度ri为：

上式中，w1，w2,，w3和w4是fisher信息ft的权重；

灰色关联度ri越大，则地形信息越好。

步骤(3.7)：判别闭环是否有效：利用人工神经网络识别无效闭环并保留有效闭环；输入新子图与历史子图的平均校正值，预处理信息和区域1、区域2的有效点数，输出值若小于阀值0.1，则该闭环为有效闭环，进入步骤(4)；否则是无效闭环，返回步骤(3.1)。

步骤(4)：如图3，后端图优化：后端图优化包括局部优化和全局优化。检测到闭环的状态成为关键状态，局部优化依据弱数据关联将关键状态的导航偏差分配给所有状态，然后进行全局优化。全局优化计算地图的全局一致性函数，若小于阀值，则融合惯导数据，输出bslam轨迹，算法结束，否则进行鲁棒滤波，去除最差的闭环，直到全局一致性小于阀值。

步骤(4.1)：输入闭环数据：初始化，令k＝0，输入n个闭环，初始化open表和close表，将所有闭环放入open表中，清空close表；

步骤(4.2)：聚类并计算一致性函数：

将所有闭环聚类，把所有的类在局部优化和全局优化中计算一致性函数：

上式中，是从惯导数据所得的auv状态，是从bslam所得的auv状态，j是位姿的数量；

步骤(4.3)：判断是否满足连续性条件：设定一个阀值，判断χ²是否小于阀值，若小于阀值进入步骤(4.7)，否则进入步骤(4.4)；

步骤(4.4)：令k＝k+1，依次计算open表中所有类的一致性函数和最小值对在open表中的类i,i＝1,2,...,n，依次将类从open表中取出并计算找到最小值并把最小值对应的类从open表中移至close表中；

步骤(4.5)：如果小于则进入步骤(4.3)，否则进入步骤(4.6)；

步骤(4.6)：对在close表中的类j,j＝1,2,...,m，依次将类从close表中取出并计算j＝1,2,...,m；如果中的最小值小于则将最小值对应的类从close表中移至open表中并执行步骤(4.6)，否则进入步骤(4.4)；

步骤(4.7)：融合惯导的关键状态的数据并输出修改后的bslam轨迹。

与现有技术相比，本发明通过对测深数据和惯导数据的处理和闭环检测，实现了对数据的全局优化，保证了全局一致性，且提高了定位与建图的实时性和一致性，且鲁棒性好，能够减弱测量数据的噪声对导航精度的影响。

以上所述并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。