一种基于LiDAR-IMU-GNSS的高速大场景建图方法

本发明涉及自主导航，尤其涉及一种基于lidar-imu-gnss的高速大场景建图方法。

背景技术：

1、近些年来，机器人领域的高速发展，使得自主导航设备逐步出现在大众视野中，并成为中国智造新赛道。这些设备中，小到扫地机器人，大到自动驾驶，均依赖于高精度的全局地图。全局地图构建的技术核心主要是利用激光传感器实时扫描周围环境，获取周围环境的几何信息，并融合其他传感器估计激光传感器的实时位姿，利用估计的位姿将激光点云数据拼接，构建全局一致性环境地图。

2、由于构建的点云地图中高精度地保留了原环境中的几何信息，因而拥有极为广泛的应用价值，其应用领域包括：(1)自动驾驶，在城市环境中构建大尺度的、无累积漂移的全局一致性点云地图，使得车辆拥有厘米级别的高精度定位，实现l4级别的无人驾驶；(2)高效测绘，激光雷达实时扫描周围环境几何信息，构建稠密点云地图，使得后端可以远程查看前线工作环境；(3)自主探索任务，机器人可根据实时构建的地图对自身环境可行性分析，进行自主决策、规划、探索，减少了人员在未知危险环境中工作，避免伤亡事故。

3、近些年来，随着slam领域的兴起，基于激光雷达的建图方法得到了快速的发展，许多研究学者通过提取激光点云的特征点进行配准，并取得了出色的建图效果。但在高速环境下，由于高速运动带来的数据畸变，以及雷达里程计的累积误差，使得大尺度的建图成为一个巨大的挑战，此外在户外大尺度场景中，还面临着激光退化场景、z-方向漂移等问题。

4、专利号为“cn111161412a”的发明专利提出一种三维激光建图方法及系统。该发明首先利用置放在机器人上的传感器获取数据，并滤除不合格的数据，然后利用由粗到精的匹配算法计算所述机器人的位姿，根据所述机器人的位姿构建新的图像，生成一致性较高的场景地图。该发明所述的技术方案中仅依赖激光传感器，虽然单一激光传感器在大多数室内低速环境可以完成环境建图，但面临高速运动，将导致激光原始数据产生严重的运动畸变，使得地图的精度大幅度下降。且对于隧道、空旷等退化环境，仅依赖激光传感器无法高精度地估计出当前位姿，甚至导致建图算法的失败。综上所述，该方案无法解决由于高速运动带来的激光数据畸变的问题，且难以在激光退化的场景下完成环境建图任务。

5、专利号为“cn113269878a”的发明专利提出一种基于多传感器的建图方法及系统，该发明通过机器人上的多传感器获取当前时刻的lidar点云数据、imu数据以及定位约束数据，基于预处理后的imu数据及lidar点云数据，去除点云运动畸变，使用历史位姿构建局部点云地图,并结合去运动畸变后的lidar点云数据，以及相对运动数据构建最大后验概率问题并进行优化求解，根据优化结果构建位姿图，将预处理后的定位约束数据作为相应位姿的约束融入位姿图。对位姿图进行优化求解，将优化后的融合位姿与去运动畸变后的lidar点云进行拼接，完成地图的创建。该方案中融合了imu传感器信息，并使用imu信息去除激光传感器的运动畸变，可以实现在高速运动下的环境建图。但该方法依然无法应对在激光退化的场景下鲁棒建图，而且由于累积误差带来的影响，导致该建图算法在大尺度范围的场景中产生较大的漂移。综上所述，虽然该方案解决了激光运动畸变的问题，但依然无法解决在激光退化环境下鲁棒建图，且面对大尺度环境依然会产生较大的累积漂移。

技术实现思路

1、本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于lidar-imu-gnss的高速大场景建图方法，充分利用多源传感器异构数据的互补优势，借助gnss传感器数据解决大尺度场景下建图漂移的问题。通过提取激光地面点，构建地面约束解决激光建图z-方向漂移问题，以及借助激光约束的信息数据，构建退化检测机制，处理激光退化环境下的建图，从而实现鲁棒、无漂移的大尺度环境地图构建。

2、为解决上述技术问题，本发明所采取的技术方案是：

3、一种基于lidar-imu-gnss的高速大场景建图方法，包括如下步骤：

4、步骤1：数据预处理；

5、首先对imu数据做积分处理，并对激光原始数据去除运动畸变，随后对激光点云分离地面点，并分别提取面点特征以及地面点云中的平面参数；

6、步骤2：激光数据关联；

7、使用当前帧的面点特征与局部地图中的平面特征做配准，构建点到平面的约束，使用当前帧中地面点云的平面参数，与局部地图中的地面点云做配准，构建平面到平面的约束；

8、步骤3：激光-imu联合优化；

9、使用误差状态卡尔曼滤波，优化步骤2中构建的约束以及imu前向传播，得到当前时刻的里程计lio，并更新局部地图；构建退化检测机制，实时检测传感器是否退化；

10、步骤4：lio-gnss空间同步；

11、利用lio以及gnss数据构建关于旋转方向上的约束，并在初始化距离内优化该值，从而将lio以及gnss数据统一到同一坐标系下；

12、步骤5：lio-gnss联合优化；

13、对步骤4中统一到同一坐标系下的lio以及gnss数据，作为新的因子加入到因子图中，优化整个因子图，并构建全局一致性点云地图。

14、进一步地，步骤1中去除运动畸变的方法为：借助imu传感器以及激光imu之间的外参，预测激光传感器在不同采集时间的位姿，并通过位姿变换统一到同一时间坐标下，实现消除激光原始数据的运动畸变；

15、首先对imu数据做积分处理，已知imu测量数据为加速度a以及角速度ω，根据imu的离散运动模型，得到以下传递关系：

16、

17、

18、

19、

20、

21、其中，pt、vt、rt、分别指在采样时刻t的传感器状态：位置、速度、姿态、角速度零偏以及加速度零偏；δt为传感器的采样时间间隔；ηbω、ηba分别表示零偏的随机游走噪声；ηb、ηb分别表示imu传感器的测量噪声；g为重力加速度；

22、由以上公式，得到imu传感器在各个测量时刻对应的状态，通过激光到imu的外参得到激光各个测量时刻的状态；使用激光各个时刻的状态对激光原始数据去除运动畸变：

23、

24、其中，表示在激光坐标系下的第i个激光点，表示激光相对于imu的坐标转换关系，rt+i、pt+i分别表示第i个激光点在激光坐标系下的姿态及位置，表示去除运动畸变后的激光点。

25、进一步地，对激光点云分离地面点采用ranrac的方法，具体如下：

26、首先根据激光传感器到地面的初始距离dg筛选初始地面点云pcg，由初始地面点云拟合平面公式，如下：

27、at[px py pz]＝-1

28、其中，px,py,pz分别指激光点的三维坐标，a为待拟合的平面参数矩阵；

29、使用qr分解求解上述最小二乘问题，得到矩阵a，对矩阵a做反向归一化，得到拟合平面的法向量n以及原点到平面的距离d，遍历初始地面点云pcg，带入以下平面公式中：

30、δd＝d-nqi

31、如果δd的值大于一定阈值，认为激光点qi为外点，进行二次剔除；

32、迭代上述步骤，最终将地面点分离出来，分离后的点云分别称为特征点云以及地面点云。

33、进一步地，提取特征的方法如下：

34、首先是特征点云，通过以下公式计算每个激光点的曲率c：

35、

36、其中，qi为目标激光点，s为包含目标激光点的点簇，qj为点簇中的第j个激光点。

37、对每个激光点按照曲率大小排序，并通过判断每个激光点的曲率是否小于设定的阈值，从而决定该激光点是否为平面点，将确定为平面点的激光点组成为面点特征点云pct；对于地面点云pcg，将点云投影到三维栅格中，计算每个栅格中点云的协方差以及均值，作为该栅格的平面参数。

38、进一步地，所述步骤2中，将提取的当前帧的特征点云以及地面点云与局部地图中的特征点云及地面点云做配准，并构建关于当前帧时刻状态量的约束，具体包括特征点云数据关联和地面点云数据关联；

39、特征点云数据关联的方法为：

40、对局部地图中的特征点云构建kd-tree，遍历当前帧中的每一个面点寻找该面点对应在局部地图中的最近的5个邻近点并对这5个邻近点构建平面方程，求得拟合平面的平面参数nm为拟合平面的平面法向量，为邻近点的均值；构建关于面点到拟合平面π的距离残差约束，如下式：

41、

42、其中，ed表示面点到拟合平面π的距离残差；

43、地面点云数据关联的方法为：

44、将局部地图中的地面点云投影到三维栅格vm中，构建每个栅格三维坐标的哈希映射，用于配准索引；遍历局部地图中的每个栅格，计算每个栅格的点云均值以及点云协方差σ，公式如下：

45、

46、

47、其中，表示三维格栅vm中的激光点，k为三维格栅vm中激光点的个数；

48、对点云协方差σ做特征值分解，求其第二大特征值λ1以及最小特征值λ2，如果这两个特征值的比值大于设定的对应阈值，则认为对应栅格中的点云为一个地平面，且能作为配准；最小特征值λ2对应的特征向量认为是该格栅中平面的法向量nm；

49、同理将当前帧点的地面点云投影至栅格中，计算每个栅格的均值以及法向量nc，利用哈希映射找到当前帧与局部地图配准的栅格vc、vm，并构建以下地平面到地平面的距离约束：

50、

51、其中，eg表示地平面到地平面的距离残差。

52、进一步地，所述步骤3的具体方法为：

53、经过步骤2构建激光数据关联后，得到以下观测方程：

54、

55、其中，x指状态量，x＝[p,v,r,bω,ba]，对观测方程在误差状态δx处进行泰勒展开，得到：

56、

57、

58、

59、其中，表示先验状态量，nd、ng分别表示关于平面约束以及地面约束的观测噪声；

60、结合imu前向传递，构建最大后验估计：

61、

62、由上式求得卡尔曼增益k：

63、j＝pht(hpht+r)-1

64、其中，p为当前状态x的协方差矩阵，r为激光观测的协方差矩阵；

65、并对状态进行更新：

66、

67、由此得到当前帧的估计位姿即激光里程计lio；遍历当前帧的特征点云以及地面点云利用优化后的位姿将当前帧的特征点云以及地面点云转到世界坐标系下，并加入到局部点云地图中，从而完成局部点云地图的更新；

68、所述退化检测机制，对于上述激光约束ed、eg构建的信息矩阵h，求该矩阵的最小特征值λ，如果该特征值λ小于经验阈值，则认为此时激光传感器已处于退化环境，由于该退化环境中激光构建的约束并不可靠，通过提高激光观测的协方差矩阵r的值，降低激光约束的权重从而保证系统的可靠性。

69、进一步地，所述步骤4中，将lio以及gnss数据均统一到东北天坐标系；

70、lio轨迹直接根据imu提供的航向角信息转到东北天坐标系下；gnss数据由以下公式转到东北天坐标系下：

71、

72、其中，λ、φ分别表示第一帧gnss数据的经度与纬度；

73、在初始化距离内对lio与gnss两条轨迹构建关于yaw角的残差dθ，如下式所示：

74、

75、其中，为激光里程计在x、y方向上的位置信息，为gnss投影到东北天坐标系下后在x、y方向上的位置信息；θ为关于yaw角的角度差，将该值补偿至旋转矩阵中，从而实现两条轨迹的空间同步。

76、进一步地，所述因子图使用incremental smoothing and mapping with thebayes tree即isam2来构建；

77、所述步骤5中，将每一帧的lio与gnss数据作为新的因子送入维护的因子图中，并优化整个因子图，得到无漂移的里程计；将去除运动畸变的原始点云利用该里程计信息投影到全局坐标系下，完成构建全局一致性地图。

78、采用上述技术方案所产生的有益效果在于：本发明提供的基于lidar-imu-gnss的高速大场景建图方法，能够充分利用各传感器的特性实现优势互补，提升建图精度以及鲁棒性，即使在激光退化环境或高速运动场景都能实现高精度建图，在大尺度场景下实现无漂移实时建图。与现有的建图方法相比，本发明提出的建图框架在鲁棒性以及精度方面均取得最佳的性能。本发明能够部署于移动机器人平台或自动驾驶车辆中，并具备实时运行能力。