茹淑慧, 王紅旗, 唐 浩

(河南理工大學(xué) 電氣工程與自動化學(xué)院，河南 焦作 454000)

0 引 言

近年來，同時定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)技術(shù)在機(jī)器人領(lǐng)域受到了廣泛的關(guān)注和應(yīng)用，被研究者普遍認(rèn)為是實現(xiàn)機(jī)器人完全自主的關(guān)鍵[1]。其中，最常用的掃描匹配技術(shù)是標(biāo)準(zhǔn)迭代最近點(iterative closest point,ICP)算法[2,3]。ICP算法通常使用相鄰兩幀掃描數(shù)據(jù)點集進(jìn)行匹配[4]，但是由于掃描數(shù)據(jù)點數(shù)量較大，導(dǎo)致該算法計算負(fù)荷較大，收斂速度較慢。許多學(xué)者對ICP算法進(jìn)行改進(jìn)，大都從掃描數(shù)據(jù)中提取特征，利用特征匹配來求解相對位姿。文獻(xiàn)[5]采用從掃描數(shù)據(jù)中提取特征點進(jìn)行ICP匹配，減少了匹配時間，提高了SLAM算法的快速性，但該方法適合于具有清晰的線段或角點特征的多邊形結(jié)構(gòu)化環(huán)境。文獻(xiàn)[6]采用了一種基于幾何統(tǒng)計特征的全局掃描匹配方法，利用“試探—驗證—求解”(probing verification solution,PVS)的匹配策略獲得掃描之間的相對位姿。文獻(xiàn)[7,8]采用多特征關(guān)聯(lián)方法來提高匹配的精度，但該方法在匹配策略中分配固定的加權(quán)值，在復(fù)雜多變的環(huán)境中缺乏適應(yīng)性。

本文采用激光掃描儀對環(huán)境進(jìn)行掃描，獲取掃描數(shù)據(jù)點集。首先對掃描數(shù)據(jù)點集進(jìn)行區(qū)域分割，提取區(qū)域的多特征信息；然后利用區(qū)域的多特征信息對ICP算法的匹配策略進(jìn)行改進(jìn)，得到與下一幀區(qū)域的相關(guān)性，并自適應(yīng)調(diào)整多特征的加權(quán)系數(shù)從而保證ICP匹配的精度，最終實現(xiàn)了移動機(jī)器人的同步建圖與定位。

1 掃描匹配的問題描述

掃描匹配算法的地圖構(gòu)建與定位是對相鄰兩個掃描數(shù)據(jù)點集進(jìn)行匹配[9]，得到兩組點集之間的旋轉(zhuǎn)矩陣和平移向量(R,T)。掃描匹配算法的示意圖，如圖1所示。

圖1 掃描匹配算法示意

設(shè)Pk-1={Pi,k-1(xi,k-1,yi,k-1)|i=1,2,…,n}為k-1時刻的掃描數(shù)據(jù)點集,Qk={Qj,k(xj,k·yj,k)|j=1,2,…,m}為k時刻的掃描數(shù)據(jù)點集。

匹配前，先將k時刻的掃描數(shù)據(jù)點集轉(zhuǎn)換到k-1時刻的坐標(biāo)下，令Q′j,k=RQj,k+T，然后以式(1)中E(R,T)達(dá)到最小值為評價標(biāo)準(zhǔn)，求解位姿變換參數(shù)R,T

(1)

式中R(Δθ)為兩組點集的旋轉(zhuǎn)矩陣，Δθ為旋轉(zhuǎn)角度，T=[Δx,Δy]′為兩組點集的位移向量，Pi,k-1和Qj,k為兩組點集中的對應(yīng)點，N為對應(yīng)點的數(shù)目。

2 基于自適應(yīng)多特征掃描匹配的SLAM方法

2.1 自適應(yīng)變閾值分割方法

原始掃描數(shù)據(jù)量較大，而且還存在噪聲點，直接進(jìn)行掃描匹配會影響定位與地圖構(gòu)建的實時性，因此，應(yīng)先對掃描數(shù)據(jù)進(jìn)行區(qū)域分割。為了保證在不同距離和角度都能正確對原始掃描數(shù)據(jù)進(jìn)行分割，采用了自適應(yīng)變閾值分割方法[10]。

由區(qū)域分割得到k-1時刻的區(qū)域分割點集Ak-1={Ai,k-1|i=1,2,…,r1}和k時刻Bk={Bj,k|j=1,2,…,r2}，其中r1和r2為區(qū)域的個數(shù)。

2.2 多特征的相似度

為了提高ICP算法效率，利用區(qū)域特征信息用于匹配，可以大大減少計算量。由2.1節(jié)得到區(qū)域點集Ak-1和Bk，選擇每個區(qū)域的中心坐標(biāo)(xc,yc)，角度α=arctan(yc/xc)，寬度w=|max{Δy}|，長度l=|max{Δx}|來表征區(qū)域的多特征信息。綜合考慮區(qū)域的多特征信息，只有與目標(biāo)“相似”最大的那個區(qū)域被認(rèn)為與其相匹配。

1)區(qū)域中心特征相似度

(2)

式中SDis為區(qū)域集Ak-1中第i個區(qū)域的中心坐標(biāo)和區(qū)域集Bk中第j個區(qū)域的中心坐標(biāo)的相似度。SDis越大，兩個區(qū)域就越靠近。距離最遠(yuǎn)的區(qū)域相似度定義為0。

2)區(qū)域角度特征相似度

(3)

式中Sα為區(qū)域集Ak-1中第i個區(qū)域的角度和區(qū)域集Bk中第j個區(qū)域的角度的相似度。Sα越大，兩個區(qū)域就越靠近，角度差異最大的區(qū)域相似度定義為0。

3)區(qū)域長度和寬度特征相似度

(4)

(5)

式中SL和SW分別為區(qū)域長度和寬度特征的相似度。兩個區(qū)域之間的長度或?qū)挾忍卣飨嗨贫仍酱?，差異就越小，長度或?qū)挾炔町愖畲蟮膮^(qū)域相似度定義為0。

在得到區(qū)域的中心、角度、長度和寬度四個特征的相似度后，區(qū)域多特征的相似度為

(6)

2.3 特征相似度權(quán)重系數(shù)

式(6)在計算區(qū)域多特征相似度時，特征的權(quán)重系數(shù)是固定的，在復(fù)雜多變的環(huán)境中適應(yīng)能力較差，為此，本文使用自適應(yīng)特征相似度加權(quán)系數(shù)。

在每次掃描匹配時，根據(jù)初始權(quán)重系數(shù)，得到初始匹配區(qū)域集

C=(Cl=(Ai,k-1,Bj,k)|i=1,2,…,r1,

j=1,2,…,r2,l=1,2,…,r3)

(7)

式中l(wèi)為匹配區(qū)域的個數(shù)，由C中匹配區(qū)域的中心坐標(biāo)，經(jīng)式(1)計算，得到相鄰兩組點集之間位姿變化Δx,Δy,Δθ。

區(qū)域集單項特征的變化量分別為

(8)

當(dāng)特征變化較大時，應(yīng)分配更小的權(quán)重系數(shù)，即，特征的權(quán)重系數(shù)與特征變化成反比。特征的權(quán)重系數(shù)為

(9)

2.4 同時定位與地圖構(gòu)建

經(jīng)匹配后，可以得到機(jī)器人k-1和k的位姿變換Δpk-1(Rk-1,Tk-1)。設(shè)以第1次掃描所建立的局部地圖M1作為全局的初始地圖[5]，第k次掃描時的局部地圖為Mk，機(jī)器人的位姿為pk，全局地圖為Mall，機(jī)器人的初始位姿為p1=(0,0,0)′。則第k次掃描時機(jī)器人的位姿pk為

pk=Δp1+…+Δpk-1

(10)

第k次掃描的局部地圖Mk坐標(biāo)變換到全局地M′k為

M′k=Δp1+…+Δpk-1

(11)

全局地圖為

Mall=M1+M′2+…+M′k

(12)

3 仿真實驗驗證與分析

仿真中所采用的數(shù)據(jù)由悉尼大學(xué)發(fā)布的數(shù)據(jù)集，具有豐富的結(jié)構(gòu)化和非結(jié)構(gòu)化特點。為進(jìn)一步研究算法的性能,分別用標(biāo)準(zhǔn)ICP匹配算法、本文算法對以上數(shù)據(jù)進(jìn)行匹配。將標(biāo)準(zhǔn)ICP算法作為基準(zhǔn)，從匹配誤差和迭代次數(shù)兩方面對以上兩種算法進(jìn)行定量比較。

3.1 匹配誤差性能分析

分別用標(biāo)準(zhǔn)ICP算法和本文算法對匹配的誤差進(jìn)行比較。從激光掃描數(shù)據(jù)集中隨機(jī)選取連續(xù)的 1000 幀數(shù)據(jù)，誤差以mm計算。結(jié)果如圖2所示。

圖2 匹配誤差對比

由圖2看出: 本文算法在匹配誤差上小于標(biāo)準(zhǔn)ICP算法。標(biāo)準(zhǔn)ICP算法考慮了所有的掃描點，甚至考慮了離群點，必然會導(dǎo)致對應(yīng)點的誤匹配，影響匹配精度。本文算法主要是基于區(qū)域的自適應(yīng)多特征進(jìn)行匹配，明顯的提高了匹配精度。

3.2 迭代性能分析

分別用標(biāo)準(zhǔn)ICP算法和本文算法對迭代次數(shù)進(jìn)行比較。從激光掃描數(shù)據(jù)中隨機(jī)選取連續(xù)的 1000 幀數(shù)據(jù)，將匹配的最大迭代次數(shù)均限制為50次。結(jié)果如圖3所示。

由圖3可以看出,本文算法在迭代次數(shù)上要遠(yuǎn)遠(yuǎn)小于標(biāo)準(zhǔn)ICP算法。標(biāo)準(zhǔn)ICP 算法的迭代次數(shù)是11.49; 本文算法的迭代次數(shù)5.34。標(biāo)準(zhǔn)ICP算法中對應(yīng)點如果選取正確，迭代次數(shù)就會很少，如果對應(yīng)點選取有誤，迭代次數(shù)就會很多。本文算法僅僅選取區(qū)域的中心點坐標(biāo)來參與運算，減少了匹配的數(shù)據(jù)量，大大減少了運算次數(shù)。

圖3 迭代次數(shù)對比

3.3 地圖建圖結(jié)果驗證

用標(biāo)準(zhǔn)ICP算法跟本文算法分別進(jìn)行定位與地圖構(gòu)建，實驗對比結(jié)果如圖4所示。其中黑線為激光掃描儀的定位結(jié)果，周圍輪廓為建立的環(huán)境結(jié)構(gòu)圖?？煽闯霰疚乃惴ū葮?biāo)準(zhǔn)ICP算法效果更好。

圖4 二種算法地圖建圖結(jié)果

4 結(jié)束語

針對標(biāo)準(zhǔn)ICP算法實現(xiàn)同時定位與地圖建圖計算量大的問題，本文采用了一種基于自適應(yīng)多特征匹配的SLAM方法。利用區(qū)域的多特征用于匹配，可以大大減少計算量，然后，自適應(yīng)調(diào)整多特征的加權(quán)系數(shù)從而保證ICP匹配的精度。通過以上實驗分析可以看出,本文算法在匹配誤差上要小于標(biāo)準(zhǔn)ICP算法,在迭代次數(shù)上遠(yuǎn)遠(yuǎn)少于標(biāo)準(zhǔn)ICP算法。因此，本文算法能夠有效地完成同時定位與地圖構(gòu)建。