动态环境下激光雷达地图构建_杨颖.pdf

年月第 卷第期计算机工程与设计 动态环境下激光雷达地图构建杨颖，王立勇，孙鹏，宋越，张政（北京信息科技大学 现代测控技术教育部重点实验室，北京 ）摘要：针对动态噪声大室外应用场景中存在里程计与建图精度较低、鲁棒性不足的问题，提出基于激光里程计与建图方法。利用惯性测量单元对每一帧点云数据进行运动补偿，进行地面分割。采用欧几里得聚类方法在非地面区域检测动态物体并去除动态物体的点云数据，在可靠区域内获取关键特征点后进行点云配准和分步骤求解位姿。构建全局静态地图，通过回环检测减少累积误差。利用 数据集和校园环境数据对方法进行验证，实验结果表明，提出方法在满足实时性的要求下激光里程计与建图精度优于目前主流算法、，具有良好的鲁棒性和环境适应性。关键词：激光雷达；地面分割；动态物体；特征提取；激光里程计；回环检测；建图中图法分类号：文献标识号：文章编号：（）：收稿日期：；修订日期：基金项目：北京信息科技大学“勤信人才”培育计划基金项目（、）；促进内涵发展科研水平提高基金项目（、）作者简介：杨颖（），女，内蒙古赤峰人，硕士研究生，研究方向为基于激光雷达的物体检测与 ；通讯作者：王立勇（），男，河南长葛人，博士，教授，博士生导师，研究方向为机电系统测控与故障诊断、车辆传动电控系统设计、控制与无人驾驶；孙鹏（），男，山东济宁人，博士，副研究员，硕士生导师，研究方向为机器视觉和自主驾驶；宋越（），男，山东威海人，博士后，研究方向为农业信息化技术、图像处理；张政（），男，河南安阳人，硕士研究生，研究方向为基于激光雷达 。：，（，）：，：；引言 技术主要分为视觉 和激光 ，由于相机受光线的影响较大，几乎不能在夜间工作，而且检测范围有盲点区域，使得视觉 应用场景受限。近年来，激光 技术发展迅速，研究者们提出大量的激光 方法。等通过提取点云数据中的边缘和平面特征点对两帧点云数据配准实现了实时激光里程计方法，此方法为激光 系统提供了雏形，为后续激光 的发展奠定了深厚的基础。等对 算法进行计算机工程与设计 年了改进，通过提取地面上的平面特征和非地面的边缘特征，采用两步（）优化位姿，是第一个完备 的 三 维 激 光 算 法。等提 出 一 种 基 于 融合激光、和视觉的里程计方法，使用最优多模态传感器融合，检测与跟踪稀疏点云的平面特征和边缘特征。等针对室外复合场景（地面水面）下的移动机器人导航问题，提出了一种由前端里程计和后端闭环优化组成的实时三维激光雷达 系统，将动态物体视为小目标，利用高度差的平面栅格法将动态物体去除。等利用三维点云结合语义信息来提高建图效率，同时滤除场景中的动态物体。在实际环境中动态物体的点云数据在匹配过程中是不稳定的噪声点，会影响位姿估计，从而导致激光里程计精度低。针对这个问题，本文通过检测动态物体，去除大量不稳定的特征点，使得稳定的特征点进行数据关联。同时上述大多数方法在室外环境缺乏有效的回环检测，机器人在室外大尺度环境下的长时间建图任务中产生不可避免的累计误差。累积误差会破坏全局一致性，对整个系统精度造成严重影响。针对此问题，本文利用全局描述子的回环检测方法解决室外环境建图中存在累计误差导致建图不精准问题。系统框架本文提出的算法的流程如图所示，由个模块组成：数据预处理模块、动态物体检测模块、激光里程计模块和激光建图模块。图算法总体框架 点云数据预处理运动过程中机器人不可避免的产生非线性运动，由于机械式激光雷达一帧数据不是在同时刻采集的，从而导致扫描的点云数据存在运动畸变。因此通过使用高频 对点云数据线性插值进行运动补偿校正，去除运动过程中产生的畸变。首先对每帧数据中的激光点时间戳标号，如图所示，公式为（）其中，为当前时刻下扫描激光点的时间戳，为一帧数据中的第一个激光点的时间戳，为激光雷达的扫描周期，为当前激光点对应的旋转角度，表示起始点和终止点之间的旋转角度差。然后通过获取离时刻最近的 在和 时刻下在世界坐标系中的姿态、测量值，利用线性插值确定时 在世界坐标系下的姿态，公式为 （）图激光点时间戳其中，和为时间插值因子。最后利用和能够得到起始激光点和当前激光点所对 的位置、和速度、得到一帧数据中每个激光点的运动畸变量，公式为（）（）式中：表示激光雷达当前帧起始点坐标系与世界坐标系之间的变换矩阵。对每个激光点进行位姿变换，转换到每第 卷第期杨颖，王立勇，孙鹏，等：动态环境下激光雷达地图构建帧的起始时刻坐标系下。动态物体检测在道路场景下存在大量 车 辆、行 人 等 动 态 物 体，去除动态物体可以避免提取不稳定的特征点，不影响正确数据关联。同时，减少了潜在配准的数量，提高了实 时性。本文使用几何约束的方式 去除动态 障 碍 物，在 地 面分割后，在非地面区域通过欧几里得聚类方法检测动态物体，剔除动态物体的点云，在可 靠 的 静 态物 体 区 域 选择特征点。地面分割与动态物体去除地面的点云数据对动态物体检测产生很大的干扰，因此需要进行地面分割，在非地面点云数据中检测动态物体。同时，长时间运动后地图在高度方向上会存在累计误差，分割出的地面点数据可作为对位姿的约束以减小高度方向上的误差。为了保证实时性和准确性采用随机采样一致性（）算法，分割效果如图所示。图地面分割结果地面分割完成后，在非地面区域去除动态物体。本文采用了欧几里得聚类方法将点云聚类，该方法在点云中心范围未知的情况下，把毫无组织的数据模型划分成不同较小的模型，根据所设定的距离阀值把三维数据进行聚类。同时为了加快聚类速度配合使用树搜索算法，。通过设置几何阈值将点云数据中的动态物体提取出来。其流程如图所示，这种组合方式虽然简单，但是算法参数设置简洁、运行高效。图动态物体分割算法流程在一帧点云数据中的某点，使用 树近邻搜索算法找到离最近的个点。如果距离满足设定的车辆、行人的几何阈值约束条件，则将点存入到集合中。当中的点云数目不在增加时，结束整个聚类过程。如果没有，需要重新确定点重复以上步骤，直到确保中的点云元素不再增加。由于动态物体，如车辆和行人，都具有较为一致的空间尺度，本文利用这个特性识别动态物体。对于任意的聚类后的点云簇，找到个维度上坐标的最大值、最小值，并根据长、宽和高阈值，判断聚类物体是否为动态物体，公式为 （）其中，、和 为个轴上坐标的最大值和最小值，、和分别为动态物体识别的尺寸阈值。选取关键特征点激光点云数据量庞大，为了提高计算效率，使用关键特征点进行点云配准。根据去除动态物体后点云的几何特征，在分割的地面点云中提取平面特征点，在非地面点云中提取边缘特征点和平面特征点。首先根据点云的坐标值计算所属的扫描层，计算公式为 （）（）（）其中，（，）为点云的坐标，为点云所属的扫描层，为激光雷达垂直角度上界，为激光雷达垂直角度下界，为激光雷达线束。为了使特征点分布相对均匀，避免特征点聚集，将每层点云按顺序划分为个区域，通过计算每个区域的曲率判断局部点云的光滑程度，公式为（，），（，（，）（）其中，是曲率，是点所在激光扫描束上的所有点的集合，（，），（，）是激光雷达坐标系下第帧点云数据中的第，个三维点。值最大且满足边缘点阈值的点为边缘特征点，值最小且满足阈值的点为平面特征点。帧间配准与位姿解算在获取特征点后，对前后两帧数据进行配准。在相邻的关键帧中，寻找到对应的特征点，计算特征点到对应边缘线 和 平 面 的 距 离。其 中 边 缘 点 到 边 缘 线 的 距 离 公 式如下（，）（，）（，）（，）（，）（，）（）其中，和为相邻的两帧，为帧扫描边缘点到帧（，）边缘线的距离，（，）表示点是第帧在激光雷达坐标系下的坐标，（，），（，）是第帧数据中直线特征上两点在激光雷达坐标系统下的坐标。同理，平计算机工程与设计 年面特征点到平面的距离公式为（，）（，）（，）（，）（，）（，）（，）（，）（，）（，）（）其中，为帧扫描边缘点到帧（，）特征平面的距离（，），（，），和（，）是第帧中个平面点在激光雷达坐标系统下的坐标。不同类型的特征点具有不同的几何特性，从而具有不同的约束位姿变量的能力。平面特征点对位姿中，有较好的约束能力，非地面点集的边缘特征点对位姿中，有较好的约束能力。为了充分利用不同类别特征点的类别信息，采用两步（）方法优化位姿。第一步，应用平面特征点用于获取位姿中的，第二步，使用非地面区域提取的边缘特征来获得位姿中的，。回环检测与建图激光建图模块是利用当前帧与局部地图配准建立全局地图，同时还能够获取低频的，优化的激光里程计。本文仍然采用分步骤的方法建立全局点云地图，最后将激光里程计模块输出的位姿与建图优化位姿融合在一起，输出一个高频高精度的位姿估计。但激光里程计会有误差积累，尤其是应用在有环路场景下会导致建图过程中出现重影现象，在大尺度建图任务中会产生漂移，本文使用非直方图的全局描述子回环检测方法。首先对点云进行分割，按顺序将把点云空间等分成个圆环，然后把每个圆环切割成等份。将分割后的点云对应到矩阵生成全局描述子，矩阵为，可表示为，（）其中，表示第个圆环第个扇形。每个元素存储每个分割单元 中所有三维点的高度最大值。两帧数据的所有对应的列向量之间越相似，说明两帧点云越相似。当描述子之间的距离函数小于设定阈值时，则认为该历史帧为回环检测的回环帧，公式为（，）（）（）其中，分别为当前帧点云数据和历史帧点云数据生成全局描述子，分别为，的第列。为了避免由于机器人位姿变化导致的全局描述子中列向量的顺序会发生改变，进而导致相似性判别失败，利用历史帧按列平移的方法解决这个问题。这样会得到个全局描述子，依次计算其与当前帧的全局描述子之间的距离，选择距离最小的作为闭环检测帧。在距离最小时，列平移量为 （，）（）根据，可通过式（）得到两帧数据之间的旋转角度，旋转的角度在配准时可作为初始位姿使用（）其中，为旋转角度，为列平移量，为全局描述子的列向量个数。实验结果与分析 数据集，由德国卡尔斯鲁厄理工学院和美国技术研究院在城市、乡村和高速公路环境中采集数据。为了充分测试本文方法在不同环境和条件下的性能，在 数据集上进行了广泛的测试。同时，在校园的室外环境使用地面移动机器人采集数据对本文方法测试，实验平台如图所示。使用松灵 轮式移动机器人，搭配速腾聚创 线激光雷达、组合惯性导航，如图（）所示。为了更好验证算法的有效性，选择的实验场地为一个环路，如图（）所示。移动机器人的平均移动速度约为，轨道长度约为 。所有算法均在 （）环境下，在硬件设备为 的 平台上实现。图实验平台 动态物体检测结果利用 数据集进行动态物体检测算法测试，选取了具有车辆、行人和自行车的道路场景，使用矩形框标记检测到的动态物体，检测结果如图所示。结果显示算法第 卷第期杨颖，王立勇，孙鹏，等：动态环境下激光雷达地图构建能够有效的检测出车辆、行人和自行车等动态物体。即使在动态物体较为密集的道路场景下，本文算法依然能够进行准确检测，如图（）所示。回环检测算法分析为了验证回环检测算法的性能，使用 数据集序列 进行 测 试，该 数 据 具 有 多 段 重 复 和 环 路 运 动 轨迹。表是有无回环检 测 算 法 误 差 对 比，其 中 轴、轴和 轴 的 误 差 是 与 真 值 之 间 的 均 方 根 误 差 （）。从表中可以看出本文添加回环检测算法在 轴、轴和 轴的 均小于没有回环检测的误差。因此本文算法通过有效 的回环检 测，校 正了三轴的误差。图动态物体检测表回环检测算法 对比轴轴轴没有回环 有回环 激光里程计精度评定 数据集测试为了充分测试本文方法在不同环境和条件下的性能使用 数据集进行测试，其中图（）为在城市街区中构建的地图，图（）为在乡村道路上构 建 的 地 图，图（）为在野外公路上构建的地图。结果显示了本文的方法与真值之间的轨迹比较，其中虚线为 获取的真值轨迹，实线是本文方法获得的车辆轨迹，可以看出所有实验轨迹基本上与 真值轨迹一致，测试结果表明本文方法可以在高速和大场景下保持良好的鲁棒性。此外，本文方法与经典的 与 方案进行了比较，结果如图所示。对比结果表明，本文的方法与其它算法比较有更加精确的估计轨迹，结果优于 与 方案。本文使用 对轨迹进行精度评定（单位：），从表看 出，使 用 数 据 序 列，、和本文方法的轨迹误差的结果，本文提出的方法 最小，建图精度最高。校园场景测试为了评估本文 算法，使用 校园 环 境 中采 集 的 实 际 数据，校园环境复杂，其中包括树木、建筑物、大量的行人和车辆。本文的建图结果与 方 案 进 行 对 比，结果如图所示。结果表明，在建筑物边缘、树木与草坪处噪 点 多 的 地 方，本 文 提 出 的 方 法 效 果 要 优 于 方法。在建图细节方面，本文方法的地图细节更加清晰，没有明显的重影现象。这 表 明 本 文 方 法 具 有 很 强的鲁棒性，能 够 克 服 复 杂 环 境 的 影 响。而 方案适用于轻量化终端，它虽然在特征提取阶段进行了筛选处理，但与本文剔除不稳定的噪点方法相比精确度不足。可以看 出 方 案 由 于 移 动 机 器 人 长 时间运行存在累积误差，导致地图中 存在明显 的 重影，而本文方法加入有效的回环检测减少累计误差，明显消除由于累计误差产生的重影。本文方法生成的三维点云图与实际环境高度一致，验证了本文方法在复杂环境中的有效性和实用性。时间性能评估本文使用数据集 对、和本文方法的耗时和处理帧率进行了测试。数据集具有 帧点云数据，统计 、和本文方法处理该序列数据的总耗时 （单位：）与每秒的处理帧数 （），表为实验统计数据。总耗时为 ，总耗时为 ，本文方法总耗时为 ，由于 在 算法框架基础上实现了计算的轻量化，使得算法在计算效率上大幅提升，在种方法中耗时和处理帧率上表现最佳。而本文方法为了提高激光里程计精度增加动态物体检测模块，同时，为了减少误差积累增加全局描述子的回环检测，计算效率优于，相比 增加了消耗时间。但在激光雷达 的工作条件下，本文方法每秒的处理 帧数据能够保证系统的实时性。计算机工程与设计 年图不同 数据序列轨迹重建结果图不同方法结果轨迹比较结束语本文提出一种适用于室外环境无 信号的激光里表 数据序列在不同方法 对比 本文方法 程计与建图的方法，该方法包括 数 据预 处理、动态 物体去除、激光里程计和回环检测 与建 图。通过 去除环 境中动态物体在可靠区域内获取关键特征，提高位姿求解的精度，通过添加全局描述子进行回环检测解决建图中存在累计误差问题，提高建图精度。使用 数据集与校园真 实 场 景 实 验 验 证，本 文 方 法 的 轨 迹 基 本 上 与 真值轨迹一致，且建图的精度优于 、方法。有效的回环检测方法消除由于累计误差产生的重影，生成与实际环境高 度一致的三 维点 云图。本文方法满足实时性的要求，在复杂环境具有良好的鲁棒性和有效性。第 卷第期杨颖，王立勇，孙鹏，等：动态环境下激光雷达地图构建图校园环境建图效果对比表基于 数据不同算法的耗时和处理帧率 本文方法 参考文献：，：，：，：，（）潘杨杰基于视觉多传感器融合的室内移动机器人定位技术研究 杭州：浙江大学，（）：，：，：，：，：，：，（）：，：，：，（）：，（）：，：，（）：（）范晶晶，王力，褚文博基于 树 和 欧 式聚 类 的 越 野 环境下行 人识 别 的研究汽车工程，（）：，：，：，（）：，？，：