一种基于LeGO-LOAM的分步式帧间位姿估计算法

技术领域

本发明涉及车辆平台的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)的原理完成地面点的标记；

θ＝atan2(ΔZ-ΔX)

ΔX＝R

ΔZ＝R

公式(1)中R

完成地面点标记后，进行点云集群的划分，其原理点云集群划分和公式(2)所示；

公式(2)中β表示点云中最临近的两个点到激光雷达中心连线的夹角，θ为两个点中深度更大的点到激光雷达的连线和两点之间连线的夹角，d

(3)提取地面特征点和边缘特征点；

首先计算每个点的曲率值，公式如下式(3)所示；

式中|S|表示点集中点的个数，||r

若特征点集中于一个区域，可能会出现某一时刻该区域不可观测导致的位姿跟踪丢失，造成位姿估计出现较大的误差，所以对点云进行分散采样，将每束点云平均划分为六个区域，在六个区域中各选取2个曲率最大的边缘特征点计入集合F

(4)分步帧间位姿计算：

针对无人车平台，将帧间位姿分为六个部分，三个旋转角度：rx俯仰角,ry偏航角和rz翻滚角，三个平移量：tx左右方向上的位移，ty竖直方向上的位移和tz前后方向上的位移；

在求取帧间位姿时，设相邻帧间，由后一帧到前一帧的旋转矩阵为R，平移向量为-t，并且将后一帧中的边缘特征点F

式(4)中R和t可用(r

相邻帧间仅匹配具有相同的集群标志的点，并采用LOAM算法中提出的“点到线”和“点到面”的匹配方案，该匹配方案的误差由下式(6)给出；

式中

针对无人车平台位姿变化集中于水平面上的运动，将帧间位姿分为两步求解，第一步为通过地面特征点求解得到(rx，rz，ty)，其最小二乘原理如下式(7)所示；

通过式(7)求解得到(rx，rz，ty)之后，将(rx，rz，ty)代入第二步，相对于LeGO-LOAM算法的仅通过边缘特征点求解(ry，tx，tz)，通过边缘特征点求解(rx，ry，rz，tx，ty，tz)，其原理如下式(8)；

通过上述步骤完成分布式帧间六自由度位姿估计。

作为本发明进一步改进，步骤(1)所述读入激光点云数据，通过激光雷达扫描周围环境的点云信息后，将激光点云进行编号以及存储其深度、水平旋角和竖直仰角。

作为本发明进一步改进，步骤(2)所述，对点云进行分类，在读入点云信息后，将激光点云划分为地面点和非地面点，之后再将点云进行聚类，对点云的集群信息进行标记，并剔除小集群点。

作为本发明进一步改进，步骤(3)所述提取地面特征点和边缘特征点，通过计算点云的曲率，在地面点中提取出地面特征点，在非地面点中提取边缘特征点，同时为防止特征点集中分布的问题，将可视范围进行等比例划分，每个区域中只选取固定个数的特征点。

作为本发明进一步改进，步骤(4)中所述的分步帧间位姿估计，将帧间位姿估计分为六个部分，针对无人车平台的运动特性，通过地面特征点求解(rx，rz，ty)，并将结果代入通过边缘特征点求解帧间位姿，并同时优化(rx，rz，ty)，最终得到车辆的帧间六自由度位姿变化结果(rx，ry，rz，tx，ty，tz)。

有益效果：本发明提出了一种基于LeGO-LOAM的分步式帧间位姿估计算法，在进行帧间位姿分步式估计时，在通过边缘特征点求解位姿时，将(rx，rz，ty)同时代入优化求解，在求解(ry，tx，tz)的同时，对(rx，rz，ty)也进行优化，从而求得较高精度的六自由度位姿变化结果，通过实验对比，在数据集和实测数据中，算法估计轨迹的精度均有提升，最高可达45.4％。

附图说明

图1是地面点提取原理图；

图2是点云分割原理图；

图3是实验测试平台；

图4是实验设备安装图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

本发明所述的一种基于LeGO-LOAM的分步式帧间位姿估计算法，具体方法如下：

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

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

(2)对点云进行分类；

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

θ＝atan2(ΔZ-ΔX)

ΔX＝R

ΔZ＝R

公式(1)中R

完成地面点标记后，进行点云集群的划分，其原理如图2和公式(2)所示。

公式(2)中β表示点云中最临近的两个点到激光雷达中心连线的夹角，θ为两个点中深度更大的点到激光雷达的连线和两点之间连线的夹角，d

首先计算每个点的曲率值，公式如下式(3)所示。

式中|S|表示点集中点的个数，||r

若特征点集中于一个区域，可能会出现某一时刻该区域不可观测导致的位姿跟踪丢失，造成位姿估计出现较大的误差，所以对点云进行分散采样。将每束点云平均划分为六个区域，在六个区域中各选取2个曲率最大的边缘特征点计入集合F

(4)分步帧间位姿计算：

本发明针对无人车平台，将帧间位姿分为六个部分，三个旋转角度：rx(俯仰角),ry(偏航角)和rz(翻滚角)，三个平移量：tx(左右方向上的位移)，ty(竖直方向上的位移)和tz(前后方向上的位移)。

在求取帧间位姿时，设相邻帧间，由后一帧到前一帧的旋转矩阵为R，平移向量为-t，并且将后一帧中的边缘特征点F

式(4)中R和t可用(r

相邻帧间仅匹配具有相同的集群标志的点，并采用LOAM算法中提出的“点到线”和“点到面”的匹配方案，该匹配方案的误差由下式(6)给出。

式中

本发明中针对无人车平台位姿变化集中于水平面上的运动，将帧间位姿分为两步求解，第一步为通过地面特征点求解得到(rx，rz，ty)，其最小二乘原理如下式(7)所示。

通过式(7)求解得到(rx，rz，ty)之后，将(rx，rz，ty)代入第二步，相对于LeGO-LOAM算法的仅通过边缘特征点求解(ry，tx，tz)，本发明通过边缘特征点求解(rx，ry，rz，tx，ty，tz)，其原理如下式(8)。

通过上述步骤完成分布式帧间六自由度位姿估计。

下面根据实际环境下的实验对本发明的技术方案验证效果与精度，对比改进算法与原LeGO-LOAM算法计算得到的轨迹精度。首先在KITTI数据集中对算法的轨迹估计精度进行评估，评估策略为通过evo将数据集真值与算法输出轨迹进行对比，求取RMSE值。采用的电脑配置如下：处理器为IntelCorei7-8700 3.2GHz，内存为16GB。

另一组测试为实测数据，采用了Velodyne-VLP16激光雷达，实验场地为北京顺沙路测试场和南京市东南大学九龙湖校区外围道路，利用RTK定位结果作为真值，评估轨迹精度。采用的电脑配置如下：处理器为IntelCorei7-8700 3.2GHz，内存为16GB。

数据集测试结果如下表1所示：

表1在KITTI数据集上轨迹估计结果RMSE对比

在实测数据集中，其估计轨迹的结果如下表2：

表2实测数据轨迹估计结果RMSE对比

从表1和表2可以看出，改进算法得到的轨迹精度对比LeGO-LOAM算法有了较为明显的提升效果。在数据集测试中，11组数据集中轨迹精度提升幅度最大可达到12.2％，并且其中9组数据都有提升。并且在实测数据中，测试场环境下，改进算法的轨迹估计精度提升幅度高达45.4％，校园外道路测试的轨迹精度提升幅度也达到了9.4％，通过测试结果可见，本发明提出的一种基于LeGO-LOAM的分步式帧间位姿估计算法的轨迹估计精度，相对于LeGO-LOAM算法有较大幅度的提升，并且最高提升幅度可达45.4％。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。