章弘凱 陳年生 范光宇

(上海電機(jī)學(xué)院電子信息學(xué)院 上海 201306)

0 引 言

定位技術(shù)是智能機(jī)器人領(lǐng)域的研究熱點(diǎn)與難點(diǎn)之一。輪式智能機(jī)器人在運(yùn)動(dòng)過程中，通過自身攜帶的里程計(jì)、激光雷達(dá)、攝像機(jī)等傳感器數(shù)據(jù)，獲得周圍的環(huán)境信息。通過這些信息和一定的定位算法，實(shí)現(xiàn)機(jī)器人自身位姿的估計(jì)，達(dá)到定位的目的。

經(jīng)過多年的研究，很多學(xué)者或技術(shù)人員，提出了各種移動(dòng)機(jī)器人的定位方法。文獻(xiàn)[1]首次提出了移動(dòng)機(jī)器人的蒙特卡羅定位(MCL)，以一定數(shù)量的粒子去估計(jì)機(jī)器人位姿的后驗(yàn)概率分布。此算法可以較好地近似機(jī)器人位姿概率分布，但是采樣樣本粒子數(shù)固定。當(dāng)機(jī)器人能較好估計(jì)出位姿時(shí)，仍然需要大量的粒子來估計(jì)位姿，計(jì)算量大，實(shí)時(shí)性差。為了解決計(jì)算量大的問題，文獻(xiàn)[2]引入了具有自適應(yīng)樣本集的KLD(Kullback-Leibler Divergence)采樣，即粒子濾波的自適應(yīng)采樣，可以隨時(shí)間改變粒子采樣數(shù)目，顯著降低粒子采樣所需的樣本數(shù)量，但系統(tǒng)魯棒性較差。文獻(xiàn)[3]在粒子濾波重采樣階段引入自適應(yīng)重采樣，顯著降低了重采樣階段的計(jì)算量。文獻(xiàn)[4]在粒子濾波的基礎(chǔ)上提出直方圖濾波算法(HF),該算法定位精度較高，但是魯棒性較差。在環(huán)境中由于動(dòng)態(tài)障礙物的影響，定位容易出現(xiàn)錯(cuò)誤，在初始位姿有偏差的情況下，定位精度存在較大誤差。文獻(xiàn)[5]提出了在基于EKF算法的框架下，融合多種傳感器的數(shù)據(jù)，適用于大范圍的小車定位導(dǎo)航，穩(wěn)定性高，但是實(shí)時(shí)性較差。文獻(xiàn)[6]提出在基于AMCL算法下，利用高斯-牛頓法將激光雷達(dá)點(diǎn)云與柵格地圖從粗到細(xì)進(jìn)行匹配，并且利用激光雷達(dá)數(shù)據(jù)來計(jì)算粒子權(quán)重，使粒子群經(jīng)過重采樣后更加集中在正確的位置，降低了機(jī)器人位姿的不確定性。文獻(xiàn)[7]利用里程計(jì)信息和激光雷達(dá)的距離信息相融合，有效降低了粒子濾波階段的粒子數(shù)量，但是系統(tǒng)魯棒性較低。文獻(xiàn)[8]通過引入動(dòng)態(tài)誤差，提高激光雷達(dá)數(shù)據(jù)的有效性，降低了噪聲對(duì)測(cè)量數(shù)據(jù)的影響。文獻(xiàn)[9]提出了一種快速聯(lián)合兼容關(guān)聯(lián)方法，提供準(zhǔn)確的觀測(cè)數(shù)據(jù)和特征匹配的關(guān)系。文獻(xiàn)[12]提出利用激光雷達(dá)得出標(biāo)志物與機(jī)器人的相對(duì)位置，計(jì)算出機(jī)器人的絕對(duì)位姿，將這些絕對(duì)位姿并入粒子采樣中，采樣分布更為集中。該方法只利用激光雷達(dá)傳感器，接收數(shù)據(jù)單一，且激光雷達(dá)易受環(huán)境的干擾，影響定位準(zhǔn)確性。文獻(xiàn)[13]提出了利用里程計(jì)信息和慣性測(cè)量單元的信息進(jìn)行融合，來修正里程計(jì)的累積誤差，提高定位精度。此方法在機(jī)器人運(yùn)動(dòng)時(shí)受到本身漂移和重力的影響，定位效果不佳。文獻(xiàn)[16-17]提出融合周圍環(huán)境的點(diǎn)線特征。來進(jìn)行環(huán)境匹配，完成機(jī)器人的定位，但視覺易受光線的影響，容易產(chǎn)生環(huán)境的誤匹配，無法對(duì)錯(cuò)誤的定位及時(shí)矯正。文獻(xiàn)[24]利用激光傳感器和視覺傳感器聯(lián)合感知環(huán)境信息，采用分階段的形式來判斷障礙物，可以較好地識(shí)別障礙物，但是存在計(jì)算量大、實(shí)時(shí)性差等問題。文獻(xiàn)[25]采用提取激光與視覺信息的一致性特征信息進(jìn)行信息融合來判斷機(jī)器人的位姿情況，可是實(shí)驗(yàn)成功率不高。

在現(xiàn)有機(jī)器人定位方法中，一般都是采用里程計(jì)、激光雷達(dá)、視覺傳感器等其中一種數(shù)據(jù)，數(shù)據(jù)來源較為單一，部分方法[7,13,24-25]采用其中兩種以上傳感器數(shù)據(jù)進(jìn)行信息融合，一定程度上提高了定位的準(zhǔn)確性。然而持續(xù)的數(shù)據(jù)融合增加了數(shù)據(jù)處理復(fù)雜度，增加了計(jì)算量，特別是包含視覺傳感器時(shí)，增加了很大的計(jì)算量以及機(jī)器人的負(fù)擔(dān)和能量消耗。針對(duì)以上問題，本文提出采用基于粒子濾波的AMCL優(yōu)化定位算法，在粒子濾波過程的預(yù)測(cè)階段，利用高精度的激光雷達(dá)數(shù)據(jù)和雙目攝像機(jī)數(shù)據(jù)來改善其提議分布，其中雙目攝像機(jī)和激光雷達(dá)數(shù)據(jù)的融合只在特定位置和時(shí)間段進(jìn)行以減少計(jì)算量；其次，改進(jìn)激光雷達(dá)的點(diǎn)云匹配過程，使得機(jī)器人能夠更好地估計(jì)其位姿。機(jī)器人平臺(tái)上的實(shí)驗(yàn)結(jié)果證明，本文方法在保證計(jì)算量的前提下，一定程度上提高了機(jī)器人的定位精度。

1 基于粒子濾波的定位算法優(yōu)化

1.1 粒子濾波原理

AMCL定位算法是目前常用的預(yù)測(cè)機(jī)器人位姿的粒子濾波方法，能實(shí)現(xiàn)機(jī)器人較為精準(zhǔn)的定位[3]。定位階段分為預(yù)測(cè)階段、測(cè)量階段和重采樣階段[4]。預(yù)測(cè)階段是根據(jù)里程計(jì)輸入的速度信息和前一時(shí)刻機(jī)器人的位姿粒子分布來采樣當(dāng)前時(shí)刻的位姿粒子群；測(cè)量階段是根據(jù)激光雷達(dá)的掃描數(shù)據(jù)作為輸入，對(duì)每一個(gè)粒子進(jìn)行權(quán)值計(jì)算；重采樣階段是根據(jù)粒子權(quán)值來對(duì)粒子進(jìn)行篩選，舍棄權(quán)值低的粒子，保留權(quán)值高的粒子，即為接近機(jī)器人真實(shí)位姿的粒子。若沒有達(dá)到足夠的粒子數(shù)來表示真實(shí)位姿，則需要重復(fù)這三個(gè)過程，最終使絕大部分粒子聚集于真實(shí)位姿最接近的區(qū)域，實(shí)現(xiàn)定位。其后驗(yàn)概率分布為[9]：

P(xt|z1∶t,u1∶t)=

(1)

1.2 粒子濾波算法流程及其優(yōu)化

1) 預(yù)測(cè)階段。首先，規(guī)定粒子數(shù)量，將粒子平均分布于規(guī)劃區(qū)域，然后輸入輪式里程計(jì)數(shù)據(jù)，根據(jù)里程計(jì)的運(yùn)動(dòng)模型，得到一個(gè)提議分布。以下為輪式里程計(jì)輸入模型。假設(shè)機(jī)器人在t-1時(shí)刻的位姿為Xt-1(xt-1,yt-1,θt-1),t時(shí)刻的位姿為Xt(xt,yt,θt)，里程計(jì)輸入數(shù)據(jù)為左輪速度VL和右輪VR,兩輪之間距離為2d，則機(jī)器人速度V和角速度ω(以順時(shí)針方向?yàn)檎?為[10-11]：

(2)

則機(jī)器人從t-1時(shí)刻到t時(shí)刻的位姿關(guān)系變化可以表示為：

(3)

2) 測(cè)量階段。測(cè)量階段需要以激光雷達(dá)數(shù)據(jù)作為輸入，得到一個(gè)較為準(zhǔn)確的機(jī)器人位姿。激光雷達(dá)輸入模型如圖1所示。

圖1 激光雷達(dá)坐標(biāo)系

經(jīng)過一段位移后，機(jī)器人位于A點(diǎn)，雷達(dá)的朝向就是機(jī)器人的朝向。此時(shí)，機(jī)器人的位姿為Xt(xt,yt,ψt)，雷達(dá)測(cè)量角為θ，測(cè)量到障礙物距離為d,設(shè)雷達(dá)測(cè)量點(diǎn)為B(x,y,β),則根據(jù)幾何關(guān)系可得[10-11]：

(4)

由此輸入激光雷達(dá)數(shù)據(jù)，得到觀測(cè)值z(mì)，對(duì)每個(gè)粒子進(jìn)行評(píng)價(jià)，根據(jù)下式對(duì)每個(gè)粒子進(jìn)行權(quán)重估計(jì)[15]：

(5)

由權(quán)值大小來判定粒子是否接近真實(shí)情況。

3) 重采樣階段。對(duì)于某一個(gè)粒子，以權(quán)重為wi的概率來接受這個(gè)粒子，并落在其接受的區(qū)間，保留權(quán)重大的粒子，舍棄權(quán)重小的粒子，為了保持粒子總數(shù)不變，復(fù)制權(quán)重大的粒子根據(jù)權(quán)重的比例重新分布粒子。將重采樣后的粒子代入到狀態(tài)轉(zhuǎn)移函數(shù)中，就能獲得新的預(yù)測(cè)粒子，經(jīng)過反復(fù)迭代，使得大部分粒子接近真實(shí)情況，獲得機(jī)器人準(zhǔn)確位姿，實(shí)現(xiàn)機(jī)器人的定位。

4) 粒子濾波優(yōu)化流程。理論上，提議分布越接近目標(biāo)分布，濾波效果越好。然而，典型的粒子濾波應(yīng)用是采用里程計(jì)運(yùn)動(dòng)模型來得到提議分布的。這種運(yùn)動(dòng)模型的優(yōu)點(diǎn)在于方便計(jì)算；缺點(diǎn)是樣本過于分散，方差較大。根據(jù)激光雷達(dá)數(shù)據(jù)采樣的粒子，方差較小，粒子分布更為集中，如下式所示：

(6)

本文提出在特定位置(路口、轉(zhuǎn)彎處和障礙物附近)和時(shí)間段采用雙目視覺和激光雷達(dá)融合的觀測(cè)數(shù)據(jù)和里程計(jì)數(shù)據(jù)相聯(lián)合來優(yōu)化提議分布。

雙目攝像機(jī)每幀數(shù)據(jù)龐大，本文采用ORB特征檢測(cè)方法，對(duì)特征點(diǎn)進(jìn)行提取[14]。

如圖2所示，在實(shí)際過程中，當(dāng)雙目攝像機(jī)識(shí)別出關(guān)鍵位置的圖像時(shí)，將激光雷達(dá)數(shù)據(jù)和雙目攝像機(jī)數(shù)據(jù)進(jìn)行匹配，對(duì)應(yīng)環(huán)境中的同一特征點(diǎn)。

圖2 雙目攝像機(jī)深度圖像

設(shè)雙目攝像機(jī)掃描到物體點(diǎn)特征的角度為以下形式：

ΘV=[θv,1θv,2…θv,n]

(7)

激光雷達(dá)掃描到物體點(diǎn)云的角度為以下形式：

ΘL=[θl,1θl,2…θl,n]

(8)

相機(jī)坐標(biāo)系下的掃描點(diǎn)的角度信息，經(jīng)過旋轉(zhuǎn)變換之后，轉(zhuǎn)換到激光雷達(dá)下的坐標(biāo)系,將角度信息進(jìn)行融合。

已知單位向量的旋轉(zhuǎn)變換如下所示[19]：

bi=Ari+Δbii=1,2，…,n

(9)

(10)

將掃描點(diǎn)的角度信息從攝像機(jī)坐標(biāo)系轉(zhuǎn)換到激光雷達(dá)坐標(biāo)系，用下式表示：

(θl,i)real=R(θv,i)real

(11)

式中：(θl,i)real為激光雷達(dá)掃描角度信息在激光坐標(biāo)系上的真值，(θv,i)real為攝像機(jī)掃描角度信息在相機(jī)坐標(biāo)系上的真值。激光雷達(dá)和雙目攝像機(jī)在收集數(shù)據(jù)時(shí)存在測(cè)量誤差，為Δθl,i和Δθv,i，若誤差符合正態(tài)分布，則兩者轉(zhuǎn)換關(guān)系為：

θl,i-Δθl,i=R(θv,i-Δθv,i)

(12)

θl,i=Rθv,i-RΔθv,i+Δθl,i

(13)

則求解旋轉(zhuǎn)矩陣R如下式所示：

(14)

(15)

R=UVT

(16)

在機(jī)器人運(yùn)行過程中，將最近一次的雙目攝像機(jī)和激光雷達(dá)聯(lián)合觀測(cè)得到的樣本，集成到提議分布中，以此將提議分布集中在可能性更高的區(qū)域。

根據(jù)式(13)，得到改進(jìn)后的提議分布函數(shù)為：

(17)

根據(jù)此提議分布，利用式(14)計(jì)算得到權(quán)重，進(jìn)行重采樣。根據(jù)優(yōu)化后的提議分布，可以忽略分布中意義不大的區(qū)域，使粒子分布更為集中，減少重采樣部分的計(jì)算量。

(18)

在重采樣階段，根據(jù)激光雷達(dá)和雙目攝像機(jī)的融合觀測(cè)數(shù)據(jù)，計(jì)算得到粒子的權(quán)重來進(jìn)行重采樣，為下一次的預(yù)測(cè)階段提供輸入，經(jīng)過反復(fù)迭代之后，得到機(jī)器人正確的位姿。具體流程為：

5) 將新的高斯分布作為提議分布，從該分布中得到新的位姿。算法過程如下：

參數(shù)St-1：上一時(shí)刻的采樣集

zt：最新的融合觀測(cè)

zVt：最新的視覺掃描

zLt：最新的激光掃描

ut-1：最新的里程計(jì)測(cè)量

St：最新的采樣集。

St={}

//視覺掃描匹配

//激光掃描匹配

else

//從極值附近采樣

fork=1,2,…,Kdo

end for

//計(jì)算高斯分布

η(i)=0

for allxj?{x1,x2,…,xK} do

end for

end for

end for

//采樣新的位姿

end if

1.3 掃描匹配優(yōu)化流程

1) ICP匹配原理。激光雷達(dá)掃描過程中，主要是根據(jù)ICP匹配算法。當(dāng)前后兩幀激光數(shù)據(jù)為同一個(gè)點(diǎn)的點(diǎn)云時(shí)，記為數(shù)據(jù)集P和Q，需要在點(diǎn)云Q中找出qi，與點(diǎn)云P中的pi相對(duì)應(yīng)，如下式所示[22]：

(19)

根據(jù)qi和pi求得旋轉(zhuǎn)變換矩陣R和平移矩陣T。如圖3所示，由于機(jī)器人只在平整地面上移動(dòng)，所以只考慮關(guān)于Z軸的旋轉(zhuǎn)。P點(diǎn)由XaYa坐標(biāo)軸，旋轉(zhuǎn)到XbYb坐標(biāo)軸，為qi所在的坐標(biāo)軸，則可計(jì)算出旋轉(zhuǎn)矩陣R。

(20)

該匹配需對(duì)一批點(diǎn)云的每一個(gè)點(diǎn)進(jìn)行最近鄰點(diǎn)匹配，計(jì)算量大，結(jié)果可能會(huì)陷入局部最優(yōu)問題。

圖3 旋轉(zhuǎn)矩陣變換

2) 點(diǎn)云匹配優(yōu)化流程。本文提出一種分組階梯式閾值匹配法。在機(jī)器人局部路徑規(guī)劃中，在時(shí)間Δt內(nèi)，激光雷達(dá)掃描到新的障礙物，在Δt內(nèi)障礙物為靜止。以激光雷達(dá)對(duì)障礙物掃描的前后兩幀的數(shù)據(jù)作為兩個(gè)集合，為M集合和N集合。根據(jù)掃描的時(shí)間先后順序?qū)和N從頭至尾平均分為k組，在M和N之間對(duì)應(yīng)的每一組的點(diǎn)云來進(jìn)行最近鄰點(diǎn)匹配，根據(jù)ICP算法，對(duì)每組的每一個(gè)點(diǎn)云進(jìn)行匹配。在一定時(shí)間內(nèi)，若匹配率達(dá)到閾值，則表明這兩組點(diǎn)云匹配成功；若匹配成功組數(shù)達(dá)到閾值，則表明這批點(diǎn)云匹配成功，以此來成功識(shí)別出該障礙物，重新進(jìn)行局部路徑規(guī)劃。算法主要流程如下：

(1) 將激光雷達(dá)的前后兩幀掃描作為點(diǎn)云數(shù)據(jù)，分別作為M點(diǎn)集和N點(diǎn)集。

(2) 將M點(diǎn)集和N點(diǎn)集分別分成k組，記為M1，M2，M3，…，Mk和N1,N2,N3，…，Nk，設(shè)每組平均個(gè)數(shù)為C。

(3) 將Mij和Nij點(diǎn)云進(jìn)行ICP匹配，(Mij表示M點(diǎn)集中第i組的第j個(gè)點(diǎn)云)利用下式計(jì)算兩個(gè)點(diǎn)云Mij和Nij的質(zhì)心。

(21)

(4) 利用SVD分解來求得變換矩陣,以下式來求得U、V。

(22)

若rank(W)=3,則求得解唯一，利用下式求得旋轉(zhuǎn)變換矩陣R和平移矩陣T′：

R=U′V′TT′=μm-Rμn

(23)

(24)

式中：p(i)用來判斷點(diǎn)匹配是否合格，E為經(jīng)過試驗(yàn)得到的距離閾值。

(6) 若Mi、Ni的匹配率達(dá)到閾值ζ(匹配率為該組成功點(diǎn)云數(shù)除以該組點(diǎn)云總數(shù))，停止迭代，則表明這兩組對(duì)應(yīng)點(diǎn)云匹配成功。若M、N的匹配組數(shù)達(dá)到閾值β，停止迭代，則表明M、N點(diǎn)集匹配成功。

2 實(shí)驗(yàn)與數(shù)據(jù)分析

實(shí)驗(yàn)采用的機(jī)器人為Autolabor Pro1,如圖5所示，是一款集機(jī)器人底盤、控制計(jì)算機(jī)、傳感器為一體的室內(nèi)導(dǎo)航移動(dòng)平臺(tái)。平臺(tái)搭載激光雷達(dá)、慣導(dǎo)、里程計(jì)等傳感器，可實(shí)現(xiàn)Navigation定位導(dǎo)航功能。

圖4 ROS機(jī)器人平臺(tái)

實(shí)驗(yàn)中采用的AMCL工具包為ROS平臺(tái)Navigation包的一部分。利用Ubuntu16.04操作系統(tǒng)進(jìn)行操作，在rviz中進(jìn)行可視化。已知地圖是利用ROS平臺(tái)的GMapping工具包進(jìn)行創(chuàng)建的，圖5所示為本校電子信息樓4樓二維地圖。

圖5 室內(nèi)全局圖

實(shí)驗(yàn)中，機(jī)器人利用HF、AMCL和本文算法進(jìn)行定位操作，場(chǎng)地分別選定為實(shí)驗(yàn)室室內(nèi)與外部走廊，如圖6、圖7所示。圖6中的(a)、(b)、(c)分別為機(jī)器人利用HF、AMCL和本文算法在實(shí)驗(yàn)室室內(nèi)的定位結(jié)果；圖7中的(a)、(b)、(c)為機(jī)器人利用這三種方法在外部走廊的定位結(jié)果。由實(shí)驗(yàn)可以得出，利用HF算法和AMCL算法，在機(jī)器人運(yùn)動(dòng)一段時(shí)間后機(jī)器人定位均出現(xiàn)了不同程度的偏差，本文算法機(jī)器人定位性能較穩(wěn)定，且定位精度較高。

(a) HF (b) AMCL (c) Proposed圖6 實(shí)驗(yàn)室室內(nèi)定位結(jié)果

(a) HF (b) AMCL (c) Proposed圖7 外部走廊定位結(jié)果

圖8給出了不同方法下機(jī)器人定位誤差的結(jié)果比較，其中：(a)為機(jī)器人的距離均方根誤差，(b)為航向角角度誤差?？梢钥闯?，本文算法在距離和角度誤差中均為最小，曲線較為平緩。HF算法精度較高，均方根誤差和角度誤差與本文方法誤差接近，但是存在波動(dòng)較大的情況。AMCL算法穩(wěn)定性較高，但定位誤差較大。

(a) 角度誤差曲線

(b) 位姿估計(jì)誤差曲圖8 誤差比較圖

表1顯示了三種方法在不同初始位姿偏差的情況下的定位結(jié)果，包括橫向(Lateral)、縱向(Longitudinal)和航向角(Yaw)。圖9為橫向誤差、縱向誤差、航向角角度誤差和時(shí)間消耗與初始位姿偏差的關(guān)系曲線，顯示了隨著初始位姿偏差的增大，各參數(shù)值的變化情況。由表1和圖9可以得出，在不同初始位姿偏差的情況下，不論是橫向，縱向還是角度偏差，本文算法誤差最小，完成定位時(shí)間最短。HF算法在初始位姿較小的情況下，定位精度較高，但是在初始位姿偏差較大的情況下，定位效果差，誤差較大。AMCL傳統(tǒng)算法穩(wěn)定性較好，但定位精度不高。

(a) 橫向定位誤差曲線

(b) 縱向定位誤差曲線

(c) 角度誤差曲線

(d) 定位時(shí)間消耗曲線圖9 不同初始位姿偏差下定位結(jié)果曲線

表1 不同初始位姿偏差下定位結(jié)果對(duì)比

偏差方法Lateral/mLongitudinal/mYaw/degTime-cost/s0.5 mAMCL0.256 20.261 50.394 70.968 4HF0.223 60.233 10.439 20.815 9本文方法0.112 70.090 70.132 00.532 11 mAMCL0.331 20.356 20.412 51.289 5HF0.891 50.521 60.826 31.865 5本文方法0.166 20.143 60.125 80.495 12 mAMCL0.564 40.552 30.421 82.102 5HF1.260 91.795 61.352 45.215 8本文方法0.210 20.125 70.158 20.856 1

3 結(jié) 語(yǔ)

本文主要研究了智能機(jī)器人的定位問題，并針對(duì)現(xiàn)有采用里程計(jì)、激光雷達(dá)、視覺攝像機(jī)等數(shù)據(jù)在定位過程中存在精度有限且計(jì)算量較大的問題，提出了基于粒子濾波的AMCL優(yōu)化定位方法。該方法利用雙目攝像機(jī)數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)改進(jìn)基于輪式里程計(jì)的提議分布，使提議分布更為集中，并且減少重采樣過程的粒子數(shù)目，其中雙目攝像機(jī)與激光雷達(dá)數(shù)據(jù)的融合只在特定位置和時(shí)間段進(jìn)行以減少計(jì)算量；利用分組階梯式閾值判斷法進(jìn)行障礙物的匹配，在保證匹配準(zhǔn)確性的情況下，降低匹配計(jì)算量。實(shí)驗(yàn)結(jié)果表明，改進(jìn)后的算法具有較高的定位精度，且能夠較好地在實(shí)驗(yàn)平臺(tái)中應(yīng)用，具有良好的實(shí)用性。