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

閆 鵬，林迎春

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

閆 鵬，林迎春

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

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

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

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

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

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

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

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

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

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

1.2 ROS智能車硬件配置

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

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

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

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

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

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

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

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

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

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

2.1.1二維里程計模型

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

2.1.2基于里程計模型的激光雷達運動畸變補償

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

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

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

圖4 激光雷達運動畸變補償示意圖

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

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

2.2 智能車位姿估計

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

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

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

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

1.粒子初始化

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

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

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

3.粒子采樣

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

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

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

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

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

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

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

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

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

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

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

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

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

3 試驗驗證

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

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

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

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

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

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

4 結(jié)論

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

[1] 趙華東,張耀輝,吳畏.智能車激光SLAM的研究與實現(xiàn)[J].汽車實用技術(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)東,張飛.移動機器人的同步自定位與地圖創(chuàng)建研究進展[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－），男，碩士，高級工程師，研究方向為移動通信、物聯(lián)網(wǎng)等，E-mail:514366153@qq.com。