一种手持式半球形视角激光雷达SLAM三维建图技术_杨红刚.pdf

年 第 期杨红刚：一种手持式半球形视角激光雷达 三维建图技术引文格式：杨红刚 一种手持式半球形视角激光雷达 三维建图技术 测绘通报，（）：一种手持式半球形视角激光雷达 三维建图技术杨红刚（中铁第四勘察设计院集团有限公司，湖北 武汉）摘要：针对室内外场景快速三维建图问题，本文提出了一种半球形视角激光雷达扫描同时定位与建图（）方法，设计了集成半球形视角激光雷达和惯性测量单元的手持便携式三维激光扫描系统。首先，根据半球形视角激光雷达扫描线结构特征，对分割的地面点云降采样，减少垂直约束冗余。然后，对惯性测量单元（）和降采样激光点云通过一个紧耦合迭代扩展卡尔曼滤波器（）进行联合位姿估计。最后，使用因子图增量式后端优化处理消除累积误差，提升点云地图精度。采用本文方法研发的原型系统对典型室内外场景进行建图试验，结果表明：本文方法绝对定位精度优于，相对定位精度优于.，在室内外一体化三维建图方面具有广阔应用前景。关键词：同时定位与建图；半球形视角激光雷达；因子图优化；手持式激光扫描系统；三维点云中图分类号：文献标识码：文章编号：（）（，）：，（），（），（），.：；近年来，小型三维激光雷达（，）传感器研发和生产技术的不断成熟为移动测量、导航及制图等应用提供了新的选择。常见的小型 可分为机械式、固态式和混合固态式三类，被广泛应用于自动驾驶领域。轻量化的背包式、手持式激光扫描系统通过对小型 和惯性测量单元（，）等传感器的高度集成，有效弥补了机载、车载等大型激光扫描系统作业难度高、在中小型场景中适用性低等不足。手持式激光扫描系统体积小、重量轻、便携化程度高，只需一人手持即可完成数据采集工作，在复杂场景中具有更大的灵活度，已被广泛应用于三维测图、建筑建模及考古探测等领域。手持式激光扫描系统在室内场景中无法接收到准确有效的全球卫星导航系统（，）信号，受用户行走时快速且不规则抖动干扰，在短时间内产生剧烈的位姿变化。稳定且稳健的同时定位与建图（收稿日期：基金项目：国家重点研发计划（）作者简介：杨红刚（），男，高级工程师，主要从事铁路勘测、工程测量技术研究及实践。：测 绘 通 报 年 第 期，）算法在扫描系统自身定位与建图的 过 程 中 发 挥 着 重 要 作 用。（）被认为是最经典的激光 方法，它根据局部平滑度从扫描线中提取边特征和面特征，通过联合优化点到边的距离和点到面的距离，估计自身运动状态。文献提出了（），通过加入地面点提取、点云分割及回环检测，有效提高了 精度。文献在 框架中融入 预积分模块，并借助因子图实现后端优化。文献借鉴文献中视觉与惯性观测数据的联合状态估计思想，用 中的特征提取替换视觉图像处理。文献采用迭代扩展卡尔曼滤波器减小线性化误差，有效降低了系统计算量。文献提出了（），使用增量 树维护地图，被视为目前最有效的三维激光 方法之一。当前 算法主要针对 、和 等多线激光扫描设备，常见的手持激光扫描系统设计方案包括多线 水平安装、小视场角的 朝前安装。这两种扫描系统及其对应的 算法都忽略了在竖直方向上获取充足激光扫描点的重要性。若激光雷达移动速度太快，扫描帧之间重叠点数量较少，导致帧间匹配失效。针对这一问题，本文提出一种半球形视角激光雷达 方法，通过一个 半球形视场角激光雷达扩充竖直方向视角，扩大室内场景覆盖视野，根据半球形视角激光雷达的扫描线结构特征设计 方法，以期实现轻量级、可便携手持式的室内外场景三维建图。手持式半球形视角激光扫描 系统手持式半球形视角激光扫描 系统集成了一个九轴、一个半球形视角激光雷达、同步控制板、工业控制计算机及电源模块，如图 所示。图 手持式半球形视角激光扫描 系统半球形视角激光雷达采用 设备，其具有 视场角的半球形激光雷达。在 的常规扫描频率下，可在 的扫描半径内获取点云。每帧点云包含 条呈同心圆分布的扫描线，每条扫描线包含最多 个扫描点。被安装在手持激光扫描系统正前方，可对用户前方的场景进行最大范围扫描。和 以严格的工业标准进行集成固定，它们之间的工业参数可为系统外参的估计提供初值。工控机内置一块四核八线程的英特尔 处理器，为 算法提供算力支持。采用一块大疆的 智能电池供电，在常规情况下续航超过 ，可以满足较大规模场景的数据采集和处理工作。半球形视角激光雷达 算法 算法流程如图 所示。首先对半球形 扫描点云进行地面点分割和降采样；然后结合 信息，通过一个紧耦合迭代扩展卡尔曼滤波器进行状态估计；最后将自动检测到的回环约束加入因子图进行后端优化，构建出全局一致的三维点云地图。图 手持式半球形激光扫描系统 方法技术流程 年 第 期杨红刚：一种手持式半球形视角激光雷达 三维建图技术.地面分割与降采样高密度激光扫描点云会消耗较大计算资源，影响 定位与建图的实时性。在原始扫描点云中提取特征点是一种比较常见的解决方案，特征点的数目和质量直接影响 的精度。在多数场景（尤其是室外场景）中，半球形 扫描出的单帧点云中落在地面上的点占比较大。图 给出了典型场景下半球形 扫描出的单帧点云分布。如果不加分类地对单帧点云进行特征提取，则会导致非地面特征点进一步减少。将地面点分割出来并对其进行降采样处理，保证单帧扫描点云空间分布更加均匀。现有的对单帧 点云进行地面分割的方法都是针对水平放置多线 设计的，不适用于本文半球形 扫描模式。因此，本文提出了一种专用于半球形 的地面分割方法。图 半球形 地面分割示意手持式半球形激光扫描系统正常作业时，设备正下方的扫描点会落在地面上。沿设备行进方向选取距离扫描中心最近的 条扫描线；在所选扫描线上，选取与设备正下方点的夹角小于 的点作为地面种子点（图 中绿色点）。使用主成分分析法对种子点集合的协方差矩阵进行特征值分解 （）式中，为 种 子 点 集 合 的 协 方 差 矩 阵；为由特征向量组成的矩阵；为由特征值降序排列（）组成的对角矩阵。由此可得到种子点集合所在平面的法向量。记种子点集合的平均中心为，则单帧扫描中的任意一点 到种子点平面的距离可表示为 （）（）根据式（）计算单帧扫描中每一个扫描点到种子点平面的距离度量。若 小于阈值，则认为该点属于地面点（图 中红色点）；否则为非地面点（图 中蓝色点）。考虑到 扫描点的位置精度会随测距增大而降低，故而将 定义为一个随测距增大而线性增大的量，表达为 （）候选扫描线数、种子点角度阈值 及距离因子 为地面点分割算法的参数，可根据实际扫描场景进行调整。使用体素化网格的方法对点云进行降采样，用体素中所有点的重心近似表示体素中其他点，在保证非地面点数量不变的前提下，减少冗余的地面点，提升算法运行效率。.位姿估计位姿和姿态估计模块是一个紧耦合的迭代扩展卡尔曼滤波器。将第一帧 自身的坐标系（）作为全局坐标系（），手持式半球形激光扫描系统的状态可表示为 （）式中，表示 在全局坐标系下的位姿；为 在全局坐标系下的速度向量；和 分别为 的角速度和加速度偏置；为全局坐标系下的重力矢量；为 与 之间的外参。首先，根据运动学方程和误差传播定律，计算第 个激光扫描帧对应的先验状态?和协方差?。然后，在历史扫描帧中搜索与当前扫描点的先验位置距离最近的 个点构成局部平面。点到面的距离计算公式为?()（）式中，为局部平面的法向量；为 个邻近点的平均中心；?为当前扫描点在全局坐标系下的先验位置。融合先验状态与点到面的距离，并在当前更新的状态?处进行线性化，构建残差计算模型为 （，），()（?）()（?）（?）（）式中，为在流形上的操作，具体定义参考文献；为（，）关于的雅可比矩阵；（，）为原始的观测噪声。最后，将状态的先验和观测结合在一起，得到式（）所示的最大后验估计问题。?（?）()（）式中，前半部分表示状态的先验，后半部分表示观测 绘 通 报 年 第 期测残差。得到的状态量包括激光扫描系统在 频率下的位姿、速度和 偏置，以及对 和 之间的外参和全局坐标系下的重力矢量估计。利用这些估计，可还原出每一帧激光采样时刻的扫描系统位姿和 外参，并对激光扫描点进行运动补偿，得到消除畸变的单帧点云。.因子图优化与三维建图长时间的状态估计会产生误差的累积，在系统中添加回环检测模块，可有效修正大规模场景中位姿的漂移。本文在极坐标系下进行当前帧与历史帧之间的相似性计算，判断扫描点云是否形成回环。如图 所示，手持式半球形激光扫描系统的状态被视为待优化的量，以状态节点形式增量式地插入到因子图中。状态估计模块估计出的位姿信息提供 扫描相邻帧之间的位姿约束，帧与历史局部子图匹配得到的位姿变换关系提供回环约束，帧间位姿约束和回环约束以因子节点的形式插入到状态节点之间。帧间位姿约束保证局部地图精度，回环约束维持大规模建图。图 因子图优化示意使用增量式平滑和建图方法对因子图进行优化，即每接收到新的 观测数据触发一次优化计算，滑动时间窗口不断向前滑动，因子图维持固定的状态变量因子个数，从而保证优化计算的效率。将因子图转化为非线性目标函数，为，()（）式中，和分别对应相邻帧之间的位姿约束和回环约束构成的因子节点。采用列文伯格马夸尔特法求解因子图中各状态节点的最优估计，得到经过回环优化的全局一致的位姿。利用状态估计出的 外参和经过因子图优化后的系统位姿，对消除畸变的单帧点云进行刚体变换，即可还原出扫描点的真实位姿。最后在全局坐标系下逐帧累积点云，得到整体场景的三维点云地图。试验与分析.试验设备及结果为了验证手持式半球形激光扫描系统 方法的精度与性能。在室内 拒止环境中，用户单手持半球形激光扫描系统进行数据采集；在室外 信号良好环境中，本文系统被固定在配备高精度 组合导航系统的移动平台上（如图 所示），同时采集本文系统和组合导航系统数据。组试验数据的详细信息见表。图 试验数据采集装备表 试验数据数据名称总帧数扫描时长 轨迹长度 信号室内.无室内.无室外.无室外.有室外.有试验中，将 组合导航解算出的轨迹作为真值，对本文 轨迹进行精度评估。为了验证系统的实时性能，对比分析了本文算法与 的运行时间差异。采用本文方法对 个典型场景的三维建图结果如图 所示。.试验结果分析.轨迹精度使用绝对轨迹误差（，）和相对位姿误差（，）的均方根误差（，）对 种激光 算法估计的轨迹进行精度评价，。为了展示单一模块对整体改进影响，本文设计了消融试验，统计对 只进行地面降采样和只进行因子图优化的轨迹精度。表 结果显示，本文 年 第 期杨红刚：一种手持式半球形视角激光雷达 三维建图技术手持式半球形激光扫描系统，单一的地面降采样模块或因子图优化模块有助于降低 的轨迹误差，且在本文算法中将两者相结合，轨迹精度最佳。图 本文方法建图结果表 轨迹精度评价数据指标地面降采样因子图优化本文算法室外.比例误差.比例误差.室外.比例误差.比例误差.运行时间表 统计了使用 算法和本文算法在同一计算平台下处理不同数据时单帧平均耗时情况。由于本文的算法沿用了 的框架，同测 绘 通 报 年 第 期样使用增量 树维护点云地图，额外的耗时主要发生在地面分割模块和因子图优化模块中。本文算法对地面点进行分割并降采样，降低了后续参与计算的点数量，整体运行时间并没有大幅增加，效果在有大量地面点的室外场景中尤为明显。试验结果表明，本文算法在单帧耗时与 相差不大，可在工控机上以大于 的频率稳定工作。表 单帧运行时间算法室内 室内 室外 室外 室外.本文.结 语本文提出了一种手持式半球形视角激光扫描系统 方法。半球形视角 具有超广视场角优点，其同心圆形状的扫描线蕴含丰富的结构特征，一方面可以有效对用户前方的场景进行最大覆盖扫描；另一方面可以扩充竖直方向视角，实现对室内场景的更大视野覆盖。根据半球形视角激光雷达的扫描线结构特征，提出了一种改进 的 算法，实现轻量级、可便携手持式的室内外场景一体化三维建图。后续需要进一步研究半球形视角激光雷达在位于室内转角处狭小空间中的 稳健性。参考文献：陈敬业，时尧成 固态激光雷达研究进展 光电工程，（）：，（）：，（）：徐寿志，程鹏飞，张玉，等 地面三维激光扫描仪的检校与测量精度评定 测绘通报，（）：宁振伟，谢刚生，钟晓兰 手持激光雷达扫描系统在农村房地一