实时里程计方法、设备及存储介质与流程

本技术涉及一种实时里程计方法、设备及存储介质，属于计算机。

背景技术：

1、随着多元传感器的采用和即时定位与地图构建(simultaneous localizationand mapping，slam)技术的发展，基于激光雷达的实时定位在自动驾驶，机器人，建筑bim模型，消防，地图测绘，资源勘测，矿井、vr\ar等方面应用需求越来越显著。要实现无人机，机器人，汽车等多种高速载体实现实时定位，要求对应的算法能实时处理输入雷达点云和惯性测量单元(inertial measurement unit，imu)等数据。

2、传统的里程计算法通过原始点云、点云几何特征点，实现帧间点云配准，实现帧间位姿变换估计，达到里程计算法的实现。

3、然而，传统的里程计算法只通过匹配两帧点云估计它们俩之间的位姿，由于噪声的影响，当两帧位姿估计不够准确时，可能导致估计的位姿漂移，从而导致存在即时定位与地图构建算法的失效的问题。

技术实现思路

1、本技术提供了一种实时里程计方法、设备及存储介质，可以解决即时定位与地图构建算法的失效的问题。本技术提供如下技术方案：

2、第一方面，实时里程计方法，所述方法包括：

3、获取点云数据和惯性测量单元数据；所述惯性测量单元数据包括加速度、加速度偏差、角速度和角速度偏差；

4、确定所述点云数据对应的稀疏面元；

5、对所述点云数据对应的点云位姿进行拟合，并使用所述惯性测量单元数据对拟合进行限制，得到第一b样条曲线；

6、通过k-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第一匹配结果；

7、基于所述第一匹配结果、所述第一b样条曲线和预设损失函数，确定第一损失函数值；

8、通过预设迭代算法对所述第一b样条曲线进行迭代优化，得到第二b样条曲线；

9、通过所述k-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第二匹配结果；

10、基于所述第二匹配结果、所述第二b样条曲线和预设损失函数，确定第二损失函数值；

11、在所述第二损失函数值与所述第一损失函数值的差值小于预设损失函数阈值的情况下，基于所述第二b样条曲线确定实时里程计。

12、可选地，所述点云数据包括n帧点云数据；所述确定所述点云数据对应的稀疏面元，包括：

13、获取第y帧点云数据；所述y为大于0、且小于或等于所述n的整数；

14、确定所述第y帧点云数据对应的第y帧稀疏面元；

15、在所述y小于所述n的情况下，令y＝y+1，执行获取第y帧点云数据的步骤。

16、可选地，所述通过k-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第一匹配结果，包括：

17、依次将第p帧稀疏面元与第p+1帧稀疏面元进行匹配，得到所述第一匹配结果；所述p依次取1至所述n的正整数。

18、可选地，所述确定所述第y帧点云数据对应的第y帧稀疏面元，包括：

19、确定所述第y帧点云数据对应的体素网格中每个体素的点云均值，作为稀疏面元均值；

20、确定所述每个体素的点云方差的最小特征值对应的法向量，作为稀疏面元法向量；

21、将所述第y帧点云数据对应的点云时间作为稀疏面元时间。

22、可选地，所述确定所述第y帧点云数据对应的体素网格中每个体素的点云均值之前，还包括：

23、确定激光雷达的当前测量范围；

24、获取单个体素的尺寸信息；

25、按照所述尺寸信息对所述当前测量范围进行划分，得到所述体素网格。

26、可选地，所述第一b样条曲线包括m个控制点和至少一个时间窗口；每个时间窗口包括h帧点云数据对应的点云位姿；所述h为大于1、且小于或者等于所述n的正整数；

27、所述确定所述第y帧点云数据对应的第y帧稀疏面元之后，还包括：

28、在当前时间窗口中包括h帧点云数据对应的点云位姿的情况下，移动时间窗口；所述h为大于1、且小于或者等于所述n的正整数；

29、将所述当前时间窗口中的点云位姿按照时间顺序保存，并作为里程计输出结果。

30、可选地，所述预设损失函数包括第一损失函数、第二损失函数和第三损失函数；所述第一损失函数用于确定稀疏面元匹配对应的匹配损失函数值；所述第二损失函数用于确定加速度损失函数值；所述第三损失函数用于确定角速度损失函数值；

31、所述基于所述第一匹配结果、所述第一b样条曲线和预设损失函数，确定第一损失函数值，包括：

32、基于所述第一匹配结果和所述第一损失函数，确定所述匹配损失函数值；

33、基于所述第一b样条曲线和所述第二损失函数，确定加速度损失函数值；

34、基于所述第一b样条曲线和所述第三损失函数，确定角速度损失函数值；

35、将所述匹配损失函数值、所述加速度损失函数值和所述角速度损失函数值相加，得到所述第一损失函数值。

36、可选地，所述通过预设迭代算法对所述第一b样条曲线进行迭代优化，包括：

37、通过所述预设迭代算法对所述控制点、所述角速度偏差和所述加速度偏差进行迭代优化。

38、第二方面，提供一种电子设备，所述设备包括处理器和存储器；所述存储器中存储有程序，所述程序由所述处理器加载并执行以实现第一方面提供的实时里程计方法。

39、第三方面，提供一种计算机可读存储介质，所述存储介质中存储有程序，所述程序被处理器执行时用于实现第一方面提供的实时里程计方法。

40、本技术的有益效果至少包括：通过获取点云数据和惯性测量单元数据；确定点云数据对应的稀疏面元；对点云数据对应的点云位姿进行拟合，并使用惯性测量单元数据对拟合进行限制，得到第一b样条曲线；通过k-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第一匹配结果；基于第一匹配结果、第一b样条曲线和预设损失函数，确定第一损失函数值；通过预设迭代算法对第一b样条曲线进行迭代优化，得到第二b样条曲线；通过k-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第二匹配结果；基于第二匹配结果、第二b样条曲线和预设损失函数，确定第二损失函数值；在第二损失函数值与第一损失函数值的差值小于预设损失函数阈值的情况下，基于第二b样条曲线确定实时里程计。可以解决即时定位与地图构建算法的失效的问题。通过提取输入点云的稀疏面元信息，用于多帧稀疏面元匹配，从而实现激光雷达和惯性测量单元的实时里程计估计采用b样条拟合一个时间窗口内的全局位姿，即使当某两帧间位姿估计失败时，由于b样条曲线的约束，也能使得即时定位与地图构建算法漂移的情况尽可能减少，同时，直到前后两次匹配和优化过程中最终优化的损失函数值的差值小于预设损失函数阈值，则判定为b样条曲线收敛，基于收敛的b样条曲线确定实时里程计，可以增加即时定位与地图构建算法的鲁棒性，从而避免即时定位与地图构建算法的失效。

41、另外，由于提取的稀疏面元的数目对比原始点云数据大幅减少，因此，可以降低算法功耗和计算资源等需求。

42、上述说明仅是本技术技术方案的概述，为了能够更清楚了解本技术的技术手段，并可依照说明书的内容予以实施，以下以本技术的较佳实施例并配合附图详细说明如后。