[发明专利]一种基于LeGO-LOAM的分步式帧间位姿估计算法

说明书

本发明公开了一种基于LeGO‑LOAM的分步式帧间位姿估计算法。首先读入激光雷达的点云信息，并将点云划分为地面点和非地面点，再标记小集群点。在对点云进行分类之后，在地面点中提取地面特征点，在非地面点中提取边缘特征点。之后对位姿进行分步求解，步骤一通过地面特点计算得到帧间位姿的翻滚角、俯仰角和竖直方向的位移，并将计算得到的三自由度位姿代入步骤二的边缘特征点计算偏航角以及水平面上位移的过程，在步骤二中对六自由度位姿同时进行优化，最后再与地图点共同优化位姿。应用本发明的方法求解的轨迹精度较LeGO‑LOAM算法的轨迹精度提升最高达45.4％。

技术领域

本发明涉及车辆平台的SLAM技术方案，尤其涉及一种基于LeGO-LOAM的分步式帧间位姿估计算法。

背景技术

无人驾驶技术中，作为车辆的感知器官，激光雷达有着举足轻重的地位，而激光SLAM(simultaneouslocalizationandmapping)正是车辆对环境与自身感知的一种手段。相较于视觉SLAM，激光雷达有着更高的精度、更强鲁棒性、更大的视角和更精准的尺度信息。

三维激光雷达可以捕获更多环境的细节，所以本文重点研究了三维激光雷达在室外环境下的实时定位与建图技术。寻找激光雷达相邻帧间转换的典型方法是迭代最近点(ICP)，ICP算法需要计算所有获得的激光点云，所以计算量巨大。为了提高ICP算法的效率和精度，提出了多种改进ICP算法，其中广义ICP提出了一种匹配相邻帧间局部平面块的方法，此后基于特征匹配的方法受到越来越多的关注，相继提出了大量的利用特征进行点云配准的算法。

基于特征匹配的LOAM算法是3D激光SLAM中最为经典的算法，LOAM算法中的特征提取算法原理为计算点在其局部区域的曲率，进而将激光雷达获取的点云通过曲率分为平面特征点和边缘特征点，根据这两类特征点完成相邻帧间的特征匹配，并开创性的提出了将位姿估计划分为高频的位姿估计和低频的位姿优化。LOAM算法作为一种普适性算法，未对于无人车平台，做出针对性的优化，所以在无人车平台上存在着运算速度和精度仍有提升空间。2018年发布的LeGO-LOAM算法针对无人车平台对LOAM算法进行了改进，该算法将平面点进一步细化为地面特征点，并且在帧间位姿求解时采用了分步求解的策略，充分利用了道路的特征，简化了帧间位姿估计的运算复杂度。但是LeGO-LOAM的帧间位姿估计策略中，在根据边缘特征点估计位姿估计时，仅将在不影响算法运行速度的前提下，对通过地面特征匹配求得的三自由度的位姿变化结果进行优化，从而达到提升整体位姿估计精度的效果。

发明内容

发明目的：针对以上问题，本发明提出一种基于LeGO-LOAM的分步式帧间位姿估计算法。在分步计算车辆帧间的位姿时，将(rx，rz，ty)也代入由特征点求取位姿的最小二乘计算中，在对(ry，tx，tz)的同时，对(rx，rz，ty)也进行优化，提高分步求解位姿的精度。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于LeGO-LOAM的分步式帧间位姿估计算法，具体方法如下：

(1)读入激光点云数据；

系统通过激光雷达获取周围环境的点云信息，并将点云信息进行处理，将激光束编号，从仰角最小的激光束开始编号，作为点云的纵坐标，激光的水平编号，从旋角为0的点开始编号，作为其横坐标，对点云进行编号，最后记录点云的深度信息、水平旋角和仰角；

(2)对点云进行分类；

点云分类通过两步完成，第一步通过地面点计算以及公式(1)的原理完成地面点的标记；

θ＝atan 2(ΔZ-ΔX)

ΔX＝R_r-1 cosα-R_r cosβ

ΔZ＝R_r-1 sinα-R_r sinβ(1)