趙華東，張耀輝，吳畏

（長(zhǎng)安大學(xué) 汽車(chē)學(xué)院，陜西 西安 710064）

引言

生活節(jié)奏日益加快的今天，掃地機(jī)器人、無(wú)人送貨車(chē)及無(wú)人清掃車(chē)等在普通居民、商場(chǎng)、景區(qū)等得到了充分使用。同時(shí)定位與建圖問(wèn)題的解決是上述產(chǎn)品完善，甚至當(dāng)下研究熱門(mén)的自動(dòng)駕駛技術(shù)實(shí)現(xiàn)的必要步驟。

目前，2D 激光SLAM的實(shí)現(xiàn)主要分為基于濾波器和基于非線(xiàn)性?xún)?yōu)化兩種方法。基于濾波器的方法包括擴(kuò)展卡爾曼濾波（EKF）、無(wú)跡卡爾曼濾波（UKF）、以及粒子濾波（PF）[1]?；诜蔷€(xiàn)性?xún)?yōu)化的SLAM方法主要是利用圖優(yōu)化的框架實(shí)現(xiàn)SLAM，使用廣泛的是由Kohlbrecher S等人于2011年提出了Hector-SLAM方法[2]。這些方法各有優(yōu)勢(shì)，本文介紹粒子濾波算法及其SALM方法的實(shí)現(xiàn)。

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

1 SLAM實(shí)現(xiàn)方法

1.1 粒子濾波算法

粒子濾波是貝葉斯濾波的一種非參數(shù)實(shí)現(xiàn)方法，貝葉斯濾波如公式（1）、公式（2）。公式中xt表示t時(shí)刻的狀態(tài)，xt-1表示t-1時(shí)刻的狀態(tài)，表示預(yù)測(cè)，bel（xt）表示更新，狀態(tài)轉(zhuǎn)移概率由p（xt|ut，xt-1）表示，p（zt|xt）表示測(cè)量概率，zt表示測(cè)量，ut表示控制。

粒子濾波算法如程序1：

程序1 粒子濾波算法

粒子濾波算法程序是輸入t-1時(shí)刻的狀態(tài)粒子集xt-1、t時(shí)刻的控制量ut和測(cè)量zt，最終返回t時(shí)刻粒子集 χt。算法是通過(guò)對(duì)輸入的粒子集χt-1中的粒子進(jìn)行處理，從而構(gòu)造一個(gè)用來(lái)表示預(yù)測(cè)置信度的粒子集。然后將粒子集轉(zhuǎn)換成近似后驗(yàn)分布bel（xt）的粒子集χt。具體說(shuō)明如下：

（3）程序第8-11行實(shí)現(xiàn)了粒子濾波的重采樣。這個(gè)過(guò)程按照權(quán)值給定的概率從粒子集中中抽取并替換M個(gè)粒子。通過(guò)這樣的權(quán)值合并到重采樣的過(guò)程，使得粒子的分布從近似后驗(yàn)bel（xt）轉(zhuǎn)變?yōu)榘凑展剑?）的后驗(yàn)分布。

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

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

1.2.1 重要性計(jì)算

粒子重要性計(jì)算如公式（4）所示。上述公式中k表示粒子索引，表示第k個(gè)開(kāi)始到t時(shí)刻的狀態(tài)粒子，c表示一致性變量，η表示歸一化系數(shù)。

2 智能車(chē)實(shí)驗(yàn)平臺(tái)

智能車(chē)實(shí)驗(yàn)平臺(tái)如圖1所。

圖1 智能車(chē)

2.1 底層控制部分及底盤(pán)

底層控制器采用 STM32芯片作為處理單元，接受控制信號(hào)控制機(jī)器人移動(dòng)，獲取IMU單元所測(cè)得的底盤(pán)機(jī)器人的位姿。STM32通過(guò)TTL-USB轉(zhuǎn)接線(xiàn)接入樹(shù)莓派USB接口，與樹(shù)莓派通訊。并通過(guò)這樣的方式將位姿信號(hào)傳送給作為機(jī)器人上層控制器的樹(shù)莓派。

底盤(pán)部分包括兩個(gè)編碼電機(jī)驅(qū)動(dòng)的車(chē)輪，電機(jī)驅(qū)動(dòng)板等。通過(guò)對(duì)兩個(gè)車(chē)輪的差速控制實(shí)現(xiàn)轉(zhuǎn)向，前進(jìn)等移動(dòng)。

2.2 核心控制部件

本文采用樹(shù)莓派 3B作為核心控制部件，用以獲取機(jī)器人的位姿信息，獲取來(lái)自單線(xiàn)激光雷達(dá)RP-lidar的點(diǎn)云信息。樹(shù)莓派使用支持 ARM 處理器的 Ubuntu操作系統(tǒng)并安裝了ROS-Kinetic版本的機(jī)器人操作系統(tǒng)。

在樹(shù)莓派中下載Gmapping的ROS功能包，設(shè)置功能包所需的參數(shù)文件。設(shè)置啟動(dòng)各節(jié)點(diǎn)的“.launch”文件，在ROS環(huán)境下編譯底盤(pán)的上層控制節(jié)點(diǎn)功能包、SLAM功能包、雷達(dá)點(diǎn)云信息節(jié)點(diǎn)功能包等。

3 底盤(pán)機(jī)器人建圖實(shí)驗(yàn)

PC機(jī)與樹(shù)莓派建立通訊，啟動(dòng)設(shè)置好的.launch文件，實(shí)驗(yàn)時(shí)控制智能車(chē)移動(dòng)，完成對(duì)其所在環(huán)境的掃描獲取點(diǎn)云信息，實(shí)時(shí)的結(jié)合慣性測(cè)量單元獲取的位姿信息，構(gòu)建場(chǎng)景地圖，場(chǎng)景如圖2所示。使用ROS下的Rviz軟件顯示建圖過(guò)程，如圖3所示。建圖結(jié)果由PC機(jī)Ubuntu操作系統(tǒng)終端輸入指令獲得并保存，如圖4所示。圖3和圖4中的所顯示的障礙物為場(chǎng)景中所放置的紙箱。試驗(yàn)結(jié)果表明，獲取的地圖精度較高，穩(wěn)定性強(qiáng)。

圖2 場(chǎng)景圖

圖3 Rviz顯示建圖過(guò)程

圖4 Gmapping獲取得地圖

4 結(jié)束語(yǔ)

本文對(duì)在智能車(chē)上實(shí)現(xiàn)基于粒子濾波算法的激光SLAM。實(shí)現(xiàn)了對(duì)智能車(chē)所處場(chǎng)景的實(shí)時(shí)定位與地圖構(gòu)建。實(shí)驗(yàn)表明在這樣的小范圍場(chǎng)景下，基于粒子濾波的SLAM方法可以獲得精準(zhǔn)的場(chǎng)景地圖。在后續(xù)的避障、導(dǎo)航等研究與實(shí)現(xiàn)中提供重要基礎(chǔ)。這也為無(wú)人駕駛技術(shù)中SLAM提供了某種程度的借鑒經(jīng)驗(yàn)。