本發(fā)明涉及海底地形同步定位與建圖領(lǐng)域，是一種bslam中地形匹配結(jié)果正確性判別方法。

背景技術(shù)：

bslam是一種使用多波束聲納獲取海底地形為信息源，從而實(shí)現(xiàn)同步定位與建圖的技術(shù)，該方法通過地形匹配獲取數(shù)據(jù)關(guān)聯(lián)，從而實(shí)現(xiàn)了對(duì)位姿圖的修正。但由于海底地形趨于平緩，且多波束測(cè)深數(shù)據(jù)是聲學(xué)回波數(shù)據(jù)，受環(huán)境影響較大，uuv姿態(tài)、海流、混響等環(huán)境特征都會(huì)對(duì)原始數(shù)據(jù)產(chǎn)生影響，這會(huì)導(dǎo)致測(cè)深數(shù)據(jù)存在較大的噪聲，因此出現(xiàn)匹配結(jié)果不準(zhǔn)確的結(jié)果是很常見的。因此，需要一種能夠判別地形匹配結(jié)果正確性的方法。本發(fā)明提出了一種bslam中地形匹配結(jié)果正確性判別方法，該方法的實(shí)現(xiàn)不依賴于其他輔助設(shè)備，只需要地形匹配數(shù)據(jù)和里程計(jì)數(shù)據(jù)，能對(duì)地形匹配的正確性進(jìn)行準(zhǔn)確的判別，有益于下一步修正位姿圖工作的展開。

目前針對(duì)bslam中地形匹配結(jié)果的判別沒有相關(guān)的方法，較為相似的是slam中對(duì)數(shù)據(jù)關(guān)聯(lián)正確性的判別。

公開日為2014年7月2日，公開號(hào)為cn103901774a，發(fā)明名稱為“高效魯棒的基于多傳感器的slam協(xié)調(diào)方法及系統(tǒng)”的專利申請(qǐng)，該方法需要依賴于多個(gè)傳感器，僅僅通過簡(jiǎn)單的加權(quán)平均獲得真正位姿，無法適用于bslam中單個(gè)傳感器狀態(tài)下出現(xiàn)錯(cuò)誤數(shù)據(jù)關(guān)聯(lián)結(jié)果的情況。

公開日為2015年8月19日，公開號(hào)為cn104850615a，發(fā)明名稱為“一種基于g2o的slam后端優(yōu)化算法方法”的專利申請(qǐng)，該方法提出了dcs1算法，該方法需要在后優(yōu)化過程中增加開關(guān)變量，加大了計(jì)算開銷，計(jì)算效率大大降低。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種bslam中地形匹配結(jié)果正確性判別方法，該方法只需要輸入地形匹配數(shù)據(jù)和里程計(jì)數(shù)據(jù)，通過不斷迭代計(jì)算一致性函數(shù)實(shí)現(xiàn)對(duì)地形匹配有效性的判別。在迭代中引入了自檢和多窗口方法，其目的分別在于避免陷入局部最優(yōu)和同時(shí)保證全局一致性和局部一致性。

本發(fā)明的有益效果在于提出的bslam中地形匹配結(jié)果正確性判別方法不需要依賴于輔助設(shè)備，計(jì)算效率高，不會(huì)陷入局部最優(yōu)且能夠同時(shí)滿足局部一致和全局一致性的要求。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

主程序包括如下步驟：

步驟一：輸入m個(gè)待判定地形匹配結(jié)果和里程計(jì)數(shù)據(jù)，初始化位姿圖，初始化錯(cuò)誤地形匹配為空集，設(shè)定最大循環(huán)次數(shù)，初始化i＝0，f(0)為無窮大，進(jìn)入步驟二；

步驟二：初始化j＝0，判斷i是否大于n，若i大于n，算法結(jié)束，否則進(jìn)入步驟三；

步驟三：判斷j是否大于待判定地形匹配結(jié)果個(gè)數(shù)m，是的話轉(zhuǎn)入步驟四，否則將第j個(gè)待判定結(jié)果暫時(shí)置為空集，更新位姿圖并使用多窗口方法計(jì)算代價(jià)函數(shù)f(j),j＝j(luò)+1,重新執(zhí)行步驟三；

步驟四：將f(j)中最小值對(duì)應(yīng)的待判定地形匹配移至錯(cuò)誤地形匹配，

m＝m-1，判斷f(j)中最小值是否小于上一次迭代代價(jià)f(i-1)，是的話執(zhí)行自檢并將輸出的代價(jià)函數(shù)設(shè)為f(i)，否則將f(j)中最小值設(shè)為f(i)；

步驟五：判斷f(i)是否小于閾值，閾值一般取300，若小于閾值進(jìn)行自檢，根據(jù)自檢結(jié)果更新位姿圖，算法結(jié)束，否則轉(zhuǎn)入步驟二。

主程序步驟四和步驟五中，自檢包括如下步驟：

步驟一：輸入k個(gè)待判定地形匹配結(jié)果和w個(gè)錯(cuò)誤的地形匹配結(jié)果，設(shè)定最大循環(huán)次數(shù)n，初始化i＝0；

步驟二：初始化j＝0，判斷i是否大于n，若大于n則算法結(jié)束，輸出待判定地形匹配結(jié)果和錯(cuò)誤地形匹配，否則進(jìn)行步驟三；

步驟三：判斷j是否大于w，若大于w則轉(zhuǎn)到步驟四，否則使用第j個(gè)錯(cuò)誤地形匹配和所有待判定地形匹配更新位姿圖，使用多窗口方法計(jì)算代價(jià)函數(shù)f(j)，j＝j(luò)+1，重復(fù)執(zhí)行步驟三；

步驟四：判斷f(j)中的最小值是否大于主程序當(dāng)前迭代代價(jià)函數(shù)f，是的話算法結(jié)束，輸出待判定地形匹配結(jié)果和錯(cuò)誤地形匹配，否則將最小值對(duì)應(yīng)的錯(cuò)誤地形匹配轉(zhuǎn)入待判定地形匹配結(jié)果，i＝i+1，轉(zhuǎn)入步驟二；

主程序步驟三和自檢步驟三中，多窗口方法計(jì)算代價(jià)函數(shù)包括以下步驟：

步驟一：輸入包含了p個(gè)節(jié)點(diǎn)的位姿圖和里程計(jì)數(shù)據(jù)，設(shè)定最大窗口數(shù)num，使用公式

f(1)＝0

求解一致性函數(shù)f，令f1＝f，設(shè)定閾值為將f按由小到大排列的95％位置處f的值，將小于閾值的f值置為0，初始化i＝0；

步驟二：判斷i是否大于num，若大于則f＝f1，算法結(jié)束，否則執(zhí)行步驟三；

步驟三：在未被其他窗口選擇的區(qū)域內(nèi)選取f的最大值所在節(jié)點(diǎn)p，初始化j＝p+1，判斷f(j)是否大于0，若大于零則j取j+1和p中較大的數(shù)直到f(j)小于等于0或j小于p，否則i_right＝j(luò)；同理，初始化j＝p-1向左搜索，判斷f(j)是否大于0，若大于零則j取j-1和1中較小的數(shù)直到f(j)小于等于0或j小于1，否則i_left＝j(luò)；判斷

是否滿足，若滿足則執(zhí)行

算法結(jié)束，否則返回步驟二。

附圖說明

圖1為主程序流程框圖；

圖2為自檢流程框圖；

圖3為多窗口求解一致性函數(shù)流程框圖。

具體實(shí)施方式

下面結(jié)合附圖舉例對(duì)本發(fā)明做更詳細(xì)地描述：

結(jié)合圖1，主程序包括如下步驟：

步驟一：輸入m個(gè)待判定地形匹配結(jié)果和里程計(jì)數(shù)據(jù)，初始化位姿圖，初始化錯(cuò)誤地形匹配為空集，設(shè)定最大循環(huán)次數(shù)，初始化i＝0，f(0)為無窮大，進(jìn)入步驟二；

步驟二：初始化j＝0，判斷i是否大于n，若i大于n，算法結(jié)束，否則進(jìn)入步驟三；

步驟三：判斷j是否大于待判定地形匹配結(jié)果個(gè)數(shù)，是的話轉(zhuǎn)入步驟四，否則將第j個(gè)待判定結(jié)果暫時(shí)置為空集，更新位姿圖并使用多窗口方法計(jì)算代價(jià)函數(shù)f(j),j＝j(luò)+1,重新執(zhí)行步驟三；

步驟四：將f(j)中最小值對(duì)應(yīng)的待判定地形匹配移至錯(cuò)誤地形匹配，m＝m-1，判斷f(j)中最小值是否小于上一次迭代代價(jià)f(i-1)，是的話執(zhí)行自檢并將輸出的代價(jià)函數(shù)設(shè)為f(i)，否則將f(j)中最小值設(shè)為f(i)；

步驟五：判斷f(i)是否小于閾值，閾值一般取300，若小于閾值進(jìn)行自檢，根據(jù)自檢結(jié)果更新位姿圖，算法結(jié)束，否則轉(zhuǎn)入步驟二。

結(jié)合圖2，自檢包括如下步驟：

步驟一：輸入k個(gè)待判定地形匹配結(jié)果和w個(gè)錯(cuò)誤的地形匹配結(jié)果，設(shè)定最大循環(huán)次數(shù)n，初始化i＝0；

步驟二：初始化j＝0，判斷i是否大于n，若大于n則算法結(jié)束，輸出待判定地形匹配結(jié)果和錯(cuò)誤地形匹配，否則進(jìn)行步驟三；

步驟三：判斷j是否大于w，若大于w則轉(zhuǎn)到步驟四，否則使用第j個(gè)錯(cuò)誤地形匹配和所有待判定地形匹配更新位姿圖，使用多窗口方法計(jì)算代價(jià)函數(shù)f(j)，j＝j(luò)+1，重復(fù)執(zhí)行步驟三；步驟四：判斷f(j)中的最小值是否大于主程序當(dāng)前迭代代價(jià)函數(shù)f，是的話算法結(jié)束，輸出待判定地形匹配結(jié)果和錯(cuò)誤地形匹配，否則將最小值對(duì)應(yīng)的錯(cuò)誤地形匹配轉(zhuǎn)入待判定地形匹配結(jié)果，i＝i+1，轉(zhuǎn)入步驟二。

結(jié)合圖3，多窗口方法計(jì)算代價(jià)函數(shù)包括以下步驟：

步驟一：輸入包含了p個(gè)節(jié)點(diǎn)的位姿圖和里程計(jì)數(shù)據(jù)，設(shè)定最大窗口數(shù)num，使用公式

f(1)＝0

求解一致性函數(shù)f，令f1＝f，設(shè)定閾值為將f按由小到大排列的95％位置處f的值，將小于閾值的f值置為0，初始化i＝0；

步驟二：判斷i是否大于num，若大于則f＝f1，算法結(jié)束，否則執(zhí)行步驟三；

步驟三：在未被其他窗口選擇的區(qū)域內(nèi)選取f的最大值所在節(jié)點(diǎn)p，初始化j＝p+1，判斷f(j)是否大于0，若大于零則j取j+1和p中較大的數(shù)直到f(j)小于等于0或j小于p，否則i_right＝j(luò)；同理，初始化j＝p-1向左搜索，判斷f(j)是否大于0，若大于零則j取j-1和1中較小的數(shù)直到f(j)小于等于0或j小于1，否則i_left＝j(luò)；判斷

是否滿足，若滿足則執(zhí)行

算法結(jié)束，否則返回步驟二。