基于激光雷达SLAM的三维点云自适应算法.pdf

第 卷 第 期 激 光 与 红 外 ，年 月 ，文章编号：（）激光应用技术基于激光雷达 的三维点云自适应算法姜晓勇，吴起威，魏璇，应凯健，陈奕磊，魏益民，王郑杭，陶慧翔（浙江科技学院 机械与能源工程学院，浙江 杭州 ；浙江大学 机械工程学院，浙江 杭州 ）摘要：（）同步定位与地图构建作为移动机器人智能感知的关键技术。但是，大多已有的 方法是在静止环境下实现的，当环境中存在移动频繁的障碍物时，建图会产生运动畸变，导致机器人无法进行精准的定位导航。同时，激光雷达等三维扫描设备获得的三维点云数据存在着大量的冗余三维数据点，过多的冗余数据不仅浪费大量的存储空间，同时也影响了各种点云处理算法的实时性。针对以上问题，本文提出一种 运动畸变去除方法和一种基于曲率的点云数据分类简化框架。它通过激光插值法优化 运动畸变，将优化后的点云数据分类简化。它能在提高 建图精度，同时也很好的消除三维点云数据中特征不明显区域的冗余数据点，大大提高计算机运行效率。关键词：；定位导航；点云处理；高斯曲率中图分类号：；文献标识码：作者简介：姜晓勇，男，教授级高工，硕士生导师，主要从事高端装备、智能制造、无损探伤、结构健康监测、机器人、机器视觉等领域研究。：收稿日期：；修订日期：，（，；，）：（），：；引言随着自主导航技术的进步、巡检机器人发展前景广阔。是巡检机器人在巡检自主导航过程中不可或缺的关键技术。但是在定位建图过程中存在易移动物体会大大降低 建图精度，影响巡检机器人的自主导航巡检效率，因此提升 定位建图精度对巡检机器人自主导航有着重要意义。此外，由于计算机视觉技术的极速发展，三维点云在巡检行业的应用越来越广泛。激光雷达等扫描仪设备获取三维点云，被广泛地应用在自动驾驶 、地形测绘 、文物保护 、四足机器人定位与导航等领域。现有的三维激光扫面设备可以快速获取大量的点云数据，大量的点云数据在存储、操作、显示上，会占用大量的计算机资源 ，包括计算机内存、等。因此，对点云数据进行精简处理，提高运行效率极其重要。迄今为止，点云数据最具代表性的精简方法通常包括不顾及点云特征方法和顾及点云特征的方法 。等 将点云所在的最小包围盒划分为相等的小长方体，取接近重心的点代表小长方体的全部点云，实现点云精简。等 提出包围盒法多适用于曲率较小的数据，对于曲率起伏较大的数据，若压缩率较大，则精简后的点云不能保留较多的特征点，会影响数据的细节表达。等 提出随机采样算法没有考虑点云数据的形状和特征，在对点云数据 随机采样时，若压缩率 较大，可能会导致点云特征信息丢失。李青蒙等 提出曲率采样算法适合于曲率较大的数据，对于平坦的数据，若压缩率较大，可能会造成点云数据的大量缺失。为了尽量保留点云数据的特征区域和轮廓信息的情况下剔除三维点云数据中存在的大量冗余数据点，提出了一种基于曲率的点云数据分类简化框架，该框架使用了 算法 提取分布均匀的质心点，能保证点云数据的均匀分布，使用高斯曲率将数据点分类，能够很好的消除三维点云数据中特征不明显区域的冗余数据点。与现有的一些点云采样方法相比，该框架能在保持相同精简率的情况下更好的保留点云数据特征明显的区域。激光 原理分析 是指机器人在未知环境下实现对自身定位的同时构建它周围世界的环境地图。问题的示意图如图 所示，图中坐标系分别为：位置固定的世界坐标系和随机器人移动不断变化的传感器坐标系，的实质是在机器人移动的同时不断求解当前传感器位置 相对于世界坐标系 的位姿矩阵，将传感器坐标系下采集的环境数据添加到世界坐标系下，增量式地构建环境地图。激光雷达扫描出的点云信息可以较好地表现出环境中的几何信息，能够体现环境三维空间结构信息，激光雷达传感器的简化模型 如图 所示。图 问题示意图 图 激光雷达传感器简化模型 点 为激光雷达扫描得到的一空间激光点，根据飞行时间差计算 点与激光雷达中心的距离为；为激光点 对应的水平扫描角度；为激光点对应的竖直扫描角度，每束激光对应的竖直角度 是固定的，由激光雷达结构决定。在激光雷达模型下，计算在激光雷达坐标系下点 的坐标，其数学表达式如式（）所示：()()()()()（）若为非多线激光雷达，则上式中 方向即为零。激 光 与 红 外 姜晓勇等基于激光雷达 的三维点云自适应算法激光 技术已经日渐成熟，目前大多数的 建图是根据点云数据实现的，但是由于点云数据的局限性，如数据量大，占用空间大，以及数据离散等问题，会导致常用的导航算法难以区分栅格地图上的有效空闲区域，占用区域和未知区域 。方法描述激光点云畸变会使扫描得到的激光点云与空间中的实际位置不一致，影响激光帧间点云匹配以及激光点云和地图匹配的准确性，为优化由于激光雷达运动造成的点云畸变问题，本节针对高频激光位姿以线性插值的方式优化激光点云畸变。对高频激光位姿进行线性插值，将激光雷达非匀速运动补偿为匀速运动；按照匀速运动模型计算出每个激光点对应的位姿转换矩阵，通过转换矩阵将所有激光点转换至该帧激光扫描结束时刻，计算出激光点在结束时刻的实际位置。对运动畸变优化后的点云数据做精简优化。图 为高频激光点云畸变纠正示意图，为高频单帧激光扫描的首激光点的时间，该帧扫描最后一个点的时间为，通过线性插值优化将激光雷达在（，）时间段内的运动近似为匀速运动，即将 处由激光雷达扫描得到的激光点转换至处，依据匀速运动理论模型将 处的激光点转换到扫描结束时 处，计算出该帧所有时刻的激光点相对 处的实际位置。图 激光点云畸变纠正示意图 运动畸变去除：高频激光位姿插值将激光雷达运动补偿为匀速运动优化处理后，将激光点云畸变按照匀速运动模型进行畸变纠正，为提高算法鲁棒性，将各个高频激光点对应的位姿转换矩阵 求出，得到激光点相对于扫描结束位置时的坐标。在（，）内根据式（）对高频激光位姿进行线性插值，计算 时对应的位姿转换矩阵：（，）()()（）为（，）时间段内得到的任意一激光点，通过式（）计算点 相对于扫描结束位置的坐标：（，）（）（）点 在扫描结束时相对激光雷达的位置为通过插值法纠正畸变得到的 点坐标，对该帧的所有高频激光点做插值法畸变纠正，纠正后的每帧激光点云就等同于激光雷达在结束位置静止扫描一周后所得到的结果。为验证优化后算法的提升效果，采用回环实验 对优化前后算法定位精度进行测试，其计算公式如式：（，）（，）（）其中，（，）为机器人起始点的坐标；（，）为机器人回到起点时算法估计的机器人位置；为算法计算的巡检机器人运动轨迹长度。实验机器人搭载 多线激光雷达，在模拟变电站环境进行回环实验测得激光 算法畸变纠正前后的定位误差结果如表 所示，从表中结果可以看出，经过插值法点云畸变纠正后的激光 算法定位精度相较于原激光 算法而言定位精度有较大的提高，验证了该畸变纠正方法有效性，可提高巡检机器人定位精度及自主导航 效率。表 畸变矫正前后误差对比 算法起点坐标（，）终点坐标（，）长度 定位误差 畸变矫正前（，）（，）畸变矫正后（，）（，）激光点云的位姿畸变通过激光插值法优化，通过激光位姿插值处理畸变量相对较大的非匀速运动，并对其进行算法的精度测试实验，验证了该畸变纠正方法的对提高 定位误差、减少定位误差激 光 与 红 外 第 卷的有效性。针对畸变纠正后的高精度 点云，还存在的大量冗余数据点，且占用大量计算机内存。在尽量保留点云数据的特征区域和轮廓信息的情况下剔除三维点云数据中冗余数据点 。对点云数据进行精简处理，在降低 定位建图误差的同时，大大提高计算运行效率对巡检机器人自主导航具有重要意义。点云处理优化框架所提出的点云简化框架通常包括两个阶段，算法的流程如图所示。第一个阶段是通过 采样算法和局部点云密度阈值选取质心点，然后用加权最小二乘法估计质心点的高斯曲率。第二阶段根据第一阶段计算得到的高斯曲率对质心点进行分类，然后对质心点邻域进行不同的采样方法简化。以下第 和 节描述了每个阶段的详细流程。图 点云简化框架 第一阶段：质心点的选取和曲率估计 自适应点云平均距离在这一阶段，首先使用平均点云距离阈值选出质心点，通过加权最小二乘法拟合质心点及其邻域的曲面，然后，估计质心点的高斯曲率。第一阶段的实现过程如图 所示。自适应点云平均距离点云平均距离通常可分为整体与局部两大类 ，用于衡量点云数据的离散程度。整体点云平均距离是若干个局部点云平均距离的均值。局部点云平均距离有两种计算方法：）使用最近点的距离代表该点的局部点云平均距离；）求该点与其 近邻域距离的均值为该点的局部点云平均距离 。如图 所示，点 和 分别为使用方法 和方法 计算点云平均距离，的点云平均距离偏小，的点云平均距离偏大，以上两种方法都存在缺陷。因此，提出了一种自适应点云平均距离计算方法。图 第一阶段实现流程图 图 点云平均距离 假设原始点云 （，），使用 算法从 中采样分布均匀的点云 （，）（），则自适应平均点云距离的计算流程为：）使用 算法从原始点云 中得到点云 。）在原始点云中搜索点云 中的点 的 近邻域，计算 近邻域与 的距离 ，计算 （），（），若式（）成立，点 的局部平均点云密度为：，否则 点 的局部平均点云密度为：。（）（）激 光 与 红 外 姜晓勇等基于激光雷达 的三维点云自适应算法其中，分别点云邻域距离的最小值、最大值和均值。）利用步骤）可得到 中个点的局部点云平均距离，则原始点云 的点云平均距离为：）点云密度阈值 可根据具体情况设置为 。在每轮采样选取质心点时，计算 算法采样得到的每个点的平均点云距离，将平均点云距离小于 的点作为质心点，记作 。基于加权最小二乘法的曲率估计加权最小二乘拟合曲面对点云局部拟合效果较好，根据点的距离约束给每个点设置一个权重，减少了离散点和边缘点对曲面拟合的影响，提高了曲率估计的精度。已知 是原始点云上的一个点，是点的邻域点集。以 为坐标原点创建局部坐标系 ，并构建四元数 （，）。槡（）（）（）（）（）（）（）（）（）（）（）（）（）其中，是旋转矩阵；是平移矩阵。利用上述矩阵变换，邻域点云数据 可表示为：，()，其中 ，。设最小二乘拟合曲面的曲面方程为：（）其中，()是曲面方程的系数，用矩阵表示为：，（）（）模型表达式为：（）（）（），（），（）其中，为 维曲面方程向量 的随机误差；为系数矩阵 的随机误差。，()（）其中，。根据最小二乘原则，系数矩阵误差的参数估计可由下式解得：（）在点 的邻域 生成二次曲面时，点 与 的距离越小，对生成曲面的影响越大，反之亦然，因此点 的权值随距离减少而变大，的权值的形式如下：（，）（，）（）其中，()表示，之间的距离；表示 中点的数量。拟合权值矩阵的形式如下：，（）其中，是系数矩阵的列权值矩阵；是系数矩阵的行权值矩阵。求解上述方程即可解得二次曲面方程待求系数，代入至式（）可得到拟合曲面。高斯曲率的求解公式如下：（）其中，槡 槡 槡激 光 与 红 外 第 卷 将 ，代入式（）可求得点 的高斯曲率。第二阶段：法向量估计和自适应分类采样首先将第一阶段得到的质心点通过高斯曲率阈值划分成特征明显区域和不明显区域。对于不同的区域分别使用不同简化率和简化算法进行点云简化。第二阶段的实现过程如图 所示。图 第二阶段实现流程图 点云法向量估计法向量计算方法有很多种，其中包括主成分分析法（），加权 方法等。本文使用主成分分析法估计点云的法向量。设点 的 近邻域为（，），点 的 协方差矩阵为：（）其中，为点 的 近邻域的均值，其数学表达式为：（）对式（）进行特征分解，得到特征值、，其中，对应的特征向量为、，则点的法向量为协方差矩阵 的最小特征值对应的特征向量。自适应分类采样设高斯曲率的阈值为，搜索质心点集 （，）的邻域，邻域内点的数量为 （本文 ），使用加权最小二乘法估计质心点 （，）的高斯曲率，记作 （，）。根据曲率阈值 将质心点分成特征明显的点集，记作，特征不明显的点集，记作。对于点 ，若，将点 加入点集，反之将点 加入点集。对于 中特征明显区域的点，使用 算法保留其邻域内较多的点，一般为邻域内点的数量的，可根据精简要求具体设置邻域内的采样率；对于 中特征不明显区域的点，首先使用 算法估计该点及其邻域的法向量，计算该点与其邻域内点的法向量夹角，记作 ，然后根据法向量夹角阈值，统计出法向量夹角小于 的邻域点的数量，记作 ，防止采样后的点云出现空洞，特征不明显的邻域内的采样率一般不高于，若 ，删除法向量夹角小于 的前 个邻域点，反之删除所有法向量夹角小于 的邻域点。实验本文算法基于 激光 算法 进行 建图。使用所提出的点云采样算法进行点云数据简化实验，并与其他三种点云简化方法（包括均匀采样法、体素采样法、曲率采样法）进行了比较。激光 运动畸变去除实验平台本文实验平台采用巡检机器人外接 激光雷达与电源进行 建图。图 为本文实验平台图。图 为优化后的激光 三维点云图。单个刚体点云简化实验点云实验实验使用斯坦福大学建立的 点云数据库中的 点云数据作为实验样本，分别使用均匀采样法、体素采样法、曲率采样法及本文提出的采样法进行点云精简。原始点云精简后的精简率见表 ，原始点云如图 所示，精简后的点云如图激 光 与 红 外 姜晓勇等基于激光雷达 的三维点云自适应算法 所示，精简后点云局部结果如图 所示。可以看出，本文提出的方法相比其他 种采样算法在精简度相似的情况下，能够更有针对性的保留点云数据的轮廓特征和大量的特征点。图 实验平台图 图 激光 三维点云图 表 点云数据精简率 采样方法精简前点数 个精简后点数 个精简率 均匀采样法 体素采样法 曲率采样法 本文采样法 图 原始点云 图 精简的点云图 图 精简后点云局部结果 大型场景点云简化实验本实验对视觉激光信息融合纠正后扫描获得大场景的三维点云数据进行简化，点云数据来源于巡检机器人 运动畸变优化后获得。原始点云精简后的精简率见表 ，原始点云如图 所示，精简后点云如图 所示，精简后点云局部结果如图 所示。可以看出，本文提出的方法相比其他 种采样算法在精简度相似的情况下，能够更有针对性的保留点云数据的轮廓特征和大量的特征点。表 点云数据精简率 采样方法精简前点数 个精简后点数 个精简率 均匀采样法 体素采样法 曲率采样法 本文采样法 图 原始点云 激 光 与 红 外 第 卷图 精简的点云图 图 精简的点云局部图 总结针对激光 运动畸变的问题，提出了一种针对高频激光插值优化方法。有效减少激光雷达运动产生的畸变，大大提高 建图精度的同时，针对激光雷达运动等三维扫描设备获得的三维点云数据存在着大量的冗余三维数据点的问题，提出了一种基于曲率的点云数据自适应简化框架。该框架能够很好的消除三维点云数据中特征不明显区域的冗余数据点。该框架由两个阶段组成：第一阶段，选取分布均匀的质心点，搜素质心点 近邻域，估计质心点的高斯曲率；第二阶段，使用高斯曲率将质心点分为特征点和非特征点，采取不同的简化方法进行精简。实验结果表明，该框架相比传统点云简化方法能更好的保留点云的特征区域和轮廓，对激光雷达扫描的点云数据用于巡检机器人的自主导航起到了促进作用。参考文献：，（）：，（）樊陈，倪益民，申洪，等 中欧智能变电站发展的对比分析 电力系统自动化，（）：，：，（）：（）周苏，李伟嘉，郭军华 基于激光雷达的停车场车辆定位算法 同济大学学报：自然科学版，（）：，（）：（）梅文胜，周燕芳，周俊 基于地面三维激光扫描的精细地形测绘 测绘通报，（）：，：，：，（）：，（）：，：，（）张会霞，朱文博 三维激光扫描数据处理理论及应用 北京：电子工业出版社，（）：（）李琪琪，花向红，赵不钒，等 一种基于曲率泊松碟采样的散乱点云精简方法 测绘通报，（）：，（）：，（）：，：，激 光 与 红 外 姜晓勇等基于激光雷达 的三维点云自适应算法 ，（）：（）曹爽，赵显富，马文 一种基于曲面变化的工业构件点云数据精简方法 测绘通报，（）：，：，（）：（）黄天天，刘波 基于曲率的植物三维点云精简算法的优化 湖南农业大学学报：自然科学版，（）：，（）李青蒙 激光扫描点云处理技术研究 大连：大连海事大学，（）：，（）李宏宇 激光雷达的点云数据处理研究 长春：硕士学位论文，（）：（）刘浩敏，章国锋，鲍虎军 基于单目视觉的同步定位与地图构建方法综述 计算机辅助设计与图形学学报，（）：，：，（）：（）李晨曦，张军，靳欣宇，等 激光雷达 技术及其在无人车中的应用研究进展 北京联合大学学报：自然科学版，（）：，（）：（）谢莉，余胜 摄像机标定算法研究 科技视界，（）：，：，（）：，（）李涛涛，何宇 最小二乘法在张正友标定法中的应用 信息与电脑：理论版，（）：，（）：（）郭裕兰，鲁敏，谭志国，等 一种新的激光雷达目标姿态估计算法 中国激光，（）：，（）光兴屿 室内环境的二维语义地图构建与路径规划研究 武汉：武汉科技大学，：，：，（）高翔 视觉 十四讲：从理论到实践 北京：电子工业出版社，激 光 与 红 外 第 卷