【技术实现步骤摘要】
本专利技术涉及机器人，涉及基于地面约束与重力优化的激光雷达惯性里程计方法、系统和设备及介质。
技术介绍
1、目前，作为智能移动机器人路径规划与自主导航的基础，同时定位与地图构建(simultaneous localization and mapping，slam)技术在近年来得到了快速发展。按照传感器类型划分，slam可分为基于视觉与基于激光雷达的两大类算法。由于视觉slam容易受到光照和视角变化的影响，且受限于计算资源，一般算法只能提供稀疏的建图结果。相对于视觉传感器，激光雷达有着较大的测量范围且可以直接测量目标距离，能够更好地反映场景的结构细节，因此被广泛应用在自动驾驶、移动机器人以及测绘等领域。
2、按照扫描模式，激光雷达可分为机械式与固态两类。机械式雷达通过旋转高频激光阵列采集环境信息，一般在水平方向上具有360°的视场角，但垂直分辨率较低，比如velodyne puck和robosense rs-lidar-16，这一类传感器价格昂贵，一般用于自动驾驶车辆。最近，固态激光雷达逐渐发展并流行起来，比如intel realsense l515和livox系列产品。这一类激光雷达通常视场角更小、点云更密集，为小型机器人提供了一种成本效益高、重量轻的解决方案。
3、然而，由于工作方式的不同，目前大多数激光里程计算法都专为某一种雷达量身设计，很少有算法可以支持多种类型的激光雷达。一帧点云数据中通常会包含成千上万个点，为了提高计算效率，很多算法选择对原始点云进行特征提取，即根据点与其相邻点的平滑度来提取线特
4、由于一帧点云数据是连续扫描获得的，激光雷达的帧内运动会使点云产生运动畸变，仅使用激光雷达进行位姿估计会产生较大的漂移。imu传感器可以测量载体运动的加速度和角速度，具有很高的更新频率，因此很多算法选择在算法中融合imu来弥补激光雷达的不足。一方面imu测量可以对激光雷达点云做运动补偿。另一方面imu的运动预测也可以作为激光雷达里程计优化的初值，加快计算的收敛速度。
5、激光雷达与imu的融合可分为松耦合与紧耦合两类，其中松耦合算法独立处理两个传感器数据来获得各自的运动约束，并将得到的约束做后融合，其优势是计算效率高，但也导致了信息损失。而紧耦合通过联合优化直接融合激光雷达与imu的测量，可以获得更高的定位准确性。大多数激光雷达-惯性里程计仅在初始化阶段对重力方向进行估计，但所估计的重力可能会随时间漂移。
6、因此，需要结合实际问题，深入、系统地对激光雷达惯性里程计的高度方向约束与重力约束进行研究，并设计出快速准确的激光雷达惯性里程计方法或系统。
技术实现思路
1、有鉴于此，为解决上述技术问题，本专利技术提出的一种激光雷达惯性里程计方法、系统和设备及介质，采用非特征提取的原始点云直接配准，进一步地，使用该配准方法提取地面点，通过地面约束减小高程累计误差。将重力作为状态量进行估计，以减少初始化时不准确的重力估计对定位精度的影响。
2、为了实现上述目的，本专利技术采用如下技术方案：
3、第一方面，本专利技术实施例提供一种激光雷达惯性里程计方法，包括以下步骤：
4、s10、实时获取激光雷达扫描数据和惯性传感器数据，通过所述惯性传感器数据预测载体当前时刻位姿，并补偿所述激光雷达扫描数据点云的运动畸变；
5、s20、将经过补偿运动畸变的当前时刻的所述激光雷达扫描数据与局部地图点云进行配准，遍历所有所述激光雷达扫描数据进行最近邻搜索；基于最近邻搜索结果，拟合平面，计算地面约束残差；
6、s30、以最小化点云配准残差、所述惯性传感器数据的预积分残差和地面约束残差为目标，构建里程计估计模型；对所述里程计估计模型进行求解，得到最终的里程计结果。
7、进一步地，所述步骤s20包括：
8、将经过运动畸变补偿的扫描点转换到世界坐标系；
9、在地图中搜索距离第i个扫描点pi＝[xi,yi,zi]最近的五个点，其中，[x,y,z]为激光雷达扫描点在世界坐标系下的坐标值，并通过这五个点拟合出一个平面ni＝[a,b,c,d]；其中[a,b,c]组成了平面法向量d为平面截距长度；
10、计算扫描点pi到拟合平面的距离ri＝axi+byi+czi+d；
11、根据平面法向量与水平面法向量夹角筛选地面点；
12、使用ransac算法对地面点进行平面拟合，计算出地面约束残差。
13、进一步地，所述步骤s20中通过使用ikd-tree进行最近邻搜索。
14、进一步地，所述步骤s30包括：
15、s301、基于步骤s20中当前时刻点云配准结果，计算点到平面的点云配准残差；
16、s302、基于步骤s10中所述惯性传感器数据的预积分，计算带有重力约束的预积分残差，即包含重力优化的imu预积分残差；
17、s303、基于步骤s20中地面点拟合结果，计算地面约束残差；
18、s304、以最小化点云配准残差、包含重力优化的imu预积分残差和地面约束残差为目标，构建激光雷达惯性里程计代价函数；
19、s305、以最小化步骤s304中代价函数为目标，构建里程计估计模型，对所述里程计估计模型进行求解，得到最终的里程计结果。
20、进一步地，所述步骤s30还包括：
21、s306、根据步骤s305中所估计里程计结果，拼接激光雷达扫描点云，获得三维环境点云地图。
22、进一步地，所述步骤s302包括：
23、计算连续两帧激光雷达扫描时刻k与k+1之间的imu预积分：
24、
25、
26、
27、式中，b表示imu坐标系，与分别为imu预积分的位置、速度和旋转部分，其定义为k时刻到k+1时刻的相对运动；r为旋转矩阵，p为三维位置向量，v为速度向量，g为重力向量，δt为相邻两帧的时间间隔；
28、对于连续两帧激光雷达扫描内的imu传感器测量，包含重力约束的相对运动约束残差定义为：
29、
30、式中，为imu传感器在连续两帧激光雷达扫描bk与bk+1内的测量，x为激光雷达惯性里程计系统状态量，分别为平移、速度和旋转量的3d误差状态，δba，δbg分别为加速度偏置与角速度偏置的误差状态，δg为重力误差状态，δtk时相邻两帧的时间间隔，log(·)表示李群到李代数的对数映射运算，角标a,g,w,b分别表示加速度计、陀螺仪、世界坐标系和载体坐标系。
31、进一步地，所述步骤s303包括：
32、根据载体位姿将地平面系数π0＝[nx,ny本文档来自技高网...
【技术保护点】
1.一种激光雷达惯性里程计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤S20包括：
3.根据权利要求1所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤S30包括：
4.根据权利要求3所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤S302包括：
5.根据权利要求3所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤S303包括：
6.根据权利要求3所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤S304构建激光雷达惯性里程计代价函数：
7.根据权利要求3所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤S305求解过程中包括：
8.一种激光雷达惯性里程计系统，其特征在于，包括：
9.一种终端设备，其包括处理器和存储器，处理器用于实现各指令；存储器用于存储多条指令，其特征在于，所述指令适于由处理器加载并执行权利要求1-7任一项所述的一种激光雷达惯性里程计方法。
10.一种计算机可读存储介质，
...【技术特征摘要】
1.一种激光雷达惯性里程计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤s20包括：
3.根据权利要求1所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤s30包括：
4.根据权利要求3所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤s302包括：
5.根据权利要求3所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤s303包括：
6.根据权利要求3所述的一种激光雷达惯性里程计方法，其特征在于，所述步骤s304构建激光雷达...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。