基于激光SLAM的導(dǎo)航車軟硬件系統(tǒng)

文/鄭建杰 張海濤 陳寅

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

SLAM的概念最早是由Smith和Cheesman等[2-3]提出的，同時將概率估計方法應(yīng)用到機(jī)器人定位與建圖。1987年，Smith Self和Cheesman提出了基于卡爾曼濾波的SLAM算法。基于卡爾曼濾波器的 SLAM 算法主要是通過卡爾曼濾波器對機(jī)器人的位姿信息不斷的估計和更新，利用里程計估計機(jī)器人所走軌跡以及機(jī)器人此時的位姿，激光測距采集的觀測數(shù)據(jù)作為機(jī)器人的真實位姿加入卡爾曼濾波器的計算中，不斷校正機(jī)器人對自身位置的估計，使估計量不斷接近真實狀態(tài)[4]。但在實際運用過程中，不管怎樣改進(jìn)和調(diào)整濾波方程，基于卡爾曼濾波器的 SLAM 算法仍然存在計算復(fù)雜度高、可靠性不強(qiáng)以及數(shù)據(jù)關(guān)聯(lián)等問題[5]。為解決非線性濾波問題，Metropolis等人提出了粒子濾波算法(Particle Filter,PF)[6]。在強(qiáng)非線性非高斯環(huán)境下，粒子濾波算法的跟蹤性能要遠(yuǎn)遠(yuǎ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所示。圖中激光雷達(dá)和里程計分別將獲取的距離信息和位姿信息送往控制單元，經(jīng)里面的SLAM算法計算、融合，得到導(dǎo)航車周圍的地圖信息及自身的位姿，并通過熱點接入網(wǎng)絡(luò)，將地圖和位姿信息傳遞到人機(jī)交互單元，實現(xiàn)周圍地圖及導(dǎo)航車的可視化。同時結(jié)合接收到的任務(wù)，將其轉(zhuǎn)化為控制指令，通過RΟS各個串口節(jié)點傳遞給執(zhí)行單元，改變電機(jī)轉(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電機(jī)，最高速可達(dá)1m/s,最大爬坡能力為15°,轉(zhuǎn)向方式采用四驅(qū)差速轉(zhuǎn)向。同時裝有增量式編碼器以及gy-85九軸AHRS慣導(dǎo)模塊。

2 SLAM功能實現(xiàn)

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

圖2：SLAM的定義

2.1 建圖

貝葉斯(Bayes)濾波大多用于解決一般的線性、高斯系統(tǒng)的后驗概率的問題,對于一些非線性,非高斯系統(tǒng)的后驗概率的問題,引入蒙特卡洛(MonteCarlo)算法,蒙特卡洛采用樣本形式而不是以函數(shù)形式對先驗和后驗概率進(jìn)行求解,不需要積分等復(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（擴(kuò)展卡爾曼濾波）和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：地圖估計，使用卡爾曼濾波對上式進(jìn)行計算。

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

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

2.2 定位

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

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

圖10：建圖結(jié)果

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

（1）預(yù)測階段。基本思想是選擇一個估計密度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)以及由過濾器維護(hù)的姿態(tài)估計集合。

3 算法試驗

3.1 室內(nèi)

基于激光slam搭建的導(dǎo)航車如圖5所示，在室內(nèi)真實環(huán)境下進(jì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 室外

在校園隨機(jī)選一茂密的小樹林，如圖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é)

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