[發明專利]一種BSLAM中地形匹配結果正確性判別方法

有效

• 申請號: 201710344524.6
• 申請日: 2017-05-16
• 公開（公告）號: CN107132521B
• 公開（公告）日: 2019-12-06
• 發明（設計）人: 李曄;馬騰;王汝鵬;叢正;貢雨森;姜言清;郭宏達;雷明;安力;何佳雨
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01S7/52
• 分類號: G01S7/52;G01S15/89;G06K9/62
• 代理公司: 暫無信息
• 代理人: 暫無信息

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。