基于粒子濾波的智能機器人定位算法

章弘凱 陳年生 范光宇

(上海電機學院電子信息學院 上海 201306)

0 引 言

定位技術是智能機器人領域的研究熱點與難點之一。輪式智能機器人在運動過程中，通過自身攜帶的里程計、激光雷達、攝像機等傳感器數據，獲得周圍的環境信息。通過這些信息和一定的定位算法，實現機器人自身位姿的估計，達到定位的目的。

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

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

1 基于粒子濾波的定位算法優化

1.1 粒子濾波原理

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

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

(1)

1.2 粒子濾波算法流程及其優化

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

(2)

則機器人從t-1時刻到t時刻的位姿關系變化可以表示為：

(3)

2) 測量階段。測量階段需要以激光雷達數據作為輸入，得到一個較為準確的機器人位姿。激光雷達輸入模型如圖1所示。

圖1 激光雷達坐標系

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

(4)

由此輸入激光雷達數據，得到觀測值z，對每個粒子進行評價，根據下式對每個粒子進行權重估計[15]：

(5)

由權值大小來判定粒子是否接近真實情況。

3) 重采樣階段。對于某一個粒子，以權重為wi的概率來接受這個粒子，并落在其接受的區間，保留權重大的粒子，舍棄權重小的粒子，為了保持粒子總數不變，復制權重大的粒子根據權重的比例重新分布粒子。將重采樣后的粒子代入到狀態轉移函數中，就能獲得新的預測粒子，經過反復迭代，使得大部分粒子接近真實情況，獲得機器人準確位姿，實現機器人的定位。

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

(6)

本文提出在特定位置(路口、轉彎處和障礙物附近)和時間段采用雙目視覺和激光雷達融合的觀測數據和里程計數據相聯合來優化提議分布。

雙目攝像機每幀數據龐大，本文采用ORB特征檢測方法，對特征點進行提取[14]。

如圖2所示，在實際過程中，當雙目攝像機識別出關鍵位置的圖像時，將激光雷達數據和雙目攝像機數據進行匹配，對應環境中的同一特征點。

圖2 雙目攝像機深度圖像

設雙目攝像機掃描到物體點特征的角度為以下形式：

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

(7)

激光雷達掃描到物體點云的角度為以下形式：

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

(8)

相機坐標系下的掃描點的角度信息，經過旋轉變換之后，轉換到激光雷達下的坐標系,將角度信息進行融合。

已知單位向量的旋轉變換如下所示[19]：

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

(9)

(10)

將掃描點的角度信息從攝像機坐標系轉換到激光雷達坐標系，用下式表示：

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

(11)

式中：(θl,i)real為激光雷達掃描角度信息在激光坐標系上的真值，(θv,i)real為攝像機掃描角度信息在相機坐標系上的真值。激光雷達和雙目攝像機在收集數據時存在測量誤差，為Δθl,i和Δθv,i，若誤差符合正態分布，則兩者轉換關系為：

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

(12)

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

(13)

則求解旋轉矩陣R如下式所示：

(14)

(15)

R=UVT

(16)

在機器人運行過程中，將最近一次的雙目攝像機和激光雷達聯合觀測得到的樣本，集成到提議分布中，以此將提議分布集中在可能性更高的區域。

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

(17)

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

(18)

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

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

參數St-1：上一時刻的采樣集

zt：最新的融合觀測

zVt：最新的視覺掃描

zLt：最新的激光掃描

ut-1：最新的里程計測量

St：最新的采樣集。

St={}

//視覺掃描匹配

//激光掃描匹配

else

//從極值附近采樣

fork=1,2,…,Kdo

end for

//計算高斯分布

η(i)=0

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

end for

end for

end for

//采樣新的位姿

end if

1.3 掃描匹配優化流程

1) ICP匹配原理。激光雷達掃描過程中，主要是根據ICP匹配算法。當前后兩幀激光數據為同一個點的點云時，記為數據集P和Q，需要在點云Q中找出qi，與點云P中的pi相對應，如下式所示[22]：

(19)

根據qi和pi求得旋轉變換矩陣R和平移矩陣T。如圖3所示，由于機器人只在平整地面上移動，所以只考慮關于Z軸的旋轉。P點由XaYa坐標軸，旋轉到XbYb坐標軸，為qi所在的坐標軸，則可計算出旋轉矩陣R。

(20)

該匹配需對一批點云的每一個點進行最近鄰點匹配，計算量大，結果可能會陷入局部最優問題。

圖3 旋轉矩陣變換

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

(1) 將激光雷達的前后兩幀掃描作為點云數據，分別作為M點集和N點集。

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

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

(21)

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

(22)

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

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

(23)

(24)

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

(6) 若Mi、Ni的匹配率達到閾值ζ(匹配率為該組成功點云數除以該組點云總數)，停止迭代，則表明這兩組對應點云匹配成功。若M、N的匹配組數達到閾值β，停止迭代，則表明M、N點集匹配成功。

2 實驗與數據分析

實驗采用的機器人為Autolabor Pro1,如圖5所示，是一款集機器人底盤、控制計算機、傳感器為一體的室內導航移動平臺。平臺搭載激光雷達、慣導、里程計等傳感器，可實現Navigation定位導航功能。

圖4 ROS機器人平臺

實驗中采用的AMCL工具包為ROS平臺Navigation包的一部分。利用Ubuntu16.04操作系統進行操作，在rviz中進行可視化。已知地圖是利用ROS平臺的GMapping工具包進行創建的，圖5所示為本校電子信息樓4樓二維地圖。

圖5 室內全局圖

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

(a) HF (b) AMCL (c) Proposed圖6 實驗室室內定位結果

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

圖8給出了不同方法下機器人定位誤差的結果比較，其中：(a)為機器人的距離均方根誤差，(b)為航向角角度誤差。可以看出，本文算法在距離和角度誤差中均為最小，曲線較為平緩。HF算法精度較高，均方根誤差和角度誤差與本文方法誤差接近，但是存在波動較大的情況。AMCL算法穩定性較高，但定位誤差較大。

(a) 角度誤差曲線

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

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

(a) 橫向定位誤差曲線

(b) 縱向定位誤差曲線

(c) 角度誤差曲線

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

表1 不同初始位姿偏差下定位結果對比

偏差方法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 結 語

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