技术摘要：
本发明公开一种基于面线角点特征提取的激光SLAM方法，包括以下步骤：(1)初始化地图和各类参数，分配内存；(2)收集3D激光雷达采集到的点云；(3)对采集到的点云进行数据预处理；(4)进行投影；(5)进行阻塞点去除；(6)提取面、线、角点特征；(7)对面、线特征进行表面法向量  全部
背景技术：
在移动机器人的研究领域中，同步定位与建图(Simultaneous  Localization  and  Mapping,SLAM)一直是一个热门的研究话题，SLAM为机器人提供导航地图和实时位置，而这 些都是机器人执行路径规划、路径跟踪的前提，因此它在移动机器人导航中占有十分重要 的位置。自1986年SLAM问题提出至今，SLAM技术在各行各业都取得了长足的发展，现今在市 场上已经涌现了一大批SLAM相关算法的应用，如：无人驾驶汽车、服务机器人、无人机、无人 潜航器、虚拟现实和增强现实等。在SLAM中常用的环境感知传感器主要有相机、激光雷达 (Light  Detection  and  Ranging,LiDAR)、雷达(Radio  Detection  and  Ranging,RADAR)和 声呐等。 激光传感器具有测距精度高、受光线影响小、抗电磁干扰等特点，但同时它也具有 不少缺点，比如容易受到雨雪雾天气的影响，每次扫描获得的数据量庞大等。当前，激光 SLAM框架一般分为前端扫描匹配、后端优化、回环检测、地图构建四个关键模块。前端扫描 匹配能给出短时间内的位姿和地图，但由于不可避免的误差累积，后端优化正是当长时间 增量式扫描匹配后优化里程计及地图信息；回环检测负责通过检测闭环而减少全局地图的 漂移现象，以便生成全局一致性地图；地图构建模块负责生成和维护全局地图。前端扫描匹 配是激光SLAM的核心步骤，工作内容是已知前一帧位姿并利用相邻帧之间的关系估计当前 帧的位姿；目前在激光SLAM中主流的扫描匹配算法包括：迭代最临近点(Iterative  Closest  Point)及变种、相关性扫描匹配(Correlation  Scan  Match)、基于优化方法 (Optimization-based  Method)、正态分布变换(Normal  Distribution  Transformation) 等。上述常用方法往往对硬件要求较高，虽然精度较高，但是在前端匹配时处理的数据量非 常庞大。作者Ji  Zhang在论文LOAM:Lidar  Odometry  and  Mapping  in  Real-time中提到了 一种基于线面特征提取的SLAM方法，该方法没有回环检测功能，利用提取的线面特征来进 行前端帧间匹配，拥有更快的匹配速度。
技术实现要素：
本发明的目的是为了克服现有技术中的不足，提供一种基于面线角点特征提取的 激光SLAM方法，该方法采用双重提取的方式，相比于常用方法降低了数据处理量，降低了硬 件要求，相比于经典的基于线面特征提取的LOAM方法，增加了回环检测功能，拥有更高精度 的定位和建图效果。 本发明的目的是通过以下技术方案实现的： 一种基于面线角点特征提取的激光SLAM方法，包括以下步骤： (1)初始化地图和各类参数，分配内存； 5 CN 111583369 A 说　明　书 2/7 页 (2)收集3D激光雷达采集到的点云； (3)对采集到的点云进行数据预处理，将超过激光雷达有效范围的点云点进行去 除； (4)对步骤(3)中预处理后的点云，进行投影，将点云投影到一个16*1800的点云球 体坐标系中； (5)对步骤(4)中的点云进行阻塞点去除； (6)对步骤(5)中的点云进行特征提取，根据对光滑度的计算，提取面、线、角点特 征； (7)对步骤(6)中提取到的面、线特征进行表面法向量估计，根据点云的曲面法向 量计算点的曲率，并根据点的不同曲率对点云进行分类，以提取出更准确的面、线特征，提 高位姿估计的准确度； (8)根据步骤(7)中提取到的面、线特征和步骤(6)中提取到的角点特征信息进行 前端配准计算，计算出载体的位姿信息和里程计信息； (9)剔除掉点云中的冗余部分，保存关键帧信息，位姿信息和载体的轨迹信息； (10)根据步骤(9)中的关键帧信息和运动轨迹信息进行回环检测； (11)根据现有地图和最新的点云对提取的特征以及现有位姿进行后端优化； (12)更新位姿，融合建图，发布地图信息和轨迹信息； (13)跳转至步骤(2)，继续运行，直到建图完成或者关闭进程。 进一步的，步骤(3)中对点云的预处理具体如下： (301)遍历所有原始点云，设置有效距离，使用滤波器，剔除掉有效距的外的点云； (302)以步骤(301)处理后的点云作为输入，计算一个刚好能包裹住该点云的立方 体，将大立方体分割成不同的小立方体，计算落在每个小立方体内的点的质心，并通过质心 近似该每个小立方体内的所有点，实现对点云的下采样处理； (303)遍历步骤(302)处理后的所有点云，对于每一个点，进行k近邻搜索，并计算 每个点与其k近邻的平均距离，若其中一个点的平均距离大于阈值d，则认为该点为野值点， 计算完后对所有野值点进行剔除； d＝mean stddev*n 其中mean为搜索到的k个点到这个点的平均距离，stddev为标准差；n取1.0。 进一步的，点云为3D点云，3D点云包含的信息包括扫描到每个点的三维坐标(x,y, z)，将某个点从三维坐标系投影到球体坐标系的具体方法如下： (401)计算竖直方向上的角度α；求出点位于竖直方向上的第几线row； (402)计算水平方向上的角度β；求出点位于水平方向上第几线col； (403)计算点距离激光雷达的深度距离 实现原本需要三个参 数(x,y,z)表示的点，用(row，col)两个值表示即可。 进一步的，步骤(5)中，阻塞点去除指的是点云中相互遮挡且又离得很近的点。 进一步的，步骤(6)的面、线、角点特征提取中，首先进行光滑度计算，光滑度计算 公式如下： 6 CN 111583369 A 说　明　书 3/7 页 根据第i点周围的s个点来计算第i点的光滑度，通常选取s＝10，其中ri为第i点的 深度距离，rj为j点的深度距离；然后，对点云中所有点的光滑度进行排序，同时设置阈值参 数b1,b2,b3,其中b1＞b2＞b3；筛选出光滑度的值大于b1值的点并视为角点进行处理；筛选出 光滑度大于b2小于b1的点并视为线特征进行处理，筛选出光滑度小于b3值的点并视为面特 征进行处理。 进一步的，步骤(7)中，对步骤(6)中提取到的线特征和面特征进行表面法向量估 计，根据点云的表面法向量计算点pi曲率的步骤具体如下: (701)选取当前点pi； (702)运用kd-tree算法查找点pi最近邻的k个点，构成协方差矩阵A； (703)计算协方差矩阵A，对应的协方差矩阵A如下： 其中k是点pi近邻点的数目， 表示最近邻元素的三维质心，λj是协方差矩阵的特 征值，Vj是第j个特征向量； (704)求解第三步中获得的协方差矩阵的特征值λ1，λ2，λ3； (705)取λ1，λ2，λ3征中最小的特征值λmin； (706)计算pi的曲率：λmin/(λ1 λ2 λ3)。 进一步的，步骤(8)中载体的位姿信息用变换矩阵T表示，T用旋转矩阵R和向量t表 示，用欧拉角表示旋转矩阵为R＝f(θyaw，θpitch，θroll)，平移向量可以表示为t＝(tx,ty,tz) , 利用步骤(7)和步骤(6)得到相邻两帧点云的特征对应关系，找到对应平面，对于筛选出来 的面特征上的点，找到与之对应的上一帧数据中距离该点最近的平面，计算出点到平面的 距离dp；找到对应的角点和线特征以后，对于筛选出来的线特征上的点，找到与之对应的上 一帧数据中距离该点最近的线特征，计算出点到线段的距离de,根据下列的非线性方程组， 利用高斯牛顿法求解出变换矩阵T； FP(tz，θpitch，θroll)＝dp Fe(tx,ty,θyaw)＝de 求解出变换矩阵后，根据初值求解出载体的位姿信息和轨迹信息。 进一步的，步骤(10)中，在进行回环检测的过程中，对当前采集到的关键帧与存储 的历史关键帧信息进行对比，如果存在类似的关键帧，同时对当前的轨迹点进行k近邻搜 索，如果搜索到r米范围内存在出现过的轨迹点，而且满足约束条件，则判定存在回环；如果 存在判定存在回环，则剔除掉与历史关键帧重合的信息，以历史保存的特征信息和关键帧 信息进行建图。 进一步的，步骤(10)中对回环检测添加的约束条件有时间约束和距离约束，如果 当前帧和重合的历史关键帧差距在t秒以内，判定回环失效；如果当前轨迹点和重合的历史 轨迹点的里程计信息差距在s米以内，则判定回环失效。 与现有技术相比，本发明的技术方案所带来的有益效果是： 在包含树和草的嘈杂野外环境中，经典LOAM算法往往会提取到不可靠的平面和直 7 CN 111583369 A 说　明　书 4/7 页 线特征，增大位姿估计的误差，本发明通过双重提取，提取到了更准确的平面和直线特征， 大程度克服了这个缺陷。当无人车在封闭的环境内快速移动时，经典LOAM算法无法达到可 靠的建图效果，本发明通过添加回环检测模块，并在回环检测中添加距离约束和时间约束， 达到了可以让无人车在封闭的环境内快速移动时仍具有很好的建图效果。 该方法采用双重提取的方式，相比于传统的激光SLAM方法降低了数据处理量，降 低了硬件要求，并且能达到实时的定位与建图效果，相比于经典的基于线面特征提取的 LOAM方法，增加了回环检测功能，提高了定位精度和建图精度。 附图说明 图1显示为本发明所述方法的具体操作处理流程图。 图2显示为本发明中需要剔除的阻塞点说明图。 图3显示为本发明说明的回环检测距离约束图。 图4显示为本发明在ROS系统中运行的节点与消息收发图，其中椭圆形代表节点信 息，矩形代表话题消息信息。 图5a和图5b显示为用同一个室内数据集对本发明中的算法和传统的LOAM算法进 行测试的效果对比图，其中图5a为传统LOAM算法的测试效果，右图5b为本发明算法的测试 效果。