智能車激光SLAM的研究與實現

趙華東，張耀輝，吳畏

（長安大學 汽車學院，陜西 西安 710064）

引言

生活節奏日益加快的今天，掃地機器人、無人送貨車及無人清掃車等在普通居民、商場、景區等得到了充分使用。同時定位與建圖問題的解決是上述產品完善，甚至當下研究熱門的自動駕駛技術實現的必要步驟。

目前，2D 激光SLAM的實現主要分為基于濾波器和基于非線性優化兩種方法。基于濾波器的方法包括擴展卡爾曼濾波（EKF）、無跡卡爾曼濾波（UKF）、以及粒子濾波（PF）[1]。基于非線性優化的SLAM方法主要是利用圖優化的框架實現SLAM，使用廣泛的是由Kohlbrecher S等人于2011年提出了Hector-SLAM方法[2]。這些方法各有優勢，本文介紹粒子濾波算法及其SALM方法的實現。

在2007年，Grisetti等人提出了基于粒子濾波實現小范圍的激光SLAM的Gmapping方法[3]。Gmapping是本文實現激光SLAM時所采用的方法，其運算速率較高，且獲取地圖精度較高。

1 SLAM實現方法

1.1 粒子濾波算法

粒子濾波是貝葉斯濾波的一種非參數實現方法，貝葉斯濾波如公式（1）、公式（2）。公式中xt表示t時刻的狀態，xt-1表示t-1時刻的狀態，表示預測，bel（xt）表示更新，狀態轉移概率由p（xt|ut，xt-1）表示，p（zt|xt）表示測量概率，zt表示測量，ut表示控制。

粒子濾波算法如程序1：

程序1 粒子濾波算法

粒子濾波算法程序是輸入t-1時刻的狀態粒子集xt-1、t時刻的控制量ut和測量zt，最終返回t時刻粒子集 χt。算法是通過對輸入的粒子集χt-1中的粒子進行處理，從而構造一個用來表示預測置信度的粒子集。然后將粒子集轉換成近似后驗分布bel（xt）的粒子集χt。具體說明如下：

（3）程序第8-11行實現了粒子濾波的重采樣。這個過程按照權值給定的概率從粒子集中中抽取并替換M個粒子。通過這樣的權值合并到重采樣的過程，使得粒子的分布從近似后驗bel（xt）轉變為按照公式（2）的后驗分布。

1.2 基于粒子濾波的SLAM方法原理

由于粒子濾波的自身的特點，直接應用于SLAM在描述地圖時，包含大量變量，這注定失敗。Murphy等人正式將R-B粒子濾波（Rao-Blackwellized Particle Filters）引入SLAM[4]。如公式（3）所示的因式分解，將表示SLAM問題的后驗概率轉換為狀態后驗概率p（x1:t|z1:t，u1:t-1）和地圖后驗概率p（m|z1:t，u1:t-1）乘積表示。式中m表示地圖特征，x1:t表示從開始到時刻t的狀態，z1:t表示開始到t時刻的測量，u1:t-1表示開始到t-1時刻的控制。這樣在解決SLAM問題時可以先獲得智能車位姿軌跡等定位信息，在此基礎上計算更新當前時刻的地圖信息。這樣已知位姿的建圖使粒子濾波解決SALM問題變得可行。

1.2.1 重要性計算

粒子重要性計算如公式（4）所示。上述公式中k表示粒子索引，表示第k個開始到t時刻的狀態粒子，c表示一致性變量，η表示歸一化系數。

2 智能車實驗平臺

智能車實驗平臺如圖1所。

圖1 智能車

2.1 底層控制部分及底盤

底層控制器采用 STM32芯片作為處理單元，接受控制信號控制機器人移動，獲取IMU單元所測得的底盤機器人的位姿。STM32通過TTL-USB轉接線接入樹莓派USB接口，與樹莓派通訊。并通過這樣的方式將位姿信號傳送給作為機器人上層控制器的樹莓派。

底盤部分包括兩個編碼電機驅動的車輪，電機驅動板等。通過對兩個車輪的差速控制實現轉向，前進等移動。

2.2 核心控制部件

本文采用樹莓派 3B作為核心控制部件，用以獲取機器人的位姿信息，獲取來自單線激光雷達RP-lidar的點云信息。樹莓派使用支持 ARM 處理器的 Ubuntu操作系統并安裝了ROS-Kinetic版本的機器人操作系統。

在樹莓派中下載Gmapping的ROS功能包，設置功能包所需的參數文件。設置啟動各節點的“.launch”文件，在ROS環境下編譯底盤的上層控制節點功能包、SLAM功能包、雷達點云信息節點功能包等。

3 底盤機器人建圖實驗

PC機與樹莓派建立通訊，啟動設置好的.launch文件，實驗時控制智能車移動，完成對其所在環境的掃描獲取點云信息，實時的結合慣性測量單元獲取的位姿信息，構建場景地圖，場景如圖2所示。使用ROS下的Rviz軟件顯示建圖過程，如圖3所示。建圖結果由PC機Ubuntu操作系統終端輸入指令獲得并保存，如圖4所示。圖3和圖4中的所顯示的障礙物為場景中所放置的紙箱。試驗結果表明，獲取的地圖精度較高，穩定性強。

圖2 場景圖

圖3 Rviz顯示建圖過程

圖4 Gmapping獲取得地圖

4 結束語

本文對在智能車上實現基于粒子濾波算法的激光SLAM。實現了對智能車所處場景的實時定位與地圖構建。實驗表明在這樣的小范圍場景下，基于粒子濾波的SLAM方法可以獲得精準的場景地圖。在后續的避障、導航等研究與實現中提供重要基礎。這也為無人駕駛技術中SLAM提供了某種程度的借鑒經驗。