一种激光惯导SLAM方法、系统、设备及存储介质

本发明涉及slam，特别是涉及一种激光惯导slam方法、系统、设备及存储介质。

背景技术：

1、同时定位与建图(simultaneous localization and mapping, slam)是无人机等移动机器人在未知环境中实现自主运行的关键技术之一。得益于视觉或激光雷达传感器的发展，人们投入了大量的精力来实现能够支持移动机器人六自由度状态估计的高性能slam算法。而基于视觉传感器的slam算法虽然能提供丰富的rgb信息，但缺乏深度信息，且对环境的光照条件敏感。而基于激光雷达的slam算法使用有源光源，能够在不受光线变化影响的情况下实现更高的测量精度，获得更密集的环境信息。

2、目前，众多专家学者针对激光惯导slam算法展开研究。2014年，loam算法由jizhang等人提出，率先开创了特征点匹配的方法。2018年，lego-loam由tixiao shan等人提出，是loam的高效轻量化改进版本，在loam的基础上引入地面点云分割，通过提取地面点与非地面点特征并分别优化，同时采用轻量级点云聚类，大幅降低了计算量，但该算法仅依赖imu对点云运动畸变进行校正，相较于激光惯性紧耦合的处理方式，其优化效果较为有限。2021年，wei xu等人提出了fast-lio，是激光惯性紧耦合框架的代表作，利用紧密耦合的迭代扩展卡尔曼滤波器将激光雷达特征点与imu数据融合，直接使用原始点云，避免了传统方法中特征提取可能带来的信息损失。次年，fast-lio的作者团队随后提出了fast-lio2。其在继承fast-lio紧耦合激光-惯性里程计框架所有优点的基础上，通过增量式kd树（ikd-tree）和增量地图更新策略实现高效增量建图，具有良好的鲁棒性。

3、然而，fast-lio和fast-lio2在数据预处理阶段，仅按照固定的时间间隔划分点云帧，使得不同帧的点云密度差异大，容易引入运动畸变。

技术实现思路

1、基于上述现有技术存在的缺陷，本发明提供了一种激光惯导slam方法、系统、设备及存储介质，解决了现有的问题。

2、本发明采用以下技术方案：

3、第一方面，本发明提供一种激光惯导slam方法，包括以下步骤：

4、通过激光雷达实时连续采集原始点云数据，并获取当前时刻的imu数据；

5、将当前时刻的imu数据输入至imu前向传播模型和协方差传播方程，预测下一时刻的运动状态和对应的协方差矩阵；所述协方差矩阵用于表征运动状态预测的不确定性；从下一时刻的协方差矩阵中提取用于评估对应运动状态不确定性的指标值，若指标值超过设定阈值时，则将当前累积采集的原始点云数据作为一个点云帧；

6、通过点云帧对地图和位姿进行实时更新。

7、优选的，所述运动状态包括位置、速度和姿态四元数，指标值包括位置不确定性指标值和姿态不确定性指标值，并获取对应的置信区间，基于对应的置信区间与设定阈值进行比较；位置不确定性指标值为位置分量的标准差，姿态不确定性指标值为姿态分量的标准差。

8、优选的，对点云帧中的每个原始点云进行运动补偿，通过补偿后的点云坐标对地图和位姿进行实时更新；所述对点云帧中的每个原始点云进行运动补偿，具体包括以下步骤：

9、选取点云帧中最后时刻所对应的imu位姿作为参考位姿；

10、基于参考位姿对各原始点云所对应的imu位姿进行位姿变换，得到补偿后的点云坐标。

11、优选的，所述地图通过八叉树结构构建，八叉树结构结构包括根节点和叶节点，每个叶节点内包含由多个原始点云封装形成的椭球面元，所述椭球面元通过中心点、主轴方向和平面方程进行表征。

12、优选的，所述八叉树结构从根节点开始，采用递归方式对空间进行划分，根据节点内点云的几何复杂度判断是否继续细分，若满足预设的细分终止条件则停止细分并使该节点成为叶节点。

13、优选的，通过点云帧对地图进行实时更新，具体包括以下步骤：

14、当点云帧的新点云加入椭球面元对应的区域时，中心点采用加权平均的方式进行增量更新；

15、结合历史协方差数据与新点云的偏差信息对对应的协方差矩阵进行更新；

16、针对八叉树不同层级的节点，每层分别设定最小点数阈值；当总点云数量超出限制时，优先移除最早加入该区域的点云数据。

17、优选的，通过点云帧对位姿进行实时更新，具体包括以下步骤：

18、获取点云帧中的当前激光点云坐标与最近椭球面元的偏差向量，将偏差向量投影到最近椭球面元的三个主轴方向，得到各主轴方向的投影距离；将各主轴方向的投影距离与对应的标准差进行加权，得到各主轴方向的协方差加权残差；将各主轴方向的协方差加权残差进行叠加，得到激光点到椭球表面的总马氏距离残差；

19、通过平面性权重对总马氏距离残差进行等价转换，得到单轴残差；

20、基于单轴残差构建加权观测方程和观测残差方程，基于加权观测方程和观测残差方程进行卡尔曼滤波，对imu预测的位姿进行校正。

21、第二方面，本发明提供一种激光惯导slam系统，包括：

22、采集模块，用于通过激光雷达实时连续采集原始点云数据，并获取当前时刻的imu数据；

23、划分模块，用于将当前时刻的imu数据输入至imu前向传播模型和协方差传播方程，预测下一时刻的运动状态和对应的协方差矩阵；所述协方差矩阵用于表征运动状态预测的不确定性；从下一时刻的协方差矩阵中提取用于评估对应运动状态不确定性的指标值，若指标值超过设定阈值时，则将当前累积采集的原始点云数据作为一个点云帧；

24、更新模块，用于通过点云帧对地图和位姿进行实时更新。

25、第三方面，本发明提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述的激光惯导slam方法。

26、第四方面，本发明提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述的激光惯导slam方法。

27、与现有技术相比，本发明采用的上述至少一个技术方案能够达到以下有益效果：

28、本发明提出了一种自适应点云帧划分方法，通过构建imu前向传播模型与协方差传播方程，实时预测运动状态并量化其不确定性，并基于协方差矩阵中的不确定性指标值自适应触发点云帧划分，克服了传统方法中因固定时间间隔划分点云帧而导致的不同帧点云密度差异大、高速运动场景下易引入运动畸变的缺陷。相比于现有技术中以固定时间窗口累积点云的方式，本发明能够根据实际运动状态动态调整点云帧的长度，在运动剧烈、不确定性高时生成更短的点云帧以抑制畸变，在运动平缓时生成更长的点云帧以提高计算效率，从而在保证状态估计精度的同时有效平衡了系统的实时性能，尤其适用于运动状态变化复杂的大规模场景。

1.一种激光惯导slam方法，其特征在于，包括以下步骤：

2.如权利要求1所述的一种激光惯导slam方法，其特征在于，所述运动状态包括位置、速度和姿态四元数，指标值包括位置不确定性指标值和姿态不确定性指标值，并获取对应的置信区间，基于对应的置信区间与设定阈值进行比较；位置不确定性指标值为位置分量的标准差，姿态不确定性指标值为姿态分量的标准差。

3.如权利要求1所述的一种激光惯导slam方法，其特征在于，对点云帧中的每个原始点云进行运动补偿，通过补偿后的点云坐标对地图和位姿进行实时更新；所述对点云帧中的每个原始点云进行运动补偿，具体包括以下步骤：

4.如权利要求1所述的一种激光惯导slam方法，其特征在于，所述地图通过八叉树结构构建，八叉树结构结构包括根节点和叶节点，每个叶节点内包含由多个原始点云封装形成的椭球面元，所述椭球面元通过中心点、主轴方向和平面方程进行表征。

5.如权利要求4所述的一种激光惯导slam方法，其特征在于，所述八叉树结构从根节点开始，采用递归方式对空间进行划分，根据节点内点云的几何复杂度判断是否继续细分，若满足预设的细分终止条件则停止细分并使该节点成为叶节点。

6.如权利要求4所述的一种激光惯导slam方法，其特征在于，通过点云帧对地图进行实时更新，具体包括以下步骤：

7.如权利要求4所述的一种激光惯导slam方法，其特征在于，通过点云帧对位姿进行实时更新，具体包括以下步骤：

8.一种激光惯导slam系统，其特征在于，包括：

9.一种计算机设备，其特征在于，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述权利要求1-7任一所述的激光惯导slam方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述权利要求1-7任一项所述的激光惯导slam方法。