基于改进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。