基于3D激光雷达和惯性测量单元的融合的测距和绘图的制作方法

本发明涉及3d激光雷达和惯性测量单元(imu)的融合，以用于姿态追踪和3d环境绘图。

背景技术：

环境的三维(3d)地图可以用于自主导航或工程测量。一些传感器(例如3d激光雷达)可用于重建三维地图。传感器可以固定在系统上或在系统上移动。移动绘图系统需要姿态追踪(测距)和三维地图生成，与固定在系统上相比，移动绘图系统更灵活，更有效地捕获三维空间。

在移动绘图系统的其他示例中，应用了不同的传感器来估计姿态并生成三维图。一些系统基于一个或多个相机来估计量距和地图。间接目测法从捕获的图像中提取特征点，并估计这些点的三维位置，这可能会导致地图上的点稀疏。直接目测法可以提供更密集的地图，但是每个图像中的点位置的准确性都与估计的姿态有关。可以进一步应用立体或视觉惯性系统来解决度量三维空间中的点的规模的问题。大多数基于相机的方法都会受到照度变化的困扰，这可能会限制其应用。

基于三维激光雷达的系统是姿态追踪和绘图的替代方法，因为三维激光雷达可以提供度量点测量并且照度不变。当系统移动时，由于三维激光雷达的点的接收时间不同，这些点可能会失真。可以通过外部传感器(例如gnss/ins)提供的运动模型或自身估计(激光雷达测距)来补偿失真。外部传感器辅助方法取决于外部传感器的可实现性。自身估计的方法(基于三维激光雷达的姿态追踪)在快速运动的情况下可能会失败，因为传感器具有相率较低的速率，或在一些类似狭窄走廊场景(三维激光雷达的重复特征)下可能会失败。

有一些基于三维激光雷达和惯性测量单元的方法可以提高快速运动的情况下的性能。大多数方法将三维激光雷达和惯性测量单元解耦，形成松散耦合的系统。这些系统将三维激光雷达和惯性测量单元视为独立的模块，因此融合后的结果无法改善单独的各模块并影响系统的性能。

技术实现要素：

一方面，本公开提供一种用于姿态追踪和三维绘图的系统，包括：

3d激光雷达，其在第一时间和第二时间处获得激光雷达扫查结果，第一时间在第二时间之前；

惯性测量单元，其与所述3d激光雷达紧密耦合；和

计算装置，其构造为：

在所述第二时间到来之前，利用在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据对所述惯性测量单元在所述第二时间处的状态进行更新，同时对在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据进行预整合；

根据预整合后的所述惯性测量单元的测量数据以及惯性测量单元与激光雷达之间的外部参数来预测所述3d激光雷达的姿态，并将预测的所述3d激光雷达的姿态应用于在所述第二时间处接收的来自所述3d激光雷达的测量数据，以得到所述3d激光雷达的去偏移的测量数据；

提取在所述第二时间处接收的来自所述3d激光雷达的测量数据的特征点；

将在所述第一时间之前的来自所述3d激光雷达的测量数据的特征点融合为局部地图；

利用预测的激光雷达的姿态和所述去偏移的测量数据，得到相对激光雷达测量；

利用所述相对激光雷达测量和所述预整合，以得到优化后的3d激光雷达的姿态，进而得到局部时间窗口内的状态的绘图估计，并将估计结果用于所述更新中。

在实施例中，所述计算装置还构造为：

利用3d激光雷达的去偏移的测量数据和优化后的3d激光雷达的姿态来将多帧去偏移的数据融合而得到全局地图。

在实施例中，对优化后的3d激光雷达的姿态添加旋转约束来优化所述全局地图使得所述全局地图与所述3d激光雷达的去偏移的测量数据相匹配，以得到具有高一致性的全局地图。

在实施例中，所述相对激光雷达测量是所述局部地图与所述3d激光雷达的去偏移的测量数据之间的对应关系。

另一方面，本公开还提供一种用于姿态追踪和三维绘图的方法，包括：

通过3d激光雷达在第一时间和第二时间处获得激光雷达扫查结果，第一时间在第二时间之前；

将惯性测量单元与所述3d激光雷达紧密耦合；

在所述第二时间到来之前，利用在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据对所述惯性测量单元在所述第二时间处的状态进行更新，同时对在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据进行预整合；

根据预整合后的所述惯性测量单元的测量数据以及惯性测量单元与激光雷达之间的外部参数来预测所述3d激光雷达的姿态，并将预测的所述3d激光雷达的姿态应用于在所述第二时间处接收的来自所述3d激光雷达的测量数据，以得到所述3d激光雷达的去偏移的测量数据；

提取在所述第二时间处接收的来自所述3d激光雷达的测量数据的特征点；

将在所述第一时间之前的来自所述3d激光雷达的测量数据的特征点融合为局部地图；

利用预测的激光雷达的姿态和所述去偏移的测量数据，得到相对激光雷达测量；

利用所述相对激光雷达测量和所述预整合，以得到优化后的3d激光雷达的姿态，进而得到局部时间窗口内的状态的绘图估计，并将估计结果用于所述更新中。

在实施例中，所述方法还包括：

利用3d激光雷达的去偏移的测量数据和优化后的3d激光雷达的姿态来将多帧去偏移的数据融合而得到全局地图。

在实施例中，对优化后的3d激光雷达的姿态添加旋转约束来优化所述全局地图使得所述全局地图与所述3d激光雷达的去偏移的测量数据相匹配，以得到具有高一致性的全局地图。

在实施例中，所述相对激光雷达测量是所述局部地图与所述3d激光雷达的去偏移的测量数据之间的对应关系。

附图说明

图1示出了根据本公开实施例的包括激光雷达和imu的姿态追踪和绘图系统的框图。

图2示出了计算系统的操作过程的示例。

图3示出了姿态追踪和绘图的详细过程的框图。

图4示出了系统的时间戳的示例。

图5示出了3d激光雷达和imu绘图流程。

具体实施方式

本公开的实施例提供了一种通过紧密耦合方法使用3d激光雷达和惯性测量单元(imu)的系统，用于姿态追踪和3d环境绘图，该系统仍可在快速运动或狭窄场景中提供稳定的结果。

在一个示例中，参照图1，其示出了姿态追踪和绘图系统的示例。该系统包括具有3d激光雷达和imu的传感器以及收集传感器数据并处理姿态追踪和地图生成的计算系统。在实施例中，传感器还可以为3d激光雷达和imu组成的传感器对。计算系统例如是笔记本计算机。该计算系统可以在环境中移动时收集点云和惯性测量。

通过将3d激光雷达测量和imu测量紧密耦合，测距比现有技术有所改进，这联合优化了来自3d激光雷达测量和imu测量的残差。该系统可以提高快速运动和类似狭窄走廊场景下的稳定性。旋转约束的绘图利用了根据3d激光雷达与imu融合的测距(通过优化imu残差使该测距与重力匹配)来生成一致的3d环境地图。也就是说，通过利用3d激光雷达的去偏移的数据和预测的3d激光雷达的姿态来将多帧去偏移的数据融合而得到3d环境绘图。

图2示出了计算系统的操作过程的示例。大体上，操作过程可以包括数据收集、3d激光雷达和imu融合的测距(即，姿态追踪)和绘图。

下面将描述3d激光雷达和imu融合的测距。

将3d激光雷达捕获的每行测量表示为scanc，并将包含一次测量中所有扫描(scan)的扫查(sweep)表示为sweeps。例如，一次扫查中的16行3d激光雷达包含16个扫描。

在以下部分中，将转换矩阵表示为其将坐标系中的点转换至坐标系是由imu预测的转换。和分别是的旋转矩阵和平移矢量。使用汉密尔顿符号下的四元数其对应于用于两个四元数相乘。和表示imu在时间k的原始测量。提取的特征在原始的捕获坐标系中表示为fa，其可以转换至坐标系作为

在离散时间i处获得激光雷达扫查结果的同时，体坐标系和分别是imu主体的参考和激光雷达中心的参考。待估计的状态是世界坐标系中的imu状态以及激光雷达和imu传感器之间的外部参数详细地，将i处的imu状态和外部参数书写为：

它们形成的完整状态为：

是imu状态在四元数中的位置、速度和方向。ba和bg是imu加速器和陀螺仪的偏差。

图3示出了3d激光雷达和imu测距流程，其简要概述了根据本发明的3d激光雷达-imu测距。利用先前的估计状态，可以使用上一个时间i到当前时间j的当前激光雷达原始点云和imu原始输入以具有对状态进行优化的新步骤。测距估计执行如下：

1)在到来之前，经由以下公式更新imu状态::

上述等式可以通过迭代求解。其中，将简写为(·)i，并且，作为四元数乘法的序列。r为与q对应的旋转矩阵，g^w是世界坐标系中的重力矢量，和是来自imu的测量。

也就是说，如等式(2)所示，利用从imu的加速器和陀螺仪的输入，可通过离散估计将前一个imu状态更新为当前imu状态其中，δt是两次连续imu测量之间的间隔，并且将激光雷达扫描时间k＝i和k＝j之间的所有imu测量整合。通过简化符号使用，将k＝j-1用作k＝j之前的在前imu时间。

2)同时，imu原始输入被预整合为下面的δpij,,δvij和δqij，以在后续的联合优化中使用。

时间i和时间j之间的主体运动可以通过预整合测量来表示：

3)3d激光雷达内部具有旋转机构以接收整个周围环境的数据。当3d激光雷达移动时，来自该3d激光雷达的原始数据会遭受运动失真，使扫查中的点与真实位置不同。为了解决该问题，在本公开中，根据imu传播来预测3d激光雷达运动并且假设在扫查过程中采用线性运动模型。在本公开的实施例中，当接收到最新的激光雷达扫查结果时，进行去偏移，即将使用预测的3d激光雷达的运动的运动失真校正步骤应用于原始数据。随后，每个点通过的线性差值进行校正，以获得去偏移的扫描得到扫查的最终姿态，其中t∈(tj′，tj]是扫查中的点的时间，tj′和tj分别是扫查开始和扫查结束的时间。

因此，通过以下公式产生去偏移的激光雷达扫描

4)接下来，应用特征提取步骤以减小数据的维数并提取中最重要的特征点存在许多有效的激光雷达点提取，例如可以选择曲率较小的点，例如，平面点。

5)根据先前对应的优化状态和将局部窗口内的先前激光雷达特征点合并为局部地图

6)利用针对的预测的激光雷达姿态，可以通过找到每个约束来找到相对激光雷达测量其中是点，ω、d是在时间p处定义的平面系数。

利用imu和另一传感器的融合，能够提供传感器对的相对姿态，如果固定第一参考系，待估计的状态和将是局部可观测的。为了适当地合并来自imu的预整合，采用各扫查之间的相对激光雷达测量来约束激光雷达姿态，参见下面的算法。在找到点对应关系之前，建立局部地图，这是因为单次扫查的点对于计算准确的对应关系来说不够密集。

局部地图包含来自nm个离散时间{o，…，p，…，i}的激光雷达特征点。其中o、p和i分别是窗口内第一次激光雷达扫查的时间、基准激光雷达扫查的时间和上一次处理过的激光雷达扫查的时间，如图4所示。局部地图根据特征建立在基准激光雷达扫查的坐标系中，特征经由先前优化的激光雷达姿势被转换(为了简便，在此部分中，将预测的转换表示为并且经由转换)。待估计的状态是ns个时间{p+1，…，i，j}处的状态，其中p+1和j是窗口中的紧接着基准激光雷达扫查的下一次激光雷达扫描的时间和当前激光扫描的时间。

下表为相对激光雷达测量的算法。

通过建立局部地图，可以在和原始之间找到对应关系。将这种对应关系定义为相对激光雷达测量，因为他们是相对于基准姿态的，并且基准姿态会随着滑动窗口而变化。提取的特征点可以是中最平坦的点或边缘的点。在下文中，以平坦点为例进行讨论。将knn(k-nearestneighbor)用于每个转换的特征点以找到中k个最近的点随后，，对于平坦点，将这些相邻的点拟合为中的平面。通过由限定的线性方程可以解出平坦点的系数，其中，ω是平面法线方向，d是到的原点的距离。将针对每个平坦特征点的表示为相对测量点中的一个。需要指出的是，在每个相对激光雷达测量中，x在中被定义，ω和d在中被定义。

7)联合非线性优化，利用相对激光雷达测量和imu预整合，获得局部窗口内状态的绘图(map)估计。将最终结果应用于更新步骤1)中的预测状态，避免imu预测的偏移。成本函数可以形成为

其中，激光雷达残差、惯性残差和边缘化残差分别定义如下，

其中，是边缘化的先验项。是相对激光雷达约束的残差，是imu约束的残差。非线性最小二乘形式的成本函数可以通过gauss-newton算法求解，其采用hδx＝-b形式。可以采用ceressolver求解这一问题。

可以根据等式(4)针对每个相对激光雷达测量得到激光雷达约束通过激光雷达准确性确定协方差矩阵可以根据状态和imu预整合获得imu约束通过schur分量可以获得

图5示出了旋转约束绘图的示例并示出了3d激光雷达和imu绘图流程，其将先前测距得到的姿态、特征点和来自3d激光雷达的全点云作为输入。测距姿态首先用作全局点云登记的先验值。随后，来自测距的旋转分量作为虚拟测量应用以约束优化。然后，将以上具有马哈拉诺比斯范数(mahalanobisnorm)的成本函数最小化，以获得状态x的绘图(map)估计。

可以通过先前的激光雷达特征点和激光雷达-imu测距得到的激光雷达姿态来形成全局地图。将特征点登记到全局地图(而不是局部地图)可以将激光雷达姿态约束到一致的世界坐标系。根据本公开的绘图方法将相对激光雷达测量ml用作3d激光雷达-imu测距中的相对激光雷达测量。可以形成将最新的激光雷达特征点与全局地图匹配的成本函数，如下所示：

其中

其中是最新的待估计的激光雷达姿态，m是具有中的特征点x和限定在中的系数ω和d的相对激光测量。随后可以使用类似的gauss-newton方法来最小化通过残差和雅可比矩阵(jacobian)和执行优化，其中θ是相应四元数q的误差状态。随着累积的旋转误差和长期操作后，融合后的全局地图无法与重力精确匹配。这可能进一步导致绘图与倾斜的地图错误地匹配。在本公开中，提供了约束的绘图方法。该方法利用来自激光雷达-imu测距的约束，这确保了最终的地图与重力匹配。

给定以下特性：沿z轴的方向具有较高的不确定性，并且该方向的其他两个自由度更加接近真实值，因此可以通过将该方向的雅可比矩阵修改为以下矩阵来约束成本函数：

其中表示最后一次迭代中的状态和信息矩阵的估计，并且是关于的方向的信息矩阵的近似值，∈x和∈y可以通过中x轴方向和y轴方向到z轴方向的信息比率获得。

之后，使用和作为雅可比行列式，并将其用于优化步骤。可以获得递增的激光雷达姿态作为δθz和δp。

利用旋转约束的最终更新的激光雷达状态将是

在实施例中，在应用于室内应用的手持系统时，3d激光雷达和imu彼此靠近并紧密耦合。在应用于室外应用的车载系统中时，3d激光雷达和imu通常彼此相距较远，例如imu安装在汽车的基础连杆上方，3d激光雷达则安装在汽车的前部。与自动校准所有平移参数不同的是，将外部平移参数的先验项添加到联合非线性优化步骤中的成本函数中，以在汽车上测试。

综上所述，在本公开中，提出了一种紧密耦合的3d激光雷达-imu姿态估计算法，以克服上述问题。来自激光雷达和imu的测量都用于联合优化。为实现实时和更一致的估计，应用先前姿态的固定滞后平滑和边缘化，然后进行旋转约束的优化。本公开所带来的技术效果如下：

提出了一种紧密耦合的激光雷达-imu测距算法。它提供具有高更新速率的实时准确状态估计。

基于激光雷达-imu测距的先验技术，旋转约束优化方法进一步优化了最终姿态和生成的点云图。即使在某些激光雷达退化的情况下，它也可以确保一致且可靠的估计。

该算法已通过广泛的室内和室外测试验证。它优于传统的仅使用激光雷达或松散耦合的激光雷达-imu算法。