融合点云强度的三维激光雷达同步建图与定位方法及系统

1.本发明涉及同步定位与地图绘制技术领域，更具体地，涉及融合点云强度的三维激光雷达同步建图与定位方法及系统。


背景技术：

2.即时定位与构图技术(slam)是指机器人首次进入缺乏全球卫星定位、环境完全未知的场景内时，基于自身携带的传感器采集周围环境数据，构建全局地图并实现自身定位。基于slam算法理论基础的不同，slam技术可分为基于滤波的slam算法与基于图优化的slam算法。其中基于滤波的slam算法以概率论为基础，根据机器人的控制输入和传感器的观测值，以最大概率估计机器人的位姿并实现建图。该方法计算量较大，很难保证实时性。基于图优化的slam算法将机器人的位姿、观测和控制输入转化为图论中的顶点和边，从而将slam问题转化为优化问题。基于图优化的slam算法具有更优的建图效果与定位精度，是目前研究的重点。
3.而基于所使用传感器的不同，slam可分为视觉slam(visual slam)与激光slam(lidar slam)。视觉visual slam通过摄像机采集图像信息作为数据源，具有信息丰富、成本低、重量轻、体积小等优点。而由于摄像机易受环境(光照、气候、目标材质等因素)影响且不易提取目标三维信息，复杂变化或特征匮乏的视觉环境将会对visual slam系统造成影响甚至使得系统失效。激光lidar slam通过激光雷达采集点云信息作为数据源，激光雷达点云数据包含丰富的信息，如x，y，z坐标信息、回波次数、强度信息等，其中3d激光雷达具有更为丰富的空间信息。且激光雷达具有较强的环境适应性，能够在更加多样的场景下实现建图与定位。
4.传统的lidar slam算法基于点云数据的几何特征进行帧间匹配，从而实现激光雷达位姿的估计。该方法在几何特征丰富的场景中，具有较高的定位精度，而在几何特征匮乏的开阔环境中，lidar slam建图效果与定位精度都将受到影响。同时，传统的lidar slam算法在后端回环优化模块，常基于里程计与历史轨迹位姿进行回环检测，而由于漂移误差的累计，该回环检测方法准确率较低且鲁棒性较差。
5.激光雷达所采集的点云数据不仅具有x,y,z三维坐标信息，还具有强度信息。点云强度信息与周围目标反射率相一致，对于不同的目标，其对应的点云强度也不一样，可通过点云强度来实现目标提取，特征匹配等功能。因此，在传统lidar slam框架的基础上融合点云强度对于提高现有算法的建图效果与定位精度是可行的。


技术实现要素：

6.本发明针对现有技术中存在的技术问题，发明提出了融合点云强度的三维激光雷达同步建图与定位方法及系统。在几何特征匮乏的场景中，它从点云数据中分别提取点云几何特征与强度特征，将强度特征作为几何特征的补充来提高特征丰富度。同时本发明基于点云强度提出了一种新的全局描述子，用以提高后端回环优化的性能。
7.根据本发明的第一方面，提供了一种融合点云强度的三维激光雷达同步建图与定位方法，包括以下步骤：
8.步骤1，对采集的点云数据进行预处理，得到预处理之后的点云数据；
9.步骤2，将预处理之后的点云数据进行2d投影，得到点云深度图与点云强度图；
10.步骤3，将预处理之后的点云数据进行地面点与非地面点分割，并基于点云深度图与点云强度图提取点云几何特征与强度特征；
11.步骤4，基于提取出的点云几何特征与强度特征，适时将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合，得到当前的位姿；
12.步骤5，基于预处理之后点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征；并基于全局描述子点云强度投影形状特征进行回环检测，并对回环检测的结果进行验证；
13.步骤6，将验证通过的回环检测结果当作约束添加进因子图中，借助因子图中对历史轨迹位姿与点云地图进行优化，得到优化后的轨迹位姿与全局地图。
14.在上述技术方案的基础上，本发明还可以作出如下改进。
15.可选的，在步骤1中，所述对采集的点云数据进行预处理包括：
16.步骤1.1.通过3d lidar对周围环境进行信息采集，得到原始点云数据；
17.步骤1.2.然后对原始点云数据进行条件滤波,设置最远距离与最大高度，将超出范围的点云数据滤除掉；
18.步骤1.3.对条件滤波后的点云数据进行强度校准。
19.可选的，在步骤2中，所述将预处理之后的点云数据进行2d投影包括：对于预处理后的点云数据，基于其水平方向角分辨率与垂直角分辨率，将其投影进2d深度图与2d强度图，其中2d深度图保存的为点云到激光雷达中心的距离，2d强度图保存的为对应点云的强度值。
20.可选的，在步骤2中，所述将预处理之后的点云数据进行2d投影包括：
21.步骤2.1.将预处理后的点云数据投影进2d深度图与2d强度图，其中2d深度图保存的为点云到激光雷达中心的距离，2d强度图保存的为对应点云校核后的强度值。
22.可选的，在步骤3中，所述基于点云深度图与点云强度图提取点云几何特征与强度特征包括：
23.步骤3.1.对强度校准后的点云数据进行地面点与非地面点的分割，赋予点云数据地面点、非地面点与离散点的标签；
24.步骤3.2.基于点云数据的2d深度图与2d强度图分别计算点云几何光滑特征与强度光滑特征；
25.步骤3.3.基于几何光滑特征的结果，在地面点云中提取点云几何平面特征点，在非地面点中提取点云几何边缘特征点；
26.步骤3.4.基于强度光滑特征的结果，在地面点云中提取点云强度平面特征点，在非地面点中提取点云强度边缘特征点。
27.可选的，在步骤4中，所述将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合包括：
28.步骤4.1.基于所提取几何特征的数量判断环境中几何特征的丰富度；
29.步骤4.2.若几何特征较为丰富，则仅基于几何特征进行相邻帧点云的匹配与优化，得到相邻帧间激光雷达的相对位姿变换；
30.步骤4.3.若几何特征较为匮乏，则另基于强度特征进行相邻帧点云的匹配与优化，将得到的匹配结果与基于几何特征的匹配结果进行定权融合。
31.可选的，在步骤5中，所述基于预处理之后点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征包括：
32.步骤5.1.将强度校准后的点云投影到以激光雷达为中心的柱面上，定义激光雷达获取的一帧点云数据和其中的每个激光点，将一帧点云数据投影到极坐标系下；
33.步骤5.2.将柱面投影后激光点云按水平方向角均匀划分为ns个扇区，按其高度均匀划分为n
p
个块区，则将所述一帧点云数据划分进ns*n
p
个子空间；
34.步骤5.3.对于每一个子空间，提取点云强度的最大值构成点云强度矩阵i；
35.步骤5.4.统计具有相同水平方向角区间的点云的密度分度，计算点云密度权重d
ij
；
36.步骤5.5.将点云密度权重与点云矩阵相乘，构建基于点云强度柱面投影的全局描述子点云强度投影形状特征。
37.可选的，在步骤5中，所述基于全局描述子点云强度投影形状特征进行回环检测，并对回环检测的结果进行验证包括：
38.步骤5.6.在获得全局描述子点云强度投影形状特征后，提取该描述子行向量与列向量；其中，行向量与列向量中元素分别为描述子矩阵对应列与对应行的元素的均值；
39.步骤5.7.基于所述列向量进行候选位置搜索，将列向量保存为vector类格式的kd树，基于当前帧描述子矩阵列向量与历史帧列向量之间欧氏距离的大小，在kd树中进行索引，搜索出n1个欧式距离最小的列向量对应点云帧作为候选帧；
40.步骤5.8.在n1个候选帧中，基于行向量进行相对角度的初步计算，计算当前帧与候选帧的描述子矩阵的余弦距离；将行向量计算所得移位th当作初值，在其附近n
th
列范围内按列进行移位并计算两个描述子之间的余弦距离；
41.步骤5.9.在基于描述子检测到回环后，首先进行时间一致性检验，基于检测到的当前帧与候选帧的朝向，分别计算其附近n帧点云对应描述子的余弦距离；
42.步骤5.10.在时间一致性检验之后，将基于描述子匹配计算的相对位姿当作初值，通过直接匹配算法或特征匹配算法进行几何一致性检验，若几何一致性检验得分小于阈值，则认为当前回环检测结果是准确的。
43.可选的，在步骤6中，所述将验证通过的回环检测结果当作约束添加进因子图中，借助因子图中对历史轨迹位姿与点云地图进行优化，得到优化后的轨迹位姿与全局地图包括：
44.步骤6.1.基于当前帧与前一帧的相对位姿变换以及前一帧在全局地图的位姿，估算当前帧在全局地图中的位姿；
45.步骤6.2.基于里程计的结果提取当前帧附近的点云数据构建局部点云地图，将当前帧与局部点云地图匹配，并将当前帧点云数据添加进地图中，从而实现点云地图更新；
46.步骤6.3.将计算得到的每帧激光雷达的位姿作为因子图的节点，将相邻帧间激光雷达的相对位姿变换关系作为因子图的边，将回环检测的结果当作新的约束添加进因子图
中，实现后端图优化，从而得到优化后的轨迹位姿与点云地图。
47.根据本发明的第二方面，提供一种融合点云强度的三维激光雷达同步建图与定位系统，包括：
48.点云数据预处理模块，用于对采集的点云数据进行预处理，得到预处理之后的点云数据；
49.点云数据计算模块，用于将预处理之后的点云数据进行2d投影，得到点云深度图与点云强度图；将预处理之后的点云数据进行地面点与非地面点分割，并基于点云深度图与点云强度图提取点云几何特征与强度特征；基于提取出的点云几何特征与强度特征，适时将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合，得到当前的位姿；
50.回环检测模块，用于基于预处理之后点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征；并基于全局描述子点云强度投影形状特征进行回环检测，并对回环检测的结果进行验证；
51.点云数据优化模块，用于将将验证通过的回环检测结果当作约束添加进因子图中，借助因子图中对历史轨迹位姿与点云地图进行优化，得到优化后的轨迹位姿与全局地图。
52.本发明的技术效果和优点：
53.本发明提出了融合点云强度的三维激光雷达同步建图与定位方法及系统，在提取点云数据的强度特征并基于强度特征进行帧间匹配，提高特征匹配的精度；同时基于点云强度柱面投影构建全局描述子点云强度投影形状特征用于回环检测，并设置一致性检验来保证回环检测的准确度，从而在几何特征匮乏的环境下实现高精度的建图与定位。本发明方案实施简单方便，实用性强，解决了相关技术存在的实用性低及实际应用不便的问题，能够提高用户体验，具有重要的市场价值。
附图说明
54.图1为本发明实施例提出的融合点云强度的三维激光雷达同步建图与定位方法流程图；
55.图2为本发明实施例基于全局描述子点云强度投影形状特征回环检测框架图。
具体实施方式
56.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
57.近年来，随着计算机与传感器技术的进步，智能移动机器人技术得到快速发展，越来越多的行业开始尝试使用智能移动机器人来代替人工的新运作模式，智能移动机器人需完成的任务包括物流配送、自动清洁、智能送餐、安全巡检等。随着智能机器人任务的越发复杂与多样，对其定位的要求越来越高，高精度的定位是智能机器人实现复杂任务的关键技术之一。
58.在室内场景中，可在场景内设置锚点，通过测量机器人到各个锚点的距离来计算
机器人的位姿。常见的室内定位方法有超宽带(ultra-wide band，uwb)定位、wi-fi定位、zigbee定位或蓝牙(bluetooth)定位等。该定位方法仅适用于室内等小范围场景且需要依赖外部传感器，不具有普适性。
59.在室外场景中，智能机器人常见的定位方法主要有全球导航卫星系统(global navigation satellite system，gnss)与惯性导航系统(inertial navigation system，ins)融合定位(gnss/ins)；基于先验地图的定位技术；即时定位与构图技术(simultaneous localization and mapping,slam)。gnss/ins组合导航定位继承了ins的连续性、高采样率和完备的导航参数(速度、位置、姿态)，同时结合gnss的位置信息实现了长时间高精度定位，具有较高的定位精度，但在很多复杂场景下(比如城市峡谷中、树荫下、高架桥下等区域)，gnss定位精度会快速退化，导致gnss/ins组合导航定位方法失效；基于先验地图的定位技术是指在机器人进入该场景前，已经得到该场景某格式的地图，通过机器人装配的传感器将在线获取的数据与先验地图进行链接，从而获得机器人当前的位姿，但该方法要求机器人事先已获取某种格式的地图。
60.传统的lidar slam算法基于点云数据的几何特征进行帧间匹配，从而实现激光雷达位姿的估计。该方法在几何特征丰富的场景中，具有较高的定位精度，而在几何特征匮乏的开阔环境中，lidar slam建图效果与定位精度都将受到影响。同时，传统的lidar slam算法在后端回环优化模块，常基于里程计与历史轨迹位姿进行回环检测，而由于漂移误差的累计，该回环检测方法准确率较低且鲁棒性较差。
61.针对现有lidar slam算法在几何特征匮乏的场景下，建图效果与定位精度降低的问题，参见图1，本发明实施例提供了融合点云强度的三维激光雷达同步建图与定位方法，包括以下步骤：
62.s1：对采集的点云数据进行预处理，得到预处理之后的点云数据；
63.具体地，所述对采集的点云数据进行预处理包括以下子步骤：
64.步骤1.1.通过3d lidar对周围环境进行信息采集，得到原始点云数据；
65.步骤1.2：对原始点云数据进行条件滤波，设置最远距离l
max
与最大高度h
max
，将超出范围的点云数据滤除掉，滤除离散点与不可靠的点；
66.步骤1.3：对条件滤波后的点云进行强度校准；激光雷达采集点云数据的度值主要与目标表面反射率、入射角度以及测量距离有关，强度值i定义为：
[0067][0068]
式中，r为激光雷达中心距离目标的距离，ρ为目标表面反射率，α为激光束与目标表面的入射角，η
all
为常量系数，包含大气透射系数。系统透射系数等。一般激光雷达在出厂前已经进行了测量距离与点云强度之间的校准，因此使用者只需要点云强度进行其与入射角的校准。点云强度与入射角之间的关系为从而使点云强度与物体表面反射率保持一致。
[0069]
需要进行说明的是，对原始点云数据进行预处理还包括：点云数据体素采样，用来降低点云数据量；点云强度降准，消除入射角度对点云强度值的影响；以及运动畸变补偿，消除相对运动对点云测量带来的误差，从而得到预处理之后的点云数据。
[0070]
s2，将预处理之后的点云数据进行2d投影，得到点云深度图与点云强度图；
[0071]
具体地，所述将预处理之后的点云数据进行2d投影包括：
[0072]
步骤2.1.将预处理之后的点云数据进行2d投影，得到点云深度图与点云强度图。对于预处理后的点云数据，基于其水平方向角分辨率与垂直角分辨率，将其投影进2d深度图与2d强度图，其中2d深度图保存的为点云到激光雷达中心的距离，2d强度图保存的为对应点云的强度值。从而得到2d的点云深度图与点云强度图，用于后续点云分割与特征提取。
[0073]
s3，将点云数据进行地面点与非地面点分割，并基于点云深度图与点云强度图提取点云几何特征与强度特征；
[0074]
具体地，所述基于点云深度图与点云强度图提取点云几何特征与强度特征包括：
[0075]
步骤3.1.对强度校准后的点云数据进行地面点与非地面点的分割，赋予点云数据地面点、非地面点与离散点的标签；
[0076]
步骤3.2.基于点云数据的2d深度图与2d强度图分别计算点云几何光滑特征与强度光滑特征；
[0077]
步骤3.3.基于几何光滑特征的结果，在地面点云中提取点云几何平面特征点，在非地面点中提取点云几何边缘特征点；
[0078]
步骤3.4.基于强度光滑特征的结果，在地面点云中提取点云强度平面特征点，在非地面点中提取点云强度边缘特征点。
[0079]
更一步地，在步骤3.2中，计算点云平滑特征提取点云几何光滑特征与强度光滑特征，点云平滑特征被定义为：
[0080][0081]
其中cr与ci分别为点云深度曲率与强度曲率，s为用于计算该点平滑特征的点集，ri与ii分别为点云对应深度值与强度值。根据曲率值的大小，将点云分为平面点与边缘点。从具有地面点标签的点云中提取具有最小ci的个强度平面特征点，并从中提取ci最小的个强度平面特征点，从具有非地面点标签的点云中提取具有最大ci的个强度边缘特征点，并从中提取ci最大的个强度边缘特征点。同理可以从具有地面点标签的点云中提取具有最小cr的个几何平面特征点，并从中提取cr最小的个几何平面特征点，从具有非地面点标签的点云中提取具有最大cr的个几何边缘特征点，并从中提取cr最大的个几何边缘特征点，得到点云几何边缘点、几何平面点、强度边缘点、强度平面点的数量
[0082]
s4，基于提取出的点云几何特征与强度特征，适时将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合；
[0083]
所述将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合包括：
[0084]
步骤4.1.基于所提取几何特征的数量判断环境中几何特征的丰富度；
[0085]
步骤4.2.若几何特征较为丰富，则基于几何特征进行相邻帧点云的匹配与优化，得到相邻帧间激光雷达的相对位姿变换；
[0086]
步骤4.3.若几何特征较为匮乏，则另基于强度特征进行相邻帧点云的匹配与优
化，将得到的匹配结果与基于几何特征的匹配结果进行定权融合。
[0087]
所述将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合包括以下步骤：
[0088]
具体地，在步骤4.2中，基于几何特征进行相邻帧点云的匹配与优化，得到相邻帧间激光雷达的相对位姿变换包括：
[0089]
从上一帧点云几何边缘特征点中搜索距离当前帧点云几何边缘特征点最近的特征点，计算其相对变换并进行lm优化，得到相邻两帧间yaw角的变换与x,y方向的位移，即同理从上一帧点云平面边缘特征点中搜索距离当前帧点云几何平面特征点最近的特征点，计算相邻两帧间roll与pitch的变换及z方向的位移，即从而得到相邻两帧间六自由度的变换，即
[0090]
设置阈值分别判断激光雷达周围场景中几何边缘特征点的丰富度与几何平面特征点的数目。对于不同数量的几何平面点分别进行不同的强度特征匹配与融合策略，最终相邻两帧间变换[t
x
，ty，tz，θ
roll
，θ
pitch
，θ
yaw
]为:
[0091][0092]
其中：为通过点云强度特征匹配得到的帧间匹配结果。
[0093]
本实施步骤可以实时基于场景中几何特征的丰富度来提取点云强度特征，并将几何特征匹配与强度特征匹配结果进行自适应定权融合，以提高特征匹配的精度。
[0094]
s5，基于预处理之后点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征；并基于全局描述子点云强度投影形状特征进行回环检测，并对回环检测的结果进行验证；
[0095]
需要进行说明的是，全局描述子为一种用于描述三维点云数据特征的方法，通过提取点云数据中的全局特征信息来表示这帧点云数据；点云强度投影形状特征全称为(intensity cylindrical projection shape context，用icpsc表示)，是通过点云数据强度值柱面投影构建的一种形状特征描述子。
[0096]
具体实施时，参见图2，步骤s5包括以下子步骤：
[0097]
步骤5.1.将强度校准后的点云投影到以激光雷达为中心的柱面上，定义激光雷达
获取的一帧点云数据为p＝{p1，p2，p3…
pn}，定义其中每个激光点为pk＝[xk，yk，zk，ηk]。其中xk，yk，zk为对应激光点在以激光雷达为中心的笛卡尔坐标系下的三维坐标，ηk为对应激光点的强度值。首先将一帧点云数据投影到极坐标系下：
[0098][0099]
其中，ρk为对应激光点距离激光雷达主光轴的距离，θk为对应激光点的水平方向角。
[0100]
步骤5.2.将柱面投影后激光点云按水平方向角均匀划分为ns个扇区，按其高度均匀划分为n
p
个块区，则将一帧点云数据划分进ns*n
p
个子空间。
[0101][0102]
其中，δs为角度区间的大小，δp为垂直高度区间的大小，h为所设置的提取描述子范围的高度阈值，可以通过激光雷达最远扫描距离与其结构最大垂直方向角确定h＝h
install
+l
max
·
tanβ。其中h
install
为激光雷达的安装高度，l
max
为激光雷达的最远扫描距离，β为激光雷达的最大方向角。
[0103]
步骤5.3.将每个子空间定义为s
ij
，其中每个子空间中点云分布为：
[0104][0105]
其中，i∈[|1，n
p
|]，j∈[|1，ns|]。
[0106]
对于每一个子空间，提取其点云强度的最大值构成点云强度矩阵i。强度矩阵i中对应元素的大小为若则令i
ij
＝0。
[0107]
步骤5.4.统计具有相同水平方向角区间的点云的密度分度，计算其点云密度权重d
ij
，点云密度加权值d
ij
被定义为：
[0108][0109]
其中，median为具有相同水平角度的子空间中点云数量的中位数，num
ij
为对应子空间点云数目。
[0110]
步骤5.5.将点云密度权重与点云矩阵相乘，构建基于点云强度柱面投影的全局描述子点云强度投影形状特征，定义为描述子矩阵中每个元素的值为：
[0111][0112]
其中为向下取整函数。
[0113]
步骤5.6.在获得全局描述子点云强度投影形状特征后，提取该描述子行向量与列向量其中行向量与列向量中元素分别为描述子矩阵对
应列与对应行的元素的均值，定义为：
[0114][0115]
式中ri为行向量对应元素值，ci为列向量对应元素值。
[0116]
步骤5.7.在回环检测阶段，首先基于列向量进行候选位置搜索。由于描述子列向量具有旋转不变性，历史列向量保存为vector类格式的kd树，基于当前帧icpsc描述子矩阵列向量与历史帧列向量之间欧氏距离的大小，在kd树中进行索引，搜索出n1个欧式距离最小的列向量对应点云帧作为候选帧。
[0117]
在本实施例中，vector类是c++stl中的一个容器类，用于实现动态数组等功能；kd树是一种用于高维空间数据结构的数据结构，可以用于快速地进行最近邻搜索和范围搜索等操作；
[0118]
步骤5.8.在n1个候选帧中，基于行向量进行相对角度的初步计算。icpsc描述子行向量值的大小不受旋转的影响，但其值的分布受激光雷达旋转的影响，且其值的位移与激光雷达的旋转角度相一致。因此可通过行向量来快速计算当前帧与候选帧之间的相对角度。
[0119][0120][0121]
其中rc为当前帧的行向量，rq为候选帧的行向量，为当前帧行向量进行了th位的平移，可以根据th的大小。为当前帧点云构建的icpsc描述子矩阵，为候选帧点云构建的icpsc描述子矩阵，为当前帧与候选帧点云基于行向量匹配的结果。可以根据的大小设置阈值进一步保留n2个候选帧。
[0122]
并计算当前帧与候选帧的icpsc描述子矩阵的余弦距离，得到更为准确的相对角度。将行向量计算所得移位th当作初值，在其附近n
th
列范围内按列进行移位并计算两个描述子之间的余弦距离。
[0123][0124][0125]
式中，为对应描述子矩阵的第i列向量，为icpsc描述子矩阵按列进行了d位的移位，式中有可能出现(th-nth)＜0或(th+nth)＞ns的情况，但icpsc描述子是按照水平方向角进行展开，其第一列与最后一列相邻。为计算的最小余弦距离，此时基于位移量d得到更为准确的相对角度。
[0126]
步骤5.9.在基于icpsc描述子检测到回环后，通过一致性检验模块验证回环检测
结果是否准确。首先进行时间一致性检验，基于检测到的当前帧与候选帧的朝向，分别计算其附近n帧点云对应描述子的余弦距离。
[0127][0128]
其中p(pc，pq)为当前帧pc与候选帧pq时间一致性计算的相似度值，若p(pc，pq)的结果大于阈值，则拒绝对应的回环检测结果。
[0129]
步骤5.10.在一致性检验之后，将基于描述子匹配计算的相对位姿当作初值，通过直接匹配算法(如：icp、ndt等)或特征匹配算法(如：shot、pfh、fpfh等)进行几何一致性检验，若几何一致性检验得分小于阈值，则认为当前回环检测结果是准确的。
[0130]
在本实施例中，本实施步骤基于点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征，基于全局描述子点云强度投影形状特征进行两步搜索匹配来实现回环检测，其后通过一致性检验模块来对回环检测的结果进行验证。将强度校准后的点云投影到以激光雷达为中心的柱面上，基于水平方向角与垂直高度，将投影后的柱面点云进行空间划分，保存每个子空间中最大的点云强度值，同时考虑相同方向角子空间点云的空间密度分布来构建全局描述子点云强度投影形状特征。在回环检测中，基于全局描述子点云强度投影形状特征进行回环搜索，并将搜索结果通过一致性检验模块进行检验。能够在几何特征匮乏的环境下实现高精度的建图与定位。后端在保留基于里程计回环检测方法的同时，增加基于点云强度特征描述子的回环检测方法，用以提高lidar slam框架后端优化的效果。
[0131]
s6，将验证通过的回环检测结果当作约束添加进因子图中，借助因子图中对历史轨迹位姿与点云地图进行优化，得到优化后的轨迹位姿与全局地图。
[0132]
具体地，在本步骤中，具体包括以下子步骤：
[0133]
步骤6.1.基于当前帧与前一帧的相对位姿变换以及前一帧在全局地图的位姿，估算当前帧在全局地图中的位姿。
[0134]rk+1
＝rk·rcur
.
[0135]
pk
+1
＝pk-r
k+1
·
pc
ur
[0136]
其中rk，r
k+1
为前一帧以及当前帧在全局地图下的旋转矩阵，pk，pk
+
1为前一帧以及当前帧在全局地图下的位置坐标，r
cur
，pcur为前一帧相对当前帧的的旋转变换与位移变换。
[0137]
步骤6.2.基于里程计的结果提取当前帧附近的点云数据构建局部点云地图，将当前帧与局部点云地图匹配，并将当前帧点云数据添加进地图中，从而实现点云地图更新。
[0138]
步骤6.3.将前端里程计计算得到的每帧激光雷达的位姿p＝{x，y，z，roll，pitch，yaw}作为因子图的节点，将相邻帧间激光雷达的相对位姿变换关系作为因子图的边。在后端检测到回环后，将回环检测的结果当作新的约束添加进因子图中，从而实现后端图优化，得到优化后的轨迹位姿与点云地图。
[0139]
其中，需要说明的是图优化是一种用于求解非线性最小二乘问题的优化算法，主要用于机器人技术和计算机视觉领域中的状态估计和建图问题。
[0140]
具体实施时，本发明技术方案提出的方法可由本领域技术人员采用计算机软件技术实现自动运行流程，实现方法的系统装置例如存储本发明技术方案相应计算机程序的计算机可读存储介质以及包括运行相应计算机程序的计算机设备，也应当在本发明的保护范围内。
[0141]
本发明实施例还提供一种融合点云强度的三维激光雷达同步建图与定位系统，包括以下模块：
[0142]
点云数据预处理模块，用于对采集的点云数据进行预处理，得到预处理之后的点云数据；
[0143]
点云数据计算模块，用于将预处理之后的点云数据进行2d投影，得到点云深度图与点云强度图；将预处理之后的点云数据进行地面点与非地面点分割，并基于点云深度图与点云强度图提取点云几何特征与强度特征；基于提取出的点云几何特征与强度特征，适时将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合，得到当前的位姿；
[0144]
回环检测模块，用于基于预处理之后点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征；并基于全局描述子点云强度投影形状特征进行回环检测，并对回环检测的结果进行验证；
[0145]
点云数据优化模块，用于将将验证通过的回环检测结果当作约束添加进因子图中，借助因子图中对历史轨迹位姿与点云地图进行优化，得到优化后的轨迹位姿与全局地图。
[0146]
在一些可能的实施例中，提供一种融合点云强度的三维激光雷达同步建图与定位系统，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如上所述的融合点云强度的三维激光雷达同步建图与定位方法。
[0147]
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，所述方法包括以下步骤：步骤1，对采集的点云数据进行预处理，得到预处理之后的点云数据；步骤2，将预处理之后的点云数据进行2d投影，得到点云深度图与点云强度图；步骤3，将预处理之后的点云数据进行地面点与非地面点分割，并基于点云深度图与点云强度图提取点云几何特征与强度特征；步骤4，基于提取出的点云几何特征与强度特征，将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合，得到当前的位姿；步骤5，基于预处理之后点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征；并基于全局描述子点云强度投影形状特征进行回环检测，并对回环检测的结果进行验证；步骤6，将验证通过的回环检测结果当作约束添加进因子图中，借助因子图中对历史轨迹位姿与点云地图进行优化，得到优化后的轨迹位姿与全局地图。2.根据权利要求1所述的融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，在步骤1中，所述对采集的点云数据进行预处理包括：步骤1.1.通过3d lidar对周围环境进行信息采集，得到原始点云数据；步骤1.2.然后对原始点云数据进行条件滤波,设置最远距离与最大高度，将超出范围的点云数据滤除掉；步骤1.3.对条件滤波后的点云数据进行强度校准。3.根据权利要求1所述的融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，在步骤2中，所述将预处理之后的点云数据进行2d投影包括：步骤2.1.将预处理后的点云数据投影进2d深度图与2d强度图，其中2d深度图保存的为点云到激光雷达中心的距离，2d强度图保存的为对应点云校核后的强度值。4.根据权利要求1所述的融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，在步骤3中，所述基于点云深度图与点云强度图提取点云几何特征与强度特征包括：步骤3.1.对强度校准后的点云数据进行地面点与非地面点的分割，赋予点云数据地面点、非地面点与离散点的标签；步骤3.2.基于点云数据的2d深度图与2d强度图分别计算点云几何光滑特征与强度光滑特征；步骤3.3.基于几何光滑特征的结果，在地面点云中提取点云几何平面特征点，在非地面点中提取点云几何边缘特征点；步骤3.4.基于强度光滑特征的结果，在地面点云中提取点云强度平面特征点，在非地面点中提取点云强度边缘特征点。5.根据权利要求1所述的融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，在步骤4中，所述将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合包括：步骤4.1.基于所提取几何特征的数量判断环境中几何特征的丰富度；步骤4.2.若几何特征较为丰富，则仅基于几何特征进行相邻帧点云的匹配与优化，得到相邻帧间激光雷达的相对位姿变换；
步骤4.3.若几何特征较为匮乏，则另基于强度特征进行相邻帧点云的匹配与优化，将得到的匹配结果与基于几何特征的匹配结果进行定权融合。6.根据权利要求1所述的融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，在步骤5中，所述基于预处理之后点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征包括：步骤5.1.将强度校准后的点云投影到以激光雷达为中心的柱面上，定义激光雷达获取的一帧点云数据和其中的每个激光点，将一帧点云数据投影到极坐标系下；步骤5.2.将柱面投影后激光点云按水平方向角均匀划分为n
s
个扇区，按其高度均匀划分为n
p
个块区，则将所述一帧点云数据划分进n
s
*n
p
个子空间；步骤5.3.对于每一个子空间，提取点云强度的最大值构成点云强度矩阵i；步骤5.4.统计具有相同水平方向角区间的点云的密度分度，计算点云密度权重d
ij
；步骤5.5.将点云密度权重与点云矩阵相乘，构建基于点云强度柱面投影的全局描述子点云强度投影形状特征。7.根据权利要求6所述的融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，在步骤5中，所述基于全局描述子点云强度投影形状特征进行回环检测，并对回环检测的结果进行验证包括：步骤5.6.在获得全局描述子点云强度投影形状特征后，提取该描述子行向量与列向量；其中，行向量与列向量中元素分别为描述子矩阵对应列与对应行的元素的均值；步骤5.7.基于所述列向量进行候选位置搜索，将列向量保存为vector类格式的kd树，基于当前帧描述子矩阵列向量与历史帧列向量之间欧氏距离的大小，在kd树中进行索引，搜索出n1个欧式距离最小的列向量对应点云帧作为候选帧；步骤5.8.在n1个候选帧中，基于行向量进行相对角度的初步计算，计算当前帧与候选帧的描述子矩阵的余弦距离；将行向量计算所得移位th当作初值，在其附近n
th
列范围内按列进行移位并计算两个描述子之间的余弦距离；步骤5.9.在基于描述子检测到回环后，首先进行时间一致性检验，基于检测到的当前帧与候选帧的朝向，分别计算其附近n帧点云对应描述子的余弦距离；步骤5.10.在时间一致性检验之后，将基于描述子匹配计算的相对位姿当作初值，通过直接匹配算法或特征匹配算法进行几何一致性检验，若几何一致性检验得分小于阈值，则认为当前回环检测结果是准确的。8.根据权利要求1所述的融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，在步骤6中，所述将验证通过的回环检测结果当作约束添加进因子图中，借助因子图中对历史轨迹位姿与点云地图进行优化，得到优化后的轨迹位姿与全局地图包括：步骤6.1.基于当前帧与前一帧的相对位姿变换以及前一帧在全局地图的位姿，估算当前帧在全局地图中的位姿；步骤6.2.基于里程计的结果提取当前帧附近的点云数据构建局部点云地图，将当前帧与局部点云地图匹配，并将当前帧点云数据添加进地图中，从而实现点云地图更新；步骤6.3.将计算得到的每帧激光雷达的位姿作为因子图的节点，将相邻帧间激光雷达的相对位姿变换关系作为因子图的边，将回环检测的结果当作新的约束添加进因子图中，实现后端图优化，从而得到优化后的轨迹位姿与点云地图。
9.一种融合点云强度的三维激光雷达同步建图与定位系统，用于实现如权利要求1-8任一项所述的一种融合点云强度的三维激光雷达同步建图与定位方法，其特征在于，包括以下模块：点云数据预处理模块，用于对采集的点云数据进行预处理，得到预处理之后的点云数据；点云数据计算模块，用于将预处理之后的点云数据进行2d投影，得到点云深度图与点云强度图；将预处理之后的点云数据进行地面点与非地面点分割，并基于点云深度图与点云强度图提取点云几何特征与强度特征；基于提取出的点云几何特征与强度特征，适时将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合，得到当前的位姿；回环检测模块，用于基于预处理之后点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征；并基于全局描述子点云强度投影形状特征进行回环检测，并对回环检测的结果进行验证；点云数据优化模块，用于将将验证通过的回环检测结果当作约束添加进因子图中，借助因子图中对历史轨迹位姿与点云地图进行优化，得到优化后的轨迹位姿与全局地图。

技术总结
本发明提供融合点云强度的三维激光雷达同步建图与定位方法及系统，实现过程包括将通过3D LiDAR采集的点云数据进行预处理，得到预处理之后的点云数据；将预处理之后的点云数据进行2D投影，得到点云深度图与点云强度图，并基于点云深度图与点云强度图提取点云几何特征与强度特征；适时将点云几何特征匹配结果与强度特征匹配结果进行自适应定权融合；基于点云强度的柱面投影与空间密度分布，构建全局描述子点云强度投影形状特征，提高回环检测的效果；基于因子图实现对历史轨迹位姿与点云地图的优化，得到优化后的轨迹位姿与全局地图。本发明通过在后端基于点云强度柱面投影构建了一种新的全局描述子，用以提高后端回环检测的鲁棒性。鲁棒性。鲁棒性。


技术研发人员：张红娟 李必军 张兴 钱闯 李文卓
受保护的技术使用者：武汉大学
技术研发日：2023.05.31
技术公布日：2023/9/5