茹淑慧, 王紅旗, 唐 浩

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

0 引 言

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

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

1 掃描匹配的問(wèn)題描述

掃描匹配算法的地圖構(gòu)建與定位是對(duì)相鄰兩個(gè)掃描數(shù)據(jù)點(diǎn)集進(jìn)行匹配[9]，得到兩組點(diǎn)集之間的旋轉(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í)刻的掃描數(shù)據(jù)點(diǎn)集,Qk={Qj,k(xj,k·yj,k)|j=1,2,…,m}為k時(shí)刻的掃描數(shù)據(jù)點(diǎn)集。

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

(1)

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

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

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

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

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

2.2 多特征的相似度

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

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

(2)

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

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

(3)

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

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

(4)

(5)

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

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

(6)

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

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

在每次掃描匹配時(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ū)域的個(gè)數(shù)，由C中匹配區(qū)域的中心坐標(biāo)，經(jīng)式(1)計(jì)算，得到相鄰兩組點(diǎn)集之間位姿變化Δx,Δy,Δθ。

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

(8)

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

(9)

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

經(jīng)匹配后，可以得到機(jī)器人k-1和k的位姿變換Δpk-1(Rk-1,Tk-1)。設(shè)以第1次掃描所建立的局部地圖M1作為全局的初始地圖[5]，第k次掃描時(shí)的局部地圖為Mk，機(jī)器人的位姿為pk，全局地圖為Mall，機(jī)器人的初始位姿為p1=(0,0,0)′。則第k次掃描時(shí)機(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í)驗(yàn)驗(yàn)證與分析

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

3.1 匹配誤差性能分析

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

圖2 匹配誤差對(duì)比

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

3.2 迭代性能分析

分別用標(biāo)準(zhǔn)ICP算法和本文算法對(duì)迭代次數(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算法中對(duì)應(yīng)點(diǎn)如果選取正確，迭代次數(shù)就會(huì)很少，如果對(duì)應(yīng)點(diǎn)選取有誤，迭代次數(shù)就會(huì)很多。本文算法僅僅選取區(qū)域的中心點(diǎn)坐標(biāo)來(lái)參與運(yùn)算，減少了匹配的數(shù)據(jù)量，大大減少了運(yùn)算次數(shù)。

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

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

用標(biāo)準(zhǔn)ICP算法跟本文算法分別進(jìn)行定位與地圖構(gòu)建，實(shí)驗(yàn)對(duì)比結(jié)果如圖4所示。其中黑線(xiàn)為激光掃描儀的定位結(jié)果，周?chē)喞獮榻⒌沫h(huán)境結(jié)構(gòu)圖。可看出本文算法比標(biāo)準(zhǔn)ICP算法效果更好。

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

4 結(jié)束語(yǔ)

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