一种基于ROS的多传感器融合地图构建方法
李明明，张 龙
（西安科技大学 通信与信息工程学院，陕西 西安 710054）
从20世纪的50，60年代开始，科学家开始对机器人技术进行研究。王天然院士指出，本世纪以来机器人的应用越来越广泛，对生产力的推动作用越来越突出，机器人领域的各项技术也在快速进步，发展形式多样化［1］。
工业级机器人大多是采用固定式机器人为主，在处理简单事件时有很高的效率，杨琨等在其研究中提出当前机器人自身的缺陷使其应用范围具有一定的局限性［2］。在对机器人进一步研究之后，李成进等也提出移动机器人存在的一些重要缺陷，由于它的分层结构设计，其分序需要很长的时间，导致反应速度变慢［3］。与此同时，由于机器人技术发展缓慢与周围环境复杂，其在执行任务中难免会遇到各种各样的问题。随着移动机器人技术迅速发展，其复杂多变的使用场景也越来越多，故提高移动机器人在面对未知环境时处理遇到的障碍物的能力显得尤为重要。TAHERI等在其研究中提出使用基于同时定位与建图（Simulta-neous Localization and Mapping，SLAM）的静态地图构建方法，其作用是先控制机器人对周围静态环境感知，将周围的障碍物等信息扫描识别［4］，然后标注到杨蕴秀等所构建的二维栅格地图里面［5］，其好处是机器人在特定环境的场景中进行作业更加方便，可以提高后续路径规划与导航的效率。但由于机器人周围环境障碍物结构复杂，使用传统方法在室内建图经常会漏扫一些特征不明显，形状多样的障碍物，如桌椅等镂空障碍物。这些镂空障碍物体积较大，在室内环境中经常出现。同时，沈斯杰等在其研究中指出，传统激光雷达建图受制于激光雷达扫描单一平面的缺点，在扫到镂空障碍物时，识别的激光数据较少而无视此处的障碍物［6］。这样的错误显然是很致命的，因为如果建好的地图中未标出障碍物，而实际环境中存在障碍物，这对移动机器人后续的自动避障作业是很麻烦的。
因此，为了解决漏扫镂空障碍物的问题，文中在静态建图的条件下，给移动机器人安装多传感器，融合激光雷达数据和深度相机数据，弥补单激光雷达建图漏扫的缺点；
为纠正轮式里程计实际使用时轮子打滑导致建图不准确的问题，将轮式里程计和惯性测量单元（Inertial Measurement Unit，IMU）融合提供位姿估计的定位数据，搭配融合后的激光数据，共同传给建图算法完成建图。利用本方法在固定障碍物的场景下能有效识别镂空障碍物，实现准确建图，帮助移动机器人完成之后的作业与任务。
1.1 Gmapping算法原理
Gmapping［7］算法是一种基于粒子滤波（Rao Blackwellized Particle Filter，RBPF）［8］框架的开源算法，能够即时构建周围环境的栅格地图，在小场景内具有计算量小，精度高的优点。其对RBPF算法进行了提议分布改进，主要是将里程计数据同激光数据联合起来提议，校准后的提议分布与真实分布更为相似。因此，将二者联合起来，则RB-PF算法［9］改进提议分布见式（1）。
式中 x1∶t为位姿信息；
m为地图信息；
u1∶t－1为里程计数据；
z1∶t为观测数据。使用最近一次观测数据改进后，提议分布见式（2）。
最后对权重粒子进行改进提议分布计算。
通常目标分布是机器人在t时刻位置的真实分布，只能使用提议分布来描述该分布。由于改进的提议分布发现激光具有单峰值特性，在其附近采样就能减少其他噪声的数据［10］。因此，在激光峰值附近采集K个样本数据后，结合高斯函数模拟提议分布。模拟好提议分布后就可以获取其位姿信息，再结合激光信息构建局部地图信息，最后生成二维栅格地图。
在实际环境中，单激光雷达建图的鲁棒性较差，地图噪声较多，与实际场景中的障碍物位置情况不符合。激光雷达建立的二维栅格地图，更重要的应用是为了移动机器人在建好的地图中自主移动并且提高它在静态环境中的避障成功率。
1.2 Cartographer算法原理
基于粒子滤波算法的SLAM的局限性就在于滤波所用的粒子在经过一次计算后就会被丢弃，下次计算再次重新选取粒子，导致SLAM的计算效率低下。THRUN等针对此问题，在2006年提出了Graph-SLAM（图优化），试图将激光雷达或摄像头采集到的数据以位姿变化的方式呈现，并将稠密点云稀疏化以降低计算消耗，根据位姿间的约束关系绘制工作环境地图，并最终将定位问题通过降维方法简化为最小二乘问题［11］。
从图1可以看出，图优化问题可以用图的方式表示，节点P是需要优化的变量，边是优化变量之间的约束。X节点可以是位姿，也可以是路标；
边可以是位姿之间的关系也可以是观测量；
图优化是把位姿和空间点一起，进行优化。随着轨迹增多，特征点的增长也很快。因此位姿图优化的意义是再优化几次以后将特征点固定住不再优化，只当作位姿估计的约束，之后主要优化位姿。
图1 图优化方法Fig.1 Method of graph optimization
Google基于机器人操作系统（Robot Operating System，ROS）开源Cartographer算法就是基于图优化方式进行建图的［12］。优点在于基于图优化的方法，在建图中加入了回环检测与闭环纠正改进，能够很好地完善全局建图［13］，对于室内镂空障碍物建图具有很好的校正作用。缺点是对计算机硬件性能要求较高，低配平台会降低回环与闭环的运行效率。
在实际仿真与建图中，基于图优化的算法相比于粒子滤波算法，由于加入了闭环检测和回环纠正，得到的地图更加完整清楚。后将会在实验部分进行具体的建图效果对比。
2.1 单线激光雷达
目前，多线激光雷达价格昂贵，均在万元级别以上，一般企业难以承受如此高昂的价格。相比来说，单线激光雷达［14］成本就低得多，可以帮助机器人规避障碍物，扫描速度快、分辨率强、可靠性高。相比多线激光雷达，单线激光雷达在角频率及灵敏度上反应更快捷，所以，在测试周围障碍物的距离和精度上更加精准。但单线雷达只能平面式扫描，不能测量物体高度，由于激光数据的特性导致它需要大量的扫描数据，所建地图无法进行大范围闭环检测［15］。
研究使用的杉川3irobotix Delta 3A激光雷达是一种三角测距原理的单线激光雷达，原理和TOF激光雷达不同。TOF激光雷达［16］是根据测量光的飞行时间来计算距离的，而三角测距激光雷达是通过摄像头的光斑成像位置来解三角形的。
从图2可以看出，O1是激光发射器，射出去的激光由虚线表示，A，B，C是3个反射点。O2是摄像头光心轴，三角形代表用来捕捉反射光斑的相机模型。A，B，C的成像点分别是A′，B′，C′。由于激光发射器和相机安装的相对位置是已知的，也就是说相机的光心轴和激光（线）的角度已知，线段O1到O2长度已知，角（O1，O2，A）也已知，根据角边角定理，三角形有唯一解，于是（O1，A）的长度是可以算出来的。同理B，C这2点距离O1的距离也是可解的。这样就获取到了障碍物到激光雷达点的测量距离。
图2 激光雷达三角测距法Fig.2 Lidar triangulation of distance
在ROS系统中，将激光雷达数据封装为sensor＿msg／LaserScan的自定义雷达专用消息包，方便各个厂家在制作自己的激光雷达时，能够有统一的格式。
2.2 深度相机数据转激光数据原理
由于激光雷达价格不菲，而摄像头价格相对便宜。使用ROS官网开源的depthimage＿to＿laser-scan功能包能够将深度摄像头和多线程激光雷达采集的三维图像信息转换为单线激光雷达的二维点云信息，并且发布／scan话题消息供建图算法使用［17］。该方法存在的问题是，雷达数据是二维的、平面的，深度图像数据是三维的，如果将三维的数据转换成二维数据，只需要取深度图的某一层即可。由于摄像头对光线感知较为明显，在室内环境下，可以对摄像头采集到的图像进行处理，将障碍物的缺陷点提取出来再转换，可以减小转换计算量。因此，深度相机较之于激光雷达无论是检测范围还是精度都有不小的差距。
如图3所示，其主要转换步骤如下。
图3 深度图转激光原理Fig.3 Principle of converting depth map to laser
步骤1：将深度图像的点p（u，v，z）转换到深度相机坐标下的点P（x，y，z）。
步骤2：计算夹角AOC见式（4）。
步骤3：将AOC角投影到对应的激光数据槽中，已知激光的范围［α，β］，激光束细分为N份，则可用laser［n］表示激光数据。点P投影到数组la-ser中的索引值n，见式（5）。
laser［n］的值为P在x轴上投影点C到相机光心O的距离r，见式（6）。
2.3 惯性测量单元
IMU传感器［18］是加速度计和陀螺仪传感器的组合。它被用来检测加速度和角速度以表示运动速度和运动强度。陀螺仪是用于测量或维护方位和角速度的设备。它是一个旋转的轮子或圆盘，其中旋转轴可以不受影响地设定在任何方向。当旋转发生时，根据角动量守恒定律，该轴的方向不受支架倾斜或旋转的影响。
实验平台所使用的是MPU6050传感器［19］，它是一个6轴运动处理传感器，集成了3轴MEMS陀螺仪，3轴MEMS加速度计以及一个可扩展的数字运动处理器（Digital Motion Processor，DMP），可用I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其I2C接口输出一个6轴信号。DMP通过调用函数对获取的数据进行四元数求解，即可得到所需姿态角。MPU6050对陀螺仪和加速度计分别用了3个16位ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的。
2.4 轮式里程计
机器人可以基于码盘数据和底盘运动学模型进行航迹推演，从而得到机器人的轨迹。对于移动机器人来说，假定机器人的初始位置已知，在一个行驶维度为一维的环境，机器人可以根据轮子的圈数实时估计自己在这个一维行驶环境的何处。在二维平面情况下，通过左右轮的转速测量单元可以推知左右轮的转圈数，显然，当左右轮的转圈数不同时，移动机器人会向左或者向右转。
目前的差分两轮轮式里程计［20］普遍使用的是光电码盘，经过光电管后，每次码盘转动都会产生一次脉冲，微控制单元（Microcontroller Unit，MCU）可以通过中断记录2次脉冲之间的时间t，已知这个时间，已知2个码盘间的距离，就可以算出来这段时间的真实速度。实际使用中，轮式里程计的轮子容易受环境因素影响出现打滑和漂移的问题，出现累计误差增大从而影响定位效果。
3.1 激光雷达数据与深度相机数据融合
基于激光雷达与深度相机各自建图的优缺点，文中提出激光雷达与深度相机融合建图方法，通过均值滤波算法将二者优势结合互补，能够很大地优化移动机器人在实际环境中的建图效果，使得其扫描到的障碍物信息更加完善，建立完整的可用于导航的静态二维栅格地图。
目前，二者融合的方案有许多，大致分为2种。一种是［21］在采集前端直接对传感器数据融合，主要对传感器中的障碍物信息加以滤波，选取确定的障碍物信息。这种方法能够将2个传感器识别的障碍物都储存起来，与融合前的相比，可识别的障碍物信息增加。
另一种方法［22］是在采集后端对SLAM生成的局部地图数据进行融合。这种方法需要反复对同一场景进行移动建图才能达到比较好的效果，比较繁杂。
文中采用前端传感器数据融合方法。由于深度相机输出的是三维深度数据；
最后建图算法需要的是二维激光数据，因此，融合算法首先利用ROS官网的三维深度相机数据转二维激光数据功能包，将三维深度相机数据转成二维激光数据，然后再利用均值滤波算法［23］进行激光和相机数据融合。均值滤波融合算法描述如下。
1）分别选取激光与相机的初始激光帧。
式中 Li和Ci分别为获取的激光数据；
xi和yi为该点角度上的距离值。
2）对激光与相机数据融合，若选取范围内的激光雷达数据中存在无效值，则替换为该处相机对应的值；
若该处激光雷达存在有效值，则对比该处激光与相机有效值的误差，大于阈值则替换为该处相机对应的有效值。
3）检查最后固定范围内的激光数据，并作为新的激光帧进行发布。
通过把二者激光帧的障碍物信息通过均值滤波算法融合，所产生的新激光数据中就拥有周围环境真实的障碍物信息。
3.2 轮式里程计与IMU数据融合
为了获得更准确的建图效果，需要将里程计和IMU通过扩展卡尔曼滤波［24］融合输出定位［25］信息，融合过程如下。
1）构建系统预测模型，初始化传感器状态矩阵与误差协方差矩阵。
2）然后获取传感器数据和对应的协方差矩阵，并计算系统的卡尔曼增益K′。
3）更新系统状态矩阵和误差协方差矩阵，完成迭代，并输出融合后的位姿估计信息。
激光通过图2方式采集周围平面中的障碍物信息，深度相机通过采集三维环境中的障碍物数据，并通过式（6）将深度图像三维投影转换为二维激光数据，然后通过式（7）均值滤波算法将二者所采集数据融合，输出包含二者障碍物信息的新激光数据，最后结合里程计与IMU传感器通过式（10）融合定位数据，与第1章的Gmapping或Car-tographer开源SLAM算法，构建完整的二维栅格地图。多传感器融合框架如图4所示。
图4 多传感器融合框架Fig.4 Frame of multi-sensor fusion
实验搭建的实物平台如图5所示，为长0.42 m，宽0.42 m，高0.88 m的轮式机器人，图中标注出各个传感器相对位置，图5（b）为机器人硬件框架图。机器人使用的主要传感器有杉川3irobotix Delta 3A激光雷达、乐视Letv Pro Xtion体感摄像头和轮式里程计，工控机使用英特尔酷睿i7－4500U中央处理器，拥有1.80 GHz主频，8 GB内存的工业控制机，下位机使用意法半导体STM32F407VG底层控制板，移动底盘采用轮式差速电机底盘。上位机使用基于Ubuntu16.04的ROS-kinetic的机器人操作系统，使用C＋＋语言编写融合代码，在室内条件下进行实验。
图5 移动平台与硬件框架Fig.5 Frame of mobile platform and hardware
由于激光雷达与深度摄像头分别建图时，其采集到的激光数据处在不同图层，在不做任何处理时，建图算法只能接收到频率高的激光数据。因此，该方法对二者的激光数据进行C＋＋代码重构，使用时间同步函数同时订阅二者／scan话题，使用回调函数对二者／scan话题数据进行处理并发布一个新的／scan话题。新话题成功解决了二者建图数据不在同一图层的问题，在实验中也取得了良好的建图效果。
图6为Gazebo仿真环境，有2块镂空桌子分别用A、B表示镂空障碍物。融合前后对比如图7所示，图7（a）、图7（b）是Gmapping建图算法使用数据融合前后对比建图，图中的红框处并不能识别出镂空障碍物；
融合深度相机后，能识别出仿真环境中的障碍物A、B。图7（c）、图7（d）是Cartog-rapher算法融合深度相机前后的对比图，之前识别不出来的桌子障碍物，在融合深度相机后，二者均能识别出镂空障碍物A，B的具体位置。
图6 Gazebo仿真环境Fig.6 Gazebo simulation environment
图7 不同方法仿真环境下融合前后建图对比Fig.7 Comparison of different methods of mapping before and after fusion in simulation environment
图8为移动平台真实室内环境，室内场景为7.6 m×3.8 m（长×宽），有2张0.48 m×0.41 m×0.76 m（长×宽×高）的镂空椅子充当镂空障碍物。融合前后的机器人建图如图9所示，图9为二维栅格地图俯视图，地图分辨率为0.05 m，图9（a）、图9（c）是未融合的2种算法建图，图中红色方框并不能识别机器人前方的镂空障碍物，且圆形红框也显示出2种算法都有不同程度的漫反射现象；
图9（b）、图9（d）是融合深度相机后的2种算法建图，之前识别不出来的椅子障碍物，也能成功识别出来，并标识出镂空障碍物A、B的具体位置。深度相机采集的是环境深度信息，由于移动机器人尺寸较高，讲台高0.22 m，激光雷达距地0 28 m，相机距地面0.88 m，激光雷达只能识别同位置水平平面的物体，深度相机对不在同一平面的特征点识别较少，且物体特征点集中，与物体颜色关系不大，因此其主要特征点在镂空障碍物上，所以未识别出特征点单一的讲台。圆形红框处的激光雷达漫反射也得到改善，建图更加准确，达到了该方法识别镂空障碍物的目的，提高了对周围镂空障碍物的识别能力。
图8 移动平台室内环境Fig.8 Indoor environment of mobile platform
图9 不同方法室内环境下融合前后建图对比Fig.9 Comparison of different methods of mapping before and after fusion in indoor environment
移动平台的建图精度可以转换为其在环境中运动轨迹的精度测试。平台在室内环境下的建图移动轨迹对比如图10（a）所示，为数据融合前后移动平台运动轨迹对比图；
从图10（b）可以看出，数据融合后的移动机器人运动轨迹优于融合之前，室内环境下融合前比融合后平均存在15 cm左右的轨迹误差，且融合后的轨迹更加平滑和稳定，表明了数据融合后的地图更符合真实环境，从而验证了融合方法的有效性。
图10 室内环境中运动轨迹与误差对比Fig.10 Comparison of motion trajectories and errors in indoor environment
1）通过仿真实验验证，相比于单激光扫描，数据融合方法能够在仿真环境下识别桌子等镂空障碍物，在建立的二维栅格地图中有清晰的障碍物轮廓，边缘平滑，建图鲁棒性有明显改善。
2）在真实环境中，通过深度相机和激光雷达的数据融合方式，成功解决了传统单激光雷达室内环境建图漏扫镂空障碍物的问题。多传感器融合的移动机器人实验平台，能够在真实环境中进行更加准确的定位，更好地识别周围环境障碍物，也互补了不同传感器之间的优点。
3）优化后的融合算法相比于单激光雷达SLAM，机器人运动轨迹更准确，障碍物识别精度明显改善，可以为后续移动机器人自主导航与避障提供一定参考。
4）目前随着传感器性能的发展和SLAM系统实时性的要求，使用神经网络等融合算法也在SLAM未来发展中有了一定的尝试。
猜你喜欢建图镂空激光雷达手持激光雷达应用解决方案北京测绘(2022年5期)2022-11-22视觉同步定位与建图中特征点匹配算法优化汽车工程师(2021年12期)2022-01-17镂空幻想小资CHIC！ELEGANCE(2022年3期)2022-01-11法雷奥第二代SCALA?激光雷达汽车观察(2021年8期)2021-09-01基于三轮全向机器人的室内建图与导航电子制作(2019年10期)2019-06-17基于激光雷达通信的地面特征识别技术中国交通信息化(2019年1期)2019-03-26一种基于多传感融合的室内建图和定位算法成都信息工程大学学报(2018年4期)2019-01-23基于激光雷达的多旋翼无人机室内定位与避障研究电子制作(2018年16期)2018-09-26拼折镂空立方体数学大王·中高年级(2018年5期)2018-06-20镂空窗花朵朵开少年漫画(艺术创想)(2018年11期)2018-03-07