2D激光slam算法在室內(nèi)建圖對(duì)比研究

孫宗濤　朱永強(qiáng)

摘要：目前無(wú)人駕駛與機(jī)器人行業(yè)發(fā)展火熱，需要對(duì)汽車與機(jī)器人所處環(huán)境進(jìn)行準(zhǔn)確的建模來(lái)得到一張高精度的地圖。激光SLAM作為目前最穩(wěn)定、最主流的定位導(dǎo)航方法而被人們廣泛使用。本文使用Gmapping算法與Hector算法在搭載低頻激光雷達(dá)的差速兩輪小車上對(duì)室內(nèi)環(huán)境進(jìn)行建模，并將兩種算法所建地圖進(jìn)行對(duì)比分析，結(jié)果表明在室內(nèi)環(huán)境下Gmapping算法所建地圖的精度更高，建圖效果優(yōu)于Hector算法。

關(guān)鍵詞：激光SLAM;gmapping算法;室內(nèi);建圖

0 ?引言

SLAM 技術(shù)發(fā)展到現(xiàn)在已經(jīng)經(jīng)過(guò)了30多年，涉及的技術(shù)領(lǐng)域眾多。其中需要多個(gè)步驟來(lái)完成，如今多樣化的算法為實(shí)現(xiàn)這些步驟提供了多種選擇，SLAM技術(shù)也是如今無(wú)人駕駛與機(jī)器人領(lǐng)域的研究的熱點(diǎn)話題。SLAM（Simultaneous Localization and Mapping），中文翻譯為同步定位與地圖創(chuàng)建。SLAM主要解決以下的問(wèn)題：當(dāng)處于未知環(huán)境時(shí)，汽車或者機(jī)器人怎樣通過(guò)對(duì)環(huán)境的偵測(cè)來(lái)對(duì)自身運(yùn)動(dòng)軌跡進(jìn)行確定，同時(shí)構(gòu)建出所處環(huán)境的地圖。

SLAM可以根據(jù)應(yīng)用場(chǎng)景、核心算法、傳感器分為很多種。例如當(dāng)采用傳感器不同時(shí)，可以分為基于激光雷達(dá)的2D和3D SLAM、基于深度攝像機(jī)的RGBD SLAM、基于視覺(jué)傳感器的visual SLAM、基于視覺(jué)傳感器和慣性單元的VIO。

目前基于激光雷達(dá)的2D SLAM技術(shù)發(fā)展成熟，在ROS平臺(tái)下主要有兩種基于激光雷達(dá)的地圖構(gòu)建方法：Gmapping和Hector算法。Gmapping算法采用Rao-Blackwellized粒子濾波方法，借助里程計(jì)信息實(shí)現(xiàn)位姿估計(jì);而Hector算法采用高斯-牛頓迭代法，通過(guò)激光數(shù)據(jù)匹配進(jìn)行位姿估計(jì)[1]。

1 ?算法介紹

Gmapping算法：

Gmapping算法是基于RBPF的一種有效解決同時(shí)定位與建圖的算法。SLAM要求解的問(wèn)題就是控制數(shù)據(jù)的u1：t和觀察數(shù)據(jù)的z1：t來(lái)求解位姿和地圖的聯(lián)合分布。RBPF解決了這兩個(gè)問(wèn)題同時(shí)求解時(shí)難度較大的問(wèn)題，使用條件概率將這兩個(gè)問(wèn)題拆分開(kāi)來(lái)求解位姿，即先進(jìn)性定位再進(jìn)行建圖。此時(shí)公式如下所示：

式中x為機(jī)器人的軌跡;m為由z觀測(cè)所得到的特征地圖;u為里程計(jì)測(cè)量值。

里程計(jì)所提供的位姿消息的不確定度較大，而激光的分布比里程計(jì)分布更接近真實(shí)值，所以說(shuō)當(dāng)把激光的信息融入到提議分布中時(shí)，那時(shí)提議分布離目標(biāo)分布會(huì)更加接近。同時(shí)因?yàn)榱Ｗ有枰獙?duì)里程計(jì)狀態(tài)的全部空間進(jìn)行全面覆蓋，在這其中只有少數(shù)的粒子是符合真實(shí)目標(biāo)分布的，因此當(dāng)計(jì)算權(quán)重時(shí)粒子的權(quán)重變化就會(huì)變得很大。如果需要把權(quán)重較小的粒子舍棄，讓權(quán)重大的粒子進(jìn)行復(fù)制來(lái)達(dá)到使粒子收斂到真實(shí)狀態(tài)的附近。這樣的操作需要進(jìn)行頻繁重采樣，造成了RBpf的另一個(gè)弊端即：發(fā)生粒子退化。這也解釋了RBpf需要大量粒子并執(zhí)行頻繁重采樣。

此時(shí)采用最近的一次觀測(cè)來(lái)改進(jìn)提議分布，因此提議分布如下

此時(shí)的提議分布使用了激光和里程計(jì)的信息，相比于公式（1）中的提議分布減緩了粒子退化的情況，并且更容易達(dá)到函數(shù)的峰值，構(gòu)建地圖的精度也更加準(zhǔn)確。

Hector算法：

Hector算法是一種魯棒性較好的2D SLAM方法和使用慣性傳感系統(tǒng)的導(dǎo)航技術(shù)。其不需要里程計(jì)，但對(duì)激光雷達(dá)要求的標(biāo)準(zhǔn)較高，需要更新頻率快、噪聲小的激光雷達(dá)。

概率柵格地圖是激光雷達(dá)描述真實(shí)世界的一種方式，經(jīng)證明能對(duì)任意環(huán)境進(jìn)行地圖表述[2]。由于激光雷達(dá)數(shù)據(jù)的離散性，無(wú)法直接用于柵格地圖的構(gòu)建與掃描匹配，所以要先通過(guò)插值獲得連續(xù)的概率柵格地圖。

獲取到柵格地圖后，需要將后續(xù)到達(dá)的雷達(dá)數(shù)據(jù)進(jìn)行掃描匹配，掃描匹配是將激光雷達(dá)掃描的實(shí)時(shí)數(shù)據(jù)與已有地圖對(duì)齊的過(guò)程，原系統(tǒng)在這一步參考了圖像對(duì)齊的方法構(gòu)建誤差函數(shù)，使用Gauss-Newton法得到最為匹配的地圖。該方式在雷達(dá)數(shù)據(jù)更新后，在上一時(shí)刻狀態(tài)附近迭代出當(dāng)前時(shí)刻的最優(yōu)匹配，并計(jì)算得到位姿增量[3]。Hector SLAM所建立的GaussNewton方程為：

式中ξ為柵格地圖旋轉(zhuǎn)矩陣，px，py，ψ分別代表x，y軸的偏移量和旋轉(zhuǎn)角度，si（ξ）則表示在世界坐標(biāo)系下激光雷達(dá)的位置，M函數(shù)為坐標(biāo)點(diǎn)的障礙物概率值。

其中H為Hessian矩陣，如下所示

2 ?試驗(yàn)設(shè)計(jì)方案

本文試驗(yàn)場(chǎng)景為室內(nèi)環(huán)境，路面比較光滑，采用差速兩輪小車來(lái)進(jìn)行兩種不同算法的建圖試驗(yàn)，當(dāng)小車在室內(nèi)運(yùn)動(dòng)時(shí)，小車搭載的激光雷達(dá)會(huì)對(duì)環(huán)境信息進(jìn)行收集和處理，同時(shí)上位機(jī)會(huì)對(duì)所處環(huán)境進(jìn)行建圖操作。

試驗(yàn)所需硬件包括：

①上位機(jī)：裝有ROS系統(tǒng)的筆記本一臺(tái);

②傳感器：輪式里程計(jì)與韓國(guó)LG激光雷達(dá);

③底層平臺(tái)：以樹(shù)莓派3B+為控制器的差速兩輪小車。

3 ?差速兩輪小車的建圖試驗(yàn)

針對(duì)本文提及到的兩種激光SLAM算法，通過(guò)差速兩輪小車搭載低頻率激光雷達(dá)在室內(nèi)環(huán)境下進(jìn)行建圖試驗(yàn)，在兩次試驗(yàn)中，小車以相同速度繞場(chǎng)地進(jìn)行地圖構(gòu)建。

試驗(yàn)結(jié)果如圖2-圖3所示。

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

本文介紹使用兩種不同的2D激光SLAM算法，在搭載低頻激光雷達(dá)的差速兩輪小車上對(duì)室內(nèi)環(huán)境進(jìn)行建模，對(duì)結(jié)果進(jìn)行了對(duì)比分析，從所建地圖中可以看出Gmapping算法在對(duì)室內(nèi)環(huán)境進(jìn)行建模具有一定優(yōu)勢(shì)，總體來(lái)說(shuō)Gmapping算法建地圖精度更高，為以后應(yīng)用低頻雷達(dá)時(shí)在室內(nèi)環(huán)境建圖時(shí)提供了一定借鑒經(jīng)驗(yàn)。

參考文獻(xiàn)：

[1]Kamarudin K， Mamduh S M， Shakaff A Y， et al.Performance analysis of the Microsoft Kinect sensor for 2D simultaneous localization and mapping （SLAM） techniques[J].Sensors， 2014， 14 （12） ：23365-87.

[2]Thrun S， Burgard W，F(xiàn)ox D. Probabilistic robotics（Intelligent Robotics and Autonomous Agents）[M]. NY：The MIT Press，2005.

[3]蘇易衡，張奇志，周亞麗.適用于低端激光雷達(dá)的優(yōu)化Hector SLAM算法[J].實(shí)驗(yàn)室研究與探索，2019，38（09）：47-51.

收稿日期：2019年12月5日。

基金項(xiàng)目：國(guó)家自然科學(xué)基金資助項(xiàng)目（51005128）。

作者簡(jiǎn)介：孫宗濤（1996-），男，山東青島人，青島理工大學(xué)機(jī)械與汽車工程學(xué)院碩士研究生;朱永強(qiáng)（1975-），男，山東萊州人，博士（后），副教授，研究方向?yàn)檐囕v動(dòng)力學(xué)、虛擬現(xiàn)實(shí)技術(shù)。