一种多线激光雷达室外小范围导航算法设计

蔡志宏，赵 慧，周 亮，杜幸运

（武汉科技大学机械自动化学院，湖北 武汉 430081 ）

1 引言

移动机器人研究的问题包含众多领域，其中同步定位与建图（SLAM）[1]被提出作为移动机器人的灵魂所在。SLAM 在过去近30年中得到广泛的关注与研究，由于传感器种类和安装方式的不同，故SLAM的实现方式存在一定的差异。SLAM主要分为激光SLAM 和视觉SLAM（VSLAM）。前者起步更早，在理论、技术和产品落地上都相对成熟，可靠性高，建图相对直观，精度高，不存在累计误差。但受到传感器自身限制，测距范围、维度不同的激光雷达价格差异甚高，特别是多线激光雷达，不管是Sick、北洋，还是Velodyne，价格从几万到几十万不等，成本控制多年来一直是激光SLAM难以攻破的难关；而视觉SLAM相比结构简单，安装方式多元化，成本低，无传感器探测距离限制[2]。但因其易受光照影响，在室外多变的环境下，几乎无法达到正常的效果，运行负荷大，依赖具有较强运算能力的中央处理器才能达到实时效果，构建出的点云地图也无法直接用于导航[3]，并且点云地图通常规模很大，对应的pcd文件（点云地图格式）也会很大，其中存在许多不必要的细节，由于空间被占用，为了在有限的内存中使用而不得不降低分辨率，从而导致地图质量下降。

在20 世纪的80 年代，文献[4]提出了栅格地图的概念，并针对该模型进行了深入的研究，确定了栅格地图的理论体系结构。其将地图分为独立存在的栅格单元，每个单元表示该区域被障碍物占据的概率值，依次用1、0、0.5来表示被占用、空闲和未知这三种状态，即使对于复杂地形也可通过改变单元状态来表示，使已探索、未探索和障碍物这三种类型的区域清楚显示出，可用于二维导航技术。并且栅格地图便于维护，能够以数组的形式存储在计算机中[5]，即使搭载运算能力一般的中央处理器依然可以达到实时的效果。

现阶段室外导航多借助高精度GPS配合惯性测量单元（In‐tertial Measurement Unit）大范围定位或是采用光线跟踪（Ray−tracing）[6]的方法对整条激光测量形成进行处理，从而达到定位效果，多用于高成本研发性室外移动平台。

针对室外小范围运行的移动平台，成本控制相对较低，无法采用成本过高的设备以及运算能力较强的工控机，虽有多线激光雷达可将室外信息描述完整，但多线雷达所得到的是三维点云数据，无法直接用于导航。故这里提出一种算法，将得到的点云地图进行转换成八叉树地图[7]后投影成信息更全面的二维栅格地图，并提取该地图信息转换成导航包所需要的消息发出，将成熟的室内定位导航技术运用于室外。为保证室外运行的实时性以及成本控制，该算法整体设计并不复杂，搭载运算能力一般的工控机依然可以达到室外的实时效果。

2 算法设计

2.1 三维空间的压缩分割

在室外，由多线激光雷达构建的点云地图内存占用通常很大，需要过多的内存空间，其中存在大量多余的信息，更是难以用于导航，无法直观提取出地图信息。而八叉树可将点云信息进行压缩，形成单独存在的离散“块”，并可调整“块”的大小，即分辨率，节省了大量空间，如图1所示。

图1 八叉树示意图Fig.1 Schematic Diagram of the Octree

八叉树地图是由其最大递归深度不断进行分割实现的。根据环境的尺寸构建出同等大小的立方体，将单位元素放入其中不断进行八等份的分割，如图2所示。分割后这些元素被八个子立方体所包含。根据空间分割理论，当子节点中的元素数量与父节点中一致时，继续分割下去数量还是一样，会造成无穷分割的结果，故停止分割，此时到达最大递归深度。

图2 八叉树原理图Fig.2 Octree Schematic

2.2 Move_Base导航包框架结构

Move_base是ROS提供的导航功能包，也是为机器人提供路径规划的中枢，它通过接收各个传感器的数据来进行机器人的全局路径规划和局部路径规划，有了局部路径便可不断转换成机器人电机的速度信息，驱动着机器人按照全局路径不断向目标点移动，完成导航。

Move_Base框架，如图3所示，可以看出中间的Move_Base模块需要接收4个消息才能正常运行，分别是左上角的AMCL模块（ROS 的导航定位模块，也叫自适应蒙特卡洛定位），左下角的Odom模块（机器人里程计信息），右上角的Map模块（全局地图信息）以及最重要的Scan模块（激光信息）。

图3 Move_Base框架图Fig.3 Move_Base Frame Diagram

2.3 八叉树地图的生成与更新

ROS中有一个基于Octomap的功能包Octomap_Server，可将接受来的点云信息（PointCloud）转化八叉树形式，生成相应的八叉树地图。

在八叉树中，一个子节点是否被占据是通过概率表达，而不能用0和1来表示。由于观测周围环境时噪声始终存在，且观测点存在不断移动的情况，故每个子节点的状态也会随时间的变化而变化。根据其推导原理，假设每个时刻分别为t=1，2，3，4…，T，则记录下每个时刻所对应的信息为i1，i2，i3，i4，…，iT。故第n个子节点所更新的信息为：

对上式做一个logit变换，把一个概率p转换到全实数空间R上：

其逆变换为：

α称之为log−odds，用L（）表示每个叶子节点的log−odds，则可以得到下面的式子：

由（4）可知，每当八叉树信息更新时，只需将新的信息放入，并且每个值均存在上下限，避免了计算溢出，计算八叉树地图时将实数转成概率即可。由八叉树的构造原理得知当需要父节点的概率时，可通过子节点计算得到，方法是取均值或所有子节点的上下限。

2.4 生成真实环境下scan模块的生成

由于Move_Base中需要接收Scan的消息，而多线激光雷达受到安装位置的影响，通常安装在较高的位置，其发出的Scan节点往往是与激光雷达同水平面方向上的激光射线，无法反映出室外复杂的多维地图环境，若Move_Base直接使用激光雷达发出的Scan消息，会造成理论导航结果与实际导航结果相差甚远，如移动机器人运行至无法通行的道路上，如低矮的台阶、高度低于雷达安装位置的若干障碍物。如图4所示，激光雷达当前安装位置所发出的Scan信息无法反映出当前环境的所有信息，如图5所示。

图4 激光雷达当前Scan信息Fig.4 Lidar Current Scan Information

图5 当前环境实际状态Fig.5 Actual State of the Current Environment

利用ROS提供的Octomap_Server完成八叉树地图的实时显示，将其投影成二维栅格地图，此时的栅格地图在经八叉树地图投影后能够描述出复杂的环境。已知二维栅格地图是单线激光Scan 扫描可生成的，故为了给Move_Base 提供可使用的Scan 消息，需对上一步得到的当前状态下动态栅格地图提取其信息，转换成Scan的消息结构并发出。

在ROS下动态观察当前栅格地图的信息，地图Map的消息格式，如表1所示。其中动态信息Data是由一组数组构成的，即上文所说的−1、1、0.5这三种数据组成，分别表示地图上各个像素点的状态（占用、非占用、未知）。

表1 消息结构Tab.1 Message Structure

Scan的消息类型，如表1所示。其中动态消息类型是由像素点到激光原点的距离（Ranges）和该像素点的强度值（Intensities）构成，只需将地图数据中被占用的像素点（即墙面信息或不可移动区域）的坐标提取出，地图中像素点是以数组形式呈现，在假象坐标系中，若实际地图中某点坐标为（x，y），则对应栅格地图中数组顺序为：

其中map.info.width代表当前栅格地图的宽度，接着计算被占用的像素点到原点之间的距离，并赋予该像素点的强度为100，即表示激光扫到的点为不可移动区域，最后以Scan消息格式发送出便可得到Move_Base所需要的Scan消息。

3 实验

3.1 实验平台及环境

这里的激光雷达采用的是Velodyne VLP−16激光雷达，系统环境为Ubuntu18.04，ROS 版本为Melodic，移动平台是基于一台后轮驱动清扫车所改装，如图6所示。在室外强光的环境下进行实验。

图6 Velodyne雷达与移动平台Fig.6 Velodyne and Mobile Platform

3.2 实验步骤

（1）Velodyne激光雷达生成的信息为三维点云信息，如图7（a）所示。在Octomap_Server 的处理下，将当前环境下的三维点云数据转化为八叉树形式，如图7（b）所示。原来的点云以方块的形式压缩表示，以不同的方式保留了地图的有效信息。

图7 点云地图转化为八叉树地图Fig.7 Point Cloud Map into Octree Map

（2）将得到的八叉树地图进行投影处理便可成为具有更多信息量的二维栅格地图，不同水平面上收到的障碍物信息集中在同一张栅格地图中，如图8（a）所示。

图8 栅格地图信息生成激光信息（scan）Fig.8 Grid Map Information Generates Laser Information（Scan）

（3）提取（2）中生成的栅格地图信息，在ROS 系统下实现该算法，将其转换成Move_Base导航包所需要的Scan模块发出，在ROS提供的RVIZ可视化界面如图8中（b）可以看出，紫色的激光点即生成的Scan完全匹配上先前的栅格地图，该激光点反映出的信息为多线雷达扫描多个平面后的完整地图信息，意味着发送至Move_Base的Scan模块可以作为室外环境的真实映射。

（4）至此Move_Base 所需的五个消息分别为/t（f位姿变换）、/odom（里程计）、/cmd_ve（l电机编码器）、/map（全局地图）、/scan（实时激光点）全部收到，为进行进一步室外路径规划和导航做好准备。

（5）在室外，使用Cartographer建图算法，通过接受上一步发出的Scan消息来对室外环境进行建图，并运行Move_Base导航包，便可在对移动平台进行导航及路径规划，达到室外避障的效果。

3.3 实验分析

在上一步中得到的激光点通过Cartographer 建图算法建立出室外的二维栅格地图，如图9（a）所示。并在移动过程中Cartog‐rapher自带的定位算法会将该激光点与建立好的栅格地图进行匹配以及与里程计信息融合的联合定位，如图9（b）所示。其中两色相间的坐标轴即为移动平台当前的精确位置以及位姿，其中一色坐标轴代表移动平台的正方向。

在完成移动平台定位后即可开始进行路径规划与导航，以达到使移动平台按照预先设置好的轨迹行驶。如图9（b）所示，收集出地图中需要经过的目标点以及移动平台行驶到该点时的位姿，生成出移动平台的行驶路径，其中一点代表下一个局部目标点，每当移动平台移动到目标点后便寻找下一个目标点，另一点代表上一步生成反映真实环境的激光点，可供Move_Base功能包使用来规避轨迹上临时出现的障碍。

图9 激光与地图匹配Fig.9 Laser and Map Match

移动平台能够不断按照寻找下一个目标点的方式走过图中轨迹，完成了在室外导航的目的，实现了在仅使用激光雷达作为传感器的情况下小范围定位导航功能。如图10所示，从启动开始，在移动的过程中，绿色激光点始终能够与地图精确配准，从未发生畸变，对于移动平台的定位十分准确，具有足够的实时效果，保证了在室外行驶的安全性。

图10 全局路径导航地图Fig.10 Global Path Navigation Map

4 结论

针对室外小范围定位导航的问题提出一种支持算法，通过多线激光雷达将点云地图转换为的八叉树地图投影成栅格地图，由地图生成相应的激光点，从而能够完整地描述出当前的室外环境，将成熟的室内导航技术运用在了室外，解决了多线激光雷达无法建立栅格地图进行路径规划与导航的问题，并整体降低了系统的成本，在对实时性要求极高的室外环境下依然可保证实时的准确定位，成功实现室外小范围定位导航。