一种基于球面投影特征的激光SLAM方法及系统技术方案

本发明专利技术公开了一种基于球面投影特征的激光SLAM方法及系统，该方法包括以下三个步骤：特征提取、前端里程计及后端建图。首先，特征提取模块用于提取激光点云投影在球面平面的平面和直线特征；前端里程计根据当前帧所提取特征进行位姿估计；后端建图则将局部地图与全局地图进行匹配,从而精确估计激光雷达在世界坐标系中的位姿。本发明专利技术的有益效果在于：特征提取算法具有优秀的准确性和鲁棒性，克服了激光雷达巨大数据量下的实时性不足问题，同时算法对旋转式激光雷达和固态激光雷达具有通用性。对旋转式激光雷达和固态激光雷达具有通用性。对旋转式激光雷达和固态激光雷达具有通用性。

A laser slam method and system based on spherical projection feature

【技术实现步骤摘要】
一种基于球面投影特征的激光SLAM方法及系统


[0001]本专利技术涉及计算机视觉定位、移动机器人运动轨迹领域，具体涉及一种基于球面投影特征的激光SLAM方法及系统。

技术介绍

[0002]我国海岸线绵长，建立海岸带地理模型能够有助于国土空间规划，沿海带的可持续开发对我国的经济社会发展具有重大意义。近年来，对海岸带进行考察测绘，建立海岸带数据库逐渐成为研究热点。然而，复杂的沿海环境使得传统的测绘手段难以奏效，无法获得精确的海岸地理信息。基于激光雷达的测量受环境影响小、数据获取快速、测量精度高，能够获得精准高密度的高程数据，实时建立地形三维空间模型。
[0003]同步定位和建图(Simultaneous Localization and Mapping，简称SLAM)不仅能够实时提供关于智能体自身位置的信息，还能够实时绘制周围环境的三维地图。在海洋考察中，机载LiDAR SLAM系统能够进行地形测绘、环境三维建模等，对于提取海岸带地理信息有极大的优势，是海岸带考察的重要手段，具有广泛的应用前景。
[0004]基于LiDAR的SLAM算法的核心是点云配准。点云配准旨在获取两个3D点云之间空间变换关系，使得变换后的3D点云尽可能重合。基于特征的激光SLAM方法使用点云中关于场景的几何特征，进而对特征点进行进一步处理。由于传统激光雷达在水平方向具有较高的分辨率，而在垂直方向上则要差得多，使得多数算法都采用了基于单个扫描线的特征提取方案。然而，单个扫描线是并不具备完整的平面和直线结构，导致提取的特征并不准确，包含较多的误分类的点，影响了算法的鲁棒性和精度。

技术实现思路

[0005]针对现有技术中存在的上述技术问题，本专利技术提出了一种基于球面投影特征的激光SLAM方法，设计合理，克服了现有技术的不足，具有良好的效果。
[0006]为了实现上述目的，本专利技术采用如下技术方案：
[0007]一种基于球面投影特征的激光雷达SLAM方法，包括以下三个步骤：
[0008]S1特征提取，根据当前帧的点云数据进行基于球面投影的统一点云映射并提取平面特征和直线特征；
[0009]S2前端里程计，根据相邻两帧点云数据所提取的特征估计两帧激光雷达的位姿变换；
[0010]S3后端建图，将累计到一定帧数目的点云数据融入全局地图，以获得激光雷达在世界坐标系下的精确位姿。
[0011]需要说明的是，所述步骤S1中，还包括
[0012]S1.1将当前帧激光点云数据投影到一个球面平面上；获取雷达点云中每一个点p的投影坐标；
[0013]S1.2根据步骤S1.1得到的二维球面图像进行平面特征提取；
[0014]S1.3根据步骤S1.2提取的平面点进行直线特征提取。
[0015]需要说明的是，所述步骤S1.1中，还包括
[0016]S1.1.1对于传统旋转式激光雷达，其投影坐标可通过下式计算：
[0017][0018]其中：t0是当前帧的起始时间戳，α是一个由激光雷达分辨率计算得到的参数；
[0019]S1.1.2对于固态激光雷达，基于下式计算点p的极坐标为其球面投影坐标：
[0020][0021]其中：是点p到原点的距离，[f
left
,f
right
],[f
down
,f
up
]分别是激光雷达的垂直和水平视野，M及N分别是投影图像的宽度和高度。
[0022]需要说明的是，所述步骤S1.2中，还包括
[0023]S1.2.1基于下式对最优平面的点法式参数进行估计：
[0024][0025]其中：π是球面图像中5
×
5的正方形窗口，c
π
是最优平面上的一个点，n
π
是最优平面的法向量；
[0026]S1.2.2基于下式计算出点p与最优平面的距离d
p
：
[0027][0028]S1.2.3筛选出具有较低距离的点集由中所有点与最优平面的距离的平均值代表窗口π的平滑度τ
π
，在包含点p的多个窗口中，根据最低平滑度的窗口π
*
指定p的平滑度和法向量再以一定的平滑度阈值选出平面点；
[0029]S1.2.4对平面点基于图进行分组，将满足下式条件的平面点加入一个边：
[0030][0031]其中，p1，p2代表球面图像中相邻的平面点，n1和n2分别是p1和p2的法线。
[0032]需要说明的是，所述步骤S1.3中，还包括
[0033]S1.3.1根据以下三种情况筛选直线点：
[0034]相邻两点p1，p2是平面特征且具有不同的平面特征标记，p1为直线点的条件可基于下式：
[0035][0036]相邻两点p1，p2深度具有显著跳变，p1为直线点的条件可基于下式：
[0037]||p2||2‑
||p1||2＞1；
[0038]存在同一行或列的相邻点p1，p2及p3，p1为直线点的条件可基于下式：
[0039][0040]其中：是点p
i
的法向量，上标i用于区分不同的点；
[0041]S1.3.2通过下采样得到新点云S，并使用KD
‑
Tree找到每个点的两个相邻点，组合成三元组，则每个三元组的方向向量n
L
和平滑度τ
L
可基于下式规定：
[0042][0043]其中：p1为中心点，p2及p3为其两个相邻点；
[0044]S1.3.3点p的法向量n
L*
和平滑度τ
L*
由具有最低平滑度的三元组L
*
指定，并以一定阈值选出直线点；
[0045]S1.3.4基于KD
‑
Tree搜索直线点的邻接关系，将满足下式条件的直线点加入一个边：
[0046][0047]其中：n1，n2是两个直线点的方向向量或法向量。
[0048]需要说明的是，在步骤S2中，还包括
[0049]S2.1将当前帧点云数据中的直线特征点集ε
k+1
和平面特征点集重投影至扫描起始点坐标系下，形成重投影点集及
[0050]S2.2针对当前帧点云数据中的直线特征点集合对于对于通过搜索当前帧与上一帧中的最相邻两点j,l，形成匹配线，并基于以下公式计算直线特征点i至匹配线的距离d
ε
：
[0051][0052]其中：及为i，j，l在坐标系L下的坐标；
[0053]S2.3针对当前帧点云数据中的平面特征点集合对于对于通过搜索当前帧与上一帧中的最相邻三点j,l,m，形成匹配面，并基于以下公式计算平面特征点i至
匹配面的距离：
[0054][0055]其中：为i，j，l及m在坐标系L下的坐标；
[0056]S2.4基于列文伯格
‑
马夸尔本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于球面投影特征的激光雷达SLAM方法，其特征在于，包括以下三个步骤：S1特征提取，根据当前帧的点云数据进行基于球面投影的统一点云映射并提取平面特征和直线特征；S2前端里程计，根据相邻两帧点云数据所提取的特征估计两帧激光雷达的位姿变换；S3后端建图，将累计到一定帧数目的点云数据融入全局地图，以获得激光雷达在世界坐标系下的精确位姿。2.根据权利要求1所述的基于球面投影特征的激光雷达SLAM方法，其特征在于，所述步骤S1中，还包括S1.1将当前帧激光点云数据投影到一个球面平面上；获取雷达点云中每一个点p的投影坐标；S1.2根据步骤S1.1得到的二维球面图像进行平面特征提取；S1.3根据步骤S1.2提取的平面点进行直线特征提取。3.根据权利要求2所述的基于球面投影特征的激光雷达SLAM方法，其特征在于，所述步骤S1.1中，还包括S1.1.1对于传统旋转式激光雷达，其投影坐标可通过下式计算：其中：t0是当前帧的起始时间戳，α是一个由激光雷达分辨率计算得到的参数；S1.1.2对于固态激光雷达，基于下式计算点p的极坐标为其球面投影坐标：其中：是点p到原点的距离，[f
left
,f
right
],[f
down
,f
up
]分别是激光雷达的垂直和水平视野，M及N分别是投影图像的宽度和高度。4.根据权利要求2所述的基于球面投影特征的激光雷达SLAM方法，其特征在于，所述步骤S1.2中，还包括S1.2.1基于下式对最优平面的点法式参数进行估计：其中：π是球面图像中5
×
5的正方形窗口，c
π
是最优平面上的一个点，n
π
是最优平面的法向量；S1.2.2基于下式计算出点p与最优平面的距离d
p
：
S1.2.3筛选出具有较低距离的点集由中所有点与最优平面的距离的平均值代表窗口π的平滑度τ
π
，在包含点p的多个窗口中，根据最低平滑度的窗口π
*
指定p的平滑度和法向量再以一定的平滑度阈值选出平面点；S1.2.4对平面点基于图进行分组，将满足下式条件的平面点加入一个边：其中，p1，p2代表球面图像中相邻的平面点，n1和n2分别是p1和p2的法线。5.根据权利要求2所述的基于球面投影特征的激光雷达SLAM方法，其特征在于，所述步骤S1.3中，还包括S1.3.1根据以下三种情况筛选直线点：相邻两点p1，p2是平面特征且具有不同的平面特征标记，p1为直线点的条件可基于下式：相邻两点p1，p2深度具有显著跳变，p1为直线点的条件可基于下式：||p2||2‑
||p1|...

【专利技术属性】
技术研发人员：谭晓军，陈楠杰，王锦萍，张乐天，介璐，
申请(专利权)人：中山大学，
类型：发明
国别省市：