文/鄭建杰 張海濤 陳寅

Simultaneous Localization and Mapping(SLAM)同步定位與地圖構(gòu)建，它被定義為解決“機器人從未知環(huán)境的未知地點出發(fā)，在運動過程中通過重復(fù)觀測到的地圖特征（比如，墻角，柱子等）定位自身位置和姿態(tài)，再根據(jù)自身位置增量式的構(gòu)建地圖，從而達到同時定位和地圖構(gòu)建的目”的問題方法的統(tǒng)稱[1]。

SLAM的概念最早是由Smith和Cheesman等[2-3]提出的，同時將概率估計方法應(yīng)用到機器人定位與建圖。1987年，Smith Self和Cheesman提出了基于卡爾曼濾波的SLAM算法?；诳柭鼮V波器的 SLAM 算法主要是通過卡爾曼濾波器對機器人的位姿信息不斷的估計和更新，利用里程計估計機器人所走軌跡以及機器人此時的位姿，激光測距采集的觀測數(shù)據(jù)作為機器人的真實位姿加入卡爾曼濾波器的計算中，不斷校正機器人對自身位置的估計，使估計量不斷接近真實狀態(tài)[4]。但在實際運用過程中，不管怎樣改進和調(diào)整濾波方程，基于卡爾曼濾波器的 SLAM 算法仍然存在計算復(fù)雜度高、可靠性不強以及數(shù)據(jù)關(guān)聯(lián)等問題[5]。為解決非線性濾波問題，Metropolis等人提出了粒子濾波算法(Particle Filter,PF)[6]。在強非線性非高斯環(huán)境下，粒子濾波算法的跟蹤性能要遠遠優(yōu)于卡爾曼濾波[7]。在此基礎(chǔ)上，Doucet 等[8]提出基于 Rao-Blackwellized 的粒子濾波器(RBPF)的方法，目前在解決SLAM問題上應(yīng)用最廣。

1 SLAM導(dǎo)航車軟硬件系統(tǒng)構(gòu)建

圖1：硬件系統(tǒng)框圖

導(dǎo)航車硬件平臺的系統(tǒng)框圖如圖1所示。圖中激光雷達和里程計分別將獲取的距離信息和位姿信息送往控制單元，經(jīng)里面的SLAM算法計算、融合，得到導(dǎo)航車周圍的地圖信息及自身的位姿，并通過熱點接入網(wǎng)絡(luò)，將地圖和位姿信息傳遞到人機交互單元，實現(xiàn)周圍地圖及導(dǎo)航車的可視化。同時結(jié)合接收到的任務(wù)，將其轉(zhuǎn)化為控制指令，通過RΟS各個串口節(jié)點傳遞給執(zhí)行單元，改變電機轉(zhuǎn)速，從而控制導(dǎo)航車的行駛。

1.1 核心處理器

導(dǎo)航車車載核心處理器采用Ιntel-NUC-8i5BEK，擁有第八代智能英特爾酷睿i5-8259U處理器，8G內(nèi)存，并搭載英特爾銳炬Plus顯卡655， HDMΙ接口x2，前端 USB3.1接口2個，后端USB3.1接口2個，2個內(nèi)部USB2.0端口，內(nèi)部內(nèi)存64G，搭載英特爾雙頻Wireless-AC 9560無線網(wǎng)卡。

1.2 其它模塊

設(shè)計的導(dǎo)航車采用鋁合金骨架+鋁合金外殼，12V59Rpm電機，最高速可達1m/s,最大爬坡能力為15°,轉(zhuǎn)向方式采用四驅(qū)差速轉(zhuǎn)向。同時裝有增量式編碼器以及gy-85九軸AHRS慣導(dǎo)模塊。

2 SLAM功能實現(xiàn)

機器人研究的問題包含許許多多的領(lǐng)域，我們常見的幾個研究的問題包括：建圖(Mapping)、定位(Localization)和路徑規(guī)劃（Path Planning）。而同步定位與建圖（SLAM）問題位于定位和建圖的交集部分，如圖2所示。SLAM需要機器人在未知的環(huán)境中逐步建立起地圖，然后根據(jù)地圖確定自身位置，從而進一步定位。

圖2：SLAM的定義

2.1 建圖

貝葉斯(Bayes)濾波大多用于解決一般的線性、高斯系統(tǒng)的后驗概率的問題,對于一些非線性,非高斯系統(tǒng)的后驗概率的問題,引入蒙特卡洛(MonteCarlo)算法,蒙特卡洛采用樣本形式而不是以函數(shù)形式對先驗和后驗概率進行求解,不需要積分等復(fù)雜計算,RBPFSLAM（Rao-Blackwellized Particle Filter） 算法的實現(xiàn)正是通過非參數(shù)化的蒙特卡洛代替貝葉斯方法來解決在大型復(fù)雜環(huán)境下概率估計問題 [9]。SALM要解決的問題就是由控制數(shù)據(jù)u1:t和觀測數(shù)據(jù)z1:t來求位姿和地圖的聯(lián)合分布P(x1:t,m|z1:t,u1:t-1)。RBPF-SLAM融合了EKF（擴展卡爾曼濾波）和PF（粒子濾波）的優(yōu)點，在降低計算復(fù)雜度的同時又有較好的魯棒性，基于 SLAM 問題的 Rao-Blackwellized 變換可以描述為：

式（1）中描述了Rao-Blackwellized分解的核心思想，用條件概率把P(x1:t,m|z1:t,u1:t-1)拆開先求解位姿，而后建圖。RBPF使用粒子濾波來估計位姿，算法總共包含四個步驟：采樣-計算權(quán)重-重采樣-地圖估計。

圖3：gmapping算法運行結(jié)構(gòu)圖

圖4：AMCL架構(gòu)圖

圖5：導(dǎo)航車實物圖

圖6：實際測試環(huán)境

圖7：建圖結(jié)果

圖8：導(dǎo)航過程

步驟1：采樣。

步驟2：計算權(quán)重。

步驟3：重采樣：舍棄低權(quán)值粒子，只保留高權(quán)值的粒子，由粒子的后驗分布和后驗概率密度，可求得地圖的后驗概率為：

步驟4：地圖估計，使用卡爾曼濾波對上式進行計算。

本系統(tǒng)基于RBPF的算法，根據(jù)激光雷達和里程計的信息，對環(huán)境地圖進行構(gòu)建，并且對自身狀態(tài)進行估計，算法的實際運行的結(jié)構(gòu)如圖3。

它主要輸入機器人底盤和激光雷達之間、底盤和里程計原點之間的坐標(biāo)變換以及激光雷達數(shù)據(jù)，經(jīng)過slam_gmapping運算后，輸出地圖和里程計原點之間的變換，建立的地圖及其相關(guān)信息。

2.2 定位

Adaptive Mentcarto Localization(AMCL),蒙特卡洛自適應(yīng)定位是一種很常用的定位算法，它通過比較檢測到的障礙物和已知地圖來進行定位，同時可對里程計誤差進行修正。它是通過改變采樣方式和濾波條件,以改進蒙特卡洛算法。在采樣階段,自適應(yīng)蒙特卡洛算法是把前一時刻的采樣點作為圓心進行采樣,這使得實際密度與估計密度接近,從而有效提高定位精度；在濾波階段，自適應(yīng)蒙特卡洛算法是給粒子賦予權(quán)值,這樣離節(jié)點真實位置越近的粒子,權(quán)值越大。[10]通過結(jié)合粒子位置和粒子權(quán)值可以估算節(jié)點的真實位置,并減小測量誤差。其定位過程主要分為預(yù)測階段和濾波階段，具體如下：

圖9：實際測試環(huán)境

圖10：建圖結(jié)果

圖11：導(dǎo)航過程

（1）預(yù)測階段?；舅枷胧沁x擇一個估計密度q(x)來代替采樣的真實概率密度p(x),即：

而后利用多個q（x）獨立樣本的加權(quán)來求近似：

其中，Np為采樣粒子個數(shù)，為權(quán)重。

（2）濾波階段。主要是通過粒子的位置以及權(quán)值的大小來提高各節(jié)點的定位精度。假設(shè)k時刻獲取的采樣粒子中，只有P1P2P3P4P7的待測點區(qū)域可構(gòu)成封閉圖形，則可得到該5個粒子的密集程度權(quán)值為：

根據(jù)這5個粒子的權(quán)值，可得到采樣集合與真實位置的相關(guān)程度：

AMCL架構(gòu)圖如圖4所示。

蒙特卡洛自適應(yīng)定位主要是輸入坐標(biāo)變換和激光掃描數(shù)據(jù)，經(jīng)過AMCL運算后，輸出地圖與里程計原點之間的坐標(biāo)變換（其實質(zhì)就是對導(dǎo)航車的定位），導(dǎo)航車在地圖上的估計姿態(tài)以及由過濾器維護的姿態(tài)估計集合。

3 算法試驗

3.1 室內(nèi)

基于激光slam搭建的導(dǎo)航車如圖5所示，在室內(nèi)真實環(huán)境下進行定位與導(dǎo)航的測試，實際測試環(huán)境如圖6所示。啟動導(dǎo)航車,繞室內(nèi)運動一周，完成slam建圖，建圖結(jié)果如圖7所示。而后給定其一個目標(biāo)點，導(dǎo)航車自主規(guī)劃一條路線，并運動到目標(biāo)點，導(dǎo)航過程如圖8所示。經(jīng)實際測試室內(nèi)構(gòu)建的地圖精度較高，且自主導(dǎo)航過程路線規(guī)劃合理，系統(tǒng)魯棒性較好。

3.2 室外

在校園隨機選一茂密的小樹林，如圖9所示。啟動導(dǎo)航車,在小樹林里運動一圈，完成slam建圖，建圖結(jié)果如圖10所示。而后給定其一個目標(biāo)點，導(dǎo)航車自主規(guī)劃一條路線，并運動到目標(biāo)點，導(dǎo)航過程如圖11所示。通過實驗檢測，室外復(fù)雜環(huán)境下導(dǎo)航車的建圖、定位精度較高，且自主導(dǎo)航過程路線規(guī)劃合理，系統(tǒng)魯棒性較好。

4 總結(jié)

本文基于激光雷達slam技術(shù)，采用RBPF粒子濾波和蒙特卡洛自適應(yīng)定位算法，搭建一款可自主建圖、定位和導(dǎo)航的無人導(dǎo)航車。通過室內(nèi)及室外復(fù)雜環(huán)境下的實驗測試，導(dǎo)航車的精度和魯棒性較好，具有較好的應(yīng)用前景。但是實驗過程中也發(fā)現(xiàn)障礙物高度低于激光雷達所在平面時，無法被檢測出來，下步我們將基于此，進行更進一步的研究和改進。