一种基于ICP的激光雷达里程计方法技术

本发明专利技术提供一种基于ICP的激光雷达里程计方法，由多尺度特征提取和运动估计两部分组成。其中，多尺度特征提取流程分为两步：1、在大尺度上，根据激光数据中点的曲率大小将点云划分成平滑点和粗糙点作为特征点，形成大尺度特征点对；2、在小尺度上，剔除噪声点和不稳定的点，保留边缘点和角点作为特征点，形成小尺度特征点对。运动估计分为两部分：1、ICP算法用在大尺度上提取的特征点对求解六自由度的运动估计；2、基于小尺度上提取的特征点对计算的切向量对可以鲁棒地求解旋转自由度；将两部分的计算结果耦合得到运动估计结果。连续的运动估计组成了激光雷达里程计。计组成了激光雷达里程计。计组成了激光雷达里程计。

【技术实现步骤摘要】
一种基于ICP的激光雷达里程计方法


[0001]本专利技术属于SLAM(SimultaneousLocalizationand Mapping，同步定位与建图)领域，具体涉及一种基于ICP的激光雷达里程计方法。

技术介绍

[0002]SLAM技术近年来发展迅速，应用广泛。无论是在自动驾驶领域还是在无人工业运输领域，亦或是服务机器人领域，SLAM涵盖了从高速到低速运动的大部分场景。服务机器人领域主要包括安防机器人、引导机器人、扫地机器人等，应用场景从广阔的室外环境到有限的室内环境。定位是SLAM中运动估计模块要完成的基本任务，连续的定位结果组成了里程计。
[0003]对于激光雷达SLAM，基于滤波的算法首先被广泛使用并应用于二维激光雷达。利用连续的激光雷达数据在环境中的分布来估计机器人的最有可能的位姿状态，这类算法通常对计算资源敏感。随着非线性求解器在SLAM中的广泛使用，涌现出大量的基于图优化的算法。但这些方法在数据关联时并没有刻意提取特征点，而是直接使用了原始的激光数据。而在3D激光雷达SLAM中，特征提取在数据预处理阶段是一个很重要的步骤，可以获得更少噪声的和更稳定的数据。此外，常见的特征提取都是在单尺度上执行的，会导致信息丢失。

技术实现思路

[0004]专利技术考虑噪声对传感器数据的影响，应用多尺度测度实现多尺度的鲁棒的特征提取，然后结合ICP和提出的切向量对获得鲁棒的运动估计。由于采用了多尺度的数据结构，可以快速进行数据的搜索和关联，节省了迭代求解过程中的时间，进而实现本方法的实时运行。
[0005]多尺度特征提取的优点之一是可以获取不同层级的信息。几何特征的构建通常对邻域的大小十分敏感，尺度的大小不易控制。但是可以利用多尺度特征的数据结构构建有效的搜索空间或省略二次搜索过程，大大加快了数据关联的速度。
[0006]本专利技术通过以下技术方案予以实现：
[0007]基于ICP的激光雷达里程计方法，包括以下步骤：
[0008]步骤1：在大尺度上，对每一帧激光数据根据点的曲率大小将点云划分成平滑点和粗糙点作为特征点，并在前一帧数据找到对应点形成特征点对；
[0009]步骤2：在小尺度上，剔除噪声点和不稳定的点，保留边缘点和角点作为特征点，并在前一帧数据找到对应点形成特征点对；
[0010]步骤3：ICP算法用在大尺度上，对提取的特征点对求解六自由度的运动估计；
[0011]步骤4：基于小尺度上提取的特征点对计算的切向量对可以鲁棒地求解旋转自由度；
[0012]步骤5：结合步骤3和步骤4中的运动估计结果输出最终的里程计计算值。
[0013]进一步地，步骤1具体如下：
[0014]步骤1.1、对激光雷达数据中曲率κ1小于0.1的，提取为平滑点，并在上一帧中找到最近关联点，形成平滑点对。曲率计算方式如式(1)所示：
[0015][0016]式(1)中，κ1为计算的曲率，p
k
是第k个激光点，p
j
是点邻域内的点，邻域大小为设置为10。
[0017]步骤1.2、对激光雷达数据中曲率κ1大于0.1的，提取为粗糙点，并在上一帧中找到最近关联点，形成粗糙点对。曲率计算方式如式(1)所示；
[0018]步骤1.3、所述步骤1.1和步骤1.2中的平滑点对和粗糙点对，必须满足式(2)的约束条件，否则弃用点对：
[0019][0020]式(2)中，κ1和κ'1是点对中两点的曲率值，是阈值，设为0.005。
[0021]所述步骤2具体包括以下子步骤：
[0022]步骤2.1、对所述步骤1中的所有特征点对，计算切向量τ
k
、直线度l
k
和切向量的变化率κ2；
[0023]步骤2.2、对所述步骤1中的平滑点对，若当前帧的点满足l
k
＞1.5且κ2<0.5，则被分类成边缘点，并形成边缘点对；
[0024]步骤2.3、对所述步骤1中的粗糙点对，若当前帧的点满足l
k
<1且l
k
<1，则被分类成角点，并形成角点对；
[0025]步骤2.4、对所述步骤2.2和步骤2.3中的边缘点对和角点对，必须满足约束条件，否则弃用点对。
[0026]进一步地，步骤2具体如下：
[0027]步骤2.1、对所述步骤1中的点对，以式(3)计算切向量τ
k
，以式(4)计算直线度l
k
，以式(5)计算切向量的变化率κ2；
[0028][0029][0030][0031]式(3)、(4)、(5)中，p
k
是激光雷达的第k个测量点，p
j
是点邻域内的点，邻域大小为设置为2。τ
k
是第k个点的切向量，l
k
是第k个点的直线度，κ2是第k个点的切向量变化率。
[0032]步骤2.2、对所述步骤1中的平滑点对，若当前帧的点满足l
k
＞1.5且κ2<0.5，则被分类成边缘点，并形成边缘点对；
[0033]步骤2.3、对所述步骤1中的粗糙点对，若当前帧的点满足l
k
<1且l
k
<1，则被分类成角点，并形成角点对；
[0034]步骤2.4、对所述步骤2.2和步骤2.3中的边缘点对和角点对，必须满足式(6)、(7)
的约束条件，否则弃用点对。
[0035][0036][0037]式(6)中，l
k
和l'
k
是点对中两点的直线度，是阈值且设置为0.05。式(7)中，κ2和κ'2是点对中两点的切向量变化率，是阈值且设置为0.05。
[0038]进一步地，步骤3具体如下：
[0039]用所述步骤1中的点对作为输入，构建如式(8)的方程；
[0040][0041]其中，p
n
和p'
n
是点对，expξ^
→
M1是李代数到李群的映射，M1∈SE(3)是变换矩阵。
[0042]进一步地，步骤4具体如下：
[0043]用所述步骤2中的点对计算的切向量对作为输入，构建如式(9)的方程；
[0044][0045]其中，τ
m
和τ
m
'是切向量对，以式(3)的方法计算，expφ^
→
R2是李代数到李群的映射，R2∈SO(3)是旋转矩阵。
[0046]进一步地，步骤5具体如下：
[0047]所述步骤3和步骤4构成的运动估计可写成式(10)；
[0048][0049]其中式(8)和式(9)在求解过程中所用到的雅可比矩阵分别是式(11)和式(12)
[0050][0051][0052]两帧之间的刚体变换结果T
L
用式(本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于ICP的激光雷达里程计方法，其特征在于，所述方法包括以下步骤：步骤1：在大尺度上，对每一帧激光数据根据点的曲率大小将点云划分成平滑点和粗糙点作为特征点，并在前一帧数据找到对应点形成特征点对；步骤2：在小尺度上，剔除噪声点和不稳定的点，保留边缘点和角点作为特征点，并在前一帧数据找到对应点形成特征点对；步骤3：ICP算法用在大尺度上，对提取的特征点对求解六自由度的运动估计；步骤4：基于小尺度上提取的特征点对计算的切向量对可以鲁棒地求解旋转自由度；步骤5：结合步骤3和步骤4中的运动估计结果输出最终的里程计计算值。2.根据权利要求1所述基于ICP的激光雷达里程计方法，其特征在于，所述步骤2具体包括以下...

【专利技术属性】
技术研发人员：刘胜兰，李福星，赵雪冬，张文轩，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：