專利名稱：一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法
技術(shù)領(lǐng)域：
本發(fā)明涉及到誤差估計(jì)領(lǐng)域，具體地說(shuō)，特別涉及到一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法。
背景技術(shù)：
移動(dòng)機(jī)器人導(dǎo)航廣泛應(yīng)用于外星球探索，采礦自動(dòng)化與安全，水下制導(dǎo)和深?？碧降阮I(lǐng)域。使用全球定位系統(tǒng)(GPQ對(duì)移動(dòng)機(jī)器人定位簡(jiǎn)單易行，但GPS信號(hào)會(huì)因植被、山脈和高樓的遮擋以及多路徑效應(yīng)而在很多區(qū)域精度變差甚至無(wú)法輸出結(jié)果，因此自包含的航位估測(cè)方法被應(yīng)用到移動(dòng)機(jī)器人定位中，通過(guò)匹配激光束是該類方法的經(jīng)典技術(shù)，方法是，將激光掃描傳感器安裝在移動(dòng)機(jī)器人前端，則依據(jù)激光測(cè)距原理，傳感器在每個(gè)采樣時(shí)刻觀測(cè)到的激光束由若干距離讀數(shù)構(gòu)成，每個(gè)讀數(shù)表征了從傳感器當(dāng)前方位到環(huán)境景物表面一個(gè)點(diǎn)的距離。通過(guò)極坐標(biāo)到笛卡爾坐標(biāo)的轉(zhuǎn)換，每個(gè)距離讀數(shù)都可等效成機(jī)器人中心坐標(biāo)系下的一個(gè)二維激光點(diǎn)。當(dāng)機(jī)器人的移動(dòng)的速度不太快或傳感器采樣頻率足夠高時(shí)， 相鄰兩采樣時(shí)刻(這里稱這兩個(gè)相鄰采樣時(shí)刻分別為新時(shí)刻、參考時(shí)刻，在這兩個(gè)時(shí)刻所觀測(cè)到的激光束分別稱為新激光束、參考激光束)機(jī)器人在相鄰兩方位處的視野會(huì)有部分重疊，即，新激光束中的一些激光點(diǎn)分別與參考激光束中的一些激光點(diǎn)兩兩對(duì)應(yīng)于實(shí)際環(huán)境景物表面的一些相同的物理點(diǎn)，那么，在全局空間中，對(duì)應(yīng)同一個(gè)物理點(diǎn)的激光點(diǎn)相關(guān)聯(lián)。搜尋新激光束和參考激光束中所包含激光點(diǎn)的關(guān)聯(lián)關(guān)系，進(jìn)而匹配這兩幀激光束，并基于此通過(guò)迭代優(yōu)化空間距離的誤差函數(shù)可求取移動(dòng)機(jī)器人相鄰時(shí)刻間的相對(duì)運(yùn)動(dòng)，但是， 當(dāng)把由此求取的相對(duì)運(yùn)動(dòng)與其它傳感器觀測(cè)(例如，GPS觀測(cè))相融合時(shí)，對(duì)激光束匹配誤差的合理估計(jì)是重要技術(shù)環(huán)節(jié)。經(jīng)對(duì)現(xiàn)有技術(shù)文獻(xiàn)的檢索發(fā)現(xiàn)，Ola Bengtsson等人在《Robotics and Autonomous Systems》2003,44 卷，29-40 頁(yè)上發(fā)表了 "Robot localization based on scan-matching__estimating the covariance matrix for the IDC algorithm" ( ((1/1^ 人和自動(dòng)化系統(tǒng)》，基于激光束匹配的機(jī)器人定位——估計(jì)IDC方法的協(xié)方差矩陣)。該文介紹了一種估計(jì)激光束匹配誤差的方法——離線法(off-line)，該方法能捕捉到當(dāng)機(jī)器人移動(dòng)過(guò)程中激光束匹配定位所產(chǎn)生的誤差，并使用協(xié)方差矩陣來(lái)表征這種誤差。這種激光束匹配誤差估計(jì)的方法存在一定問(wèn)題(a)該方法估計(jì)的僅是傳感器觀測(cè)誤差所引起的最終匹配誤差，而假設(shè)兩幀激光束中激光點(diǎn)之間的關(guān)聯(lián)是準(zhǔn)確的。事實(shí)上，對(duì)于現(xiàn)代的基于激光測(cè)距掃描原理的商用激光傳感器的精度有了顯著的改善，因此，與激光傳感器觀測(cè)噪聲所引起的最終激光束匹配誤差與激光點(diǎn)間誤關(guān)聯(lián)所引起的誤差相比基本可以忽略。引證文獻(xiàn)所提供的方法忽略了激光束匹配問(wèn)題中的主要誤差源。(b)基于簡(jiǎn)單的歐式距離尺度，并使用基于最小距離累加和的迭代優(yōu)化函數(shù)確定誤差，忽略了這種迭代優(yōu)化方法本身的局部最小現(xiàn)象，因此，該方法估計(jì)出的最終誤差是不全面的。綜上所述，針對(duì)現(xiàn)有技術(shù)的缺陷，特別需要一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法，以解決現(xiàn)有技術(shù)的不足。

發(fā)明內(nèi)容
本發(fā)明針對(duì)上述現(xiàn)有技術(shù)的不足和缺陷，提供了一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法，解決了背景技術(shù)中存在的不足，提高了激光束匹配誤差估計(jì)的可靠性和準(zhǔn)確性。本發(fā)明所解決的技術(shù)問(wèn)題可以采用以下技術(shù)方案來(lái)實(shí)現(xiàn)—種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法，其特征在于，所述方法包括如下步驟1)針對(duì)激光束匹配誤差估計(jì)問(wèn)題的特點(diǎn)構(gòu)建條件隨機(jī)場(chǎng)概率圖模型，此步驟僅需執(zhí)行一次；2)將移動(dòng)機(jī)器人在相鄰兩個(gè)采樣時(shí)刻測(cè)得的激光束作為一對(duì)；3)提取得到2、中所述激光對(duì)中激光點(diǎn)之間的局部特征和配對(duì)特征；4)如果具有現(xiàn)成的局部特征和配對(duì)特征的權(quán)重，則直接進(jìn)行下一步，否則先針對(duì)所構(gòu)建的條件隨機(jī)場(chǎng)概率圖模型進(jìn)行權(quán)重參數(shù)的學(xué)習(xí)，再進(jìn)行下一步；5)將相鄰時(shí)刻采集的激光束、局部特征和配對(duì)特征、局部特征和配對(duì)特征的相應(yīng)權(quán)重輸入至步驟1)中所構(gòu)建的條件隨機(jī)場(chǎng)，通過(guò)和積概率推理得到推理概率表；6)將幻中得到的推理概率表每一列中最大元素對(duì)應(yīng)孤立點(diǎn)狀態(tài)的列去除，僅保留可找到關(guān)聯(lián)關(guān)系的節(jié)點(diǎn)，相應(yīng)地演化出新的推理概率表；7)計(jì)算6)中新推理概率表中每一列的熵，并根據(jù)熵值的大小對(duì)節(jié)點(diǎn)再次選擇，得到緊湊節(jié)點(diǎn)集，相應(yīng)地演化出新的推理概率表；8)將7)中的推理概率表的每一個(gè)節(jié)點(diǎn)的狀態(tài)按照各自的推理概率降序排列，然后依據(jù)該節(jié)點(diǎn)所有可能關(guān)聯(lián)狀態(tài)推理概率的全局均值和局部均值，確定該節(jié)點(diǎn)的緊湊狀態(tài)集，相應(yīng)地，演化出新的推理概率表；9)利用步驟7)得到的緊湊節(jié)點(diǎn)集和步驟8)得到的緊湊狀態(tài)集，通過(guò)采樣，求取激光束匹配的協(xié)方差。在本發(fā)明的一個(gè)實(shí)施例中，所述對(duì)局部特征和配對(duì)特征的權(quán)重的學(xué)習(xí)過(guò)程包括如下步驟a.依據(jù)步驟2、采集相鄰的兩幀激光束，用手工的方法對(duì)它們所包含激光點(diǎn)之間的匹配關(guān)系進(jìn)行標(biāo)定；b.提取該相鄰兩幀激光束中激光點(diǎn)的局部特征和配對(duì)特征；c.運(yùn)行步驟a和步驟b 10次，得到10組激光束對(duì)和它們所對(duì)應(yīng)的10組標(biāo)定關(guān)聯(lián)關(guān)系、局部特征和配對(duì)特征；d.將步驟c中的結(jié)果輸入步驟1)所構(gòu)建的條件隨機(jī)場(chǎng)概率圖模型中，并使用最大偽似然學(xué)習(xí)法得到各種特征的最佳權(quán)重。在本發(fā)明的一個(gè)實(shí)施例中，求得所述緊湊節(jié)點(diǎn)集的具體步驟為①將去除了孤立點(diǎn)狀態(tài)的推理概率表的列依據(jù)節(jié)點(diǎn)的熵從大到小排列；②計(jì)算所有節(jié)點(diǎn)熵的全局熵均值和局部熵均值，并找到熵值最接近這些熵均值的節(jié)點(diǎn)，將這些節(jié)點(diǎn)連同具有最大熵和最小熵的節(jié)點(diǎn)作為η個(gè)基準(zhǔn)點(diǎn){氏} =1;③圍繞每個(gè)基準(zhǔn)點(diǎn)選取m個(gè)節(jié)點(diǎn)，所選取的mXn個(gè)節(jié)點(diǎn)組成緊湊的節(jié)點(diǎn)集N。。m。
在本發(fā)明的一個(gè)實(shí)施例中，所述求取激光束匹配的協(xié)方差的具體步驟為①在N。。m中，分別從圍繞η個(gè)節(jié)點(diǎn)基準(zhǔn)點(diǎn)的節(jié)點(diǎn)子集中選取η個(gè)節(jié)點(diǎn)，即從每一個(gè)子集中都隨機(jī)地選取一個(gè)節(jié)點(diǎn)；②對(duì)于選取的η個(gè)節(jié)點(diǎn)中的每一個(gè)節(jié)點(diǎn)，分別從圍繞g個(gè)狀態(tài)基準(zhǔn)點(diǎn)的狀態(tài)子集中選取g個(gè)狀態(tài)；③基于該η個(gè)節(jié)點(diǎn)及其各自的g個(gè)狀態(tài)，構(gòu)建gn個(gè)相鄰激光束中激光點(diǎn)關(guān)聯(lián)配置； 依據(jù)該η個(gè)節(jié)點(diǎn)的索引，從參考激光束中找出對(duì)應(yīng)的η個(gè)激光點(diǎn)的機(jī)器人中心坐標(biāo)系下的笛卡爾坐標(biāo)；依據(jù)步驟②的結(jié)果，對(duì)應(yīng)每個(gè)節(jié)點(diǎn)，重新在激光束中找出g個(gè)激光點(diǎn)的機(jī)器人中心坐標(biāo)系下的笛卡爾坐標(biāo)；④基于步驟③的結(jié)果，計(jì)算出gn組相對(duì)運(yùn)動(dòng)的解；⑤在構(gòu)建每條激光點(diǎn)關(guān)聯(lián)配置時(shí)，同時(shí)記錄相應(yīng)推理概率表中每個(gè)對(duì)應(yīng)元素的值，即所涉及節(jié)點(diǎn)的某個(gè)狀態(tài)的推理概率，然后將它們相乘便得要求的似然；關(guān)聯(lián)配置的似然作為每組相對(duì)運(yùn)動(dòng)解的權(quán)重，據(jù)此求取激光束匹配的加權(quán)協(xié)方差；⑥重復(fù)以上步驟30次。與現(xiàn)有技術(shù)相比，本發(fā)明具有如下有益效果1.考慮了激光束匹配中的主要誤差源，更全面地建模了激光束匹配中的誤差。所估計(jì)的激光束匹配誤差更符合真實(shí)情況。2.以機(jī)器學(xué)習(xí)的手段智能地管理并歸納激光點(diǎn)局部及全局的多重幾何特征，比基于簡(jiǎn)單歐式距離的同類方法(見(jiàn)引證背景文獻(xiàn))具有更嚴(yán)密的概率基礎(chǔ)。3.基于熵分級(jí)和推理似然分級(jí)限定了粒子采樣空間，有效地降低了方法執(zhí)行時(shí)間。


圖1為本發(fā)明所述方法的流程示意圖；圖2為本發(fā)明實(shí)施例中進(jìn)行實(shí)驗(yàn)驗(yàn)證的環(huán)境；圖3為本發(fā)明實(shí)施例中所構(gòu)建的條件隨機(jī)場(chǎng)概率圖模型示意圖；圖4為本發(fā)明實(shí)施例中求得的推理概率表示意圖；圖5為本發(fā)明實(shí)施例中求得的推理概率表的三維示意圖；圖6為本發(fā)明實(shí)施例中對(duì)推理概率表的節(jié)點(diǎn)選擇示意圖；圖7為本發(fā)明實(shí)施例中激光束匹配誤差估計(jì)的效果圖。
具體實(shí)施例方式為了使本發(fā)明實(shí)現(xiàn)的技術(shù)手段、創(chuàng)作特征、達(dá)成目的與功效易于明白了解，下面結(jié)合具體圖示，進(jìn)一步闡述本發(fā)明。本實(shí)施例中，將激光傳感器安裝于機(jī)器人平臺(tái)前端，并通過(guò)無(wú)線自組網(wǎng)遠(yuǎn)程遙控移動(dòng)機(jī)器人平臺(tái)的運(yùn)動(dòng)。激光傳感器的有效量程被設(shè)定為3. 5米，可視范圍為180度，每個(gè)采樣周期內(nèi)輸出361個(gè)激光測(cè)距值，可根據(jù)機(jī)器人當(dāng)前的運(yùn)動(dòng)方位將其等效為激光點(diǎn)在機(jī)器人中心坐標(biāo)系下的笛卡爾坐標(biāo)。實(shí)驗(yàn)在圖2所示的辦公室環(huán)境中進(jìn)行。本實(shí)施例包括如下步驟
第一步，針對(duì)激光束匹配誤差估計(jì)問(wèn)題的特點(diǎn)，構(gòu)建條件隨機(jī)場(chǎng)(CRF)概率圖模型，該步驟僅需運(yùn)行一次即可。本實(shí)施例中所構(gòu)建的鏈?zhǔn)綏l件隨機(jī)場(chǎng)圖模型示于圖3， 它只包含兩種勢(shì)函數(shù)局部勢(shì)(Local potential)禾口配對(duì)勢(shì)(Pairwise potential)。其中將參考激光束和新激光束所包含激光點(diǎn)作為觀測(cè)量z。{Llei, Lr2ef, Llil...，L。}表示參考激光束中的N個(gè)激光點(diǎn)。它們各自的可能的關(guān)聯(lián)狀態(tài)映射為條件隨機(jī)場(chǎng)圖中的節(jié)點(diǎn)。 {^Lw, Llew, L3new,..., 表示新激光束中的M個(gè)激光點(diǎn)。條件隨機(jī)場(chǎng)圖中的任一節(jié)點(diǎn)Xi 都對(duì)應(yīng)了 M+1種狀態(tài)的似然。例如，Xi的狀態(tài)1表示Xi所對(duì)應(yīng)的參考激光束中的激光點(diǎn)I^f 與新激光束的激光點(diǎn)ALw關(guān)聯(lián)的似然。Xi狀態(tài)2表示i^f與新激光束的激光點(diǎn)聯(lián)的似然，依次類推，狀態(tài)M+1表示I^f在新激光束中找不到相關(guān)聯(lián)的激光點(diǎn)的似然，即，grf是孤立點(diǎn)的似然。ΦΑ。表示局部勢(shì)，Φρ(Χ ，Χρ表示配對(duì)勢(shì)。它們基于局部和配對(duì)特征，及學(xué)習(xí)得到的相應(yīng)權(quán)重，從兩幀激光束的激光點(diǎn)在機(jī)器人中心坐標(biāo)系下的笛卡爾坐標(biāo)算得。第二步，將移動(dòng)機(jī)器人在相鄰兩個(gè)采樣時(shí)刻測(cè)得的激光束作為一對(duì)。相鄰兩激光束分別為參考激光束LMf和新激光束Lmw，每束激光束由所包含的激光點(diǎn)在機(jī)器人坐標(biāo)系下的笛卡爾坐標(biāo)組成。它們可進(jìn)一步表示為一Lref = I ^refa, )^refa] 丄,Lnew = | ^new1 ？ ^news, }卜丄其中參考激光束和新激光束分別包含N和M個(gè)激光點(diǎn)。第三步，提取第一步得到的相鄰時(shí)刻的激光束中激光點(diǎn)之間的局部特征和配對(duì)特征。本實(shí)施例中提取兩束激光束中激光點(diǎn)之間的距離、角、測(cè)地線和輻射等幾何特征作為局部特征，提取配對(duì)序列、配對(duì)孤立點(diǎn)和配對(duì)距離作為配對(duì)特征。第四步，如果已有現(xiàn)成的學(xué)習(xí)參數(shù)，則直接跳轉(zhuǎn)下一步；在第一次使用本方法進(jìn)行激光束匹配的誤差估計(jì)時(shí)，需通過(guò)學(xué)習(xí)確定所構(gòu)建條件隨機(jī)場(chǎng)中各種特征權(quán)重。該步驟僅需進(jìn)行一次即可，具體步驟是①.依據(jù)第二步，采集相鄰的兩幀激光束。用手工的方法標(biāo)定它們所包含激光點(diǎn)之間的匹配關(guān)系，可使用圖像等傳感器的觀測(cè)作為參考。②.依據(jù)第三步，提取該相鄰兩幀激光束中激光點(diǎn)的局部特征和配對(duì)特征。③.運(yùn)行①②10次，得到10組激光束對(duì)和它們所對(duì)應(yīng)的10組標(biāo)定關(guān)聯(lián)關(guān)系、局部特征和配對(duì)特征。④.將③中的結(jié)果輸入第一步所構(gòu)建的條件隨機(jī)場(chǎng)中，并使用最大偽似然 (Maximum pseudo-likelihood, MPL)方法學(xué)習(xí)得到各種特征的最佳權(quán)重。第五步，將第二步得到的相鄰兩幀激光束和第三步得到的局部特征和配對(duì)特征，以及學(xué)習(xí)得到的特征的權(quán)重輸入第一步所構(gòu)建的條件隨機(jī)場(chǎng)圖模型，通過(guò)和積 (Sum-product)概率推理得到推理概率表P。P的每一列元素值對(duì)應(yīng)了圖模型中相應(yīng)節(jié)點(diǎn)處于不同狀態(tài)的似然，即參考激光束中任一激光點(diǎn)與新激光束中所有激光點(diǎn)相關(guān)聯(lián)的似然。 圖4為推理概率表的示意圖(以參考激光束和新激光束中均包含4個(gè)激光點(diǎn)為例)，推理概率表中的一列表征了參考激光束中一個(gè)點(diǎn)的所有可能狀態(tài)關(guān)聯(lián)的概率。例如，圖中第一列的前四個(gè)元素分別表示參考激光束中的第一個(gè)激光點(diǎn)與新激光束中第一個(gè)、第二個(gè)、第三個(gè)、第四個(gè)激光點(diǎn)關(guān)聯(lián)的概率；第五個(gè)元素表示該參考激光點(diǎn)為孤立點(diǎn)(找不到關(guān)聯(lián))的概率。圖5是本實(shí)施例實(shí)測(cè)得到的推理概率表的三維可視化圖(兩激光束均包含240個(gè)激光點(diǎn))。第六步，將P中每一列中最大元素對(duì)應(yīng)孤立點(diǎn)狀態(tài)的列去除，即僅保留可找到關(guān)聯(lián)關(guān)系的節(jié)點(diǎn)集{Nin}，對(duì)應(yīng)地，整理P為Pp第七步，計(jì)算P1中每一列的熵，并依據(jù)熵值的大小對(duì)節(jié)點(diǎn)集{Nin}中的節(jié)點(diǎn)進(jìn)行選擇，得到緊湊節(jié)點(diǎn)集N。。m，具體步驟是①.將推理概率表P1中的列依據(jù)節(jié)點(diǎn)集{Nin}中節(jié)點(diǎn)的熵從大到小排列。相應(yīng)地， P1演變?yōu)镻2。②.計(jì)算所有節(jié)點(diǎn)熵的全局熵均值和局部熵均值，并找到熵值最接近這些熵均值的節(jié)點(diǎn)，將這些節(jié)點(diǎn)連同具有最大熵和最小熵的節(jié)點(diǎn)作為η個(gè)基準(zhǔn)點(diǎn){戧} =1。全局熵均值指用全部節(jié)點(diǎn)的熵求得的均值。局部熵均值指基于降序排列的節(jié)點(diǎn)的熵，在一個(gè)區(qū)間內(nèi)求得的熵均值。圖6顯示了多種局部熵均值，比如，接近全局熵均值的節(jié)點(diǎn)將全局區(qū)間分為兩個(gè)子區(qū)間從最大熵的節(jié)點(diǎn)到全局熵均值節(jié)點(diǎn)的區(qū)間、從全局熵均值節(jié)點(diǎn)到最小熵節(jié)點(diǎn)的區(qū)間。在這兩個(gè)區(qū)間內(nèi)可分別計(jì)算1/2局部熵均值。依次類推，可計(jì)算1/4局部熵均值，1/8 局部熵均值。在本實(shí)施例中η選取為5。③.圍繞每個(gè)基準(zhǔn)點(diǎn)Bi選取m個(gè)節(jié)點(diǎn)。所選取的mXn個(gè)節(jié)點(diǎn)組成緊湊的節(jié)點(diǎn)集， 它們可記為Nc。m =... {iV|n}r=1]，對(duì)應(yīng)Nram提取P2中的mXn個(gè)列組成新的推理概率表P3。在本實(shí)施例中m選取為5。第八步，將P3中每個(gè)節(jié)點(diǎn)的狀態(tài)按照各自的推理概率降序排列，然后通過(guò)計(jì)算推理概率的全局均值和局部均值確定g個(gè)基準(zhǔn)點(diǎn){刀}〖=1。相應(yīng)地，P3重新組合為P4，然后圍繞各基準(zhǔn)點(diǎn)選取h個(gè)狀態(tài)，并最終確定該節(jié)點(diǎn)的緊湊的狀態(tài)集。因每個(gè)節(jié)點(diǎn)均有各自的關(guān)聯(lián)狀態(tài)空間，所以狀態(tài)空間限定需對(duì)緊湊節(jié)點(diǎn)集N。。m中的所有節(jié)點(diǎn)依次操作，而且所對(duì)應(yīng)的節(jié)點(diǎn)的緊湊狀態(tài)集S。。m&表現(xiàn)為(mXn)X(hXg)矩陣，相應(yīng)的，推理概率表P4進(jìn)一步演化為 P5O在本實(shí)施例中g(shù)和h分別選取為3和5。第九步，通過(guò)采樣緊湊節(jié)點(diǎn)集N。。m和緊湊狀態(tài)集S。。m，求取激光束匹配的協(xié)方差，具體步驟是①.重復(fù)如下操作30次②.在N-中，分別從圍繞η個(gè)節(jié)點(diǎn)基準(zhǔn)點(diǎn)的節(jié)點(diǎn)子集中選取η個(gè)節(jié)點(diǎn)。S卩，從每一個(gè)子集中都隨機(jī)地選取一個(gè)節(jié)點(diǎn)。則該η個(gè)節(jié)點(diǎn)分別來(lái)自不同誤差等級(jí)的節(jié)點(diǎn)子集。③.對(duì)于選取的η個(gè)節(jié)點(diǎn)中的每一個(gè)，分別從圍繞g個(gè)狀態(tài)基準(zhǔn)點(diǎn)的狀態(tài)子集中選取g個(gè)狀態(tài)。④.基于該η個(gè)節(jié)點(diǎn)，及其各自的g個(gè)狀態(tài)，構(gòu)建gn個(gè)相鄰激光束中激光點(diǎn)關(guān)聯(lián)配置。依據(jù)該η個(gè)節(jié)點(diǎn)的索引，從參考激光束中找出對(duì)應(yīng)的η個(gè)激光點(diǎn)的機(jī)器人中心坐標(biāo)系下的笛卡爾坐標(biāo)。依據(jù)步驟③的結(jié)果，對(duì)應(yīng)每個(gè)節(jié)點(diǎn)，從新激光束中找出g個(gè)激光點(diǎn)的機(jī)器人中心坐標(biāo)系下的笛卡爾坐標(biāo)。⑤.基于步驟④的結(jié)果，計(jì)算出gn組相對(duì)運(yùn)動(dòng)的解。每組解RT = [R，T] (R代表機(jī)器人的航位角，T代表機(jī)器人的位移)可通過(guò)優(yōu)化下式來(lái)求得，
權(quán)利要求
1.一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法，其特征在于，所述方法包括如下步驟1)針對(duì)激光束匹配誤差估計(jì)問(wèn)題的特點(diǎn)構(gòu)建條件隨機(jī)場(chǎng)概率圖模型，此步驟僅需執(zhí)行一次；2)將移動(dòng)機(jī)器人在相鄰兩個(gè)采樣時(shí)刻測(cè)得的激光束作為一對(duì)；3)提取得到2、中所述激光對(duì)中激光點(diǎn)之間的局部特征和配對(duì)特征；4)如果具有現(xiàn)成的局部特征和配對(duì)特征的權(quán)重，則直接進(jìn)行下一步，否則先針對(duì)所構(gòu)建的條件隨機(jī)場(chǎng)概率圖模型進(jìn)行權(quán)重參數(shù)的學(xué)習(xí)，再進(jìn)行下一步；5)將相鄰時(shí)刻采集的激光束、局部特征和配對(duì)特征、局部特征和配對(duì)特征的相應(yīng)權(quán)重輸入至步驟1)中所構(gòu)建的條件隨機(jī)場(chǎng)，通過(guò)和積概率推理得到推理概率表P ；6)將P中每一列中最大元素對(duì)應(yīng)孤立點(diǎn)狀態(tài)的列去除，僅保留可找到關(guān)聯(lián)關(guān)系的節(jié)點(diǎn)集{Nin}，相應(yīng)地演化出新的推理概率表P1 ；7)計(jì)算P1中每一列的熵，并根據(jù)熵值的大小對(duì)節(jié)點(diǎn)集{Nin}的節(jié)點(diǎn)進(jìn)行選擇，得到緊湊節(jié)點(diǎn)集N。。m，相應(yīng)的，演化出新的推理概率表P3 ；8)將每一個(gè)節(jié)點(diǎn)的狀態(tài)按照各自的推理概率降序排列，然后通過(guò)計(jì)算推理概率的全局均值和局部均值，確定該節(jié)點(diǎn)的緊湊狀態(tài)集S。。m，相應(yīng)的，推理概率表進(jìn)一步演化為P4，隨后又演化為P5 ；9)利用步驟7)得到的緊湊節(jié)點(diǎn)集N。。m和步驟8)得到的緊湊狀態(tài)集S。。m，通過(guò)采樣，求取激光束匹配的協(xié)方差。
2.如權(quán)利要求1所述的一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法，其特征在于，所述對(duì)局部特征和配對(duì)特征的權(quán)重的學(xué)習(xí)過(guò)程包括如下步驟a.依據(jù)步驟幻采集相鄰的兩幀激光束，用手工的方法標(biāo)定它們所包含激光點(diǎn)之間的匹配關(guān)系；b.提取該相鄰兩幀激光束中激光點(diǎn)的局部特征和配對(duì)特征；c.運(yùn)行步驟a和步驟blO次，得到10組激光束對(duì)和它們所對(duì)應(yīng)的10組標(biāo)定關(guān)聯(lián)關(guān)系、 局部特征和配對(duì)特征；d.將步驟c中的結(jié)果輸入步驟1)所構(gòu)建的條件隨機(jī)場(chǎng)概率圖模型中，并使用最大偽似然學(xué)習(xí)法得到各種特征的最佳權(quán)重。
3.如權(quán)利要求1所述的一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法，其特征在于，求得所述緊湊節(jié)點(diǎn)集N。。m的具體步驟為(1)將推理概率表P1中的列依據(jù)節(jié)點(diǎn)集{Nin}中節(jié)點(diǎn)的熵從大到小排列，相應(yīng)地，P1演變?yōu)镻2 ；(2)計(jì)算所有節(jié)點(diǎn)熵的全局熵均值和局部熵均值，并找到熵值最接近這些熵均值的節(jié)點(diǎn)，將這些節(jié)點(diǎn)連同具有最大熵和最小熵的節(jié)點(diǎn)作為η個(gè)基準(zhǔn)點(diǎn){雙} =1;(3)圍繞每個(gè)基準(zhǔn)點(diǎn)選取m個(gè)節(jié)點(diǎn)，所選取的mXη個(gè)節(jié)點(diǎn)組成緊湊的節(jié)點(diǎn)集N。。m，對(duì)應(yīng) Ncoffl提取P2中的mXn個(gè)列組成新的推理概率表P3。
4.如權(quán)利要求1所述的一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法，其特征在于，所述求取激光束匹配的協(xié)方差的具體步驟為①在N。。m中，分別從圍繞η個(gè)節(jié)點(diǎn)基準(zhǔn)點(diǎn)的節(jié)點(diǎn)子集沖選取η個(gè)節(jié)點(diǎn)，即從每一個(gè)集中都隨機(jī)地選取一個(gè)節(jié)點(diǎn)；②對(duì)于選取的η個(gè)節(jié)點(diǎn)中的每一個(gè)節(jié)點(diǎn)，分別從圍繞g個(gè)狀態(tài)基準(zhǔn)點(diǎn)的狀態(tài)子集 {礙})=1中選取g個(gè)狀態(tài)；③基于該η個(gè)節(jié)點(diǎn)及其各自的g個(gè)狀態(tài)，構(gòu)建gn個(gè)相鄰激光束中激光點(diǎn)關(guān)聯(lián)配置；依據(jù)該η個(gè)節(jié)點(diǎn)的索引，從參考激光束中找出對(duì)應(yīng)的η個(gè)激光點(diǎn)的機(jī)器人中心坐標(biāo)系下的笛卡爾坐標(biāo)；依據(jù)步驟②的結(jié)果，對(duì)應(yīng)每個(gè)節(jié)點(diǎn)，重新在激光束中找出g個(gè)激光點(diǎn)的機(jī)器人中心坐標(biāo)系下的笛卡爾坐標(biāo)；④基于步驟③的結(jié)果，計(jì)算出相對(duì)運(yùn)動(dòng)的解；⑤在構(gòu)建每條激光點(diǎn)關(guān)聯(lián)配置時(shí)，同時(shí)記錄推理概率表P5中每個(gè)對(duì)應(yīng)元素的值，即所涉及節(jié)點(diǎn)的某個(gè)狀態(tài)的推理概率，然后將它們相乘便得要求的似然；關(guān)聯(lián)配置的似然作為每組相對(duì)運(yùn)動(dòng)解的權(quán)重，據(jù)此求取激光束匹配的加權(quán)協(xié)方差；⑥重復(fù)以上步驟30次。
全文摘要
本發(fā)明公開(kāi)了一種適用于移動(dòng)機(jī)器人導(dǎo)航的激光束匹配誤差估計(jì)方法，所述方法首先將移動(dòng)機(jī)器人在相鄰兩個(gè)采樣時(shí)刻測(cè)得的激光束作為一對(duì)，針對(duì)激光束匹配誤差特性構(gòu)建條件隨機(jī)場(chǎng)概率圖模型，并提取出所述激光束對(duì)的局部特征和配對(duì)特征，然后將所述特征及相應(yīng)權(quán)重輸入至所構(gòu)建的條件隨機(jī)場(chǎng)，通過(guò)和積概率推理得到推理概率表，最后根據(jù)推理概率表分別求得緊湊節(jié)點(diǎn)集和緊湊狀態(tài)集，并通過(guò)采樣上述緊湊節(jié)點(diǎn)集和緊湊狀態(tài)集求取表征激光束匹配誤差的協(xié)方差。本發(fā)明的優(yōu)點(diǎn)在于考慮了激光束匹配中的主要誤差源，以機(jī)器學(xué)習(xí)的手段智能地管理并歸納激光點(diǎn)局部及全局的多重幾何特征，有效地提高了對(duì)激光束匹配誤差的估計(jì)準(zhǔn)確度，同時(shí)降低了方法執(zhí)行時(shí)間。
文檔編號(hào)G06F19/00GK102338867SQ201110184609
公開(kāi)日2012年2月1日 申請(qǐng)日期2011年7月1日 優(yōu)先權(quán)日2011年7月1日
發(fā)明者孫作雷, 曾連蓀, 黃平平 申請(qǐng)人:上海海事大學(xué)