閆 鵬，林迎春

基于改進(jìn)Gmapping的室內(nèi)ROS智能車同步定位與建圖系統(tǒng)設(shè)計(jì)

閆 鵬，林迎春

（電信科學(xué)技術(shù)第四研究所有限公司，陜西 西安 710061）

室內(nèi)移動(dòng)機(jī)器人是近年來研究的熱點(diǎn)問題，同步定位與建圖技術(shù)更是移動(dòng)機(jī)器人的關(guān)鍵技術(shù)之一。為此文章設(shè)計(jì)了基于機(jī)器人操作系統(tǒng)（ROS）智能車的同步定位與建圖系統(tǒng)，以Gmapping為核心算法，首先采用電機(jī)編碼器構(gòu)建的里程計(jì)對單線激光雷達(dá)的每幀數(shù)據(jù)進(jìn)行運(yùn)動(dòng)畸變補(bǔ)償，以運(yùn)動(dòng)畸變補(bǔ)償后的激光雷達(dá)數(shù)據(jù)和里程計(jì)數(shù)據(jù)作為Gmapping算法輸入；然后通過Gmapping算法粒子初始化，構(gòu)建考慮觀測量的提議分布并對粒子進(jìn)行采樣以估計(jì)智能車位姿，以重采樣重要性系數(shù)對粒子進(jìn)行重采樣并設(shè)置重采樣閥值，利用二值貝葉斯濾波器對每個(gè)粒子的地圖狀態(tài)更新；最后基于Gmapping算法輸出智能車的位姿估計(jì)和地圖數(shù)據(jù)。試驗(yàn)結(jié)果表明，所設(shè)計(jì)的同步定位與建圖系統(tǒng)，在小尺寸環(huán)境下對智能車位姿估計(jì)和構(gòu)建的地圖較為準(zhǔn)確，滿足設(shè)計(jì)目標(biāo)要求。

室內(nèi)ROS智能車；Gmapping算法；同步定位與建圖系統(tǒng)；激光雷達(dá)

同步定位與地圖構(gòu)建（Simultaneous Localiza- tion And Mapping, SLAM）技術(shù)是自動(dòng)駕駛、人工智能等領(lǐng)域的關(guān)鍵技術(shù)。目前，2D 激光SLAM的實(shí)現(xiàn)主要分為基于濾波器和基于非線性優(yōu)化兩種方法[1]。1986年，正在研究空間不確定性的描述和變換表示的SMITH等[2]發(fā)表了一篇關(guān)于SLAM的開創(chuàng)性文章，并提出了概率SLAM的概念。早期主要運(yùn)用概率統(tǒng)計(jì)方法（卡爾曼濾波和粒子濾波）研究SLAM問題。2002年MONTEME- RLO等[3]提出基于粒子濾波的Fast SLAM1.0和Fast SLAM2.0算法，采用RBPF（Rao-Blackwellised Particle Filter）方法將SLAM解耦成定位和建圖，采用粒子來表示機(jī)器人的位姿，而環(huán)境特征的估計(jì)利用擴(kuò)展卡爾曼濾波（Extended Kalman Filtering, EKF）解析計(jì)算。2003年MONTEMERLO等[4]提出一種求取重要性函數(shù)的方法（Fast SLAM2.0），優(yōu)化了粒子濾波器粒子退化的問題，即通過重采樣判斷減少重采樣次數(shù)。綜上所述，Gmapping算法是基于柵格地圖實(shí)現(xiàn)的Fast SLAM2算法，并得到廣泛應(yīng)用。

本文基于機(jī)器人操作系統(tǒng)（Robot Operating System, ROS）智能車設(shè)計(jì)了SLAM系統(tǒng)，依托ROS智能車為載體，通過單線激光雷達(dá)傳感器，采用ROS下的Gmapping算法模塊對智能車進(jìn)行位姿估計(jì)和環(huán)境地圖構(gòu)建。

1 同步定位與建圖系統(tǒng)設(shè)計(jì)

1.1 系統(tǒng)方案總體框架

本系統(tǒng)硬件系統(tǒng)總體設(shè)計(jì)（見圖1）由兩塊主控芯片及相關(guān)附屬模塊組成，STM32F底層主控芯片一方面控制著舵機(jī)、慣性測量單元（Inertial Measurement Unit, IMU）、電機(jī)驅(qū)動(dòng)及電機(jī)編碼器按口、蜂鳴器、發(fā)光二極管（Light-Emitting Diode, LED）顯示屏和電池等模塊，同時(shí)與ROS主控芯片通信連接。ROS主控芯片Jetson TX1內(nèi)置ubuntu系統(tǒng)，下載了與ROS和Gmapping相關(guān)的功能包及依賴包，并通過雷達(dá)轉(zhuǎn)接板接收激光雷達(dá)數(shù)據(jù)。

圖1 系統(tǒng)方案總體框架

ROS中的Gmapping功能包是基于ROS框架下實(shí)現(xiàn)的開源openslam包，Gmapping功能包是根據(jù)里程計(jì)和激光雷達(dá)的信息，構(gòu)建環(huán)境地圖和對智能車位姿估計(jì)，調(diào)用過程如圖2所示。Gmapping功能包訂閱三個(gè)節(jié)點(diǎn)信息，廣播四個(gè)節(jié)點(diǎn)信息，其中訂閱信息包括智能車底盤與激光雷達(dá)的坐標(biāo)變換信息，智能車底盤與里程計(jì)原點(diǎn)的坐標(biāo)變換信息和激光雷達(dá)的每幀數(shù)據(jù)；輸出信息包括地圖與里程計(jì)的坐標(biāo)變換信息、二維柵格地圖、地圖相關(guān)數(shù)據(jù)，以及位姿估計(jì)分散程度數(shù)據(jù)。

1.2 ROS智能車硬件配置

智能車硬件由車模、電機(jī)、電機(jī)編碼器、舵機(jī)、電池、ROS主控、底層主控和IMU等主要部件組成，主要硬件配置參數(shù)如表1所示。

表1 硬件配置參數(shù)

類型參數(shù) 底層主控型號(hào)：STM32F407VET6；主控芯片：STM32F407 電機(jī)額定電流：0.36 A；額定電壓：12 V；功率：4.32 W；額定扭矩：1 kg·cm；堵轉(zhuǎn)扭矩：4.5 kg·cm 電機(jī)編碼器型號(hào)：GMR；線數(shù)：500 p/r ROS主控型號(hào)：Jetson TX1；GPU：NVIDIA MaxwellTM架構(gòu)，配備256個(gè)NVIDIA CUDA?核心；內(nèi)存：4 GB 激光雷達(dá)型號(hào)：鐳神M10P串口板 舵機(jī)型號(hào)：S20F大扭矩?cái)?shù)字舵機(jī)；反應(yīng)速度：0.18 s/60 ° 電池型號(hào)：12 V聚合物鋰電池；容量：12 V 9 800 mAh IMU型號(hào)：MPU6050；加速度線性誤差：0.1g；陀螺儀線性誤差：0.1 (°)/s

2 基于粒子濾波的Gmapping同步定位與建圖

SLAM問題可以描述為機(jī)器人在未知環(huán)境中從一個(gè)未知位置開始移動(dòng)，在移動(dòng)過程中根據(jù)位置和地圖進(jìn)行自身定位，同時(shí)在自身定位的基礎(chǔ)上建造增量式地圖，實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航[5]。通過所有給定的測量+1，控制+1和運(yùn)動(dòng)位姿先驗(yàn)估計(jì)，來估計(jì)運(yùn)動(dòng)位姿+1和環(huán)境地圖的后驗(yàn)概率，如圖3所示。

圖3 同步定位與建圖原理示意圖

2007年GRISETTI等[6]提出一種基于粒子濾波的建圖算法（Gmapping），該算法通過改進(jìn)提議分布和選擇性重采樣的方式改善了粒子退化程度。通過高精度的傳感器觀測數(shù)據(jù)代替低精度里程計(jì)數(shù)據(jù)來計(jì)算提議分布，使其與目標(biāo)分布的差別縮減，同時(shí)引入重采樣閥值，即超過閥值才進(jìn)行重采樣。

Gmapping算法主要由以下幾部分組成：

1）前端數(shù)據(jù)處理部分：包括電機(jī)編碼器構(gòu)建里程計(jì)和激光雷達(dá)運(yùn)動(dòng)畸變補(bǔ)償；2）后端運(yùn)動(dòng)狀態(tài)估計(jì)部分：通過粒子濾器對運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)，其主要流程為構(gòu)建粒子提議分布、粒子采樣、計(jì)算粒子權(quán)重，以及自適應(yīng)重采樣；3）柵格地圖構(gòu)建更新部分：對柵格地圖進(jìn)行實(shí)時(shí)更新。

2.1 數(shù)據(jù)預(yù)處理

2.1.1二維里程計(jì)模型

在二維平面環(huán)境下，運(yùn)動(dòng)學(xué)模型采用三個(gè)變量來描述，二維平面橫縱坐標(biāo)及其航向角（位姿）。本文利用里程計(jì)模型對智能車進(jìn)行運(yùn)動(dòng)學(xué)建模，該模型是將里程計(jì)測量當(dāng)作控制，并對里程計(jì)測量進(jìn)行分解，包括初始旋轉(zhuǎn)、平移和第二次旋，并用正態(tài)分布對誤差建模，構(gòu)建的里程計(jì)為

2.1.2基于里程計(jì)模型的激光雷達(dá)運(yùn)動(dòng)畸變補(bǔ)償

采用的一幀激光雷達(dá)數(shù)據(jù)為1 636個(gè)激光束，首尾激光束發(fā)射間隔為83.3 ms。智能車運(yùn)動(dòng)會(huì)導(dǎo)致一幀數(shù)據(jù)中每束激光的測量信息是在不同坐標(biāo)系下獲得，從而導(dǎo)致測量的環(huán)境信息發(fā)生錯(cuò)誤。

激光雷達(dá)運(yùn)動(dòng)畸變補(bǔ)償如圖4所示，黑色坐標(biāo)系為世界坐標(biāo)系，以智能車起始時(shí)智能車后軸中心參考點(diǎn)為原點(diǎn)；曲線是以智能車后軸中心參考點(diǎn)的智能車軌跡；曲線上的坐標(biāo)系為激光雷達(dá)坐標(biāo)系。

由式（1）計(jì)算可得，測量點(diǎn)用激光雷達(dá)坐標(biāo)表示。0為某一幀數(shù)據(jù)起始時(shí)世界坐標(biāo)系下的點(diǎn)；A為第束激光世界坐標(biāo)系下的點(diǎn)；0為第一束激光測量點(diǎn)；C為第二束激光測量點(diǎn)。當(dāng)存在畸變時(shí)，在A激光雷達(dá)坐標(biāo)系的測量點(diǎn)C會(huì)被誤認(rèn)為是在0激光雷達(dá)坐標(biāo)系測得，真實(shí)測量點(diǎn)C被C替代。

圖4 激光雷達(dá)運(yùn)動(dòng)畸變補(bǔ)償示意圖

為消除這一畸變誤差，通過里程計(jì)模型計(jì)算預(yù)估軌跡對畸變進(jìn)行補(bǔ)償。首先把激光雷達(dá)坐標(biāo)系下的C變換到世界坐標(biāo)下，接著再進(jìn)行一次坐標(biāo)變換，把世界坐標(biāo)系下的C點(diǎn)變換到0點(diǎn)激光雷達(dá)坐標(biāo)下，從而消除因激光雷達(dá)自身運(yùn)動(dòng)而產(chǎn)生的測量畸變誤差。

由于激光雷達(dá)安裝在智能車的縱向?qū)ΨQ面上，在二維平面上，激光雷達(dá)坐標(biāo)系原點(diǎn)在后軸中心參考點(diǎn)正前方。因此，雷達(dá)坐標(biāo)系與車體坐標(biāo)系的坐標(biāo)變換只需平移即可測量雷達(dá)坐標(biāo)系與車體坐標(biāo)系原點(diǎn)的水平距離。智能車起始時(shí)，車體坐標(biāo)系原點(diǎn)是世界坐標(biāo)系原點(diǎn)，即可完成雷達(dá)坐標(biāo)系與世界坐標(biāo)系的轉(zhuǎn)換參數(shù)標(biāo)定。

2.2 智能車位姿估計(jì)

Gmapping算法將全SLAM問題分解成智能車定位問題和基于位姿估計(jì)的環(huán)境地圖估計(jì)問題，即將全SLAM后驗(yàn)估計(jì)分解為

式中，1:t為1到時(shí)刻時(shí)智能車位姿的集合；為環(huán)境地圖；1:t為1到時(shí)刻的激光雷達(dá)觀測量集合；1:t為1到時(shí)刻的智能車控制量集合，即里程計(jì)測量量的集合；(A|B,C)為在B和C條件下A的概率值。

對軌跡進(jìn)行分解，將軌跡估計(jì)轉(zhuǎn)化成一個(gè)增量估計(jì)問題，即

式中，為歸一化因子，Gmapping算法主要流程如下：

1.粒子初始化

在初始時(shí)刻進(jìn)行全局定位時(shí)，粒子是隨機(jī)分布在地圖上；當(dāng)初始位置已知，粒子則分布在初始位置周圍。

2.構(gòu)建提議分布

RBPF SLAM的提議分布為里程計(jì)模型，其計(jì)算雖較為簡單，但是會(huì)產(chǎn)生大范圍的樣本粒子，且重采樣后粒子耗散多。而Gmapping算法通過觀測量1: t來構(gòu)建提議分布，改善此問題，即

3.粒子采樣

通過改進(jìn)粒子提議采樣分布，Gmapping算法用一幀激光雷達(dá)掃描與地圖的匹配，得到一個(gè)觀測似然峰值區(qū)域(i)，在峰值區(qū)域，可視為常數(shù)。采用高斯分布來描述該區(qū)域，其均值與方差是由區(qū)間內(nèi)的個(gè)次采樣點(diǎn)確定，可計(jì)算時(shí)刻位姿的后驗(yàn)概率，即

4.重采樣重要性系數(shù)

采樣重要性系數(shù)是重采樣的依據(jù)，每個(gè)粒子的重要性系數(shù)是目標(biāo)分布與建議分布比，其中目標(biāo)分布是智能車軌跡的真實(shí)概率密度分布，根據(jù)文獻(xiàn)[7]得到重要性系數(shù)遞歸公式，即

5.自適應(yīng)重采樣

重采樣是為了提高權(quán)重較高粒子位姿下的粒子數(shù)，降低低權(quán)重粒子位姿下的粒子數(shù)，粒子總數(shù)應(yīng)保持不變。根據(jù)粒子權(quán)重離散程度設(shè)置重采樣閥值，當(dāng)離散程度較小時(shí)，即eff>，大量粒子聚集在某處，不需要進(jìn)行重采集；當(dāng)離散程度較大時(shí)，eff<，需進(jìn)行重采樣?？偠灾档土酥夭蓸哟螖?shù)，減少粒子耗散。

式中，為粒子總個(gè)數(shù)；ω為第個(gè)粒子重采樣重要性系數(shù)。

2.3 柵格地圖構(gòu)建及更新

環(huán)境地圖是環(huán)境中物體的列表及其屬性，可見地圖更新就是更新地圖的列表及其屬性。

式中，為柵格地圖中柵格的總數(shù)，0<≤；m為柵格的位置屬性。

本文選擇基于位置的柵格地圖，索引號(hào)與特定的位置對應(yīng)，柵格地圖為每一個(gè)柵格分配二值隨機(jī)變量，表示該位置是否被占用，占用柵格地圖構(gòu)建算法是對所有隨機(jī)變量進(jìn)行近似后驗(yàn)估計(jì)，即

式中，為地圖；為第個(gè)柵格單元的位置屬性。

對整個(gè)地圖的后驗(yàn)概率進(jìn)行因式分解，并用邊緣概率乘積近似求解，每個(gè)柵格單元邊緣概率計(jì)算是一個(gè)靜態(tài)二值估計(jì)問題，因此，可以采用二值貝葉斯濾波器對其進(jìn)行求解，即

式中，l,i和l-1,i分別為時(shí)刻和-1時(shí)刻第個(gè)柵格單元的后驗(yàn)概率對數(shù)；()為第個(gè)柵格單元的初始先驗(yàn)概率，取0.5。

3 試驗(yàn)驗(yàn)證

在室內(nèi)10 m×8 m小場景測試所設(shè)計(jì)同步定位與建圖系統(tǒng)的建圖效果，如圖5所示。通過控制ROS智能車?yán)@中間大桌子一圈，構(gòu)建的地圖如圖6所示，可以看到前后門、墻體、大桌子等典型物體在地圖上得以顯示，且能真實(shí)反映出環(huán)境中各物體相對位置信息，說明本系統(tǒng)在室內(nèi)小場景下，建圖效果較好。

圖5 室內(nèi)10 m×8 m小場景

圖6 室內(nèi)10 m×8 m小場景建圖效果

為了測試移動(dòng)障礙物對本系統(tǒng)的影響，在室內(nèi)小場景下，擺放了一個(gè)能移動(dòng)的紙箱子，如圖7所示。在開始建圖時(shí)，智能車能構(gòu)建有移動(dòng)紙箱子的地圖，在移動(dòng)紙箱子離開原有位置后，智能車激光雷達(dá)再對紙箱子原來位置進(jìn)行掃描時(shí)，則不會(huì)掃描到障礙物。因此，這位置下障礙物的置信度會(huì)降低，經(jīng)激光雷達(dá)多次掃描后，置信度為0，同時(shí)在構(gòu)建的地圖上會(huì)顯示紙箱子原來待過的位置沒有障礙物，建圖歷程如圖8所示。

圖7 移動(dòng)障礙物下室內(nèi)小場景

圖8 移動(dòng)障礙物下室內(nèi)小場景建圖歷程

4 結(jié)論

本文設(shè)計(jì)的基于ROS智能車的Gmapping同步定位與建圖系統(tǒng)在小尺寸場景下應(yīng)用良好，所構(gòu)建的地圖與真實(shí)環(huán)境信息基本一致，且能解決移動(dòng)障礙物對建圖效果的影響問題。本文基于高精度編碼器的里程計(jì)模型，利用正態(tài)分布對其誤差建模，采用高精度里程計(jì)模型消除激光雷達(dá)自身運(yùn)動(dòng)畸變，減少激光雷達(dá)數(shù)據(jù)噪聲，提高了Gm- apping算法的魯棒性。今后為了使SLAM系統(tǒng)能夠準(zhǔn)確地構(gòu)建大尺寸場景地圖，需要對Gmapping算法、建圖方法，以及里程計(jì)構(gòu)建模型等方面進(jìn)行優(yōu)化。

[1] 趙華東,張耀輝,吳畏.智能車激光SLAM的研究與實(shí)現(xiàn)[J].汽車實(shí)用技術(shù),2020,45(23):39-41.

[2] SMITH R,CHEESEMAN P.On the Representation of Spatial Uncertainty[J].International Journal of Robo- tics Research,1986,9:56-68.

[3] MONTEMERLO M,THRUN S,KOLLER D,et al.Fast SLAM:A Factored Solution to the Simultaneous Loc- alization and Mapping Problem[C]//Proceedings of AAAI Natioal Conference on Artificial Intelligence. Palo Alto:AAAI Press,2002:593-598.

[4] MONTEMERLO M,THRUN S,ROLLER D,et al.Fast SLAM an Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Prova- bly Converges[C]//Proceedings of the 18th Internati- onal Joint Conference on Artificial Intelliaence. Acapulco:ACM Press,2003:1151-1156.

[5] 陳衛(wèi)東,張飛.移動(dòng)機(jī)器人的同步自定位與地圖創(chuàng)建研究進(jìn)展[J].控制理論與應(yīng)用,2005,22(3):455-460.

[6] GRISETTI G.STACHNISS C.BURGARD W. ImprovedTechniques for Grid Mapping with Rao-Blackwellized Particle Filters[J].IEEE Transactions on Robotics, 2007,23(1):34-46.

[7] DOUCET A,FREITAS A,MURPHY K,et al.Rao- Blackwellised Particle Filtering for Dynamic Baye- sian Networks[C]//Proceedings of the 16th Conferenceon Uncertainty in Artificial Intelligence.San Francisco: Harper Collins Publishers,2000:176-183.

Design of Indoor ROS Intelligent Car Synchronous Positioning and Drawing System Based on Improved Gmapping

YAN Peng, LIN Yingchun

( Fourth Research Institute of Telecommunication Technology Company Limited, Xi'an 710061, China )

Indoor mobile robots are a hot issue in recent years, and synchronous positioning and mapping technology is one of the key technologies of mobile robot.To this end,a synchronous positioning and the mapping system is designed in this paper,based on robot operating system(ROS) intelligent car, which takes Gmapping as the core algorithm.Firstly, to compensate for the motion distortion per frame of the data of the single-line lidar with the odometer constructed by the motor encoder,lidar data and odometer data after motion distortion compensation is used as the input of the Gmapping algorithm;Then particle initialized via the Gmapping algorithm,construct a proposed distribution considering the observed quantity and sampling the particles to estimate the intelligent parking poses,resampling the particles with the resampling importance coefficient and setting the resampling threshold,update the map state of each particle with a binary bayesian filter;At last,the pose estimation and map data of intelligent car based on the Gmapping algorithm.The test results show that the designed synchronous positioning and the mapping system are more accurate in the intelligent parking pose estimation and construction map in a small size environment, which meets the design target requirements.

Indoor ROS intelligent car;Gmapping algorithm;Synchronous localization and drawing mapping system;Lidar

U461.91

A

1671-7988(2023)19-61-06

10.16638/j.cnki.1671-7988.2023.019.012

閆鵬（1976－），男，碩士，高級(jí)工程師，研究方向?yàn)橐苿?dòng)通信、物聯(lián)網(wǎng)等，E-mail:514366153@qq.com。