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

本发明专利技术公开了一种基于LeGO

【技术实现步骤摘要】
一种基于LeGO
‑
LOAM的分步式帧间位姿估计算法


[0001]本专利技术涉及车辆平台的SLAM技术方案，尤其涉及一种基于LeGO
‑
LOAM的分步式帧间位姿估计算法。

技术介绍

[0002]无人驾驶技术中，作为车辆的感知器官，激光雷达有着举足轻重的地位，而激光SLAM(simultaneouslocalizationandmapping)正是车辆对环境与自身感知的一种手段。相较于视觉SLAM，激光雷达有着更高的精度、更强鲁棒性、更大的视角和更精准的尺度信息。
[0003]三维激光雷达可以捕获更多环境的细节，所以本文重点研究了三维激光雷达在室外环境下的实时定位与建图技术。寻找激光雷达相邻帧间转换的典型方法是迭代最近点(ICP)，ICP算法需要计算所有获得的激光点云，所以计算量巨大。为了提高ICP算法的效率和精度，提出了多种改进ICP算法，其中广义ICP提出了一种匹配相邻帧间局部平面块的方法，此后基于特征匹配的方法受到越来越多的关注，相继提出了大量的利用特征进行点云配准的算法。
[0004]基于特征匹配的LOAM算法是3D激光SLAM中最为经典的算法，LOAM算法中的特征提取算法原理为计算点在其局部区域的曲率，进而将激光雷达获取的点云通过曲率分为平面特征点和边缘特征点，根据这两类特征点完成相邻帧间的特征匹配，并开创性的提出了将位姿估计划分为高频的位姿估计和低频的位姿优化。LOAM算法作为一种普适性算法，未对于无人车平台，做出针对性的优化，所以在无人车平台上存在着运算速度和精度仍有提升空间。2018年发布的LeGO
‑
LOAM算法针对无人车平台对LOAM算法进行了改进，该算法将平面点进一步细化为地面特征点，并且在帧间位姿求解时采用了分步求解的策略，充分利用了道路的特征，简化了帧间位姿估计的运算复杂度。但是LeGO
‑
LOAM的帧间位姿估计策略中，在根据边缘特征点估计位姿估计时，仅将在不影响算法运行速度的前提下，对通过地面特征匹配求得的三自由度的位姿变化结果进行优化，从而达到提升整体位姿估计精度的效果。

技术实现思路

[0005]专利技术目的：针对以上问题，本专利技术提出一种基于LeGO
‑
LOAM的分步式帧间位姿估计算法。在分步计算车辆帧间的位姿时，将(rx，rz，ty)也代入由特征点求取位姿的最小二乘计算中，在对(ry，tx，tz)的同时，对(rx，rz，ty)也进行优化，提高分步求解位姿的精度。
[0006]技术方案：为实现本专利技术的目的，本专利技术所采用的技术方案是：一种基于LeGO
‑
LOAM的分步式帧间位姿估计算法，具体方法如下：
[0007](1)读入激光点云数据；
[0008]系统通过激光雷达获取周围环境的点云信息，并将点云信息进行处理，将激光束编号，从仰角最小的激光束开始编号，作为点云的纵坐标，激光的水平编号，从旋角为0的点开始编号，作为其横坐标，对点云进行编号，最后记录点云的深度信息、水平旋角和仰角；
[0009](2)对点云进行分类；
[0010]点云分类通过两步完成，第一步通过地面点计算以及公式(1)的原理完成地面点的标记；
[0011]θ＝atan2(ΔZ
‑
ΔX)
[0012]ΔX＝R
r
‑1cosα
‑
R
r
cosβ
[0013]ΔZ＝R
r
‑1sinα
‑
R
r
sinβ#(1)
[0014]公式(1)中R
r
‑1表示仰角为α的激光束的长度，R
r
表示仰角为β的激光束的长度，当θ大于阈值时，则该点标记为非地面点，否则标记为地面点，计入地面点集合
[0015]完成地面点标记后，进行点云集群的划分，其原理点云集群划分和公式(2)所示；
[0016][0017]公式(2)中β表示点云中最临近的两个点到激光雷达中心连线的夹角，θ为两个点中深度更大的点到激光雷达的连线和两点之间连线的夹角，d1为两点中深度更大的点的深度，d2为另一个点的深度，若θ大于阈值这说明两个点不为一个集群，否则两个点为一个集群，在完成所有点的集群划分后，对点的集群进行标记并剔除小集群点；
[0018](3)提取地面特征点和边缘特征点；
[0019]首先计算每个点的曲率值，公式如下式(3)所示；
[0020][0021]式中|S|表示点集中点的个数，||r
i
||表示点i的深度，通过式(3)得到点云中每个点的曲率值，在非地面点中，点的曲率越大则表明此处点云深度变化幅度越大，并计入边缘特征点集合
[0022]若特征点集中于一个区域，可能会出现某一时刻该区域不可观测导致的位姿跟踪丢失，造成位姿估计出现较大的误差，所以对点云进行分散采样，将每束点云平均划分为六个区域，在六个区域中各选取2个曲率最大的边缘特征点计入集合F
e
，或四个曲率值最小的地面特征点计入集合F
p
；
[0023](4)分步帧间位姿计算：
[0024]针对无人车平台，将帧间位姿分为六个部分，三个旋转角度：rx俯仰角,ry偏航角和rz翻滚角，三个平移量：tx左右方向上的位移，ty竖直方向上的位移和tz前后方向上的位移；
[0025]在求取帧间位姿时，设相邻帧间，由后一帧到前一帧的旋转矩阵为R，平移向量为
‑
t，并且将后一帧中的边缘特征点F
e
和地面特征点F
p
按照下述公式(4)投影至前一帧中，式中代表k时刻雷达坐标系下的点i的坐标，而表示k+1时刻雷达坐标系下点i的坐标；
[0026][0027]式(4)中R和t可用(r
x
，r
y
，r
z
，t
x
，t
y
，t
z
)表示，如下式(5)可见：
[0028][0029]相邻帧间仅匹配具有相同的集群标志的点，并采用LOAM算法中提出的“点到线”和“点到面”的匹配方案，该匹配方案的误差由下式(6)给出；
[0030][0031]式中表示边缘特征点，和表示k时刻中与匹配的两个边缘特征点的坐标，表示地面特征点，和表示k时刻中与匹配的地面点的坐标，d
ξ
表示边缘点匹配中的“点到线”的距离，d
φ
表示地面点匹配中的“点到面”的距离；
[0032]针对无人车平台位姿变化集中于水平面上的运动，将帧间位姿分为两步求解，第一步为通过地面特征点求解得到(rx，rz，ty)，其最小二乘原理如下式(7)所示；
[0033][0034]通过式(7)求解得到(rx，rz，ty)之后，将(rx，rz，ty)代入第二步，相对于LeGO
‑
LOAM算法的仅通过边缘特征点本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于LeGO
‑
LOAM的分步式帧间位姿估计算法，其特征在于，具体方法如下：(1)读入激光点云数据；系统通过激光雷达获取周围环境的点云信息，并将点云信息进行处理，将激光束编号，从仰角最小的激光束开始编号，作为点云的纵坐标，激光的水平编号，从旋角为0的点开始编号，作为其横坐标，对点云进行编号，最后记录点云的深度信息、水平旋角和仰角；(2)对点云进行分类；点云分类通过两步完成，第一步通过地面点计算以及公式(1)的原理完成地面点的标记；θ＝atan2(ΔZ
‑
ΔX)ΔX＝R
r
‑
1 cosα
‑
R
r cosβΔZ＝R
r
‑1sinα
‑
R
r
sinβ#(1)公式(1)中R
r
‑1表示仰角为α的激光束的长度，R
r
表示仰角为β的激光束的长度，当θ大于阈值时，则该点标记为非地面点，否则标记为地面点，计入地面点集合完成地面点标记后，进行点云集群的划分，其原理点云集群划分和公式(2)所示；公式(2)中β表示点云中最临近的两个点到激光雷达中心连线的夹角，θ为两个点中深度更大的点到激光雷达的连线和两点之间连线的夹角，d1为两点中深度更大的点的深度，d2为另一个点的深度，若θ大于阈值这说明两个点不为一个集群，否则两个点为一个集群，在完成所有点的集群划分后，对点的集群进行标记并剔除小集群点；(3)提取地面特征点和边缘特征点；首先计算每个点的曲率值，公式如下式(3)所示；式中|S|表示点集中点的个数，||r
i
||表示点i的深度，通过式(3)得到点云中每个点的曲率值，在非地面点中，点的曲率越大则表明此处点云深度变化幅度越大，并计入边缘特征点集合若特征点集中于一个区域，可能会出现某一时刻该区域不可观测导致的位姿跟踪丢失，造成位姿估计出现较大的误差，所以对点云进行分散采样，将每束点云平均划分为六个区域，在六个区域中各选取2个曲率最大的边缘特征点计入集合F
e
，或四个曲率值最小的地面特征点计入集合F
p
；(4)分步帧间位姿计算：针对无人车平台，将帧间位姿分为六个部分，三个旋转角度：rx俯仰角,ry偏航角和rz翻滚角，三个平移量：tx左右方向上的位移，ty竖直方向上的位移和tz前后方向上的位移；在求取帧间位姿时，设相邻帧间，由后一帧到前一帧的旋转矩阵为R，平移向量为
‑
t，并且将后一帧中的边缘特征点F
e
和地面特征点F
p
按照下述公式(4)投影至前一帧中，式中代表k时刻雷达坐标系下...

【专利技术属性】
技术研发人员：潘树国，章辉，高旺，赵涛，谭涌，
申请(专利权)人：东南大学，
类型：发明
国别省市：