基于MEMS激光雷达的智能轮椅SLAM研究

崔林威， 崔建伟， 黄子正， 李 响， 姜汇策

(东南大学 仪器科学与工程学院，江苏 南京 210096)

我国人口老龄化问题日益严峻，正在步入老龄社会，老年人和残障人士的生活自理问题日益突出。目前老年人和残障人士多采用传统的无动力式轮椅和电动式轮椅，面对复杂的室外环境时，仅依靠使用者无法保障安全出行，因此无法满足使用者的出行需求[1]。另外，随着无人驾驶、激光SLAM(Simultaneous Localization and Mapping,即时定位与地图构建)技术的迅速发展，依靠三维激光雷达获取的点云进行建图，可完成室内外环境下的路径规划，这对于无人轮椅的研究有一定的借鉴意义。因此，针对现有轮椅的出行方式，为提高老年人生活、出行的便捷性，开展轮椅的导航研究对实现轮椅的智能化、提高老年人的出行质量有重要意义。

多传感器融合的智能式轮椅将是未来的发展趋势，目前国内外正在开展有关智能轮椅的导航研究。丁佳伟[2]将脑机接口技术与SLAM技术进行融合，将环境地图构建、自主避障、路径规划与导航技术加入到脑控轮椅中，实现脑控轮椅的自主导航。于福超等[3]设计了基于北斗导航定位的智能导诊轮椅，采用北斗导航定位作为全局路径规划路径信息，结合使用者操作实现精准行走来实际解决医院的导诊问题。张毅等[4]研究了基于ROS(Robot Operating System,机器人操作系统)的智能轮椅室内导航，采用激光测距仪并运用基于扩展卡尔曼滤波器的SLAM方法进行建图，使轮椅具备语音自主导航等功能。Ferracuti等[5]提出了Human-in-the-Loop框架，在室内场景轮椅导航到目的地过程中，根据路径规划错误后人产生的EEG电位作为导航算法的附加输入，实现了路径的实时修改。Shan等[6]提出一种轻量级和地面优化的激光雷达测距和建图算法(LeGO-LOAM)，适用于具有可变地形的复杂环境，可对移动机器人进行位姿估计，并可实时估计地面车辆的六自由度位姿，可用于无人车的建图与位姿估计。

以上研究在一定程度上实现了轮椅的自主导航，但也存在二维雷达检测精度不足、北斗定位信息存在误差，以及在复杂环境下导航功能下降的问题。因此，本文结合百度地图和MEMS三维激光雷达研究了智能轮椅的室外SLAM算法，在移动区域未建图时，借助百度地图的路径规划信息使用轮椅，同时利用激光雷达进行三维建图；完成建图后，仅依靠导航、避障算法完成轮椅的移动和环境感知，从而提高了轮椅的智能化水平和安全性能。

1 系统方案

所设计的系统旨在通过ROS和MEMS固态激光雷达完成轮椅室外行驶过程中的环境建模、路径规划和避障功能，结合百度地图导航与激光雷达完成三维环境建模，进而在不依靠百度地图的情况下完成路径规划，从而实现轮椅的自主移动。

系统的硬件部分包括MEMS固态激光雷达、工控机、轮椅电机和编码器。其中，MEMS固态雷达用于扫描轮椅前方道路的激光点云；工控机安装Ubuntu 16.04操作系统和ROS平台，用于获取、处理点云数据，完成室外环境建模和路径规划，并将移动指令发送给轮椅内部的嵌入式控制板，驱动轮椅电机控制轮椅的前、后、左、右移动；编码器用于获取轮椅的精确移动信息，并提供里程计信息。机器人硬件系统如图1所示。

图1 机器人硬件系统

2 智能轮椅三维环境建图

2.1 三维激光雷达

为了使智能轮椅具备导航功能，需要在轮椅移动过程中预先使用雷达、深度摄像头等传感器对环境进行建图，建图的精确度会影响到路径规划的准确性。考虑到摄像头易受室外强光、反射等环境因素的影响，且二维激光雷达不能全面反映路面信息，因此选用速腾聚创公司的激光雷达RS-LiDAR-M1。该款雷达相比其他激光雷达，不但同样具有体积轻巧、成本低等优点，而且便于安装在移动机器人上。该雷达等效于125线激光雷达，在2D MEMS振镜快速震动的同时发射高频率激光束对外界环境进行持续扫描，经过测距算法得到三维空间点云数据后便可获得环境的深度信息，从而获取准确的三维路面信息，使机器人更好地感知周围的环境，MEMS固态激光雷达如图2所示，其参数如表1所示[7]。

图2 MEMS固态激光雷达

表1 RS-LiDAR-M1激光雷达参数对比

2.2 激光点云获取

在ROS平台中，激光雷达可以作为子节点以话题(Topic)的形式输出点云的三维坐标。通过订阅该话题便可获取实时点云数据，同时可以通过可视化工具Rviz进行点云的可视化呈现[8]。

图3为RS-LiDAR-M1在室外采集的激光点云数据利用Rviz工具呈现的点云可视化结果。由图3可以看出，MEMS激光雷达获得的点云比较密集，能够真实反映出环境中的人、物体等信息，有助于提高室外环境建模的准确度。

图3 MEMS雷达室外激光点云

2.3 室外三维场景建模

SLAM技术是移动机器人研究的热点，用于实现机器人的实时建图和定位，包括地图表示、前端雷达数据处理、后端噪声处理、回环检测和地图融合4个步骤[9-10]。激光SLAM能够稳定、准确地建图，主流算法有：基于关键点的LOAM及改进算法LeGO-LOAM 和A-LOAM、LIO等[11]。LeGO-LOAM算法具有轻量化的特点，能够在低功耗嵌入式系统上实现实时姿态估计，其运行时间短，可用更少的计算时间实现较高的位置估计精度，可进行地面点云优化等。这些优点适合应用于智能轮椅的室外建图，因此选用LeGO-LOAM算法进行三维建图。

另外，本文选用的激光雷达采集点云的速度为每秒75万点，高密度的点云能够保证建图算法的精确度，但同时也增加了工控机的处理负担和建图的时间，因此设计了融合体素栅格滤波器和LeGO-LOAM算法的建图算法，包括点云滤波、点云分割、点云特征提取、位姿估计、激光点云建图共5个部分，具体流程如下。

(1) 点云滤波。

在LeGO-LOAM算法中加入体素栅格滤波器，实现对原始点云进行降采样的效果，以降低点云的密度。在处理时将原始点云分割成一个个3D网格，并计算每个网格内的点的质心，用于近似表示该3D网格内的其他点，同时在处理过程中采用下采样，可以加速点云处理过程且不破坏点云本身的几何结构[12]。式(1)为计算在一个体素网格内的点云质心的公式。

(1)

式中：(x,y,z)为质心坐标；(xi,yi,zi)为体素网格内的第i个点；k为网格内点的数量。

(2) 点云分割。

点云分割部分用于将地面点云和非地面点云分割开，同时对激光点云进行离群点去除和分簇，只留下点数量较多的点云簇。

MEMS激光雷达每扫描一次路面会发送一帧数据，假设t时刻获取的一帧点云为Pt={p1,p2,…,pn}，其中pi为t时刻的点云Pt中的一个点，可以将该点云投影到分辨率为600像素×125像素的二维图像中，每个点在图像中的像素即为实际物体距离激光雷达的距离。如图4所示，激光雷达竖直方向的扫描角度为-12.5°～12.5°，由于激光雷达水平安装，地面点必然出现在[-12.5°，1°] 扫描线上，因此将出现在该角度范围内的点视为地面点。

图4 激光雷达竖直方向扫描角度

对于剩下的点，使用基于图像的分割方法对距离图像进行分簇，同一类的点标记上唯一的标识，并将低于30个点的类作为噪声点处理，从而可以将一些微小物体点(如树叶、云等)作为噪声去除，减少相邻帧之间微小物体不重复出现对帧间匹配造成的干扰。这样保存下来的一帧数据就是静态物体的点云数据，并且具有每一类的标签、在距离图像中的行列索引和像素(距离)值。

(3) 点云特征提取。

为了获得t时刻和t-1时刻的姿态变换关系，需要从已分簇的点云里提取出点云的边缘特征和平面特征，首先将距离图像在水平方向上分成多个子图像，然后计算子图像中每一行点的曲率，根据曲率选择出边缘点和平面点，获得点集合。

将一帧图像划分为6个子图像，每个子图像的分辨率为100像素×125像素，在每个子图像中选取t时刻的点云Pt的一个点pi，在pi竖直方向上，左右各找5个点，构建集合S，并对每个子图像按照式(2)计算点在集合S中的曲率：

(2)

(4) 位姿估计。

位姿估计模块包括特征点关联和姿态解算，其中特征点关联是为了将k时刻的点云和k+1时刻的点云关联起来，姿态解算是为了估计雷达的位姿。本文选取分辨率为0.9°的高精度编码器BCE25H5H23D获取轮椅的移动信息。

LeGO-LOAM算法基于提取的特征点构建相邻两次扫描的约束关系，并使用两次LM优化算法得到姿态变换矩阵。其中特征点关联使用Scan-to-Scan方式，分为边缘点匹配和平面点匹配两个部分，计算点到直线的距离和点到平面的距离,姿态解算根据匹配的特征点云估计激光雷达的位姿。

(5) 激光点云建图。

LeGO-LOAM算法使用迭代最近点(Iterative Closest Point,ICP)算法进行点云配准，在得到一帧点云和相邻帧的姿态变换信息后，会调用一次建图函数，将t时刻的激光点云和t-1时刻的全局地图特征点云进行匹配，并将其加入到全局地图中，实现对已有地图的实时更新，利用GTASM优化得到最终的全局地图[13]。

3 百度地图与激光雷达融合导航

3.1 百度地图路径规划信息

为了实现轮椅的自主导航，需要在移动区域完成雷达建图。对于激光雷达没有完成建图的区域，通过开发基于百度地图路径规划的轮椅移动方式，轮椅使用者依据百度地图API函数提供的路径规划信息操控轮椅进行移动，同时雷达利用LeGO-LOAM算法进行建图，建图完毕后就可以不依靠百度地图完成实时路径规划与导航[14]。

为了调用百度地图API的路径规划函数，需要遵循以下步骤：① 在百度地图开放平台申请百度账号；② 申请成为百度开发者；③ 使用百度账号获得服务密匙(ak)；④ 发送URL，调用百度地图的相关服务。进而可编写JavaScript脚本调用DrivingRoute函数和search函数实现路径的查找与显示，百度地图API-路径规划如图5所示。

图5 百度地图API-路径规划

3.2 轮椅路径规划及导航

设计的轮椅的导航方式包括未建模和已建模两种情况，当后台不存在建模信息时，依靠百度地图，人为操控轮椅完成移动。在完成室外建图后，仅依靠道路感知算法和MEMS激光雷达完成轮椅的自主移动功能，即用户可获取指定目标点的移动路径，同时轮椅自主避障，此时依赖ROS功能包完成导航。

ROS提供了两个功能包：① move_base，实现机器人导航中的最优路径规划；② amcl，实现点云地图中的机器人定位。机器人只需要发布必要的传感器信息和导航的目标位置，ROS即可完成轮椅的实时路径规划、避障和移动指令的发布。导航算法实施如下：首先，导航功能包需要采集MEMS激光雷达发布的点云信息，以达到实时避障的效果，激光雷达以10 Hz的频率向该功能包发布话题为/rslidar_points的点云信息；然后，功能包订阅机器人发布的里程计信息和坐标变换完成自身定位，同时结合道路感知算法完成实时避障；最后，功能包输出cmd_vel话题的控制指令，用以控制机器人完成相应的运动。

3.3 路面感知算法

为了精确识别路面信息，提高轮椅安全性能，调用了RoboSense的路面感知算法RoboSense Smart Sensor感知SDK，协助导航算法完成道路物体感知。该SDK融合了基于几何规则的传统点云算法和基于数据驱动的深度学习算法，可以为自动驾驶决策规划模块提供像素级的全方位、结构化、高精度的感知信息。SDK包括驱动模块、通信模块和感知模块，其中驱动模块提供数据解析功能；通信模块用于对外信息的发送和接收；感知模块作为核心子模块，提供整体感知功能。

RoboSense Smart Sensor感知SDK能够输出以下信息：① 障碍物检测信息，包括行人、骑行者、小车、大车、拖车等；② 可行驶区域信息，将路面物体围成的封闭区域作为可行驶区域；③ 车道线信息，输出检测和预测到的车道线信息；④ 道路边界信息，以检测和预测的方式输出边界信息。通过调用SDK输出的路面感知结果即可获得准确的路面信息，从而保证轮椅实时导航过程中的安全性。

4 实验

笔者在激光雷达上研究了三维激光点云获取与基于体素网格滤波器和LeGO-LOAM的三维建图算法，然后进行了智能轮椅的导航技术研究，包括基于百度云和三维建图的路径规划和路径规划过程中的实时路面物体感知算法，下面按照本文设计的导航方式对上述研究进行实验验证。

4.1 建图及导航算法仿真

在实际测试前，对文中提出的导航模式、建图和导航算法进行了仿真，以保证实际应用时的安全性。首先在Gazebo界面中导入一款建筑内部的3D模型，如图6(a)所示，然后验证未建图的导航模式，利用键盘模拟人为使用百度地图的路径规划信息操控轮椅移动，在移动过程中利用改进的LeGO-LOAM算法进行建模，建好的三维地图如图6(b)所示。

图6 三维建图仿真

为了利用3.2节中的ROS功能包进行导航，需要将图6(b)的三维地图转换为二维栅格地图，采用Octomap库完成二维栅格地图的转换，转换后的二维栅格地图如图7所示。

图7 二维栅格地图

最后对导航算法进行仿真，将图7导入到导航算法中，当操作者需要到达地图的某个位置，可直接点击地图，结合避障算法，机器人可到达指定位置，路径规划信息和机器人的实时位置如图8所示。仿真实验表明，在未建图时利用激光雷达进行三维建图，然后将其转化为二维栅格地图，结合导航算法能够实现机器人导航，在实际控制轮椅时，订阅导航功能包的cmd_vel信息可获得轮椅的移动指令。

4.2 智能轮椅室外三维建模

根据图1构建的装有激光雷达的智能轮椅如图9所示，并在学校内部进行了室外三维建模实验。

图8 自主导航仿真

图9 装有激光雷达的智能轮椅

在室外环境未建图时，轮椅无法对陌生环境进行路径规划和导航移动，此时利用百度云进行路径规划，人工操控轮椅在室外进行移动建图。图10为基于百度云开发的地图可视化操作界面，用户可利用该界面操控轮椅到达目标位置。

图10 基于百度云的地图可视化界面

按照图10中的路径控制轮椅移动，同时使用激光雷达采集实时激光点云。利用融合体素网络滤波器的LeGO-LOAM算法进行三维建图，并进行点云滤波、点云分割、地面点优化等处理，原始点云如图11(a)，滤波后的点云如图11(b)所示。在Rviz界面上显示的已建好的三维地图如图12所示， RS-LiDAR-M1采集的高密度点云配合建图算法可使建好的图能更真实地反映路面环境。

图11 激光雷达点云滤波

图12 激光点云三维建图

为了与上述建图算法进行对比，采用LOAM算法也进行了三维建图，实验发现，在室外建图时，LOAM算法提取的特征较多，往往会将草丛、树叶等特征提取出来。但在实际应用中，建好的地图只需要包含车道线、路面障碍等信息即可，特征过多也会造成地图冗余。另外，LeGO-LOAM算法在运行速度上降低了60%，能够更高效率地完成建图。

4.3 智能轮椅导航感知实验

在完成建图后，根据导航仿真实验过程，将图12构建的三维地图转换为二维地图，并导入到ROS的导航算法包内，在该段路径进行了导航实验。

轮椅在长20.5 m的路径内进行了实验，轮椅能够在完成建图的情况下，到达指定目标位置，同时在移动过程中能够调用感知算法识别路面的物体信息，实现实时避障功能，室外实验及路面感知效果如图13所示，其中图13(a)为轮椅和实验人员，图13(b)为导航过程中调用物体感知算法获取的路面识别信息，进而机器人能够根据障碍物信息主动避障。在图13(b)中可以看到识别到的车辆(CAR)、车道线(红色直线)、其他障碍(Barrier)等，轮椅在进行目标分类时，智能轮椅导航指标如表2所示。

图13 室外实验及路面感知效果

表2 智能轮椅导航指标

另外，在实验过程中，受灌木、草丛等因素的影响，可能导致轮椅出现误识别现象。路面目标检测结果与部分错误分类结果的对比图如图14所示。由图14可以看到，出现了将Barrier误识别为CAR的结果，在误识别时需要机器人将识别结果、路径规划信息与人为控制灵活结合起来，辅助完成轮椅的移动，避免因误识别导致出现轮椅安全问题。

图14 路面感知效果对比

5 结束语

在ROS平台下研究了智能轮椅的室外场景建模、路径规划与导航感知算法。针对激光雷达采集的三维激光点云密度较大的问题，在LeGO-LOAM算法中融合了体素网格滤波器，在保证建图精度的同时减少了建图时间；然后研究了百度地图和激光雷达融合的轮椅导航移动方式，同时在导航移动时调用路面感知SDK获取实时的路面环境信息，实现了实时避障，该导航模式能够保障轮椅导航过程中的安全性能，为智能轮椅的研发提供一定的借鉴。另外，下一步还需要提高轮椅的识别精确度和鲁棒性，将轮椅路径规划信息、百度地图和外部人为辅助更好地融合起来，并部署主动安全模块，通过多种激光雷达、测距模块等多种传感器融合实现轮椅的主动安全功能，进一步提高轮椅的安全性能。