复杂环境移动机器人多传感器融合SLAM研究.pdf

【108】第45卷 第08期 2023-08收稿日期：2022-04-02作者简介：唐元文（1997-），男，江西赣州人，硕士研究生，研究方向为机器人技术。通讯作者：刘祚时（1963-），男，江西永新人，教授，硕士，主要从事智能机器人技术、矿山设备自动化方面的研究工作。复杂环境移动机器人多传感器融合SLAM研究Research on multisensor fusion SLAM for mobile robots in complex environments唐元文，刘祚时*TANG Yuan-wen，LIU Zuo-shi*（江西理工大学 机电工程学院，赣州 341001）摘 要：室内复杂环境下单一传感器移动机器人SLAM误差较大，效果较差，对此，提出一个融合多种传感器的三维RTAB-Map SLAM方案。首先在ROS系统中仿真环境，并通过里程计仿真与SLAM仿真验证该方案的可行性。在仿真的基础上进行SLAM实验，通过对比实验结果验证采用优化协方差矩阵的EKF融合里程计数据能够提升SLAM效果。实验结果表明，优化协方差矩阵的EKF融合里程计能够减少RTAB-Map SLAM中的错误点云的同时提升SLAM精度。关键词：移动机器人；SLAM；RF2O；EKF；RTAB-Map中图分类号：TP242 文献标志码：A 文章编号：1009-0134(2023)08-0108-050 引言SLAM（Simultaneous Localization and Mapping,同时定位与地图构建），其具体指的是搭载特定传感器的主体在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自身的运动1。SLAM最早在1988年由Smith1,2等人提出，发展至今已有30余年3。扩展卡尔曼滤(Extended Kalman Filter，EKF)4是最早被采用的SLAM方法，目前也是最为广泛采用的SLAM方法之一，由Smith等人提出。Thrun5等人提出基于蒙特卡洛方法的粒子滤波(Particle Filter)解决SLAM问题。随着研究的深入，Thrun等人开发了基于稀疏扩展信息滤波的高效SLAM方法SEIF6。Lu和Milios在1997提出了图优化SLAM(Graph-SLAM)7基本思路为通过传感器观位姿之间的软约束，在图优化过程中校正移动机器人的位姿以便最小化地图中的错误。由于搭载单个传感器的移动机器人在复杂环境中SLAM有着过于依赖单一传感器、感知环境方式单一等缺点，因此采用多传感器在复杂环境8中SLAM是目前机器人研究领域的研究热点之一。文献9提出基于扩展卡尔曼滤波和互补融合的组合数据融合方法，但是欠缺三维地图构建。文献10提出一种不依赖轮式里程计数据的多传感器融合定位算法，融合IMU与RF2O激光里程计数据，可以提升定位精度，但是没有计算RF2O的里程计协方差矩阵，提升到更高的精度。对于上述问题，本文在ROS机器人控制系统中，通过扩展卡尔曼滤波融合传统的轮式里程计与基于范围流的RF2O激光里程计对进行定位，并在融合过程中改进激光里程计数据的协方差矩阵，增加精度较高的激光里程计在EKF中的影响，进而提升定位精度，并通过这些里程计估计的移动机器人位姿，进行RTAB-Map SLAM仿真与实地实验，对SLAM结果进行分析，验证了RF2O激光里程计的高精度与改进协方差后EKF所融合的里程计数据对RTAB-Map SLAM性能的增强。1 移动机器人同时定位与地图构建大部分SLAM都需要外部输入里程计提供位姿估计，本章对论文所采用的里程计与SLAM算法进行介绍。1.1 轮式里程计轮式里程计是成本最低的一种里程计，通常由编码器获取底盘各个车轮的角速度进而求出移动机器人的运动状态。图1 移动机器人轮式里程计设 世 界 坐 标 系 下 移 动 机 器 人 位 移 第 i 个 位 姿 为Si=xi,yi,iT，移动机器人在机器人坐标系下的位移为xi 第45卷 第08期 2023-08【109】yi iT，轮式里程计估算下一个位姿Si+1=xi+1,yi+1,i+1T：111cossin0sincos0001iiiiiiiiiiiiixxxyyy+=+(1)将移动机器人的运动看作一个逐步累加的过程，当i=n时，移动机器人的位姿为：111111cossinsincosnniiiiinniiiiinniixxxyyyxy=+=+=+(2)1.2 激光里程计RF2O（Range Flow-based 2D Odometry）11是一种根据激光传感器连续范围扫描来估计其平面运动的方法。图2 RF2O激光里程计如图所示，假设R(t,)为激光范围扫描，其中t是时间，0,N)R是激光扫描坐标，N是该激光扫描的长度。任意点P在激光雷达坐标系的极坐标为(r,)，与点P的角坐标直接相关的扫描坐标可以以如下公式表达：11122NNNkFOV=+=+(3)其中FOV为扫描的范围。对于移动机器人的速度估计，R2FO根据范围流约束方程12，提出激光雷达的速度约束方程：(),sincoscossinsincos0 x sy sstR kR kvvrrxyR kR+=(4)其中R和Rt为极坐标中r对角度和时间t的偏导，x、y、vx,s、vy,s和s分别表示激光雷达x轴和y轴上的坐标、x轴和y轴上的速度和激光雷达在平面上的角速度。1.3 EKF融合里程计信息k-1时刻里程计数据输入的位姿估计列向量为xk-1=xk-1,yk-1,0,0,0,k-1T，对应二维平面上的x轴、y轴上的坐标以及航向角，预测的卡尔曼滤波状态与协方差矩阵为：1 kkkxF x=(5)1kkTkkkPF PFQ=+(6)其中Fk为状态转移矩阵，Pk-1为协方差矩阵，Qk为噪声协方差矩阵。计算k时刻对应的卡尔曼增益Kk：()1TTKkkKkkKP HH P HR=+(7)其中Hk是将状态值转换为测量值的转换矩阵。计算最优状态向量与协方差：()kkkkkxxKzH x=+(8)kkkkPPK H P=(9)更新卡尔曼滤波状态，将计算得的最优状态向量xk设为下一次计算中的xk-1，对协方差矩阵Pk也进行相同的步骤。在下次计算中使用另一个里程计估计的位姿预测卡尔曼滤波状态。其步骤可总结如下：1）通过里程计数据对卡尔曼滤波状态进行预测；2）计算卡尔曼增益并计算最优状态向量与协方差；3）更新卡尔曼滤波器状态，回到步骤1)选用另一个里程计数据进行计算。里程计数据中，协方差矩阵表示里程计数据的不确定性。RF2O激光里程计有着较高的精度，因此直接使用默认的里程计协方差矩阵将会降低最终的定位精度，需要截取部分激光里程计数据，求对应的协方差矩阵。激光里程计输出的位姿信息的频率由激光雷达频率决定，从记录的激光里程计数据中截取对应激光雷达频率的数量的位姿信息。设里程计数据中截取n个数据，转化为n个66矩阵Xi：iiiiiiixyzX=(10)其中xi,yi,zi,i,i和i分别代表x轴，y轴z轴上的位置，横滚角，俯仰角和偏航角。计算该组数据的协方差:111111TnnniiiiiiiPXXXXnnn=(11)1.4 RTAB-Map SLAM算法本文所采用的RTAB-Map(Real-Time Appearance-Based Mapping)13是一种基于图优化的SLAM算法，由其作者在2013年提出并在2018年扩展14，支持使用者采用多种传感器同时进行视觉与激光雷达SLAM15。本文采用的RTAB-Map框架如图3所示，输入RGBD图像、激光雷达数据与里程计数据，输出优化后的移动机器人运动轨迹、地图数据、优化后的TF变换、三维点云地图和二维栅格地图。【110】第45卷 第08期 2023-08RTAB-Map的工作流程为：1）将输入数据同步。对于输入的数据不总是以相同的速率和准确的时间输入，RTAB-Map会同步输入数据的帧速率。图3 RTAB-Map SLAM流程2）生成局部三维点云地图与局部二维栅格地图。RTAB-Map对RGBD图像使用块匹配算法(block matching algorithm)16生成点云。激光雷达观测数据里的每一束光线生成占用网格，由所有生成的占用网格组成二维栅格地图。3）回环检测与闭环检测。RTAB-Map提取RGBD图像中的特征点并将其转换为视觉词汇表，根据词汇签名生成定位点进行回环检测，以此检测移动机器人是否在已经构建地图的区域。RTAB-Map通过激光扫描进行接近检测定位与当前位置环境相似的区域，同样检测移动机器人是否在先前建图的区域。4）图优化。当回环检测和接近检测触发时，RTAB-Map以图优化的方法最小化地图中的错误。RTABMap集成了三种图优化方法：基于树的网络优化器(TORO)、图优化通用框架(g2o)和GTSAM。5）全局地图拼接。全局地图由局部地图拼接而成，RTAB-map将局部地图转换为对应位姿。当回环检测和接近检测触发时，局部地图由新的优化后位姿组成全局地图。2 里程计与SLAM仿真2.1 里程计仿真通过Gazebo仿真平台追踪虚拟移动机器人的部件获取里程计真值，在仿真中通过ROSBag对信息进行记录。采集采用Gazebo运动插件中的轮式里程计与RF2O激光里程计做出位姿估计，与此同时使用扩展卡尔曼滤波融合两个里程计信息估计移动机器人位姿。通过evo SLAM评估工具绘出里程计所估计的轨迹，并以里程计真值作为参考轨迹评估其精度，如图4与表1所示。(a)轮式里程计轨迹 (b)激光里程计轨迹 (c)融合里程计轨迹 (d)改进协方差矩阵融合里程计图4 里程计估计轨迹与机器人实际轨迹表1 里程计估计轨迹精度评估平均误差均方根误差方差轮式里程计0.3389010.36226864.437806激光里程计0.1984730.1181136.863711融合里程计0.2917060.31358483.289785改进融合里程计0.1884790.1339068.821920 根据图4和表1,分析四种里程计数据在Gazebo仿真平台中的估计轨迹与误差。改进的融合里程计有着最高的精度，其平均误差为0.188479，而轮式里程计信息误差最高，其平均误差为0.338901。分析其原因，轮式里程计容易积累误差，误差偏大，而激光雷达精度较高，不容易累积误差。改进的激光里程计协方差矩阵后能够正确地表示里程计数据的不确定度，激光里程计因为更低的不确定度对融合的轨迹影响更大。2.2 SLAM仿真通过Gazebo仿真平台构建如图5所示的物理环境。分别采用4种里程计数据进行RTAB-Map SLAM测试，在RVIZ机器人可视化平台中实时记录SLAM结果(如图6所示)，验证这些里程计数据用于同时定位与地图构建的可行性。图5 仿真实验物理环境 (a)轮式里程计SLAM (b)激光里程计SLAM第45卷 第08期 2023-08【111】(c)融合里程计SLAM (d)改进融合里程计SLAM图6 SLAM仿真结果根据图6，四种里程计数据都能够用于RTAB-Map SLAM。由于激光里程计输出位姿估计频率较低，导致采用激光里程计的SLAM结果中存在错误位置的点云。3 SLAM实地实验本文在真实场景中使用实体移动机器人实验平台进行测试，排除理想情况对SLAM的影响，如：仿真中，编码器没有误差；激光雷达测量精度不受距离影响，其输出频率恒定，不受机械结构影响；RGBD相机获得的RGB图像和深度图像完美配准，没有畸变效应等。本文在真实的环境中使用实验室的移动机器人实验平台，研究四种里程计在实际情况下的SLAM结果。3.1 移动机器人实验平台本文所采用的移动机器人(如图7所示)配备增量式光电编码器、Astra-Pro RGBD相机和YdLidar G4机械式单线激光雷达等传感器。移动机器人通过下位机与上位机对移动机器人进行操控，其中下位机为STM32单片机，负责收集底盘数据与操控机器人运动等工作，上位机为树莓派4B，负责收集RGBD相机与激光雷达数据，对下位机发送操控信息并接收下位机的底盘数据，进行数据的分析与处理等工作。(a)实验平台外观 (b)移动机器人软、硬件框架图7 移动机器人实验平台3.2 室内复杂环境SLAM实验本章采用移动机器人实验平台在真实环境中（如图8所示）进行了四次不同里程计数据的SLAM测试，在RVIZ机器人可视化平台中实时记录SLAM结果，如图9所示。图8 实验场景在实际场景中用激光测距仪测量部分场景的特征长度作为真实值，在RVIZ中使用测量功能测量SLAM结果中相同特征长度作为测量值，将测量值与真实值对比评估SLAM结果的精确度，如表2所示，分别用lw,|w|,lr,|r|,le,|e|,la,|a|表示轮式里程计、激光里程计、EKF融合的里程计和改进协方差矩阵的EKF融合里程计特征长度与绝对误差。(a)采用轮式里程计SLAM结果(b)采用激光里程计SLAM结果(c)融合里程的SLAM结果【112】第45卷 第08期 2023-08(d)改进协方差矩阵融合里程计SLAM结果图9 RTAB-Map SLAM结果在基于轮式里程计的SLAM结果中，左边的灰色铁柜与右边的黄色木柜以及办公桌等物体都较为扭曲，有着较大的误差，相比于激光里程计的SLAM结果，地图中间的空旷区域，尤其是各个桌子之间的错误位置点云更少。激光里程计SLAM结果中误差较小，但是空旷区域中错误位置点云较多，部分能够通行的狭窄区域已经被错误位置点云完全填满。融合两种里程计SLAM结果误差偏大，空旷区域错误位置点云较少，而改进协方差矩阵后，融合两种里程计SLAM结果误差更少。表2 特征定量分析表编号真实值lwlrlela17.726006.846147.980358.064437.3161522.320001.902552.178531.957062.3598434.123004.151003.975214.208134.3316844.034004.360244.129654.362734.1628050.873000.951820.813870.923500.86043编号真实值|w|r|e|a|17.726000.879860.254350.338430.4098522.320000.417450.141470.362940.0398434.123000.028000.147790.085130.2086844.034000.326240.095650.328730.1288050.873000.078820.059130.050500.01257 为了能够客观地分析基于各个里程计的SLAM精度与错误点云的占比，采用平均绝对误差（MAE）与平均绝对百分比误差（MAPE）统计SLAM中的误差，采用Cloud Compare点云工具估算错误的点云体积。表3 SLAM分析表统计指标轮式里程计SLAM 激光里程计SLAM EKF融合里程计SLAM 改进融合里程计SLAM MAE0.3460740.1396780.2331460.159948MAPE9.02864%4.42376%7.20455%3.34323%错误点云体积1.8932.3671.6841.521 图9与表2的测试结果表明：1）四种里程计都可以用于RTAB-Map SLAM，实际实验结果比仿真结果差；2）采用激光里程计的SLAM结果精度最高，而轮式里程计精度最低，这是因为激光里程计精度大于轮式里程计，轮式里程计在现实条件下表现较差，图优化也不能完全消除地图中的错误；3）由于激光里程计输出位姿频率较慢，RTAB-Map会将局部地图对应错误的位姿，导致采用激光里程计的SLAM结果存在较多错误点云，图优化反而会加剧这种情况，而轮式里程计构建的地图错误主要来自轮式里程计较差的精度。EKF融合两种里程计后增加输出位姿频率，减少错误点云的产生，而改进协方差矩阵的EKF融合里程计有着不错的精度，错误点云更少。4 结语本文通过改进协方差矩阵加强激光里程计对EKF的影响，提升精度，并分别进行了仿真测试与实际测试。仿真和实验结果表明：在室内复杂场景中，采用改进协方差矩阵的EKF融合里程计能够解决轮式里程计RTAB-Map构建的地图精度较差、采用激光里程计RTAB-Map构建的地图存在错误点云的问题，提升RTAB-Map SLAM精度与效果。本文采用的RTAB-Map能够通过图优化减少地图错误，但是不能够及时消除地图中错误点云。本文下一步工作将对RTAB-Map进行改进，减少因为位姿输出频率较低而产生的错误点云。进一步改善室内复杂场景内多传感器SLAM系统。参考文献：1 高翔,张涛,视觉SLAM十四讲:从理论到实践M.北京:电子工业出版社,2019.2 Randall Smith,Matthew Self.Estimating Uncertain Spatial Relationships in RoboticsJ.CoRR,2013,abs/1304.3111.3 中国公路学报编辑部.中国交通工程学术研究综述2016J.中国公路学报,2016,29(6):1-161.4 朱建斌,赵健康,崔超,等.基于EKF的UWB/IMU组合导航系统鲁棒性设计J.计算机仿真,2020,37(12):47-52,57.5 Thrun S,Fox D,Burgard W,et al.Robust Monte Carlo localization for mobile robotsJ.Artificial Intelligence,2001,128(1/2):99-141.6 Thrun,S.Simultaneous Localization and Mapping with Sparse Extended Information FiltersJ.Int.j.robot.res,2004,23(7/8):693-716.7 Borrmann D,Elseberg J,Kai L,et al.Globally consistent 3D mapping with scan matchingJ.Robotics and Autonomous Systems,2008,56(2):130-142.8 倪志康,厉茂海,林睿,等.基于三维激光雷达与RTK融合的SLAM研究J.制造业自动化,2020,42(7):51-54.9 李航,黄波,廖映华.基于非线性多传感器复合定位融合算法应用研究J.机床与液压,2023,51(5):41-47.10张礼睿,孙勇智,刘力铭,等.无里程计移动机器人多传感器融合定位算法J.组合机床与自动化加工技术,2023,588(2):109-112.【下转第166页】【166】第45卷 第08期 2023-08置由不同的电机所驱动，具有其相对独立性，其拥有简易的自定义功能，可以记住数个形态调整方案，为按摩椅姿态的“一键”调整提供了可能性。2.3 导轨及办公智能设计2.3.1 车辆导轨系统在传统商务车导轨的基础上进行了一定的改进：把原有的普通导轨进行了一定程度的加长。与传统车型滑轨不同，按摩椅安装在水平导轨上，椅背等结构用铰链固定，在后方无乘员的情况下可以自由调整前后方空间大小和靠背角度，及能体验到舒适的乘车座椅模式，更能在路面平稳时，将座椅调整到近乎水平的状态，给乘车人员最大程度休息放松。2.3.2 车侧小桌板设计为了保证商务人员的工作效率性和稳定性，在按摩椅的基础上，在右侧扶手处添加了小桌板设计，如图7所示。在MPV的目标客户群体中，所需要的是可以拜访一定办公文件，笔记本电脑等的空间，因此在车侧小桌板容纳空间的前方还放置了一定的线材收纳装置，如HDMI视频转接线，电源线等等。在线材的基础上，商务人员可以将自己的笔记本电脑与车内的空间互联，从而形成一个高效的沉浸性工作空间。与此同时，在休息时间也可以起到一定的娱乐作用。由于按摩椅侧面的扶手空间有限，为了保证车内空间的完全利用，这里采用抽拉式小桌板的设计。图7 车侧小桌板设计2.3.3 投影系统在加长型座椅导轨的基础上，可以在乘坐者面前留有足够的办公空间，为了保证这一空间的合理利用，设计了投影系统。其分为可收缩电动幕布和车顶投影仪两大部分。在正常的乘坐状态中，电动幕布处于收缩状态，不影响商务人员的乘坐体验。在商务人员有一定的办公要求的，便可以将按摩椅沿导轨推到最后进入办公状态，同时操作电动幕布下放，将笔记本电脑与前文提到的线材收纳系统相连接，形成完整的办公空间，大大提高了办公者的工作效率。在此基础上，还可以为车内其余人员进行各类展示，为商务人员提供了巨大的便利。3 结语针对MPV车型中后座椅只有“乘坐”这一单一功能，提出了新型座舱设计方案,对第二排空间进行合理改进。在按摩座椅的设计基础上，对形态调整系统、改进型导轨系统、车侧桌板系统进行探索。该设计能满足用户按摩休息需要，并且能保障移动办公的需求。本设计获得第十届上海市“上图杯”先进成图技术与创新设计大赛上汽大通“酷客杯”创新设计竞赛一等奖。参考文献：1 李骏鹏.基于生育转变理论的中国人口结构预测D.江西财经大学,2021.2 史效东,任媛.我国城市居民通勤时间的变化特征基于两省一市的追踪调查J.太原城市职业技术学院学报,2020(7):15-16.3 何明卫,寸议鹏,石庄彬,等.小汽车用户通勤时间容忍阈值的特征及影响因素J.深圳大学学报(理工版),2021,38(4):340-346.4 郁淑聪,孟健,张渤.浅谈汽车智能座舱发展现状及未来趋势J.时代汽车,2021(5):10-11.5 陈俊,智能座舱的内外都需要元器件的精心打造J.电子产品世界,2021,28(8).6 杜曾宇,黄晓延,蒙锦珊.智能座舱的关键技术J.时代汽车,2021(5):143-144.7 杜莎.汽车座舱从“工具”演变为“智能助手”,整车厂传统的设计开发流程面临哪些挑战J.汽车与配件,2020(15):58-59.8 陈海峰.智能座舱:带来不一样的用车体验J.上海信息化,2020(10):41-44.11Jaimez M,Monroy J G,Gonzalez-Jimenez J.Planar Odometry from a Radial Laser Scanner.A Range Flow-based ApproachC/IEEE International Conference on Robotics&Automation.IEEE,2016.12Spies H,Jaehne B,Barron J L.Range Flow EstimationJ.Computer Vision&Image Understanding,2002,85(3):209-231.13Labb M,Michaud F Online global loop closure detection for large-scale multi-session graph-based SLAMC.2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.IEEE,2014:2661-2666.14M Labb,Michaud F.RTAB-Map as an open-source lidar and visual simultaneous localization and mapping library for large-scale and long-term online operation:LABB and MICHAUDJ.Journal of Field Robotics,2018,36.15赵伟卓.基于视觉与激光雷达的ROS机器人自主导航研究D.沈阳理工大学,202216Dazhi Y,Walsh W M,Zibo D.Block Matching Algorithms:Their Applications and Limitations in Solar Irradiance ForecastingC/PV Asia Pacific Conference.2014.【上接第112页】