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

权利要求书

1.一种BSLAM中地形匹配结果正确性判别方法，其特征在于包含以下步骤：

步骤一：输入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.1：输入k个待判定地形匹配结果和w个错误的地形匹配结果，设定最大循环次数n，初始化i＝0；

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

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

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

所述的多窗口方法计算代价函数其特征包括以下步骤：

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

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

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

步骤3.3：在未被其他窗口选择的区域内选取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；判断

是否满足，若满足则执行

，

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