[发明专利]一种BSLAM中地形匹配结果正确性判别方法

说明书

本发明公开的是一种BSLAM中地形匹配结果正确性判别方法，主要是一种用于AUV测深数据同步定位与建图(Bathymetric Simultaneous Localization and Mapping,BSLAM)中判断得到的地形匹配结果是否正确的判别方法。该方法只需要输入地形匹配数据和里程计数据，通过不断迭代计算一致性函数实现对地形匹配有效性的判别。在迭代中引入了自检和多窗口方法，其目的分别在于避免陷入局部最优和同时保证全局一致性和局部一致性。本发明的有益效果在于提出的一种BSLAM中地形匹配结果正确性判别方法不需要依赖于辅助设备，计算效率高，不会陷入局部最优且能够同时满足局部一致和全局一致性的要求。

技术领域

本发明涉及海底地形同步定位与建图领域，是一种BSLAM中地形匹配结果正确性判别方法。

背景技术

BSLAM是一种使用多波束声纳获取海底地形为信息源，从而实现同步定位与建图的技术，该方法通过地形匹配获取数据关联，从而实现了对位姿图的修正。但由于海底地形趋于平缓，且多波束测深数据是声学回波数据，受环境影响较大，UUV姿态、海流、混响等环境特征都会对原始数据产生影响，这会导致测深数据存在较大的噪声，因此出现匹配结果不准确的结果是很常见的。因此，需要一种能够判别地形匹配结果正确性的方法。本发明提出了一种BSLAM中地形匹配结果正确性判别方法，该方法的实现不依赖于其他辅助设备，只需要地形匹配数据和里程计数据，能对地形匹配的正确性进行准确的判别，有益于下一步修正位姿图工作的展开。

目前针对BSLAM中地形匹配结果的判别没有相关的方法，较为相似的是SLAM中对数据关联正确性的判别。

公开日为2014年7月2日，公开号为CN103901774A，发明名称为“高效鲁棒的基于多传感器的SLAM协调方法及系统”的专利申请，该方法需要依赖于多个传感器，仅仅通过简单的加权平均获得真正位姿，无法适用于BSLAM中单个传感器状态下出现错误数据关联结果的情况。

公开日为2015年8月19日，公开号为CN104850615A，发明名称为“一种基于g2o的SLAM后端优化算法方法”的专利申请，该方法提出了DCS1算法，该方法需要在后优化过程中增加开关变量，加大了计算开销，计算效率大大降低。

发明内容

本发明的目的在于提供一种BSLAM中地形匹配结果正确性判别方法，该方法只需要输入地形匹配数据和里程计数据，通过不断迭代计算一致性函数实现对地形匹配有效性的判别。在迭代中引入了自检和多窗口方法，其目的分别在于避免陷入局部最优和同时保证全局一致性和局部一致性。

本发明的有益效果在于提出的BSLAM中地形匹配结果正确性判别方法不需要依赖于辅助设备，计算效率高，不会陷入局部最优且能够同时满足局部一致和全局一致性的要求。

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

主程序包括如下步骤：

步骤一：输入m个待判定地形匹配结果和里程计数据，初始化位姿图，初始化错误地形匹配为空集，设定最大循环次数，初始化i＝0，F(0)为无穷大，进入步骤二；

步骤二：初始化j＝0，判断i是否大于n，若i大于n，算法结束，否则进入步骤三；

步骤三：判断j是否大于待判定地形匹配结果个数m，是的话转入步骤四，否则将第j个待判定结果暂时置为空集，更新位姿图并使用多窗口方法计算代价函数f(j),j＝j+1,重新执行步骤三；

步骤四：将f(j)中最小值对应的待判定地形匹配移至错误地形匹配，m＝m-1，判断f(j)中最小值是否小于上一次迭代代价F(i-1)，是的话执行自检并将输出的代价函数设为F(i)，否则将f(j)中最小值设为F(i)；

步骤五：判断F(i)是否小于阈值，阈值一般取300，若小于阈值进行自检，根据自检结果更新位姿图，算法结束，否则转入步骤二。