一种基于图优化的多工程激光SLAM点云拼接方法和系统技术方案

本发明专利技术公开一种基于图优化的多工程激光SLAM点云拼接方法和系统，其中，多工程激光SLAM点云拼接方法包括：以submap点云为关键帧进行单工程激光SLAM回环检测，得到单工程回环检测约束；若任意关联两组工程之间存在重叠点云，则拼接重叠点云处的submap点云得到多工程拼接约束；若submap点云存在GPS控制点，则根据submap点云关联的GPS控制点计算得到GPS控制点约束；使用单工程回环检测约束、多工程拼接约束和GPS控制点约束，建立submap点云的图优化模型；使用Ceres非线性优化库求解图优化模型，得到submap点云的位姿修正量，达到单工程SLAM点云非刚性位姿修正的目的。本发明专利技术技术方案能解决现有技术中多工程激光SLAM解算点云拼接时将单个工程视为一个刚体，整体不能达到最佳拼接一致性的问题。最佳拼接一致性的问题。最佳拼接一致性的问题。

【技术实现步骤摘要】
一种基于图优化的多工程激光SLAM点云拼接方法和系统


[0001]本专利技术涉及激光雷达
，尤其涉及一种基于图优化的多工程激光SLAM点云拼接方法和系统。

技术介绍

[0002]手持和背负式激光雷达扫描仪通常基于SLAM(Simultaneous Localization and Mapping，同步定位与地图构建)算法进行建图，通过生成密集的激光点云描述物体的三维物理特征，具有直观性和便利性，因此被广泛应用于建筑物三维建模、建筑立面量测、体积测量与方量量测等方面。在实际作业过程中，若测区范围较大，限于设备单次采集的点云数据大小限制，往往需要分开进行数据采集，然后对每次采集的点云数据单独进行SLAM数据解算，最后使用点云拼接算法将多组工程解算的点云拼接到一起，生成一个完整的点云。其中，多组工程之间的点云拼接算法是关键，多工程之间的点云拼接算法能够保证独立采集的单个工程点云拼接到整体后，不同工程的点云之间无分层现象。
[0003]经典的点云拼接算法是刚性匹配算法，刚性匹配算法假设待匹配点云是一个刚体，两个点云之间的相对位姿能够通过一个平移参数和一个旋转参数进行描述。常用的点云匹配算法主要有：(1)以ICP(Iterative ClosestPoint，迭代最近点)算法及其变体为代表的直接匹配法。经典的ICP算法迭代优化的代价函数是两帧点云中匹配点对的距离值，通过最小化匹配点对的距离值得到最优位姿变换，该ICP算法对距离值的初值精度非常敏感，对于精度较差的初值，其迭代可能会收敛到错误结果，且由于每次迭代需要重新匹配搜索最近的匹配点，该算法的时间开销较大，为一阶收敛。(2)以NDT(Normal Distribution Transform，整体分布变换)算法及其变体为代表的基于点云数学特性匹配法。NDT算法利用点云的高斯分布特性，通过最大化匹配点对的概率密度函数求得最优变换，不需要对每个点搜索对应的匹配点，提高了运算速度，对初值精度要求较ICP算法低。多层正态分布变换(ML
‑
NDT)算法将点云分为不同的层级单元，代价函数替换为马哈拉诺比斯距离，提高了匹配精度，但时间开销增加。
[0004]然而，受限于激光SLAM算法的基本原理，以上刚性匹配算法并不适用于两个激光SLAM解算点云之间的拼接。激光SLAM算法通过对序列帧点云进行相对匹配，得到帧间的相对位姿，最后累积相对位姿得到全局位姿。此阶段是激光里程计递推匹配，递推匹配会导致累积误差不断变大，使用此全局位姿解算点云时，由于帧间匹配精度不同，最后SLAM解算得到的整体点云局部间的精度不同，并不能视为一个刚体，若只用一个平移参数和一个旋转参数描述两组SLAM解算点云之间的相对位姿，会导致拼接后的点云局部一致性较好，但整体并不能达到最佳的拼接一致性。

技术实现思路

[0005]本专利技术提供一种基于图优化的多工程激光SLAM点云拼接方案，旨在解决现有技术提供的刚性匹配算法不适用于两个激光SLAM解算点云的拼接，整体点云不能视为一个刚
体，导致整体不能达到最佳的拼接一致性的问题。
[0006]为实现上述目的，根据本专利技术的第一方面，本专利技术提出了一种基于图优化的多工程激光SLAM点云拼接方法，包括：
[0007]拼接多帧激光点云得到submap点云，以submap点云为关键帧进行单工程激光SLAM回环检测，得到单工程回环检测约束；
[0008]判断多工程点云中任意关联两组工程之间是否存在重叠点云；
[0009]若判定任意关联两组工程之间存在重叠点云，则根据广义迭代最近点算法拼接重叠点云处的submap点云，得到多工程拼接约束；
[0010]判断submap点云是否存在GPS控制点；
[0011]若判定submap点云存在GPS控制点，则根据submap点云关联的GPS控制点计算得到GPS控制点约束；
[0012]使用单工程回环检测约束、多工程拼接约束和GPS控制点约束，建立submap点云的图优化模型；
[0013]使用Ceres非线性优化库求解图优化模型，得到submap点云的位姿修正量，使用位姿修正量修正多工程点云每一单工程的局部位姿。
[0014]优选的，上述多工程激光SLAM点云拼接方法中，以submap点云为关键帧进行单工程激光SLAM回环检测，得到单工程回环检测约束的步骤包括：
[0015]使用特征匹配算法匹配单工程点云中的帧间点云，得到帧间点云的相对位姿；
[0016]根据帧间点云的相对位姿拼接预定数量的帧间点云，得到submap点云；
[0017]根据submap点云的相对位姿，使用回环检测算法计算submap点云的回环检测误差，得到单工程回环检测约束。
[0018]优选的，上述多工程激光SLAM点云拼接方法中，使用特征匹配算法匹配单工程点云中的帧间点云，得到帧间点云的相对位姿的步骤包括：
[0019]根据点云计算公式：
[0020]P
i
＝R
·
P
i+1
+P
[0021]计算得到目标点云帧，其中，P
i+1
为待匹配点云帧，P
i
为目标点云帧，P为平移参数，R为旋转参数；
[0022]使用待匹配点云帧和目标点云帧，根据点到平面距离公式：
[0023][0024]和点到直线距离公式：
[0025][0026]分别计算得到点到平面距离和点到直线距离，其中，d
p
和d
e
分别表示点到平面距离以及点到直线距离，为待匹配点云帧中的特征点，和为目标点云帧中的特征点；
[0027]使用点到平面距离和点到直线距离，得到距离残差函数；
[0028]使用最小二乘法求解距离残差函数，得到帧间点云的相对位姿。
[0029]优选的，上述多工程激光SLAM点云拼接方法中，根据submap点云的相对位姿，使用回环检测算法计算submap点云的回环检测误差，得到单工程回环检测约束的步骤包括：
[0030]根据回环检测误差公式：
[0031][0032]计算得到回环检测误差，其中，T
i
和T
j
为回环检测关联的两帧submap点云的相对位姿，为回环检测误差；
[0033]使用回环检测误差，根据回环检测约束优化方程：
[0034][0035]计算得到单工程回环检测约束，其中，为回环约束协方差矩阵，F
l
为单工程回环检测约束。
[0036]优选的，上述多工程激光SLAM点云拼接方法，根据广义迭代最近点算法拼接重叠点云处的submap点云，得到多工程拼接约束的步骤，包括：
[0037]根据多工程拼接误差公式：
[0038][0039]计算得到多工程拼接误差，其中，T
i
和T
j
分别目标工程和待拼接工程相关联的两帧submap点云的位姿，为多本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于图优化的多工程激光SLAM点云拼接方法，其特征在于，包括：拼接多帧激光点云得到submap点云，以submap点云为关键帧进行单工程激光SLAM回环检测，得到单工程回环检测约束；判断多工程点云中任意关联两组工程之间是否存在重叠点云；若判定任意关联两组工程之间存在重叠点云，则根据广义迭代最近点算法拼接所述重叠点云处的submap点云，得到多工程拼接约束；判断所述submap点云是否存在GPS控制点；若判定所述submap点云存在GPS控制点，则根据所述submap点云关联的GPS控制点计算得到GPS控制点约束；使用所述单工程回环检测约束、所述多工程拼接约束和所述GPS控制点约束，建立所述submap点云的图优化模型；使用Ceres非线性优化库求解所述图优化模型，得到所述submap点云的位姿修正量，使用所述位姿修正量修正所述多工程点云每一单工程的局部位姿。2.根据权利要求1所述的多工程激光SLAM点云拼接方法，其特征在于，以submap点云为关键帧进行单工程激光SLAM回环检测，得到单工程回环检测约束的步骤，包括：使用特征匹配算法匹配单工程点云中的帧间点云，得到所述帧间点云的相对位姿；根据所述帧间点云的相对位姿拼接预定数量的所述帧间点云，得到所述submap点云；根据所述submap点云的相对位姿，使用回环检测算法计算所述submap点云的回环检测误差，得到所述单工程回环检测约束。3.根据权利要求2所述的多工程激光SLAM点云拼接方法，其特征在于，所述使用特征匹配算法匹配单工程点云中的帧间点云，得到所述帧间点云的相对位姿的步骤，包括：根据点云计算公式：P
i
＝R
·
P
i+1
+P计算得到目标点云帧，其中，P
i+1
为待匹配点云帧，P
i
为目标点云帧，P为平移参数，R为旋转参数；使用所述待匹配点云帧和所述目标点云帧，根据点到平面距离公式：和点到直线距离公式：分别计算得到点到平面距离和点到直线距离，其中，d
p
和d
e
分别表示点到平面距离以及点到直线距离，为待匹配点云帧中的特征点，和为目标点云帧中的特征点；使用所述点到平面距离和点到直线距离，得到距离残差函数；使用最小二乘法求解所述距离残差函数，得到所述帧间点云的相对位姿。4.根据权利要求2所述的多工程激光SLAM点云拼接方法，其特征在于，所述根据所述submap点云的相对位姿，使用回环检测算法计算所述submap点云的回环检测误差，得到所
述单工程回环检测约束的步骤，包括：根据回环检测误差公式：计算得到回环检测误差，其中，T
i
和T
j
为回环检测关联的两帧submap点云的相对位姿，为回环检测误差；使用所述回环检测误差，根据回环检测约束优化方程：计算得到所述单工程回环检测约束，其中，为回环约束协方差矩阵，F
l
为单工程回环检测约束。5.根据权利要求1所述的多工程激光SLAM点云拼接方法，其特征在于，所述根据广义迭代最近点算法拼接所述重叠点云处的submap点云，得到多工程拼接约束的步骤，包括：根据多工程拼接误差公式：计算得到多工程拼接误差，其中，T
i
和T
j
分别目标工程和待拼接工程相关联的两帧sub...

【专利技术属性】
技术研发人员：请求不公布姓名，
申请(专利权)人：武汉绿土图景科技有限公司，
类型：发明
国别省市：