專利名稱：一種基于幾何統(tǒng)計特征的全局掃描匹配方法
技術(shù)領(lǐng)域：
本發(fā)明涉及智能移動機(jī)器人領(lǐng)域，主要用于移動機(jī)器人在未知動態(tài)環(huán)境中定位與 建模，但方法本身不限于此。掃描匹配方法的研究和應(yīng)用涉及醫(yī)療、考古、救援、軍事、虛擬 現(xiàn)實、移動機(jī)器人等諸多領(lǐng)域。重現(xiàn)非結(jié)構(gòu)化未知環(huán)境的原貌，無疑對完成救災(zāi)、勘察等困 難危險的任務(wù)有很大的幫助。而從掃描匹配研究中衍生出的對于靜態(tài)、動態(tài)、非剛性物體識 別的研究，也具有較大的應(yīng)用價值。區(qū)別于醫(yī)療、考古等研究中對于物體模型高精確性再現(xiàn) 的需要，移動機(jī)器人研究對掃描匹配的實時性提出了更高的要求，以保證機(jī)器人可以準(zhǔn)確 快速地完成既定任務(wù)，如災(zāi)后救援。掃描匹配結(jié)果的好壞直接體現(xiàn)了機(jī)器人在三維環(huán)境中 定位的準(zhǔn)確性。
背景技術(shù)：
掃描匹配是尋找不同位姿下傳感器掃描數(shù)據(jù)間的對應(yīng)關(guān)系，估計相對位姿變換， 將掃描數(shù)據(jù)合并在一起，恢復(fù)物體或者環(huán)境原貌的一類方法的總稱。掃描匹配技術(shù)的處理 對象是利用測距儀采集的掃描點集合。由于掃描匹配具有同時建立環(huán)境模型和估計相對位 姿的能力，它被引入到移動機(jī)器人定位與建圖的研究中，并成為與基于濾波器(如擴(kuò)展卡 爾曼濾波)的定位方法，以及基于貝葉斯理論(如馬爾科夫和蒙特卡洛)的定位方法并列 的一類重要的移動機(jī)器人定位方法。該技術(shù)直接利用掃描的原始數(shù)據(jù)，建圖結(jié)果更能體現(xiàn) 環(huán)境原貌。目前機(jī)器人領(lǐng)域?qū)呙杵ヅ涞难芯咳找嫔钊耄〉昧艘幌盗杏袃r值的成果，如基 于圖的同時定位與建圖方法(Graph-SLAM)等。根據(jù)是否依賴掃描間相對位姿的初始估計，掃描匹配分為局部匹配和全局匹配。 局部匹配方法的位姿估計通常來自于里程計，在此基礎(chǔ)上迭代地尋找對應(yīng)掃描點，校正相 對位姿直至收斂使掃描重疊在一起。ICP(IterativeClosest/Corresponding Point)和 NDT(Normal Distribution Transformation)是此類方法的代表。局部匹配方法在靜態(tài)環(huán) 境且位姿估計誤差較小時取得理想結(jié)果，當(dāng)環(huán)境變化或者位姿估計誤差較大時則不能保證 收斂到全局最優(yōu)解。這種情況下則需要全局匹配方法。全局匹配方法大都從掃描數(shù)據(jù)中提 取特征，通過特征匹配求解位姿。比如有的方法為每個掃描點建立局部坐標(biāo)系，利用霍夫變 換在掃描點周圍提取霍夫點構(gòu)成特征。這種特征易受到噪聲掃描點、觀測位姿及運動物體 的影響?；蛘哂械姆椒ú捎脙煞N直方圖描述整個掃描，利用互相關(guān)操作獲得掃描間相對位 姿，但是受到重疊區(qū)域大小及環(huán)境變化的影響。還有方法利用累積角度函數(shù)描述掃描的分 割段，并為分割段間建立位置關(guān)聯(lián)。該方法更能體現(xiàn)掃描的局部特性，但是位置關(guān)聯(lián)仍然易 受到觀測位姿和環(huán)境變化的影響。

發(fā)明內(nèi)容
本發(fā)明從特征定義、匹配策略等角度提出了魯棒性更高，適應(yīng)動態(tài)環(huán)境的全局匹 配方法。掃描點根據(jù)是否取自同一物體表面歸為一類，即看作一個分割段，掃描點集合便由 若干分割段構(gòu)成。新方法從分割段和掃描點兩個層次定義特征，利用掃描點法線方向的統(tǒng)計信息進(jìn)行描述，提高特征定義的魯棒性。分割段特征是對掃描數(shù)據(jù)整體的粗略描述。掃描 點特征定義在其所在的分割段上，不受周圍運動物體干擾，可以細(xì)致地刻畫分割段局部的 性質(zhì)。最終的全局掃描匹配方法采用“試探-驗證-求解”的策略。根據(jù)分割段的可辨別度 和相似度建立對應(yīng)關(guān)系，利用掃描點特征相似度進(jìn)行匹配得到試探解；依據(jù)評價法則驗證 試探解的正確性；在試探解基礎(chǔ)上找到所有可對應(yīng)的分割段并求解掃描間的相對位姿。已 經(jīng)在多個室內(nèi)未知環(huán)境真實激光數(shù)據(jù)集上通過實驗驗證了方法的有效性。新方法可以正確 完成匹配，校正位姿，而動態(tài)物體和變化區(qū)域則在特征匹配的過程中被標(biāo)定。新方法可應(yīng)用 于靜態(tài)或動態(tài)環(huán)境，在定位誤差累積較大時可輔助發(fā)現(xiàn)閉環(huán)。雖然全局匹配方法效率略低 于局部匹配方法，但在動態(tài)環(huán)境及閉環(huán)上的應(yīng)用不可替代。
四

圖1掃描數(shù)據(jù)預(yù)處理前(a)后(b)示意圖。在該數(shù)據(jù)集中每次激光掃描采集180° 范圍共361個點。(a)圖為預(yù)處理前的情況，預(yù)處理后(b)同一分割段上的掃描點通過線段 連接在一起，法線方向通過短線段表示。圖2分割段(特征圓弧)可辨別度示意圖。該圖反映了使用特征圓弧表示的不規(guī) 則分割段，陰影區(qū)域表示分割段的可辨別度大小。圖3掃描點特征抽取示意圖。該圖描述了掃描點P的特征如何定義，α表示其法 線方向，L表示掃描點關(guān)聯(lián)微段劃分的長度，1和r分別表示投影的左右邊界，分割段上掃描 點的法線方向使用有向線段表示。圖4兩次掃描分別運用里程計(a)和新方法(b)匹配的結(jié)果，圖中圓點代表激光 掃描點。位置1對應(yīng)掃描1，位置2對應(yīng)掃描2。(a)圖反映出里程計誤差較大，掃描不一 致；(b)圖反映出利用新方法可以完好地完成匹配，矯正位姿。(c)圖放大了墻角處運用新 方法進(jìn)行匹配的結(jié)果。圖5MingUeZ J.數(shù)據(jù)集上連續(xù)匹配149個掃描建立環(huán)境平面圖。其中矩形內(nèi)為標(biāo) 記的動態(tài)區(qū)域。圖中標(biāo)注了機(jī)器人運行的軌跡，以及通過新方法發(fā)現(xiàn)的環(huán)境中可能動態(tài)變 化的區(qū)域。圖6Fr079數(shù)據(jù)集上連續(xù)匹配1525個掃描建立環(huán)境平面圖。圖中1和2標(biāo)注了兩 個墻角。圖7Albert-b-laSer數(shù)據(jù)集上連續(xù)匹配401個掃描建立環(huán)境平面圖。圖中矩形框 內(nèi)為不規(guī)則區(qū)域，標(biāo)記了環(huán)境中的動態(tài)區(qū)域。
五具體實施例方式5· 1 ^>害1] 牛寺 11 (Segment feature description)5.1.1 預(yù)處理在提取分割段特征之前需要對激光數(shù)據(jù)進(jìn)行預(yù)處理，包括分割和估計法線方向兩 個步驟。分割是根據(jù)掃描點的位置分布將可能采集自同一物體的掃描點劃分為一個分割 段。分割段描述了掃描點的整體分布；掃描點依托于分割段便擁有了更多信息。分割方法綜合考慮激光點之間的距離，以及激光點連線與激光束之間夾角的關(guān) 系，降低角度過小或者距離過遠(yuǎn)時因為采點稀疏所帶來的影響。掃描點數(shù)過少的分割段在 該步驟被過濾。這些分割段代表的環(huán)境信息少，有些則是錯誤的激光讀數(shù)或者動態(tài)物體。
分割之后對于每個掃描點P兩側(cè)一定范圍內(nèi)的掃描點，采用線性回歸的方法估計 回歸直線，將垂線方向作為該掃描點P的法線方向。法線方向經(jīng)變換后均指向傳感器。后 文提及的法線方向與此一致。圖1顯示了預(yù)處理前后的結(jié)果，同一分割段上的掃描點通過 線段連接在一起。5. 1.2分割段特征分割段形狀各異，為了統(tǒng)一描述和比較，本文定義分割段S的特征Fs如下Fs=(LsICs)(1)Ls表示順序連接分割段上所有掃描點的總長度，δ/表示掃描點法線方向變化 的方差，Cs e {-1，0，1}表示分割段整體的凹凸性(數(shù)值依次表示凹、平、凸)。由于不同區(qū) 域掃描點疏密程度不同，在統(tǒng)計5sa時根據(jù)掃描點兩側(cè)一定范圍內(nèi)掃描點的密度加權(quán)。分 割段可以呈現(xiàn)多次凹凸變化的形態(tài)，在此分別統(tǒng)計分割段整體及左半側(cè)區(qū)域的平均法線方 向，據(jù)此獲知分割段整體的凹凸性Cs。以上特征定義實際將復(fù)雜的分割段簡化為線段或者圓弧。假設(shè)法線方向滿足一致 分布，當(dāng)《#0時分割段對應(yīng)一段長度！^的圓弧，圓心角范圍、-忑5as,忑δ , Cs決定開口
方向。當(dāng)分割段對應(yīng)圓周時，圓心角取得最大范圍(-1 ^0，相應(yīng)的5sa取極值π/芯。當(dāng) Sas =0時，分割段對應(yīng)一條長度Ls的線段。5. 1. 3分割段可辨別度通常越長越不規(guī)則的分割段越容易與其他分割段相區(qū)別，對匹配的貢獻(xiàn)越大。為 了描述不同分割段可被辨別的程度，本文定義分割段S的可辨別度Ds為1)若分割段特征表示為線段，Ds為0 ；2)若分割段特征表示為圓弧，Ds表示如圖2所示的陰影面積，即將特征圓弧圓心 置于縱軸并與橫軸相切，圓弧與橫軸以及外接矩形所圍成的面積。Ds越大表示分割段可辨別度越高。由于激光讀數(shù)存在誤差，真實線段對應(yīng)的分割 段的5sa通常不為0，表示為圓心角很小的圓弧。假設(shè)激光讀數(shù)誤差滿足高斯白噪聲，則真 實線段越長越容易辨別。5. 1. 4分割段相似度為了在不同掃描上尋找相似的分割段進(jìn)行匹配，定義任意分割段S1* &間的相似 度$為 ⑵其中凡tAnaX^?！?，凡=LYmax^d ,L -min(Z L } ’
{L,mn/Anax ’ A x ^ 0[Αη η /^max ’ ^max 本 0^min _ mm ( , ‘ ^ }，
Anax = max{zv ZsJ ,Jmin,Jmax。相似度實際描述了特征線段或者
特征圓弧之間幾何形狀上的相似程度。&表示分割段長度比例，RS表示圓心角范圍重疊比 例，表示凹凸性及圓心角的總體差異。根據(jù)定義，
且$越大相似
度越高。5. 2(Scan point feature description)5. 2. 1掃描點特征
分割段特征是對掃描數(shù)據(jù)整體的粗略描述，精確匹配還需要更好地刻畫掃描數(shù)據(jù) 局部的性質(zhì)，為此本文引入分割段S上掃描點P的特征FP。如圖3所示，以點P為原點，法 線方向α ρ為橫軸建立局部坐標(biāo)系。沿點P處切線向兩側(cè)依次等距離劃分，每段記為Si稱 為“掃描點關(guān)聯(lián)微段”，將S上的點依次投影到對應(yīng)微段Si中直至S的端點。Si中統(tǒng)計的是 S上連續(xù)的一段，S卩如果S投影的過程中由于彎曲而投影到了之前統(tǒng)計結(jié)束的微段上則提 前終止投影過程。由此點P的特征Fp記為
FP=[L,UBP{l,r),{FSi \i = [llL\,...,\rlL-\))(3)L為劃分長度，UPB(1，r)表示投影距離原點P最遠(yuǎn)的左右邊界，
I / = [//1」，…，「r/Z]j是所有微段Si特征的集合，掃描點關(guān)聯(lián)的微段數(shù)目為「r/q-μ/Ζ」。根
據(jù)定義，掃描點特征不隨觀測位姿變化而改變。5. 2. 2掃描點關(guān)聯(lián)微段特征掃描點關(guān)聯(lián)微段Si實際是具有局部坐標(biāo)系信息的分割段，其特征 <記為F、=[LSj,Ul^l+S5as),CSi,U^{t,d))(4)、表示微段長度，&表示微段整體凹凸性，Us"表示掃描點投影垂直距離的分布， 表示微段內(nèi)掃描點相對α ρ的平均偏角，《表示偏角的方差。與分割段特征一致，假設(shè)微
段上法線方向滿足一致分布，關(guān)聯(lián)微段簡化為特征線段或者圓弧，表示圓心角范圍，其 位置由Si和V5"決定，開口方向由 <決定。分割段特征Fs可以看作關(guān)聯(lián)微段特征《的特殊形式
Fsi-(LsrSlXs)^ FSi =[LSi,Ul(-SdaSi,SSas),Csr0)(5)5. 2. 3掃描點關(guān)聯(lián)微段相似度與分割段相似度類似，微段相似度S' F是用于在匹配掃描點時描述不同微段S1* &之間的相似程度
_4] A=Wulc^1-CAi⑷&表示微段長度比例，Rs表示圓心角范圍重疊比例，&表示投影分布的重疊比例。 根據(jù)定義，S' Fe
且S' F越大相似度越高。5. 2. 4掃描點匹配度在本文提出方法中，精確匹配是通過掃描點匹配完成的。任意掃描點P1和P2，將 其局部坐標(biāo)系重疊在一起，則部分關(guān)聯(lián)微段對應(yīng)，二者相匹配的程度Mp表示如下Mp= (ηι; n2, n3' n4) = (sF，oB，s' F，o' B) (7)Sf表示對應(yīng)微段的平均相似度，Ob表示掃描點特征的跨度UpB的重疊比例。將相 似度大于一定閾值的對應(yīng)微段看作好的匹配，則S'B分別表示好匹配微段的平均 相似度和所占比例。$和％刻畫了掃描點特征的平均匹配情況，而S'B則可以避 免因分割段局部變化導(dǎo)致平均刻畫較差時，錯誤地認(rèn)為掃描點不匹配。最好的匹配度描述 為Mbest = (1,1,1,1),匹配度的數(shù)值表示如下
權(quán)利要求
1.一種基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于，步驟包括(1)激光傳感 器數(shù)據(jù)預(yù)處理；( 提取預(yù)處理后數(shù)據(jù)分割段的特征；C3)提取預(yù)處理后激光掃描點的特 征；⑷采用“試探-驗證-求解”的策略獲取不同掃描間相對位姿。其中步驟⑴在步驟 ⑵⑶⑷之前；步驟⑵⑶順序不限，可顛倒、可并行；步驟⑷在最后。
2.如權(quán)利要求1所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于其步驟(1)激光傳感器數(shù)據(jù)預(yù)處理中包含(1.1)激光數(shù)據(jù)分割和(1. 激光掃描點法線方向估計 兩步。
3.如權(quán)利要求1所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于其步驟(2)中數(shù)據(jù)分割段特征的定義符合以下三元組
4.如權(quán)利要求1所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于其步驟(3)中激光掃描點特征的定義符合以下三元組
5.如權(quán)利要求4所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于激光掃描 點特征定義三元組中·Kh.=L以」，...，「推1丨是所有掃描點關(guān)聯(lián)微段“特征的集合，其中掃描 點關(guān)聯(lián)微段特征的定義符合以下四元組
6.如權(quán)利要求1所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于其步驟(4)中“試探-驗證-求解”中試探過程為從不同掃描間選取一對分割段，根據(jù)分割段相似 度以及掃描點匹配度尋找對應(yīng)掃描點，利用最小二乘法或者牛頓方法求解分割段間相對位 姿作為試探解。
7.如權(quán)利要求6所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于其中的分 割段相似度按照以下方式計算
8.如權(quán)利要求6所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于其中的掃 描點匹配度按照以下方式計算Sf = Rh ■ Rl . Ra--j-rMp= (η” n2, η3，n4) = (SF, 0B, S' f, 0' b)r 4\ 4VmF =cos ？K"'-1)2 'Σω· =1V L /=1J i=l其中S' F表示掃描點關(guān)聯(lián)微段相似度，4和&2分別表示不同微段的凹凸性，&和\分 別表示法線偏角的方差，&表示投影分布的重疊比例，&表示微段長度比例，R5表示圓心 角范圍重疊比例。而四元組Mp描述了不同掃描間的匹配結(jié)果，艮表示對應(yīng)微段的平均相似 度，Ob表示掃描點特征的跨度U/的重疊比例，S'B分別表示好匹配微段的平均相似度和所占比例。則是最終掃描點匹配度的計算值。
9.如權(quán)利要求1所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于其步驟 (4)中“試探-驗證-求解”中驗證過程為根據(jù)一定的準(zhǔn)則判斷試探解正確與否以及解的好 壞，準(zhǔn)則包含以下三項中任意幾項(a)掃描點對平均匹配誤差艮小于一定閾值；(b)掃描 間互相可見區(qū)域的最大比例孓大于一定閾值；(c)可見區(qū)域中重疊區(qū)域比例&大于一定閾 值。結(jié)果評價按照下式計算
10.如權(quán)利要求1所述的基于幾何統(tǒng)計特征的全局掃描匹配方法，其特征在于其步驟 (4)中“試探-驗證-求解”中求解過程為在試探解的基礎(chǔ)上將不同掃描轉(zhuǎn)換到同一坐標(biāo) 系下，根據(jù)分割段覆蓋的角度范圍獲得對應(yīng)分割段，求解所有對應(yīng)分割段的相對位姿，過濾 其中求解結(jié)果與試探解不一致的對應(yīng)分割段，利用剩余對應(yīng)分割段上的對應(yīng)掃描點，利用 最小二乘法或者牛頓方法等求解掃描間相對位姿。將驗證結(jié)果評價最高的試探解作為最終 解。
全文摘要
本發(fā)明中基于幾何統(tǒng)計特征的全局掃描匹配方法是在研究如何同步完成移動機(jī)器人定位、環(huán)境輪廓刻畫、環(huán)境變化區(qū)域判別等任務(wù)時提出并實現(xiàn)的。方法除可用于移動機(jī)器人環(huán)境建模外，還可作為災(zāi)難救援、礦井勘察等特定任務(wù)的輔助工具。方法利用了激光掃描數(shù)據(jù)上幾何信息的統(tǒng)計結(jié)果定義特征，首先通過分割掃描，定義分割段特征簡化掃描整體描述，進(jìn)而采用掃描點特征刻畫掃描局部性質(zhì)，掃描間相對位姿則通過“試探-驗證-求解”的策略獲得匹配分割段獲得位姿試探解，驗證解正確性，驗證成功則利用對應(yīng)點特征及最小二乘法求解相對位姿。驗證最佳的解作為最終解。這樣方法刻畫了環(huán)境輪廓同時矯正了機(jī)器人位姿，環(huán)境變化則根據(jù)特征匹配結(jié)果進(jìn)行了辨別。
文檔編號G01C22/00GK102135620SQ20101003230
公開日2011年7月27日 申請日期2010年1月21日 優(yōu)先權(quán)日2010年1月21日
發(fā)明者郭瑞 申請人:郭瑞