移動機器人改進激光SLAM算法研究

陳 丹，吳 欣

西安理工大學 自動化與信息工程學院，西安710048

智能化機器人在面臨復雜不確定地形環境時如何確定自身位置，并提供優質的地圖具有重要的理論研究價值和實際應用意義[1]。同步定位與地圖構建（simultaneous localization and mapping，SLAM）[2-3]利用機器人攜帶的傳感器在未知環境下觀測目標，根據機器人與目標之間的相對位置和里程信息來估計機器人與目標點的全局坐標。SLAM 問題一直是機器人研究中的一個基礎且重要的開放性研究熱點。2017年，Wongsuwan等人[4]提出糾正梯度細化方法對傳統RBPF-SLAM進行改進，該改進方法可以應用于多種類型的機器人傳感器上。2018年，Jo等人[5]提出了一種新的映射表示方法以及由基本映射和單獨映射組成的分解方案，在保證地圖精度的基礎上節約環境地圖的存儲空間。2020年，楊爽等[6]提出一種通過搭建地標數據庫和位姿推導模型，解決了移動機器人在SLAM過程中定位失真問題。

求解SLAM 問題的方法大致可分為基于概率估計的方法和基于非概率估計的方法兩大類。基于概率估計的方法主要有擴展卡爾曼濾波（extended Kalman filter，EKF）、FastSLAM與粒子濾波（particle filter，PF）等。粒子濾波的優點在于解決如狀態遞歸估計或高維非線性、非高斯時變系統的概率推理等復雜問題。粒子濾波作為一種有效解決SLAM 問題的方法仍然存在粒子退化問題[7]。后來，Murphy 等人[8]提出采用RBPF（Rao-Blackwellised particle filter）求解SLAM問題。2007年，Grisetti 等人[9]應用了基于有效樣本容量的選擇性重采樣策略，在一定程度上限制了重采樣次數，但對緩解粒子耗盡的現象并沒有做出有效的改進。2016年，田夢楚等人[10]提出智能優化算法，采用優勝劣汰機制，提高了精度，但該算法對小權重粒子吸引較小，易造成粒子退化現象。2018年，劉云濤[11]采用基于蝴蝶優化的粒子濾波算法處理重采樣之后的粒子，該算法避免陷入局部最優且改善粒子耗散現象，但在粒子間吸引時任意兩粒子間交互計算，實時性較差。2019年，吳正越等人[12]提出了基于最小方差重采樣方法（minimum sampling variance，MSV）的一種優化SLAM算法，在動態環境下取得了更好的建圖和定位效果，但是算法復雜度增加且只能達到有限精度。2020年，王彥等人[13]針對重采樣過程導致粒子多樣性降低問題，提出一種自適應優化組合重采樣方法，但改進算法只能在一定程度上提高建圖精度，對于構建尺寸較大的復雜環境地圖還有待提升。

迭代最近點（iterative closest point，ICP）算法是一種點云匹配算法，是基于激光雷達掃描相鄰幀，尋找激光雷達兩個掃描點之間的轉換關系。當掃描點數增大時，ICP 會承受過高的計算成本。為了提高ICP 算法的效率和準確性，人們提出了許多改進方法。2016年，Lee等人[14]提出了一種在線學習的方法來進行三維對象配準，實現了更好的魯棒性和穩定收斂性，但是迭代過程中可能不能收斂到全局最優解。2018 年，Guan 等人[15]利用體素格對點云數據進行重采樣，采用尋找對應點的方法來提高點云配準效率。2019 年，Wu 等人[16]通過引入最大相關準則作為相似性度量，提出一種新的魯棒尺度ICP算法。這些算法雖然提高了點云配準效率，但也丟失了部分配準精度，使得對點云初始位姿的選取更加嚴格。

因此本文著重提供良好的點云初始態，從采樣點的選取和匹配策略方面對激光雷達快速點云配準算法進行了改進，同時針對傳統重采樣過程中粒子多樣性喪失問題，提出了一種改進傳統粒子濾波重采樣方法，最后基于ROS系統進行真實較復雜場景下的地圖構建。

1 RBPF-SLAM的算法改進

1.1 改進ICP算法

ICP 算法配準的目標是通過一個三維變換矩陣將源點云集p={pi} 轉換為目標點云集q={qj}，且盡可能使兩個點云坐標在同一位置重合。兩點云位置配準誤差為：

求解誤差函數式（1）的最小值，獲得下一次位姿變換估計值：

式中，“⊕”表示旋轉平移操作符，Sref為旋轉平移參考面，k為匹配算法迭代次數，索引i和j分別代表點云p和q中的點，R和T分別為旋轉和平移矩陣。ICP算法原理簡單、直觀，但算法的效率和全局最優收斂取決于初始迭代值。

主成分分析（principal component analysis，PCA）是一種有效的簡化檢測數據集方法，可以減少數據集維數，反映數據集對方差貢獻的最大特征。因此，相似度大的兩片點云，通過PCA 算法只要把其參考坐標系調整到一致，即可達到粗配準目的，粗配準為點到線ICP點云精配準提供了良好的點云初始變換。

傳統ICP 配準算法采用兩點間歐氏距離最近點作為配準點，易受到噪聲的干擾。因此，本文在對應點匹配中，將最近的點度量從兩點之間的歐氏距離改變為到兩點連線的垂直距離，對點云采用PCA 粗配準后再進行點到線ICP算法精確配準，以提高匹配精度和收斂速度，實現移動機器人的精確定位。點到線ICP解決曲面匹配問題，可以表示如下：給出一個參考Sref和一組點云{pi}，最小化點pi旋轉平移到其在表面Sref上投影的距離：

式中ni為曲面在投影點處的法線，Qk是對點云粗配準后得到的初始變換值。

基于點到線的改進ICP配準算法步驟：

（1）設置距離誤差閾值δ，改進算法采用點到線的距離作為誤差；

（2）將當前幀的點云數據根據初始位姿投影到參考坐標系中；

（3）對于點云中的每一個點pi，在參考幀中找到與它最近的兩個點和；

（4）計算配準誤差，誤差計算采用下式：

（5）當誤差小于誤差閾值δ，停止配準迭代，否則返回步驟（2）。

1.2 改進重采樣策略

優化的重采樣算法對于粒子濾波來說既可保持樣本的有效性和多樣性，還可提高濾波器的性能。傳統重采樣方法完全丟棄了權值較小的粒子，多次迭代后削弱了粒子的多樣性，導致收斂精度不足。針對該問題，本文提出一種改進重采樣方法，將所有粒子按權值進行分類，根據預先設定的粒子權值閾值wt，在大權重和小權重粒子集合內分別進行采樣，最后對新的粒子集進行權重歸一化，進入下一步濾波過程。與傳統重采樣方法相比，改進重采樣著重于在大權重集合內進行采樣，大量復制粒子，同時在小權重集合內采用隨機重采樣方法獲取粒子，在保證高權重粒子數的同時低權重粒子的選取機會增大，增加粒子多樣性，進而提高機器人狀態的估計精度。

改進重采樣算法步驟如下：

（1）設定重采樣門限閾值Nth，當有效粒子數Neff=（其中表示歸一化權值），小于閾值Nth時，進行重采樣步驟（2），否則進行下一時刻的濾波過程。

（2）對權重大于閾值wt的粒子進行重采樣，將高權值粒子進行復制生成M個粒子，復制次數為粒子總數N與粒子權重的乘積取整floor(N×w)，高權值粒子集記為，其中r=1,2,…,M表示粒子個數。

（3）對權重小于閾值wt的粒子，進行隨機重采樣生成低權值粒子集記為，其中j=1,2,…,N-M表示粒子個數。

（4）分別對步驟（2）和步驟（3）產生的兩部分粒子進行排序，得到新的粒子集。

（5）對新的粒子集進行權重歸一化，完成后面的濾波過程。

1.3 改進RBPF-SLAM算法

RBPF-SLAM算法[17]將SLAM問題分解為機器人位姿估計和基于估計得到的位姿進行地圖構建兩部分進行描述。核心思想是利用外部傳感器信息z1:t=z1,z2,…,zt-1和里程計信息u1:t=u1,u2,…,ut-1估計環境地圖m和機器人軌跡x1:t=x1,x2,…,xt的聯合后驗概率密度p(x1:t,m|z1:t,u1:t-1)，該后驗概率為：

式中，p=p(x1:t|z1:t,u1:t-1)表示利用激光雷達觀測信息和里程計信息估計機器人位姿；p(m|x1:t,z1:t)表示通過當前機器人姿態更新地圖m。

本文對傳統RBPF-SLAM 算法從掃描匹配和粒子重采樣兩方面進行改進，改進后的RBPF-SLAM算法步驟如下：

（1）點云匹配。當機器人在移動過程中獲得來自激光雷達的掃描數據時，采用本文改進點云配準算法，對機器人位姿進行精確估計。

（2）抽樣。對上一代第i個粒子估計得到當前時刻粒子。選取激光雷達匹配結果作為優化提議分布，如式（6）所示：

（3）更新權重。根據權重計算公式（7）計算出每個粒子的權值。

（4）重采樣。當Neff

（5）更新地圖。根據第i個粒子表示的移動機器人位姿和當前觀測情況，對第i個粒子構建的地圖進行更新。

2 地圖構建算法性能仿真與分析

本文基于激光雷達點云配準算法和粒子濾波重采樣算法兩方面對RBPF-SLAM 算法進行了改進。針對這兩種改進策略，分別從算法配準精度、估計性能以及算法運行時間進行了性能仿真分析，同時將本文所改進重采樣粒子濾波算法與傳統重采樣粒子濾波以及文獻[12]所提MSV算法進行了性能對比。

2.1 改進點云配準算法性能

用激光雷達數據集對原始ICP算法和本文改進ICP算法進行點云匹配仿真實驗，原點云數據與目標點云數據的配準結果如圖1 所示，其中藍色點云代表源點云，紅色點云代表目標點云。圖1（a）為原始點云分布，圖1（b）為傳統ICP配準后點云分布，圖1（c）為本文改進ICP算法配準后點云分布。由圖1 可知，圖（c）中紅色覆蓋面積大于圖（b），因此本文改進配準算法的點云配準準確度更高。在機器人定位過程中，優良的配準算法可以進一步提高激光雷達點云的配準精度，從而實現移動機器人的精確定位。

圖1 傳統ICP和改進ICP算法配準后點云分布Fig.1 Point cloud distribution after registration of traditional ICP and improved ICP algorithms

此外，本文對傳統ICP 算法和改進ICP 算法的運行時間進行了比較，對于相同點云數據集，傳統ICP 算法進行點云配準運行時間需要12.537 s。而本文不但改進了精配準算法，同時增加了粗配準，改進精配準算法與原始ICP配準算法相比，算法運行時間大大減小。因此加上粗配準時間依舊比原始ICP 精配準算法運行時間短，總共時間為8.063 s，比傳統ICP 算法直接配準時間節省了4.474 s，由此說明改進算法實時性更好。

2.2 改進重采樣算法性能

為了驗證本文基于改進重采樣方法的粒子濾波狀態估計性能，選取典型的單變量非平穩模型進行仿真分析。該模型具有典型的非線性特征，且后驗分布具有雙峰特性，其狀態方程和觀測方程分別為：

其中，k為狀態時間，x(k)為狀態向量，z(k)為觀測向量，v(k-1)和n(k)是零均值高斯隨機變量，其方差分別為Q(k-1)和R(k)。

對目標位置估計誤差性能采用均方根誤差（RMSE）進行評價：

其中，x(k)和(k)為k時刻的濾波狀態真實值和估計值，N是蒙特卡羅模擬次數。

本文對改進重采樣粒子濾波、MSV算法[12]和傳統重采樣粒子濾波三種算法進行了性能對比。仿真中Q(k-1)和R(k)分別取10 和1，狀態初始值x(0)=0.1,N取100次[18]，粒子數設定為100。圖2 是三種粒子濾波算法對狀態估計值的仿真對比，圖3是三種粒子濾波算法狀態估計均方根誤差。由圖2、圖3可知，本文提出的改進重采樣粒子濾波狀態估計值與真實值接近，均方根誤差均小于其他兩種算法，說明本文所提算法提高了粒子的多樣性和估計精度。

圖2 三種重采樣算法濾波狀態估計Fig.2 Three resampling algorithms for filtering state estimate

圖3 三種重采樣算法濾波均方根誤差Fig.3 Three resampling algorithms for RMSE

圖4為原始重采樣算法、MSV算法和本文改進重采樣粒子濾波三種算法濾波后粒子分布情況。由圖4 可知改進重采樣粒子濾波算法擬合精度高，大部分粒子分布緊密靠近真值，只存在個別粒子樣本發散，可以有效提高地圖估計精度。

圖4 三種重采樣算法濾波后粒子分布Fig.4 Particle distribution after filtering by three resampling algorithms

為了進一步分析改進算法的實時性，對傳統重采樣、MSV 算法和本文改進重采樣粒子濾波三種算法在相同初始粒子數情況下進行多次仿真實驗，統計算法運行時間均值，如表1 所示。可以看出，在相同粒子個數下，改進算法的運行時間低于傳統算法以及MSV算法，驗證了本文所改進重采樣粒子濾波算法在地圖構建具有一定的實時性。

表1 三種算法運行時間對比Table 1 Comparison of running time of three algorithms

3 Turtlebot機器人SLAM實驗

在實際情況下，機器人的工作環境比仿真環境更加復雜，環境特征更加豐富，存在行人等不確定因素。為了驗證改進算法在這些情況下的有效性，分別采用傳統重采樣算法、MSV 算法以及本文改進重采樣與配準算法，基于Turtlebot機器人對西安理工大學教五樓室內和室外走廊環境分別進行了SLAM 實驗。構建地圖時所需的粒子個數如表2所示，每個環境重復10次實驗。實驗場景1是室外走廊，實驗場景2為室內，由于室內障礙物較多，環境復雜，構建地圖所需粒子數多于室外所需粒子數。

表2 構建地圖粒子數Table 2 Number of particles to construct a map

圖5（a）和（b）分別為進行地圖構建的室外走廊和室內真實環境。

圖5 地圖構建真實場景Fig.5 Map building real scene

采用三種RBPF-SLAM算法構建的網格圖如圖6和圖7 所示。由圖6 和圖7 紅色圈標記地方可以看出，本文所提改進算法構建的地圖符合真實場景，原始算法構建的地圖在拐角處存在偏移和重影問題，而改進算法構建的地圖更清晰、更整齊。對比圖6（a）、（b）和（c），圖（a）中原始算法沒有構建出障礙物完整的形狀，圖（b）中MSV 算法對障礙物的構建不夠完整，地圖中滅火器和電梯口位置構建不清晰。而圖（c）中本文改進重采樣與點云配準算法構建的地圖信息更加精確地反映環境信息，線條較為清晰，表示障礙物對應所在位置的單元柵格狀態更為確定，同時對于電梯口等拐角位置信息的構建更加完整。

圖6 實驗室外走廊環境下構建地圖效果圖Fig.6 Effect of map constructed in experimental outdoor corridor environment

圖7 中間部分箭頭所標注的紅色圈為室內擺放的障礙物，可以看出室內靠墻兩邊的書桌位置以及中間矩形障礙物的形狀被清晰地構建出來。圖7（c）所構建的障礙物效果也明顯優于采用原始構圖算法獲得的圖7（a）和基于MSV的構圖算法獲得的圖7（b）。對于障礙物的大小、形狀和位置構建效果提升也很明顯，可以建立正確且一致性良好的環境地圖，算法的定位精度得到了一定程度上的改善。構建地圖的差異主要是因為原始SLAM算法在粒子濾波重采樣過程中，注重復制大權值粒子和頻繁的重采樣，導致粒子多樣性喪失、粒子退化，從而導致機器人位姿與估計位姿不重合，地圖構建過程中出現地圖傾斜與墻體重影。而MSV算法采用計算最小采樣方差的方式，使得重采樣前后粒子分布保持一致，差異達到最小化，緩解了樣本貧化問題，但是計算復雜度增加，粒子分布比較集中，缺乏多樣性，只能達到有限精度。本文所提改進算法克服了原始算法和MSV算法的缺點，并且由表2 還可以看出，改進算法用于地圖構建，用更少的粒子數，構建出更加精確的地圖，減小了算法計算復雜度，進一步驗證了改進算法構建環境地圖的有效性。

圖7 實驗室環境下構建地圖效果圖Fig.7 Effect of map rendering in laboratory environment

4 總結

本文提出了一種基于激光雷達的改進RBPF 算法。采用改進ICP算法完成激光掃描點集配準后，通過引入改進重采樣的粒子濾波算法，減少了粒子退化，在保留好粒子并創造新的有用粒子基礎上增加粒子多樣性，提高了移動機器人的定位性能和構建地圖的準確性。將其與傳統SLAM算法進行了性能仿真對比，改進算法的估計精度優于傳統SLAM算法，能更準確地表達真實粒子的狀態。最后利用Turtlebot 機器人在真實場景下對改進算法地圖構建進行實驗。結果表明改進SLAM算法比原始SLAM算法構建地圖準確性更高，能清晰地表達障礙物的形狀及其位置信息，更能清晰準確體現所構建環境地圖的細節及邊緣信息。