基于移動機器人的地圖構建技術*

杜 晨, 杜 煜

(1.北京聯合大學 北京市信息服務工程重點實驗室，北京 100101；2.北京聯合大學 機器人學院，北京 100101)

0 引 言

作為機器人自主定位導航技術的關鍵，同時定位與地圖構建(simultaneous localization and mapping，SLAM)技術也正成為關注焦點[1,2]。移動機器人的SLAM技術包括對機器人的狀態和環境信息的同時估計[3]。

Rao-Blackwellized粒子濾波器(Rao-Blackwellized particle filter,RBPF)是解決SLAM技術的方法之一。經過多年的發展也出現了很多改進辦法，國內的王田橙[4]、國外PopovlG[5]等人都在不同程度上對RBPF-SLAM算法做出了改進。

本文在RBPF-SLAM方法的基礎上，將采樣集中在觀測信息的可能性區域，減少了與目標分布的誤差，使得采樣的粒子更加符合真實環境狀態，建立更準確的地圖環境；并且引入分層重采樣優化策略，通過控制閾值，維持盡可能多的粒子多樣性，有效地解決了粒子退化問題。最后，為了驗證算法的有效性，以Bulldog移動機器人平臺為基礎，開展了相關實驗。并針對實驗平臺，優化了掃描處理步驟，加快了掃描處理時間，有效降低了實驗運行時間。

1 基于RBPF-SLAM算法的優化

1.1 RBPF-SLAM原理

RBPF-SLAM問題可理解為:初始地圖m0與初始位姿x0已知的情況下，從最初到時刻t的傳感器觀測信息z1:t=z1,…,zt，與移動機器人里程計的控制信息u1:t-1=u1,…,ut-1，來估計p(x1:t,m|z1:t,u1:t-1)，即機器人軌跡x1:t=x1…,xt與地圖m的聯合后驗概率分布。通過貝葉斯濾波器可以對其進行分解

p(x1:t,m|z1:t,u1:t-1)=p(m|x1:t,z1:t)p(x1:t|z1:t,u1:t-1)

(1)

RBPF-SLAM算法采用粒子濾波器來估算機器人的位姿，其中，每一個粒子代表一條可能的軌跡。包含4個主要步驟：采樣、權重計算、粒子重采樣、地圖更新。在權重計算階段可以通過限制概率密度函數來進行

(2)

式中η=1/p(zt|z1:t-1,u1:t-1)為貝葉斯定律中歸一化因子，所有粒子擁有相同的η。

1.2 優化的RBPF-SLAM

1.2.1 融合觀測信息的提議分布

機器人在實際移動過程中，激光雷達作為機器人的外部傳感器具有較高的精度，似然函數區域比較集中；而基于里程計的運動模型，因里程計自身誤差較大，似然函數區域比較分散，導致了兩者之間重合的地方很少。

融合觀測信息[6]，將采樣集中在觀測信息的可能性區域。可以得到關于粒子權重方差的最佳提議分布，減少與目標分布的誤差，使得采樣的粒子更加符合真實環境狀態，建立更準確的地圖環境。此時的提議分布

進一步更新式(2)，得到

在似然函數的峰值進行采樣，可以得到化簡之后的權重計算

1.2.2 重采樣優化策略

引入有效粒子數Neff來估計當前粒子集代表目標函數的近似程度

為解決粒子退化問題[7，8]，本文引入了一種分層重采樣優化策略。首先設置采樣粒子高權重閾值ωk=2/N和低權重閾值ωL=1/(2N)，其中N代表粒子數。根據設定的閾值，把粒子權值分為3個區間，較高權值區間、較低權值區間和中等權值區間。其中，對中等權值區間的粒子不做處理，對具有較高和較低權值區間的粒子按權值大小由高到低進行排序，之后再計算每個粒子被選擇的概率

p(i)={a+[rank(i)/(Nh+Nl-1)]·

(b-a)}/(Nh+Nl)

(7)

式中 rank(i)為粒子根據權重排序后的序號，a,b為系數，Nh和Nl分別為高權重粒子數和低權重粒子數。

1.2.3 優化RBPF-SLAM算法流程

1)首先，估計機器人初始位姿；2)使用迭代最近鄰ICP算法進行點云匹配；3)進行采樣，計算目標分布，進而求得提議分布；4)在融合觀測信息的提議分布中采樣求得新粒子集，計算粒子權重；5)進行優化重采樣策略更新粒子權值；6)地圖更新。

2 移動機器人系統

2.1 移動機器人硬件平臺

本實驗平臺為長90 cm、寬70 cm 、高80 cm的Bulldog機器人，實物圖如圖1(a)所示。

本實驗選取Rslidar激光雷達傳感器以及里程計進行實驗。Rslidar激光雷達集合了16個激光收發元器件，可以按照設定的頻率進行掃描。里程計能夠記錄機器人走過的路程以及方向等信息。其內部存在一個用串口線連接的PC，外部PC連接Bulldog的路由，通過SSH協議實現通信。

Bulldog輪式機器人包含感知模塊、決策模塊和底盤控制模塊等。當控制層對Bulldog輪式機器人進行控制時，感知層中的傳感器會接收到自身以及周圍場景的信號。之后，將信號節點發布到內部PC中，外部PC連接Bulldog的路由，實現與內部PC的通信。在訂閱節點信息的同時，對這些數據進行處理，根據各自算法的需求，來完成相應的實驗。

2.2 移動機器人軟件系統

機器人操作系統(robot operating system,ROS)是移動機器人研究的基礎，ROS的提出推動了機器人產業的發展。系統框架分為:節點(node)、節點管理器(master)、消息(message)、話題(topic)。其中，節點是核心，可以執行簡單的程序語句[9]。

內部PC為Linux(Ubuntu14.04)上運行的ROS，主要節點簡化圖如圖1(b)所示。

圖1 移動機器人及ROS的主要節點

在ROS框架下，激光雷達傳感器用激光雷達節點來實現激光雷達數據幀的讀取，由/scan話題完成激光數據的發布工作。里程計節點得到里程計信息，由/odom完成里程計信息的發布工作。所有話題都會發布到公共主題當中，等待其它節點訂閱。ROS中還提供了一個可視化工具，可以實現實時在線地圖構建的顯示。

由于實驗平臺實現的功能較簡單，僅在移動機器人已經行進了給定距離后才開始處理掃描數據，隨著掃描數據的增多，運行時間會加大。因而，在掃描處理上也做出了優化改進，增加了限定時間的掃描處理條件，以便更加快速的處理掃描。

3 實驗環境與分析

3.1 實驗環境與結果

選取由簡單到復雜的兩個實驗場景，分別用傳統RBPF-SLAM算法即Gmapping算法、其它改進RBPF-SLAM算法[7]以及本文優化的RBPF-SLAM算法進行實驗。

實驗場景一：近似地圖為80 m×2 m，環境比較簡單，包括長走廊，電梯口等，真實場景如圖2(a)所示。三種算法使用相同的粒子數，構建地圖結果如圖2(b)、(c)、(d)所示。

圖2 三種算法分別構圖結果

圖2(b)、(c)、(d)是移動機器人從A點到B點再到C點使用不同算法在RVIZ上顯示的構圖情況。選取的柵格大小同樣為5 cm×5 cm。從構建地圖的最終結果來看，隨著時間增加，圖2(b)中會出現邊緣地方不一致性現象，如圖中標出的地方；圖2(c)和圖2(d)構圖效果較好，可以明顯地看出D點凹凸的輪廓。

在場景一中分別進行5次實驗，計算這兩種算法創建相同一致性地圖時，需要的平均運行時間。實驗時，機器人平均速度為0.4 m/s，改進算法需要258 s，本文優化的RBPF-SLAM算法需要232 s，比改進算法更快。

實驗場景二：近似地圖為6 m×8 m，存在一些障礙物，如辦公桌椅、書柜、長桌等，如圖3(a)所示。同樣使用三種算法進行對比實驗。查看構建一致性地圖時，三種算法使用粒子數的情況。圖3(b)是本文優化的RBPF-SLAM算法構建的地圖結果。

圖3 實驗場景及本文算法構圖結果

當構建的地圖結果中A點、B點和C點都能清晰看出輪廓時，記錄三種算法使用粒子數的情況，傳統RBPF-SLAM算法使用38個粒子，改進RBPF-SLAM算法使用20個粒子，本文優化的RBPF-SLAM算法只需要18個粒子。

3.2 實驗結果分析

從構建地圖結果中可以看出，當實驗環境較簡單(實驗場景一)，使用相同粒子數時，其它研究者的改進算法以及本文優化的RBPF-SLAM算法構圖結果更加可靠，地圖出現不一致性現象的情況較少。說明將采樣更集中在觀測信息的可行性區域，使得采樣的粒子更加符合真實環境的狀態，可以改善地圖構建不一致性。而構建相同精度的地圖時，與改進RBPF-SLAM算法相比較，本文優化的RBPF-SLAM算法所需要的平均運行時間更短，算法的效率更高。

當實驗環境復雜一些(實驗場景二)，構建相同清晰度的地圖時，本文優化的RBPF-SLAM算法使用的粒子數更少，有效地降低了算法的計算量。

為了對算法的粒子多樣性進行驗證，同樣用三種算法在使用相同30個粒子時，查看重采樣過程中粒子多樣性變化情況，如圖4所示。

圖4 重采樣過程中粒子多樣性變化

由圖4可以看出：本文優化的RBPF-SLAM算法，在重采樣過程中，不同粒子數明顯多于傳統的RBPF-SLAM算法；與改進的RBPF-SLAM算法相比較，粒子多樣性的波動范圍會更大一些。也說明了優化算法不僅可以緩解粒子退化現象，而且粒子的多樣性更好一些。

4 結 論

本文優化的Rao-Blackwellized粒子濾波的同時定位與地圖構建算法，在提議分布中融合了激光雷達的觀測信息，將采樣集中在觀測信息的可能性區域，減少了與目標分布的誤差，提高了準確度，有效地減少了地圖不一致性。而通過控制權值的閾值，維持盡可能多的粒子多樣性的分層重采樣優化策略，有效緩解了粒子退化現象。而進一步優化的掃描處理，增加了限定時間的掃描處理條件，加快了處理速度，降低了運行時間。下一步工作，嘗試在更大的實驗環境下驗證優化RBPF-SLAM算法的魯棒性。