基于多约束后端优化的实时全景彩色点云地图构建方法及其装置

本发明属于结构健康监测以及激光点云建图，更具体地说，涉及一种基于多约束后端优化的实时全景彩色点云地图构建方法及其装置。

背景技术：

1、重复结构化场景，如隧道、长走廊或配电网系统中，具有高度规则和重复的几何特性，这给检测工作带来了一些挑战。在重复结构化的环境中，墙壁、地板和线缆可能呈现出高度一致性，使得区分不同部分变得困难。另外，在隧道或长走廊中，光照条件可能不均匀或变化无常，影响视觉传感器的表现，并且这类场景可能缺乏足够的纹理特征，使基于图像的检测方法难以区分不同的区域。长距离和大规模的结构也可能难以用传统测量方法全面覆盖。但是激光点云可以通过高分辨率的三维数据捕捉，提供精确的空间几何信息，帮助准确地识别和分割不同的结构元素。

2、目前激光雷达和摄像机是机器人导航领域最常用的传感器。激光雷达能够快速、精确地获取环境三维信息，但三维点云比较稀疏,且只有距离信息；摄像机能够获得致密、丰富的信息，无法获取深度。将二者进行数据融合，可得到较理想的三维重建结果。近年来，三维全向激光雷达和全景相机的使用越来越多，使致密的全景三维重建成为可能。

3、因此，激光雷达与视觉传感器之间的传感器融合成为了目前激光点云slam系统的主流趋势，在重复的结构化场景中，可以较好地进行点云地图的构建。

4、目前的激光slam点云地图构建方法大多存在如下四类普遍的问题：1、计算效率有待提高，利用特征点匹配得到的激光里程计容易浪费计算量。2、许多点云地图构建方法的鲁棒性有待提高，存在退化环境的问题，也不适配快速运动或高噪声的工作环境，另外适用的雷达型号也有限制。3、许多方法缺少回环检测和后端优化，在大范围工作场景下容易产生明显的累计误差。4、与其他传感器的数据融合效果有待进一步提升。

5、重复结构化场景，如隧道、长走廊或配电网系统中，具有高度规则和重复的几何特性，这给检测工作带来了一些挑战。在重复结构化的环境中，墙壁、地板和线缆可能呈现出高度一致性，使得区分不同部分变得困难。另外，在隧道或长走廊中，光照条件可能不均匀或变化无常，影响视觉传感器的表现，并且这类场景可能缺乏足够的纹理特征，使基于图像的检测方法难以区分不同的区域。长距离和大规模的结构也可能难以用传统测量方法全面覆盖。但是激光点云可以通过高分辨率的三维数据捕捉，提供精确的空间几何信息，帮助准确地识别和分割不同的结构元素。

6、目前激光雷达和摄像机是机器人导航领域最常用的传感器。激光雷达能够快速、精确地获取环境三维信息，但三维点云比较稀疏,且只有距离信息；摄像机能够获得致密、丰富的信息，无法获取深度。将二者进行数据融合，可得到较理想的三维重建结果。近年来，三维全向激光雷达和全景相机的使用越来越多，使致密的全景三维重建成为可能。

7、因此，激光雷达与视觉传感器之间的传感器融合成为了目前激光点云slam系统的主流趋势，在重复的结构化场景中，可以较好地进行点云地图的构建。

8、目前的激光slam点云地图构建方法大多存在如下四类普遍的问题：1、计算效率有待提高，利用特征点匹配得到的激光里程计容易浪费计算量。2、许多点云地图构建方法的鲁棒性有待提高，存在退化环境的问题，也不适配快速运动或高噪声的工作环境，另外适用的雷达型号也有限制。3、许多方法缺少回环检测和后端优化，在大范围工作场景下容易产生明显的累计误差。4、与其他传感器的数据融合效果有待进一步提升。

9、所以亟需设计一种基于多约束后端优化的实时全景彩色点云地图构建方法及其装置。

技术实现思路

1、为了解决上述问题，本发明提供的彩色点云地图构建方法具有较高的匹配精度、较快的匹配速率以及较强的鲁棒性，并且能够实时显示点云地图的构建进度。

2、实现本发明目的的技术方案是提供一种基于多约束后端优化的实时全景彩色点云地图构建方法，包括以下步骤：

3、s100、使用激光雷达传感器获得高精度的三维激光点云数据，使用全景相机获得高分辨率的全景彩色rgb图像，将gps装置和惯性测量单元imu整合为pos系统，使用惯性测量单元imu获取系统姿态数据，使用gps定位装置获取装置的位置参数；标定pos系统相对于激光雷达传感器和全景相机的位置和姿态数据；

4、s200、使用增量式k-d树数据结构，直接将去除运动畸变的三维激光点云与局部地图进行匹配，通过迭代扩展卡尔曼滤波方法将激光雷达传感器与惯性测量单元imu的数据融合起来，生成更准确的运动估计，获得激光里程计信息；

5、s300、后端加入几何一致性回环检测判断是否出现回环，检测到回环之后采用多约束因子图优化消除激光里程计的累计误差；

6、s400、通过gps提供的装置位置参数和imu提供的装置姿态参数，获得装置整体的pos数据，利用pos数据和激光雷达与全景相机间的相对位置姿态关系，得到激光点所关联的全景图片以及其外方位元素，构建彩色点云地图。

7、进一步的，步骤s2具体步骤如下：

8、s210、构建imu运动方程，将imu运动方程离散化后通过imu数据进行积分，将积分结果进行前向传播，预测当前装置的粗略位姿估计，然后将积分结果反向传播去除当前帧的点云运动畸变；利用去除畸变后的点云构建激光点云观测方程，计算残差；

9、s220、使用增量式k-d树的数据结构进行地图管理，k-d树以里程计发布频率合并新的点云帧而动态增长，只保留激光雷达当前位置周围边长为l的大型局部区域中的地图点；当检测区域移动到地图边界时，地图区域会随之更新到新的矩形区域并将之前区域中的部分剔除；k-d树增量式地管理节点，为每个节点添加一个属性，来判断以节点为根节点的子树是否平衡；如果添加了一个新的点到树中，需要在原树中从下至上地寻找到最后一个不平衡的节点，将节点以下的子树中的所有点都提取出来构建一个新的平衡树；

10、s230、通过迭代扩展卡尔曼滤波器求解出位姿，将其转化为最大后验估计问题，状态增益为：

11、k＝(htr-1h+p-1)-1htr-1(1)

12、经过重复迭代后，第k+1次迭代时更新的状态；

13、

14、其中，为广义加符号，为广义减，即广义加的逆运算，k为卡尔曼滤波器增益，为在k时刻的状态估计，h为观测模型，将真实状态空间映射成观测空间，r为协方差矩阵，p为后验估计误差协方差矩阵；

15、滤波算法收敛后，更新最优状态估计为而后利用更新之后的状态量获得前端激光里程计。

16、进一步的，步骤s210中，所述的imu运动方程具体构建方式如下：

17、

18、式中，i表示imu所处的坐标系，g表示全局坐标系，l为雷达坐标系，p为位置，r为姿态，v为速度，g为重力矢量，na和nω表示am和ωm的测量噪声，ba和bω是由nba和nbω影响的随机游走高斯噪声，imu与激光雷达之间坐标系的刚体变换为：

19、itl＝(irl,ipl)  (4)

20、将imu的连续时间运动方程离散化，生成离散模型为：

21、

22、式中：为广义加符号，其物理含义为在处添加一个空间的小扰动，其中是一种n维的流行，在这里n＝24，xi,ui,wi分别表示imu的状态、输入和噪声，定义如下：

23、

24、进一步的，步骤s210中，所述的残差计算具体步骤包括：

25、s211、在初始零时刻，装置将其状态更新为其估计值；

26、s212、在非零时刻，将每个测量得到的激光点lpj投影到全局坐标系下，变换关系为：

27、

28、s213、在之前已经构建的点云地图上搜索其最近的5个点，根据其最近邻点，基于激光点云观测模型中的法向量和质心gqj拟合一个局部小平面，根据所述激光点云观测模型可得在k时刻，残差与原始测量噪声的关系式如下：

29、

30、式中：是关于处的雅可比矩阵，来源于原始测量噪声即为残差，

31、进一步的，步骤s230中，使用迭代扩展卡尔曼滤波器求解出位姿的具体步骤包括：求解未知状态xk与传播状态和协方差之间存在的先验高斯分布关系：

32、

33、除了此先验分布外，还有一个源于残差计算时测量方程的状态分布将二者结合可得状态xk的后验分布，其等价表示为及其最大后验估计：

34、

35、其中

36、通过迭代扩展卡尔曼滤波器求解该最大后验估计问题，状态增益k＝(htr-1h+p-1)-1htr-1；经过重复迭代后，第k+1次迭代时更新的状态滤波算法收敛后，更新最优状态估计为而后利用更新之后的状态量获得前端激光里程计。

37、进一步的，步骤s300中的几何一致性回环检测，具体包括如下步骤：

38、s310、从连续的点云扫描中提取几何特征；

39、s320、当检测装置回到之前访问过的位置时，匹配当前扫描和历史扫描，以识别潜在的回环；

40、s330、对候选回环进行几何一致性检查，比较点云间的对应点距离或角度的一致性，获取扫描与历史扫描之间的几何关系；

41、s340、构建一致性群组；

42、s350、设置合适的阈值，只有超过阈值的一致性群组才会被认为是有效的回环；一旦检测到回环，执行回环点云匹配，优化所述激光雷达的里程计数据中的位姿，得到回环检测的点云匹配数据。

43、进一步的，步骤s300中，所述的多约束因子图优化方法，具有如下特征：

44、装置状态节点间有3个因子timu、todom和tloop进行约束，分别为imu因子、里程计因子和回环因子；imu因子由每两个雷达关键帧之间的imu数据积分得到；里程计因子由每两个关键帧之间的位姿变换得到，若是检测出回环，则在两帧点云之间添加回环因子。

45、根据本发明的另一方面，提供了一种基于多约束后端优化的实时全景彩色点云地图构建装置，包括：数据获取模块、地图构建模块、回环检测和后端优化模块以及全景图像融合模块；

46、所述数据获取模块，用于获取三维激光点云数据、全景彩色rgb图像、imu加速度数据和旋转速率数据以及装置的gps位置参数；

47、所述地图构建模块，用于根据imu数据前向传播和后向去除运动畸变，使用增量式k-d树数据结构构建点云地图，使用迭代扩展卡尔曼滤波器求解位姿，获得激光里程计信息；

48、所述回环检测模块，用于检测出回环和进行因子图优化，消除系统累计误差；

49、所述全景图像融合模块，用于融合全景图片构建彩色点云地图。

50、根据本发明的另一方面，提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现本发明的基于多约束后端优化的实时全景彩色点云地图构建方法及其装置中的步骤。

51、根据本发明的又一方面，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现本发明的基于多约束后端优化的实时全景彩色点云地图构建方法及其装置中的步骤。

52、相比于现有技术，本发明至少具有如下有益效果：

53、本发明通过获取装置的imu数据、全景图像数据和激光雷达点云数据，对所述的imu数据进行预积分，将积分结果进行前向传播，预测当前装置的粗略位姿估计，然后将其反向传播去除当前帧的点云运动畸变。利用去除畸变后的点云构建激光点云观测模型并计算残差。使用一种增量式k-d树数据结构，直接将所述去除运动畸变的三维激光点云与局部地图进行匹配，通过一种迭代扩展卡尔曼滤波方法将激光雷达传感器与惯性测量单元imu的数据融合起来，生成更准确的运动估计，获得激光里程计信息。使用几何一致性回环检测判断所述的里程计数据是否存在回环，若存在回环则采用因子图对前端里程计因子、imu积分因子和回环因子进行优化。最后将构建的点云地图与全景图像进行数据融合，获得激光点所关联的全景影像及其外方位元素，将全景图像中对应点的颜色属性值赋值给激光点云，实现点云的赋色。