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

权利要求书

1.一种AUV地形匹配导航的鲁棒BSLAM方法，其特征在于：包含如下步骤：

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

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

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

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

所述的步骤(3)具体为：

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

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

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

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

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

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

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

上式中，(y_t,k-h_k(x_t))²表示状态x_t处k个测量点的校正值，N是采样点的个数，y_t,k表示采样点k的测量深度，h_k(x_t)表示历史子图中对应点的深度；

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

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

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

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

x₀＝[max(R_T(i)),min(h_T(i)),max(σ_T(i)),max(F_T(i))](i＝1,2,...,k)；

灰色关系系数为：

上式中，分辨率ρ∈[0,1]，R_T是地形粗糙度，H_T是地形熵，σ_T是地形标准差，F_T是Fisher信息；

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

上式中，w₁，w₂,w₃和w₄是Fisher信息F_T的权重；

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

2.根据权利要求1所述的一种AUV地形匹配导航的鲁棒BSLAM方法，其特征在于：所述的步骤(4)具体为：

步骤(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表中取出并计算如果中的最小值小于则将最小值对应的类从CLOSE表中移至OPEN表中并执行步骤(4.6)，否则进入步骤(4.4)；

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