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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附图说明

图1是本申请一个实施例提供的实时里程计方法的流程图；

图2是本申请一个实施例提供的实时里程计装置的框图；

图3是本申请一个实施例提供的电子设备的框图。

具体实施方式

下面结合附图和实施例，对本申请的具体实施方式做进一步详细描述。以下实施例用于说明本申请，但不用来限制本申请的范围。

首先，对本申请涉及的若干名词进行介绍。

B样条曲线(b-spline)：是指在数学的子学科数值分析里的一种特殊的表示形式。它是B-样条基曲线的线性组合。由Isaac Jacob Schoenberg创造。

李群(Lie Group)：李群是一种只有一个运算的、比较简单的代数结构；是可用来建立许多其他代数系统的一种基本结构。

在数学中，李群(Lie group)是具有群结构的实流形或者复流形，并且群中的加法运算和逆元运算是流形中的解析映射。李群在数学分析、物理和几何中都有非常重要的作用。

图1是本申请一个实施例提供的实时里程计方法的流程图，该方法至少包括以下几个步骤：

步骤101，获取点云数据和惯性测量单元数据；其中，惯性测量单元数据包括加速度、加速度偏差、角速度和角速度偏差。

步骤102，确定点云数据对应的稀疏面元。

本实施例中，点云数据包括N帧点云数据。相应地，确定点云数据对应的稀疏面元，包括：获取第Y帧点云数据；Y为大于0、且小于或等于N的整数；确定第Y帧点云数据对应的第Y帧稀疏面元；在Y小于N的情况下，令Y＝Y+1，执行获取第Y帧点云数据的步骤。

其中，确定第Y帧点云数据对应的第Y帧稀疏面元，包括：确定第Y帧点云数据对应的体素网格中每个体素的点云均值，作为稀疏面元均值；确定每个体素的点云方差的最小特征值对应的法向量，作为稀疏面元法向量；将第Y帧点云数据对应的点云时间作为稀疏面元时间。

本实施例中，稀疏面元具有位置、法向量和生成时间。在激光雷达(LIDAR)坐标系中以当前测量空间，即，将激光雷达可360度上下左右旋转的最大测量范围对应立方体，作为体素划分的立方体，按照单个体素的长度，把该立方体划分为单个体素，得到体素网格，统计其中每个体素的点云均值作为稀疏面元均值，方差的最小特征值对应的法向量作为稀疏面元法向量，点云时间作为稀疏面元时间。

此时的稀疏面元的位置是在激光雷达坐标系中，可以通过B样条曲线拟合激光雷达的全局位姿，从而把稀疏面元坐标变换至全局坐标系中，以通过K-维树最近邻(K-Nearest Neighbour,KNN)算法依次对相邻两帧稀疏面元进行匹配。

其中，全局坐标系是指以激光雷达采集得到第一帧点云数据时的位置为原点构建得到的坐标系。

具体地，确定第Y帧点云数据对应的体素网格中每个体素的点云均值之前，还包括：确定激光雷达的当前测量范围；获取单个体素的尺寸信息；按照尺寸信息对当前测量范围进行划分，得到体素网格。

步骤103，对点云数据对应的点云位姿进行拟合，并使用惯性测量单元数据对拟合进行限制，得到第一B样条曲线。

其中，第一B样条曲线包括至少一个控制点和至少一个时间窗口；每个时间窗口包括H帧点云数据对应的点云位姿；H为大于1、且小于或者等于N的正整数。

本实施例中，采用李群SE(3)作为连续全局位姿所在空间，B样条曲线拟合的时间范围为[0,tmax)，其中，tmax由控制点的数目和相邻控制点之间的时间间隔确定。

本实施例中，任意t时刻对应的全局位姿可以通过下式表示：

式中，B

通过B样条曲线，可以求导计算某一时刻t激光雷达坐标系对应的位姿p(t)，角速度w(t)和加速度a(t)。稀疏面元可以用t时刻对应的位姿p(t)变换到全局坐标系下，以用于通过K-维树最近邻算法依次对相邻两帧稀疏面元进行匹配。

因为B样条曲线拟合的时间窗口范围为[0,tmax)，为了达到实时性，控制点数目不可能设置过多，因此，本实施例中，移动B样条曲线作滑动窗口优化，每一个时间窗口有一个时间戳tB,点云的时间戳转化为B样条曲线时间t，可以通过下式表示：

t＝tP-tB

式中，tB为时间窗口对应的时间戳；tP为点云时间，即稀疏面元时间。

在当前稀疏面元对应的稀疏面元时间超过时间窗口范围的最大表示时间的情况下，换言之，在时间窗口仅能包括H帧点云数据对应的位姿，且当前的时间窗口中已经包括H帧点云数据对应的位姿的情况下，移动时间窗口，并把离当前稀疏面元最久的点云位姿依时间顺序保存作为里程计输出结果。

具体地，确定第Y帧点云数据对应的第Y帧稀疏面元之后，还包括：在当前时间窗口中包括H帧点云数据对应的点云位姿的情况下，移动时间窗口；H为大于1、且小于或者等于N的正整数；将当前时间窗口中的点云位姿按照时间顺序保存，并作为里程计输出结果。

本实施例中，通过帧与帧之间的惯性测量单元数据来限制B样条曲线拟合的全局位姿的角速度和加速度，以使得到的位姿估计更准确，同时，也使得优化参数包括惯性测量单元数据的角速度偏差和加速度偏差。

其中，优化参数是指需要通过预设迭代算法进行迭代优化的参数，优化函数还包括控制点，通过预设迭代算法对第一B样条曲线进行迭代优化，包括：通过预设迭代算法对控制点、角速度偏差和加速度偏差进行迭代优化。

可选地，预设迭代算法包括数值梯度(Numerical Gradient)算法或者解析梯度(Analytical Gradient)算法。

通过预设迭代算法对第一B样条曲线进行迭代优化，包括：通过预设迭代算法对控制点、角速度偏差和加速度偏差进行迭代优化。

步骤104，通过K-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第一匹配结果。

其中，通过K-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第一匹配结果，包括：依次将第P帧稀疏面元与第P+1帧稀疏面元进行匹配，得到第一匹配结果；P依次取1至所述N-1的正整数。

比如：以一个时间窗口中的帧的数目N为4为例，则依次对第一帧和第二帧、第二帧和第三帧、第三帧和第四帧进行K-维树最近邻算法的匹配。

本实施例中，K-维树最近邻算法的最近邻查找空间是稀疏面元的7维向量空间，包括位置，法向量和生成时间。匹配的输出结果为所有的匹配结果，匹配形式为第P帧的稀疏面元对应第P+1帧的另一个稀疏面元。

步骤105，基于第一匹配结果、第一B样条曲线和预设损失函数，确定第一损失函数值。

本实施例中，预设损失函数包括第一损失函数、第二损失函数和第三损失函数；第一损失函数用于确定稀疏面元匹配对应的匹配损失函数值；第二损失函数用于确定加速度损失函数值；第三损失函数用于确定角速度损失函数值。

具体地，第一损失函数可以通过下式表示：

式中，

其中，n

n

式中，n

第二损失函数可以通过下式表示：

第三损失函数可以通过下式表示：

式中，a

基于第一匹配结果、第一B样条曲线和预设损失函数，确定第一损失函数值，包括：基于第一匹配结果和第一损失函数，确定匹配损失函数值；基于第一B样条曲线和第二损失函数，确定加速度损失函数值；基于第一B样条曲线和第三损失函数，确定角速度损失函数值；将匹配损失函数值、加速度损失函数值和角速度损失函数值相加，得到第一损失函数值。

步骤106，通过预设迭代算法对第一B样条曲线进行迭代优化，得到第二B样条曲线。

步骤107，通过K-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第二匹配结果。

步骤108，基于第二匹配结果、第二B样条曲线和预设损失函数，确定第二损失函数值。

由于每次优化后，B样条曲线的控制点，惯性测量单元对应的加速度偏差和角速度偏差均发生了改变，因此，对应的帧的全局位姿也随之改变，通过K-维树最近邻算法对相邻两帧稀疏面元进行匹配得到的匹配结果也会变化。因此，我们重复匹配和优化B样条曲线，直到前后两次匹配和优化过程中最终优化的损失函数值的差值小于预设损失函数阈值，则判定为收敛。

步骤109，在第二损失函数值与第一损失函数值的差值小于预设损失函数阈值的情况下，基于第二B样条曲线确定实时里程计。

在第二损失函数值与第一损失函数值的差值大或者等于预设损失函数阈值的情况下，则继续匹配和优化B样条曲线。

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

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

图2是本申请一个实施例提供的实时里程计装置的框图。该装置至少包括以下几个模块：数据获取模块210、面元确定模块220、第一匹配模块230、位姿拟合模块240、第一确定模块250、曲线优化模块260、第二匹配模块270、第二确定模块280和第三确定模块290。

数据获取模块210，用于获取点云数据和惯性测量单元数据；惯性测量单元数据包括加速度、加速度偏差、角速度和角速度偏差；

面元确定模块220，用于确定点云数据对应的稀疏面元；

第一匹配模块230，用于通过K-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第一匹配结果；

位姿拟合模块240，用于对点云数据对应的点云位姿进行拟合，并使用惯性测量单元数据对拟合进行限制，得到第一B样条曲线；

第一确定模块250，用于基于第一匹配结果、第一B样条曲线和预设损失函数，确定第一损失函数值；

曲线优化模块260，用于通过预设迭代算法对第一B样条曲线进行迭代优化，得到第二B样条曲线；

第二匹配模块270，用于通过K-维树最近邻算法依次对相邻两帧稀疏面元进行匹配，得到第二匹配结果；

第二确定模块280，用于基于第二匹配结果、第二B样条曲线和预设损失函数，确定第二损失函数值；

第三确定模块290，用于在第二损失函数值与第一损失函数值的差值小于预设损失函数阈值的情况下，基于第二B样条曲线确定实时里程计。

相关细节参考上述方法实施例。

需要说明的是：上述实施例中提供的实时里程计装置在进行实时里程计时，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将实时里程计装置的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。另外，上述实施例提供的实时里程计装置与实时里程计方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。

图3是本申请一个实施例提供的电子设备的框图。该设备至少包括处理器301和存储器302。

处理器301可以包括一个或多个处理核心，比如：4核心处理器、8核心处理器等。处理器301可以采用DSP(Digital Signal Processing，数字信号处理)、FPGA(Field－Programmable Gate Array，现场可编程门阵列)、PLA(Programmable Logic Array，可编程逻辑阵列)中的至少一种硬件形式来实现。处理器301也可以包括主处理器和协处理器，主处理器是用于对在唤醒状态下的数据进行处理的处理器，也称CPU(Central ProcessingUnit，中央处理器)；协处理器是用于对在待机状态下的数据进行处理的低功耗处理器。在一些实施例中，处理器301可以在集成有GPU(Graphics Processing Unit，图像处理器)，GPU用于负责显示屏所需要显示的内容的渲染和绘制。一些实施例中，处理器301还可以包括AI(Artificial Intelligence，人工智能)处理器，该AI处理器用于处理有关机器学习的计算操作。

存储器302可以包括一个或多个计算机可读存储介质，该计算机可读存储介质可以是非暂态的。存储器302还可包括高速随机存取存储器，以及非易失性存储器，比如一个或多个磁盘存储设备、闪存存储设备。在一些实施例中，存储器302中的非暂态的计算机可读存储介质用于存储至少一个指令，该至少一个指令用于被处理器301所执行以实现本申请中方法实施例提供的实时里程计方法。

在一些实施例中，电子设备还可选包括有：外围设备接口和至少一个外围设备。处理器301、存储器302和外围设备接口之间可以通过总线或信号线相连。各个外围设备可以通过总线、信号线或电路板与外围设备接口相连。示意性地，外围设备包括但不限于：射频电路、触摸显示屏、音频电路、和电源等。

当然，电子设备还可以包括更少或更多的组件，本实施例对此不作限定。

可选地，本申请还提供有一种计算机可读存储介质，所述计算机可读存储介质中存储有程序，所述程序由处理器加载并执行以实现上述方法实施例的实时里程计方法。

可选地，本申请还提供有一种计算机产品，该计算机产品包括计算机可读存储介质，所述计算机可读存储介质中存储有程序，所述程序由处理器加载并执行以实现上述方法实施例的实时里程计方法。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。