基于改进Gmapping的室内ROS智能车同步定位与建图系统设计.pdf

10.16638/ki.1671-7988.2023.019.012 10.16638/ki.1671-7988.2023.019.012 基于改进 Gmapping 的室内 ROS 智能车同步 定位与建图系统设计 闫 鹏，林迎春（电信科学技术第四研究所有限公司，陕西 西安 710061）摘要：室内移动机器人是近年来研究的热点问题，同步定位与建图技术更是移动机器人的关键技术之一。为此文章设计了基于机器人操作系统（ROS）智能车的同步定位与建图系统，以 Gmapping 为核心算法，首先采用电机编码器构建的里程计对单线激光雷达的每帧数据进行运动畸变补偿，以运动畸变补偿后的激光雷达数据和里程计数据作为 Gmapping 算法输入；然后通过 Gmapping 算法粒子初始化，构建考虑观测量的提议分布并对粒子进行采样以估计智能车位姿，以重采样重要性系数对粒子进行重采样并设置重采样阀值，利用二值贝叶斯滤波器对每个粒子的地图状态更新；最后基于 Gmapping 算法输出智能车的位姿估计和地图数据。试验结果表明，所设计的同步定位与建图系统，在小尺寸环境下对智能车位姿估计和构建的地图较为准确，满足设计目标要求。关键词：室内 ROS 智能车；Gmapping 算法；同步定位与建图系统；激光雷达 中图分类号：U461.91 文献标识码：A 文章编号：1671-7988(2023)19-61-06 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,Xian 710061,China)Abstract: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 作者简介：闫鹏（1976），男，硕士，高级工程师，研究方向为移动通信、物联网等，E-mail:。62 汽 车 实 用 技 术 2023 年 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.Keywords:Indoor ROS intelligent car;Gmapping algorithm;Synchronous localization and drawing mapping system;Lidar 同步定位与地图构建（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功能包订阅三个节点信息，广播四个节点信息，其中订阅信息包括智能车底盘与激光雷达的坐标变换信息，智能车底盘与里程计原点的坐标变换信息和激光雷达的每帧数据；输出信息包括地图与里程计的坐标变换信息、二维栅格地图、地图相关数据，以及位姿估计分散程度数据。图 2 Gmapping 算法功能包调用示意图 第 19 期 闫 鹏，等：基于改进 Gmapping 的室内 ROS 智能车同步定位与建图系统设计 63 1.2 ROS 智能车硬件配置 智能车硬件由车模、电机、电机编码器、舵机、电池、ROS 主控、底层主控和 IMU 等主要部件组成，主要硬件配置参数如表 1 所示。表 1 硬件配置参数 类型 参数 底层主控 型号：STM32F407VET6；主控芯片：STM32F407 电机 额定电流：0.36 A；额定电压：12 V；功率：4.32 W；额定扭矩：1 kgcm；堵转扭矩：4.5 kgcm 电机编码器 型号：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。通过所有给定的测量 zt+1，控制 ut+1和运动位姿先验估计 xt，来估计运动位姿 xt+1和环境地图m 的后验概率，如图 3 所示。图 3 同步定位与建图原理示意图 2007 年 GRISETTI 等6提出一种基于粒子滤波的建图算法（Gmapping），该算法通过改进提议分布和选择性重采样的方式改善了粒子退化程度。通过高精度的传感器观测数据代替低精度里程计数据来计算提议分布，使其与目标分布的差别缩减，同时引入重采样阀值，即超过阀值才进行重采样。Gmapping 算法主要由以下几部分组成：1）前端数据处理部分：包括电机编码器构建里程计和激光雷达运动畸变补偿；2）后端运动状态估计部分：通过粒子滤器对运动状态进行估计，其主要流程为构建粒子提议分布、粒子采样、计算粒子权重，以及自适应重采样；3）栅格地图构建更新部分：对栅格地图进行实时更新。2.1 数据预处理 2.1.1 二维里程计模型 在二维平面环境下，运动学模型采用三个变量来描述，二维平面横纵坐标及其航向角（位姿）。本文利用里程计模型对智能车进行运动学建模，该模型是将里程计测量当作控制，并对里程计测量进行分解，包括初始旋转、平移和第二次旋，并用正态分布对误差建模，构建的里程计为 ()()trans1rot111trans1rot11rot1rot2cossinttttttttxxyy+=+（1）式中，t和1 t分别为里程计在 t 和 t-1 时刻的航向角；(tx,ty)和(1 tx,1 ty)分别为里程计在 t 和 t-1 时刻的位置坐标；rot1和 rot2分别为里程计的第一次和第二次转角；t和 t-1分别为智能车在 t 和 t-1时刻的航向角；(xt,yt)和(xt-1,yt-1)分别为智能车在 t和 t-1 时刻的位置坐标；1rot和2rot为智能车第一次和第二次转角。2.1.2 基于里程计模型的激光雷达运动畸变补偿 采用的一帧激光雷达数据为 1 636 个激光束，首尾激光束发射间隔为 83.3 ms。智能车运动会导致一帧数据中每束激光的测量信息是在不同坐标系下获得，从而导致测量的环境信息发生错误。激光雷达运动畸变补偿如图 4 所示，黑色坐标系为世界坐标系，以智能车起始时智能车后轴中心参考点为原点；曲线是以智能车后轴中心参考点的智能车轨迹；曲线上的坐标系为激光雷达坐标系。由式（1）计算可得，测量点用激光雷达坐标表示。A0为某一帧数据起始时世界坐标系下的点；Ak为第 k 束激光世界坐标系下的点；C0为第一束激光测量点；Ck为第二束激光测量点。当存在畸变时，在 Ak激光雷达坐标系的测量点 Ck会被误认64 汽 车 实 用 技 术 2023 年 为是在 A0激光雷达坐标系测得，真实测量点 Ck被 Ck替代。图 4 激光雷达运动畸变补偿示意图 为消除这一畸变误差，通过里程计模型计算预估轨迹对畸变进行补偿。首先把激光雷达坐标系下的 Ck变换到世界坐标下，接着再进行一次坐标变换，把世界坐标系下的 Ck点变换到 A0点激光雷达坐标下，从而消除因激光雷达自身运动而产生的测量畸变误差。由于激光雷达安装在智能车的纵向对称面上，在二维平面上，激光雷达坐标系原点在后轴中心参考点正前方。因此，雷达坐标系与车体坐标系的坐标变换只需平移即可测量雷达坐标系与车体坐标系原点的水平距离。智能车起始时，车体坐标系原点是世界坐标系原点，即可完成雷达坐标系与世界坐标系的转换参数标定。2.2 智能车位姿估计 Gmapping算法将全SLAM问题分解成智能车定位问题和基于位姿估计的环境地图估计问题，即将全 SLAM 后验估计分解为 ()()()1:1:1:1:1:1:1:1:,|,|,|,ttttttttppp=xm zuxzum zx（2）式中，x1:t为 1 到 t 时刻时智能车位姿的集合；m为环境地图；z1:t为 1 到 t 时刻的激光雷达观测量集合；u1:t为 1 到 t 时刻的智能车控制量集合，即里程计测量量的集合；p(A|B,C)为在 B 和 C 条件下 A 的概率值。对轨迹进行分解，将轨迹估计转化成一个增量估计问题，即 ()()()()1:1:1:11:1 1:11:1|,|,|,tttttttttttpppp=xzuz xx xuxzu （3）式中，为归一化因子，Gmapping 算法主要流程如下：1.粒子初始化 在初始时刻进行全局定位时，粒子是随机分布在地图上；当初始位置已知，粒子则分布在初始位置周围。2.构建提议分布 RBPF SLAM 的提议分布为里程计模型，其计算虽较为简单，但是会产生大范围的样本粒子，且重采样后粒子耗散多。而 Gmapping 算法通过观测量 z1:t来构建提议分布，改善此问题，即 ()()()()()()()()()111111111|,|,|,|,itttttiittttttiittttpppp=xxmz uzx mxxuzxmu （4）3.粒子采样 通过改进粒子提议采样分布，Gmapping 算法用一帧激光雷达扫描与地图的匹配，得到一个观测似然峰值区域 L(i)，xt在峰值区域，可视为常数。采用高斯分布来描述该区域，其均值与方差是由区间内的 K 个次采样点确定，可计算 t 时刻位姿的后验概率，即 ()()()()()11|,|,|,|,tttttttttitttpppp=x xu z mz x mx xuz x mxL （5）()()()()()()11111|,|,Kiitjtjtijijttpp=xz xmx xu （6）()()()()()()()()()()11Tj111|,|,Kiijtjttijiiitjtjttpp=xz xmx xuxx（7）()()()()()1111|,|,Kiiijtjjtttjxpp=z xmx xu （8）()()()()()()111|,iiiitttttttpzNxxmu （9）式中，L(i)为t时刻第i个粒子的观测似然峰值区域；)(it为 t 时刻第 i 个粒子在观测似然峰值区域的高斯分布均值；)(it为 t 时刻第 i 个粒子在观测似然峰值区域的高斯分布方差；(i)为 t 时刻第个 i 粒子的归一化因子。4.重采样重要性系数 采样重要性系数是重采样的依据，每个粒子第 19 期 闫 鹏，等：基于改进 Gmapping 的室内 ROS 智能车同步定位与建图系统设计 65 的重要性系数是目标分布与建议分布比，其中目标分布是智能车轨迹的真实概率密度分布，根据文献7得到重要性系数递归公式，即 ()()()()()()()()()()()1:1:1:111:1:1:1|,|,iiitttiiittiiitttp=xzuxzu （10）式中，),()(11)(1)(1it:it:it:|p uzx为目标分布；),()(11)(1)(1it:it:it:|uzx为建议分布；)(it和)(1it 分别为第 i 个粒子在 t 和t-1 时刻的重采样重要性系数。5.自适应重采样 重采样是为了提高权重较高粒子位姿下的粒子数，降低低权重粒子位姿下的粒子数，粒子总数应保持不变。根据粒子权重离散程度设置重采样阀值，当离散程度较小时，即 NeffT，大量粒子聚集在某处，不需要进行重采集；当离散程度较大时，NeffT，需进行重采样。总而言之，降低了重采样次数，减少粒子耗散。()eff211NiiN=（11）式中，N 为粒子总个数；i为第 i 个粒子重采样重要性系数。2.3 栅格地图构建及更新 环境地图是环境中物体的列表及其属性，可见地图更新就是更新地图的列表及其属性。123,Nm m mm=mL （12）式中，N 为栅格地图中栅格的总数，0nN；mN为栅格的位置属性。本文选择基于位置的栅格地图，索引号 N 与特定的位置对应，栅格地图为每一个栅格分配二值随机变量，表示该位置是否被占用，占用栅格地图构建算法是对所有随机变量进行近似后验估计，即 ()()1:1:1:1:|,|,Nttittipp=mzxm zx （13）式中，m 为地图；mi为第 i 个栅格单元的位置属性。对整个地图的后验概率进行因式分解，并用边缘概率乘积近似求解，每个栅格单元边缘概率计算是一个静态二值估计问题，因此，可以采用二值贝叶斯滤波器对其进行求解，即 ()()()()1:1:,1,1:1:|,log1|,log1ittt itiittiipllppp=+m zxm zxmm （14）式中，lt,i和 lt-1,i分别为 t 时刻和 t-1 时刻第 i 个栅格单元的后验概率对数；p(mi)为第 i 个栅格单元的初始先验概率，取 0.5。为计算反演测量模型 p(mi|z1:t,x1:t)，设在 t 时刻 mi在传感器扫描范围内，首先通过确定波束索引 k 和智能车距离栅格单元 mi的中心测量距离 ri来计算反演模型，采用均值为ktz，方差为 的高斯分布描述 p(ri|xt,zt)，采用极大似然估计得到参数，且当 ri大于ktmax z时，概率密度恒为 0，即 ()()22iiirxxyy=+（15）（16）,sensargminjjk=（17）()(|,)(|,)kittitttp rzp mzN z=xx，（18）式中，ri为智能车距离栅格单元的中心测量距离；mi为激光雷达波束索引号；ktz为 t 时刻激光雷达第k 束激光的测量值；(y,x,)为 t 时刻智能车的位姿；(xi,yi)为栅格单元 mi的位置坐标。3 试验验证 在室内10 m8 m小场景测试所设计同步定位与建图系统的建图效果，如图 5 所示。通过控制ROS 智能车绕中间大桌子一圈，构建的地图如图6 所示，可以看到前后门、墙体、大桌子等典型物体在地图上得以显示，且能真实反映出环境中各物体相对位置信息，说明本系统在室内小场景下，建图效果较好。图 5 室内 10 m8 m 小场景 66 汽 车 实 用 技 术 2023 年 图 6 室内 10 m8 m 小场景建图效果 为了测试移动障碍物对本系统的影响，在室内小场景下，摆放了一个能移动的纸箱子，如图 7所示。在开始建图时，智能车能构建有移动纸箱子的地图，在移动纸箱子离开原有位置后，智能 图 7 移动障碍物下室内小场景 (a)移动障碍物出现 (b)移动障碍物消失 图 8 移动障碍物下室内小场景建图历程 车激光雷达再对纸箱子原来位置进行扫描时，则不会扫描到障碍物。因此，这位置下障碍物的置信度会降低，经激光雷达多次扫描后，置信度为 0，同时在构建的地图上会显示纸箱子原来待过的位置没有障碍物，建图历程如图 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 UncertaintyJ.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 ProblemC/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 ConvergesC/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.Improved Techniques for Grid Mapping with Rao-Blackwellized Particle FiltersJ.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 NetworksC/Proceedings of the 16th Conference on Uncertainty in Artificial Intelligence.San Francisco:Harper Collins Publishers,2000:176-183.