基于改進Gmapping的室內ROS智能車同步定位與建圖系統設計

閆 鵬，林迎春

基于改進Gmapping的室內ROS智能車同步定位與建圖系統設計

閆 鵬，林迎春

（電信科學技術第四研究所有限公司，陜西 西安 710061）

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

室內ROS智能車；Gmapping算法；同步定位與建圖系統；激光雷達

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

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

1 同步定位與建圖系統設計

1.1 系統方案總體框架

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

圖1 系統方案總體框架

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

1.2 ROS智能車硬件配置

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

表1 硬件配置參數

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

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

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

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

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

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

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

2.1 數據預處理

2.1.1二維里程計模型

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

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

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

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

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

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

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

由于激光雷達安裝在智能車的縱向對稱面上，在二維平面上，激光雷達坐標系原點在后軸中心參考點正前方。因此，雷達坐標系與車體坐標系的坐標變換只需平移即可測量雷達坐標系與車體坐標系原點的水平距離。智能車起始時，車體坐標系原點是世界坐標系原點，即可完成雷達坐標系與世界坐標系的轉換參數標定。

2.2 智能車位姿估計

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

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

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

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

1.粒子初始化

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

2.構建提議分布

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

3.粒子采樣

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

4.重采樣重要性系數

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

5.自適應重采樣

重采樣是為了提高權重較高粒子位姿下的粒子數，降低低權重粒子位姿下的粒子數，粒子總數應保持不變。根據粒子權重離散程度設置重采樣閥值，當離散程度較小時，即eff>，大量粒子聚集在某處，不需要進行重采集；當離散程度較大時，eff<，需進行重采樣。總而言之，降低了重采樣次數，減少粒子耗散。

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

2.3 柵格地圖構建及更新

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

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

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

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

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

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

3 試驗驗證

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

圖5 室內10 m×8 m小場景

圖6 室內10 m×8 m小場景建圖效果

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

圖7 移動障礙物下室內小場景

圖8 移動障礙物下室內小場景建圖歷程

4 結論

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

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