一種基于幾何統(tǒng)計特征的全局掃描匹配方法

3 等:一種基于幾何統(tǒng)計特征的全局掃描匹配方 1 掃描匹配是尋找不同位姿下傳感器掃描數(shù)據(jù)間的對應(yīng)關(guān)系估計相對位姿變換將掃描數(shù)據(jù)合并在一起恢復(fù)物體或者環(huán)境原貌的一類方法的總稱．掃描匹配技術(shù)的處理對象是利用測距儀的掃描點集合．由于掃描匹配具有同時建立環(huán)境模型和估計相對位姿的能力因此被引入到移動機器人定位與建圖的研究中并成為與基于濾波器(如擴展卡爾曼濾波)的定位方法以及基于理論(如和蒙)的定位方法并列的一類重要的移動機器人定位方法．該技術(shù)直接利用掃描的原始數(shù)據(jù)建圖結(jié)果更能體現(xiàn)環(huán)境原貌．目前機器人領(lǐng)域?qū)呙杵ヅ涞难芯咳找嫔钊肴〉靡幌盗杏袃r值的成果如基于圖的(hsndphM)］．根據(jù)是否依賴掃描間相對位姿的初始估計掃描匹配分為局部匹配和全局匹配．局部匹配方法中掃描的位姿估計通常來自于里程計在此基礎(chǔ)上迭代地尋找不同掃描間的對應(yīng)掃描點校正掃描間相對位姿直至收斂使掃描在一起．迭代最近鄰法(Ieoet/CoeponngntP)］和正態(tài)分布轉(zhuǎn)換法(oluonaoan，T)］是此類方法的代表．局部匹配方法在靜態(tài)環(huán)境且位姿估計誤差較小時取得理想結(jié)果但當(dāng)環(huán)境變化或者位姿估計誤差較大時無法保證收斂到全局最優(yōu)解這種情況下則需要全局匹配方法．全局匹配方法大都從掃描數(shù)據(jù)中提取特征通過特征匹配求解相對位姿．文獻為每個掃描點建立局部坐標(biāo)系利用變換在掃描點周圍提取構(gòu)成特征．這種特征易受到噪聲掃描點觀測位姿及運動物體的影響．采用兩種直方圖描述整個掃描，利用互相關(guān)操作獲得掃描間相對位姿但是直方圖描述受到區(qū)域大小及環(huán)境變化的影響．文獻［6］利用累積角度函數(shù)描述掃描的分割段并為分割段間確立相對位置關(guān)系該方法更能體現(xiàn)掃描的本文從特征定義匹配策略等角度研究提出適應(yīng)動態(tài)環(huán)境和噪聲數(shù)據(jù)擾動的全局掃描匹配方法．掃描點根據(jù)是否取自同一物體表面歸為一類即看作一個分割段．新方法從分割段和掃描點兩個層次定義特征利用掃描點法線方向的統(tǒng)計信息進行描述提高特征定義的魯棒性．分割征是對掃描數(shù)據(jù)整體的粗略描述．掃描點特征定義在其所在的分

割段上可以細(xì)致地刻畫分割段局部的幾何性質(zhì);由于與運動物體不屬于同一分割段因此掃描點特征不受周圍運動物體干擾．最終的匹配方法采用試探—驗證—求解的策略:在不同掃描間根據(jù)分割征建立分割段對應(yīng)關(guān)系利用對應(yīng)分割段上掃描點特征進行匹配得到試探解;依據(jù)評價法則驗證試探解的正確性;在試探解基礎(chǔ)上找到所有可對應(yīng)的分割段;利用所有對應(yī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)用不可替代．2分割征描在提取分割征之前需要對激光數(shù)據(jù)進行預(yù)處理包括分割和估計法線方向兩個步驟．分割是根據(jù)掃描點的位置分布將可能自同一物體的掃描點劃分為一個分割段．掃描點依托于分割段便擁有信息;分割段描述掃描點的整體分布．分割方法參照文獻7綜合考慮激光點之間的距離以及激光點連線與激光束之間夾角的關(guān)系，降低距離過遠或者夾角過小時因為采點稀疏所帶來的影響．掃描點數(shù)過少的分割段在該步驟被過濾．這些分割段代表的環(huán)境信息少有些則是錯誤的激光讀數(shù)或者動態(tài)物體．分割之后對于每個掃描點P兩側(cè)一定范圍內(nèi)的掃描點采用線性回歸的方法估計回歸直線將垂線方向作為該掃描點P的法線方向．法線方向經(jīng)變換后均指向傳感器．后文提及的法線方向與此一致．2．2分割分割段形狀各異為統(tǒng)一描述和比較本文定義分割段S的特征FS如下:SδSS表示凹平凸)由于不同區(qū)域掃描點疏密程度不同在統(tǒng)計δαS密度．分割段可以呈現(xiàn)多次凹凸變 24的形態(tài)在此分別統(tǒng)計分割段整體及左半側(cè)區(qū)域 例，

δα－C

S2平均法線方向據(jù)此獲知分割段整體的凹凸性SS以上特征定義實際是將復(fù)雜的分割段簡化為線段或者圓?。僭O(shè)法線方向滿足一致分布當(dāng)δα≠0時分割段對應(yīng)一段長度LS的圓弧圓心角范圍S

1異．相似度實際描述了特征線段或者特征圓弧之間SF越大相似度越高． S(－3δα3δα)C決定開口方向 當(dāng)分割段對應(yīng) 掃描點特征描 SS周時圓心角取得最大范圍(π，πδαS極值π3當(dāng)δα=0時分割段對應(yīng)一條長度L 3 線段

3通常越長越不規(guī)則的分割段越容易與其他分割對匹配的貢獻越大．為描述不同分割段可DS:若分割征表示為圓弧DS表示如圖1所示的陰影面積即將特征圓弧圓心置于縱軸使圓弧．

確匹配還需要更好地刻畫掃描數(shù)據(jù)局部的性質(zhì)此本文引入分割段S上掃描點P的特征FP．如圖2所示曲線代表分割段P代表掃描點箭頭代表掃描點的法線方向．以點P為原點法線方向αP為橫軸建立局部坐標(biāo)系．沿點P處切線向兩側(cè)依次等距離劃分劃分長度為L每段記為Si掃描點關(guān)聯(lián)微段將S上的點依次投影到對應(yīng)微段Si中直至S的端點．Si中統(tǒng)計的是S上連續(xù)的一段即如果S投影的過程中由于分割段彎曲而投影到了之前統(tǒng)計結(jié)束的微段上則提前終止投影過程．由此點P的特征FP記為F=(LUB(l，r){'i=l/L…r/L}) 圖 分割段(特征圓弧)可辨別ig eget(featurearc)discernibleSDS越大表示分割段可辨別度越高．由于激光讀數(shù)存在誤差真實線段對應(yīng)的分割段的δα通常不為0，表示為圓心角很小的圓弧．假設(shè)激光讀數(shù)誤差滿足S 定義任意分割段S1和S2間的相似度SF為

其中，UB(l，r)表示投影距離原點P最遠的左右邊界{'ii=l/L…r/L}是所有微段Si特，P的集合掃描點關(guān)聯(lián)的微段數(shù)目為r/L－l/L．根，P圖 ig2Scanpoint S=R·R

掃描點關(guān)聯(lián)微段Si

1+Cδα－C

息的分割段其特征'其中，1， =0RL=L

S S 12 = (L，Uα(μα–3δαμα+3δα)C，UH(td))

槡 槡 L

，Lmax≠

，

=min，δmax≠

其中LS表示微段長度 Hi i

表示投影的掃描點到點P LminmLSLS}，LmaxmaxLSLS 角，δα表示偏角的方差．與分割征一致假設(shè)微 RL表示分割段長度比例，Rδ表示圓心角范圍 3 等:一種基于幾何統(tǒng)計特征的全局掃描匹配方 或者圓弧，Uα表示圓心角范圍局部坐標(biāo)系下的位置 step2 初始化．初始化所有分割征和掃由S和UH決定開口方向由μα和C決定． 描點特征以及分割段對應(yīng)列表LS． 分割征

可以看作關(guān)聯(lián)微征F

step 挑選．從 中依次選取對應(yīng)分割 FS(LS，δαC iFL，Uα(－3δα，3δα)C，) step 試探．從PS中選取掃描點 槡Si槡 '掃描點時描述不同微段S1和S2之間的相似程度:

初始化掃描點對應(yīng)列表LP;過濾對應(yīng)點對若剩余比例過小則轉(zhuǎn)至step3;否則代入求解函數(shù)E(T)中獲得分割段相對位姿SS'=R·R step 驗證TSLS1LS2 H1+ α－C

過正確匹配驗證通過則繼續(xù)至step5;否則轉(zhuǎn)到S1δ

S2δ其中，RLRδ與分割段相似度SF中的含義類似分別表示微段長度比例和圓心角范圍比例．RH表

stepstep 求解根據(jù)TS

LLS'

位姿并驗證驗證失敗則從LS'移除否則驗證是否與 T一致一致則繼續(xù)否則

移除最后LS' 成的．任意掃描點P1和P2將其局部坐標(biāo)系在一起則部分關(guān)聯(lián)微段對應(yīng)二者相匹配的程度MPFO'

若T比現(xiàn)有解優(yōu)則替代現(xiàn)有解轉(zhuǎn)至step3通常當(dāng)掃描數(shù)據(jù)的分割段越多時掃描點特征初始化的時間復(fù)雜度越低．分割段對應(yīng)列表LS包含了LS1和LS2上所有分割段對按如下方式初始化:點特征的跨度UB的比例．將相似度大于一定 記LS和LS中分割段個數(shù)分別為N和N;LS中P 值的對應(yīng)微段看作好的匹配則SF和OB分別表示好匹配微段的平均相似度和所占比例SF和OB刻畫掃描點特征的平均匹配情況而SF和OB則可以避免因分割段局部變化導(dǎo)致平均刻畫效果較差時為掃描點不匹配最好的匹配度描述為Mt=1

素被劃分為N1塊每塊N2對分割段是根據(jù)LS2中分割段可辨別度DS由高到低排序的而每塊上相同索引的元素是根據(jù)分割段相似度SF由高到底排序4. =cos(π·

ω(

–1)2)

4ω 掃描點對應(yīng)列表

∑ ∑i i

全局掃描匹配方法

根據(jù)法線方向的偏角過濾過濾錯誤的對應(yīng) 滿足分布通常錯誤對應(yīng)的偏角偏差較大P統(tǒng)計Pcor中對應(yīng)點對法線方向的偏角均值μΔα4.1

差δΔα對應(yīng)點對根據(jù)ω=VMSlg(1+DS)輸入不同觀測位姿下的掃描數(shù)據(jù)LS1

辨別度權(quán)值ωLS1的局部坐標(biāo)系下LS2位姿(x，y)代表二維位置θ代表偏轉(zhuǎn)角算法主要流程如下．step1預(yù)處理對掃描數(shù)據(jù)進行分割估計掃

掃描點匹配度越高對應(yīng)分割段的相似度可辨別度越高則貢獻越大．將偏角不滿足一定置信度的對應(yīng)對移除根據(jù)匹配度高低過濾過濾局部變化區(qū)域的 24局部變化區(qū)域的對應(yīng)點匹配度相對較低．假設(shè)同一分割段上正確對應(yīng)的掃描點的匹配度滿足分布統(tǒng)計所有對應(yīng)點匹配度的均值與方差保留匹配度高的對應(yīng)點對將匹配度低于一定置信度的對應(yīng)點對移除．迭代以上過程直至沒有對應(yīng)點對被移除將剩TS=rmn(E(T)

在一對多或者多對一的情況．然后將剩余點對代入式(1)中求解掃描間相對位姿變化T并驗證解．驗證中連續(xù)分割段上不能的區(qū)域則是局部發(fā)生變化的區(qū)域不能匹配的分割段則是移動的物體．取驗證結(jié)果值最大的解作為最終解．由于分割段的數(shù)量有限且有序試探過程是有限的若掃描完全不同則匹配總會失敗否則該方法總可以找到正確的匹配位置．→==ω

(R+(R+t－P)］2)，(→

5實驗結(jié)果 sinθ和t=(xy)–

據(jù)集進行這些數(shù)據(jù)均是移動機器人通過SICK是P1i變換到P2i位置的旋轉(zhuǎn)矩陣和平移矢量，ωi與前述過濾準(zhǔn)則中一致．求解TS實際表示縮小所有掃描點到對應(yīng)點切線的距離可以參照文獻［2］和文獻［8］進行求解．采用點到切線距離允許掃描點沿著切線方向平移．當(dāng)分割段不規(guī)則時解唯一若是線段則可以有無數(shù)解．由于真實激光存在誤差所有對應(yīng)點的切線方向是不同的當(dāng)對應(yīng)點對足夠多時在多線性約束下解唯一且匹配結(jié)果是線段4. 3節(jié)中的試探過程是通過分割段匹配獲取相對位姿若分割段對應(yīng)錯誤則勢必位姿誤差較大．如果新位姿下經(jīng)驗證滿足以下約束則說明匹配結(jié)果可以接受:掃描點對平均匹配誤差ET小于一定閾值VT大于一定可見區(qū)域中區(qū)域比例OT大于一定值例如平均匹配誤差最大05mm，至少一方有40%區(qū)域可被另一方看到可見區(qū)域中至少60%重疊驗證結(jié)果VT數(shù)值表示如下．

光傳感器得到的具體參數(shù)參照表1這里主要驗中方法的實現(xiàn)參照前文所述．若試探結(jié)束所有解1Table1ParaeersandexerintalresultsforMnuzlbertb000009．9．223970．J中所有掃描的位姿添加噪聲利用ICP方法與自己匹配驗證正確性．這樣環(huán)境成為靜止的且準(zhǔn)確位姿為000)．由于本文方法不依賴位姿估計因此不受位姿噪聲影響．根據(jù)新方法SV=cos(E)sin(V·T

)sin(O·π)

相似度掃描點匹配度總能取得最大值1試探結(jié)束后總可以取得準(zhǔn)確位姿此時完全可見且完全重 SS4. 若試探解TS通過驗證則在其基礎(chǔ)上根據(jù)分割 ．依次求

疊．本文在三個數(shù)據(jù)集上均將所有掃描與自我匹配，結(jié)果100%均獲得準(zhǔn)確位姿見表1．真實情況下每次激光的掃描點幾乎不可能一樣而且掃描數(shù)據(jù)受到觀測位姿影響還可能有動態(tài)區(qū)域．接下來本文將數(shù)據(jù)集中連續(xù)兩次的掃描進行匹配模擬機器人行走解Lcor中對應(yīng)分割段的相對位姿若求解位姿的 過程統(tǒng)計正確匹配的成功率結(jié)果如表1所示．度與試探解偏差較大則認(rèn)為不一致．這些分割常代表錯誤的對應(yīng)或者移動的物體．剩余分割段中

然幾乎所有相鄰掃描均能匹配成功仍然存在匹配失敗的情況．圖345中的環(huán)境輪廓均是連接激光掃描點構(gòu)成曲線代表機器人行走軌跡．圖3采用3 等:一種基于幾何統(tǒng)計特征的全局掃描匹配方 inuzJ．?dāng)?shù)據(jù)集描述機器人在房間內(nèi)的運動跟據(jù)匹配情況標(biāo)注部分區(qū)域可能為動態(tài)區(qū)域．圖4采用Fr079數(shù)據(jù)集機器人從房間走出并沿走廊探索但是在圖示中掃描1和2匹配失敗．5采用Albertb-la-ser數(shù)據(jù)集機器人在走廊里行動而方框內(nèi)區(qū)域是機器人通過門看到的不規(guī)則的一些室況．匹配結(jié)果的試探次數(shù)并統(tǒng)計平均結(jié)果如表1所示，inguezJ．為例平均有972個分割段則最多的試9722≈94次而最佳匹配結(jié)果平均出現(xiàn)在257次91次試探是浪費的．圖4中因為觀測位姿的不同掃描點2被分割成細(xì)小的分割段為匹配引入大量歧義并最終導(dǎo)致失?。趯嶋H應(yīng)用中為防止因方法失敗導(dǎo)致機器人無法矯正位姿可以適當(dāng)減小判斷共線的約束;

5401個掃描g．h另外可以利用里程計的讀數(shù)只要保證在一定范圍內(nèi)能夠找到正確匹配的掃描即可．方法適應(yīng)不規(guī)則環(huán)境但當(dāng)環(huán)境異常簡單或者過于紛亂時則容易失?。椒梢栽诤苌俚脑囂酱螖?shù)下找到最優(yōu)解其主要原因在于分割段對應(yīng)列表LS的初始化順序．在使匹配失敗也可以等待獲取更好的掃描進行匹配．本文方法在enmM15GHz電腦上實驗對于360個點的掃描平均每秒完成匹配3～4次1805～6次6結(jié)束語文中提出一種利用幾何信息統(tǒng)計結(jié)果定義特征的全局掃描匹配方法．通過分割掃描定義分割征簡化掃描的整體描述進而采用信息更加豐富的掃描點特征刻畫掃描局部性質(zhì)而不同掃描之間的相對位姿則通過試探驗證求解的策略獲得．方法首先通過分割段匹配獲得位姿估計的試探解然后驗證其正確性驗證成功則對應(yīng)所有的分割段利用對應(yīng)點特征求解掃描位姿．將通過驗證的結(jié)果最佳的解作為相對位姿的最終解．通過理論和實驗可以驗證所提出方法的有效性和正確性其可以較好地完成匹配任務(wù)矯正位姿并辨別環(huán)境中的動態(tài)變化區(qū)域．該方法兼顧了掃描整體和局部的性質(zhì)面對實驗中動態(tài)存在噪聲擾動的環(huán)境體現(xiàn)較好的魯棒性可應(yīng)用于靜態(tài)或者動態(tài)環(huán)境． ［1］BoannDElsebergJLingeannKetal．byConsistent3D withScanhg．bsandAutonomous 24tems2008，56(2):130［2］BelPJ，McKayND．AhdforRgittinof3D1992，14(2):239256［3］BbrP，StrasserWTheNoalDitibutinsanfom:ANewApproachtoLaserScanchng//ProcoftheIEEE/RSJInternationalConferenceonInligentRobsandSytes．LasVegas，USA，2003，Ⅲ:27432748［4］TomonoMAScanhngMhodUingudnInvariantSignatureforblLocalizationandMapBuilding//ProcoftheIEEEInternationalConferenceonbosandun．NewOlens，USA，2004Ⅰ:866871MsmglstheIEEEInternationalConferenceonbosandun．Ro

maItaly2007:4820［6］HoKL，．LoopCloueDeeconinSLAMbyCombiningViulandSpatialppne．sandAutonomousSs，2006，54(9):740749inTrackingwithLaserScannerinOutdoorSiStructuredEnnment//ProcoftheIEEE/RSJInternationalConferenceonInligentbsandSytes．SendiJapan2004Ⅰ:8893［8］CeniAAnICPntUingaPointToLineMetic//ProcofIEEE