一种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，若小于阈值进行自检，根据自检结果更新位姿图，算法结束，否则转入步骤二。

主程序步骤四和步骤五中，自检包括如下步骤：

步骤一：输入k个待判定地形匹配结果和w个错误的地形匹配结果，设定最大循环次数n，初始化i＝0；

步骤二：初始化j＝0，判断i是否大于n，若大于n则算法结束，输出待判定地形匹配结果和错误地形匹配，否则进行步骤三；

步骤三：判断j是否大于w，若大于w则转到步骤四，否则使用第j个错误地形匹配和所有待判定地形匹配更新位姿图，使用多窗口方法计算代价函数f(j)，j＝j+1，重复执行步骤三；

步骤四：判断f(j)中的最小值是否大于主程序当前迭代代价函数f，是的话算法结束，输出待判定地形匹配结果和错误地形匹配，否则将最小值对应的错误地形匹配转入待判定地形匹配结果，i＝i+1，转入步骤二；

主程序步骤三和自检步骤三中，多窗口方法计算代价函数包括以下步骤：

步骤一：输入包含了p个节点的位姿图和里程计数据，设定最大窗口数num，使用公式

f(1)＝0

求解一致性函数f，令f1＝f，设定阈值为将f按由小到大排列的95％位置处f的值，将小于阈值的f值置为0，初始化i＝0；

步骤二：判断i是否大于num，若大于则f＝f1，算法结束，否则执行步骤三；

步骤三：在未被其他窗口选择的区域内选取f的最大值所在节点p，初始化j＝p+1，判断f(j)是否大于0，若大于零则j取j+1和p中较大的数直到f(j)小于等于0或j小于p，否则i_right＝j；同理，初始化j＝p-1向左搜索，判断f(j)是否大于0，若大于零则j取j-1和1中较小的数直到f(j)小于等于0或j小于1，否则i_left＝j；判断

是否满足，若满足则执行

算法结束，否则返回步骤二。

附图说明

图1为主程序流程框图；

图2为自检流程框图；

图3为多窗口求解一致性函数流程框图。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1，主程序包括如下步骤：

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

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

步骤三：判断j是否大于待判定地形匹配结果个数，是的话转入步骤四，否则将第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，若小于阈值进行自检，根据自检结果更新位姿图，算法结束，否则转入步骤二。

结合图2，自检包括如下步骤：

步骤一：输入k个待判定地形匹配结果和w个错误的地形匹配结果，设定最大循环次数n，初始化i＝0；

步骤二：初始化j＝0，判断i是否大于n，若大于n则算法结束，输出待判定地形匹配结果和错误地形匹配，否则进行步骤三；

步骤三：判断j是否大于w，若大于w则转到步骤四，否则使用第j个错误地形匹配和所有待判定地形匹配更新位姿图，使用多窗口方法计算代价函数f(j)，j＝j+1，重复执行步骤三；步骤四：判断f(j)中的最小值是否大于主程序当前迭代代价函数f，是的话算法结束，输出待判定地形匹配结果和错误地形匹配，否则将最小值对应的错误地形匹配转入待判定地形匹配结果，i＝i+1，转入步骤二。

结合图3，多窗口方法计算代价函数包括以下步骤：

步骤一：输入包含了p个节点的位姿图和里程计数据，设定最大窗口数num，使用公式

f(1)＝0

求解一致性函数f，令f1＝f，设定阈值为将f按由小到大排列的95％位置处f的值，将小于阈值的f值置为0，初始化i＝0；

步骤二：判断i是否大于num，若大于则f＝f1，算法结束，否则执行步骤三；

步骤三：在未被其他窗口选择的区域内选取f的最大值所在节点p，初始化j＝p+1，判断f(j)是否大于0，若大于零则j取j+1和p中较大的数直到f(j)小于等于0或j小于p，否则i_right＝j；同理，初始化j＝p-1向左搜索，判断f(j)是否大于0，若大于零则j取j-1和1中较小的数直到f(j)小于等于0或j小于1，否则i_left＝j；判断

是否满足，若满足则执行

算法结束，否则返回步骤二。