摘要

代码：github
原文：原文

摘要：激光雷达里程计是自动驾驶和自主移动机器人领域的关键技术。然而，当前的大多数工作集中在非线性优化方法上，使用传统的迭代扩展卡尔曼滤波（IEKF）框架来解决问题仍然存在许多挑战：IEKF仅在观测方程上迭代，依赖于初始状态的粗略估计，这不足以完全消除输入点云中的运动失真；在复杂运动的状态估计中，系统过程噪声难以确定；以及不同传感器载体之间的运动模型变化。为了解决这些问题，我们提出了基于双迭代扩展卡尔曼滤波的激光雷达里程计（I2EKF-LO）。这种方法不仅在观测方程上迭代，还利用状态更新来迭代减轻激光雷达点云中的运动失真。此外，它根据先前预测的置信水平动态调整过程噪声，并为不同的传感器载体建立运动模型，以实现准确高效的状态估计。全面的实验表明，I2EKF-LO在激光雷达里程计领域达到了卓越的精度和计算效率水平。

图1. 在 HIT-TIB 数据集（步行序列）中 $I^2$ EKF-LO 的映射结果。当 $I^2$ EKF-LO 仅在观测过程上迭代时，它退化为普通的 IEKF-LO。与 IEKF-LO 相比，$I^2$ EKF-LO 在细节处理上表现更好。而 KISS-ICP 在使用相同分辨率的情况下，在这个序列上完全失败了。

一、介绍

随着自动驾驶、增强现实（AR）、虚拟现实（VR）等技术的进步，同时定位与地图构建（SLAM）成为了一个重要的研究领域[1]。LiDAR传感器（包括机械旋转LiDAR和固态LiDAR）因其能够直接获取环境深度信息、具有高精度测量以及不易受外部干扰的特点，广泛受到青睐。它们通常与惯性测量单元（IMU）配合使用，实时估计传感器载体的运动状态，并构建环境的点云地图。在LiDAR-惯性里程计（LIO）领域，FAST-LIO[2][3]代表了一个重要的成果，通过迭代扩展卡尔曼滤波器（IEKF）紧密集成IMU和LiDAR测量，从而实现高精度和低时间开销的状态估计。尽管LIO系统通过加入IMU测量能够达到更高的精度，但在以下场景中，LiDAR里程计仍然具有重要意义：（1）载体的运动状态超出IMU的测量范围，或IMU发生故障[4]；（2）载体在非惯性参考系下运行[5]；（3）在LiDAR和IMU外部参数标定过程中[6][7]。

近年来，LiDAR里程计领域涌现出许多优秀的工作，如F-LOAM[8]、CT-ICP[9]、KISS-ICP[10]、Traj-LO[11]、MULLS[12]和DLO[13]等，其中大多数基于非线性优化方法。尽管FAST-LIO系列展示了迭代扩展卡尔曼滤波器在LIO系统中的卓越表现，但基于卡尔曼滤波框架的LiDAR里程计系统并不常见。FAST-LO[6]继承了FAST-LIO在IEKF框架下的思想，通过用统一运动模型替代IMU来提供系统先验。然而，将IEKF框架直接应用于LiDAR里程计时，暴露出了几个问题：

没有额外传感器支持的LO系统通常从较差的先验开始。IEKF只对观测过程进行迭代，并未完全消除与当前状态相关的运动畸变对输入点云的影响。
在基于IEKF的LIO系统中，过程噪声被认为是恒定的。然而，LO系统对统一运动假设的信任度随着系统运动状态的变化而变化，导致过程噪声的波动。
在状态转移过程中，大多数现有方法将旋转和位移分开处理。然而，对于特定的传感器载体运动模型，旋转和位移之间可能存在耦合。
为了解决这些挑战，我们提出了I2EKF-LO，一种基于双重迭代扩展卡尔曼滤波器的LiDAR里程计。术语“I2”表示双重迭代过程。它使用我们为LiDAR里程计问题设计的I2EKF，精确消除输入点云中的运动畸变。我们还考虑到不同传感器载体运动模式的差异，并在状态估计过程中动态调整系统过程噪声。具体来说，我们的贡献如下：

我们提出了I2EKF，一个全面的双重迭代扩展卡尔曼滤波器框架，用于LiDAR里程计，通过对观测方程和预测过程的畸变去除阶段进行迭代，从而提高点云质量，进而提高状态估计的精度。
通过利用测量创新[14]，我们量化了当前状态下统一运动模型的置信度，动态调整过程噪声，以增强系统在各种运动强度下的鲁棒性。
考虑到不同传感器载体的运动模型之间旋转和位移的耦合关系，我们引入了SE(3)变换，以灵活管理旋转和位移操作。
在公共数据集和实际环境中的广泛实验测试表明，I2EKF-LO在当前LiDAR里程计算法中达到了卓越的精度和计算效率。为了进一步为社区做出贡献，我们公开了源代码。

二、相关工作

LOAM [15] 是LiDAR里程计的经典代表之一。它根据点云的空间曲率将点云分为两类：平面特征点和线特征点，分别构造点到平面的距离约束和点到线的距离约束，并通过非线性优化方法求解状态。LOAM以10 Hz的频率实时运行，利用两种点云配准模式：扫描到扫描和扫描到地图。然而，由于处理数千个点云需要大量计算且缺乏有效的地图管理策略，其计算效率受到限制。F-LOAM是LOAM的改进版本，省略了扫描到扫描的过程，改为直接进行扫描到地图的配准，显著提高了系统的计算效率，同时保持了较高的精度。然而，F-LOAM未考虑点云的运动畸变，因此在实际环境中的表现有所折扣。Lego-LOAM [16] 是对LOAM的另一改进，通过地面特征提取和两步优化过程提高了精度和操作效率，使其更适合资源受限的地面机器人。然而，它在适应非水平LiDAR安装的车辆时效果较差。LoamLivox [17] 是针对固态LiDAR定制的，解决了狭窄视场和不规则扫描模式的挑战，通过高质量的特征提取和动态目标滤波，提高了精度和效率，但在剧烈运动下会变得不稳定。

Dellenbach等人[9] 提出了CT-ICP，介绍了将传感器运动的连续时间表示作为时间上的连续函数的概念，估计点云帧开始和结束时车辆的运动状态。CT-ICP在Kitti数据集[18]上展示了卓越的精度，但状态维度的增加导致了更高的计算成本和收敛问题。KISS-ICP是一种简单高效的LiDAR里程计系统，通过点对点ICP估计状态，无需依赖大量参数，确保在各种环境和运动模式下的稳定运行。然而，它在点云稀疏时效果较差。Yuan等人[19] 引入了VoxelMap，这是一种基于滤波框架的LiDAR里程计，考虑了LiDAR测量的不确定性，并提出了一种自适应体素地图表示。它在Kitti数据集上展现了出色的精度，更多地关注于地图特征的表示和维护，而不是直接处理点云中的运动畸变。

我们的工作受到FAST-LIO [2] 的启发，它构建了系统运动的离散模型，利用IMU集成进行状态预测，并通过IMU测量反向传播精确消除点云中的运动畸变。通过将点到平面的距离作为残差观测，并使用IEKF（迭代扩展卡尔曼滤波器）紧密耦合IMU和LiDAR测量，进行状态估计。FAST-LIO2 [3] 引入了增量kdtree [20] 用于高效、快速的地图维护。I2EKF-LO继承了FAST-LIO系列的优势，更适合于IMU测量信息缺失且仅有点云输入的工作环境。

三、方法

A. 系统概览

系统架构如图 I 所示。EKF-LO 根据传感器载体的类型构建了一个统一的运动模型，作为系统状态的预测先验。根据运动的强度，可能需要将传入的点云分割成帧。利用 $I^2$ EKF，系统迭代消除点云中的运动失真，识别点到平面的匹配关系，并构建点到平面的距离残差。此外，基于对统一运动假设的信心，它动态调整过程噪声，最后整合先验来更新状态。点云使用后验状态注册到世界坐标系中，并使用增量式 k-d 树[20]进行管理。

B. 运动学模型

系统的状态向量定义为：

$$x\triangleq {\left[R^G{t}^G{v}^G\omega \right]}^T\in \mathcal{M},(1)$$

其中状态空间 $\mathcal{M}=SO(3)\times {\mathbb{R}}^9$，$G$ 表示全局坐标系，$R^G,t^G$ 分别表示在点云帧结束时，激光雷达坐标系在全局框架中的旋转和平移。激光雷达在全局框架中的线性速度由 $v^G$ 表示，其在机体框架中的角速度由 $\omega$ 表示。这些速度被建模为由高斯噪声 $n_v,n_{\omega }$ 驱动的随机游走。

1. 统一运动模型：基于统一运动模型的离散状态转移如下所示：

$$x_{k+1}=x_k⊞\left(f\left(x_k,w_k\right)\right),(2)$$

这里，表示在[2]、[3]中定义的状态流形上的“加号”，$w_k={\left[n_{v,k}{n}_{\omega ,k}\right]}^T$ 表示过程噪声。

1. 统一运动模型：对于手持设备，统一运动假设如下：在第 k 个激光雷达点云帧的持续时间 $t_k$ 内，假设全局框架中的线性速度和机体框架中的角速度是恒定的，并且等于第 (k-1) 帧期间的速度。状态转移函数定义为

$$f(x,w)=\left[\begin{array}{c}\omega \Delta t\\ v^G\Delta t\\ n_v\Delta t\\ n_{\omega }\Delta t\end{array}\right].(3)$$

2. 统一运动模型：对于地面移动机器人（其中旋转和平移是强耦合的），统一运动假设定义为：在第 k 个激光雷达点云帧的持续时间 $t_k$ 内，假设机体框架中的线性速度和角速度是恒定的，并且等于第$(k-1)$帧期间的速度。状态转移函数定义为

$$\begin{array}{r}f(x,w)=\left[\begin{array}{c}\omega \Delta t\\ (R^G⊞\omega \Delta t)(R^G{)}^{-1}{v}^G\Delta t\\ ((R^G⊞\omega \Delta t)(R^G{)}^{-1}-I)v^G+n_v\Delta t\\ n_{\omega }\Delta t\end{array}\right].\end{array}$$

两个连续激光雷达点云帧之间的时间间隔用 $\Delta t$ 表示，便于定义系统随时间的动态演变。

2. 观测模型：与[3]中概述的方法一致，提出的 $I^2$ EKF-LO 框架不从原始点云中提取特征。相反，它基于点到平面距离的直接计算构建了一个观测模型。这种方法利用点云中固有的详细信息来提高状态估计的准确性。观测关系形式化为如下：

$$0=h_j(x_k,n_j^L)\triangleq u_j^T(R_k^G(p_j^L+n_j^L)+t_k^G-q_j^G),(5)$$

其中 $n_j^L$ 表示激光雷达测量噪声，$p_j^L$ 表示激光雷达帧中点的坐标。向量 $u_j^T$ 是与地图中点 $p_j^L$ 匹配的平面的法向量，而 $q_j^G$ 是此平面上的一个点。

C. 点云预处理

固态激光雷达的失真移除过程的准确性，其特点是其非重复性扫描模式，显著受到初始状态估计的影响。受到[17]的启发，$I^2$ EKF-LO框架包含了一个预处理阶段，当前帧的点云，跨越一个时间间隔 $\Delta t$，被时间上细分为n个段。这种分割将每个段的时间间隔减少到 $\frac{1}{n}\Delta t$。这样的预处理策略缩小了由统一运动假设提供的初始状态和真实状态之间的差距，从而在涉及快速车辆运动的场景中，便于获得更准确的结果。

四. 双迭代扩展卡尔曼滤波器

A. IEKF回顾

让我们回顾一下为什么FAST-LIO系列的IEKF框架需要迭代求解？首先，点云匹配的质量取决于当前帧状态估计的准确性。使用更接近真实状态的状态将当前点云转换到世界坐标系中，可以实现更精确的匹配。这种改进的匹配反过来有助于得到更精细的解决方案。此外，由于观测方程的非线性，使用更接近真实状态的状态近似进行线性化有助于最小化线性化过程中引入的误差。这种增强的线性化程度可以带来更好的解决方案。这种“鸡生蛋，蛋生鸡”的关系需要通过迭代来求解最优值。IEKF中的迭代专门集中在观测组件上。在激光雷达里程计问题中，没有IMU测量的辅助，初始值的预测质量较差，这将在失真校正过程中对输入点云带来大的误差干扰。同样，点云失真校正的过程也遵循上述循环逻辑。只有使用接近真实状态的状态来校正点云，才能获得更准确的点云数据，进而促进后续迭代中更好的估计。

因此，我们引入了双迭代扩展卡尔曼滤波器（$I^2$ EKF），它在预测过程的失真移除步骤上进行额外的迭代，以最小化点云失真的影响，同时在观测过程上进行迭代，以减轻不匹配和非线性效应。此外，认识到在不同车辆运动状态下对统一运动模型的信心不同，$I^2$ EKF在迭代过程中动态调整过程噪声，以增强系统在不同运动条件下的鲁棒性。

B. $I^2$ EKF中统一运动模型的前向传播

与[2]类似，$I^2$ EKF-LO采用以下前向传播关系：
$$\begin{array}{l}{\hat{x}}_{k+1}={\bar{x}}_k⊞f\left({\bar{x}}_k,0\right),\\ {\hat{P}}_{k+1}=F_{{\tilde{x}}_k}{\bar{P}}_k{F}_{{\tilde{x}}_k}^T+F_{w_k}{Q}_k{F}_{w_k}^T,\end{array}(6)$$
其中 ${\bar{x}}_k$ 和 ${\bar{P}}_k$ 分别表示第 k 帧的后验状态和协方差。同样，${\hat{x}}_{k+1}$ 和 ${\hat{P}}_{k+1}$ 表示第 k+1 帧的预测状态和协方差。矩阵 $Q_k$ 被定义为噪声 $w_k$ 的协方差矩阵。矩阵 $F_{{\tilde{x}}_k}$ 和 $F_{w_k}$ 具体定义如下：

$$\begin{array}{rl}& F_{{\tilde{x}}_k}=\frac{\partial \left(x_{k+1}⊞{\hat{x}}_{k+1}\right)}{\partial {\tilde{x}}_k}{|}_{{\tilde{x}}_k=0,w_k=0},\\ & F_{w_k}={\frac{\partial \left(x_{k+1}⊞{\hat{x}}_{k+1}\right)}{\partial w_k}|}_{{\tilde{x}}_k=0,w_k=0},\end{array}(7)$$

其中误差状态向量由 ${\tilde{x}}_k=x_k\boxminus {\hat{x}}_k$ 表示，$\square$ 表示在状态流形上的“减号”，这在[2]、[3]中定义。

C. PEKF中的迭代更新
每次迭代更新包括两个主要部分：迭代失真校正更新和迭代观测更新。此外，基于观测迭代中第一次收敛的结果，我们动态调整过程噪声。

1. 迭代失真校正更新：鉴于激光雷达传感器在收集每个点云帧期间不是静止的，原始点云包含运动引起的失真。为了减轻这些失真，$I^{2}EKF$将当前帧 $k$ 的点云，收集时间为 $\Delta t_k$，转换到帧 k 结束时的时间 $t_k$ 的激光雷达坐标系中。对于在帧 k 中的时间为 $t_i$ 生成的激光雷达坐标系中的点 $p_i^{L_i}$，并且给定帧 k-1 的后验状态变换 ${\bar{T}}_{k-1}^G$，$I^{2}EKF$ 寻找时间 $t_i$ 时激光雷达坐标系在世界坐标系中的变换 ${\hat{T}}_i^G$，从而获得失真校正后的点。
   $$p_i^{L_k}={\left({\hat{T}}_k^G\right)}^{-1}{\hat{T}}_i^G{p}_i^{L_i},(8)$$

其中

$$T^G=\left[R^G|t^G\right].(9)$$

关于第III-B节提到的两种统一运动模型，各自的公式如下：
对于统一运动模型 1

$$\begin{array}{rl}{\hat{R}}_i^G& ={\bar{R}}_{k-1}^G\text{.interpolate(scale, }{R}_k^G\text{)}\\ {\hat{t}}_i^G& ={\bar{t}}_{k-1}^G+\text{scale}(t_k^G-t_{k-1}^G)\\ \text{scale}& =\frac{t_i-t_{k-1}}{t_k-t_{k-1}}\end{array}(10)$$

对于统一运动模型 2

$$\begin{array}{rl}{\hat{T}}_i^G& ={\bar{T}}_{k-1}^G\text{.interpolate(scale, }{\hat{T}}_k^G\text{)}\\ \text{scale}& =\frac{t_i-t_{k-1}}{t_k-t_{k-1}}\end{array}(11)$$

2. 迭代观测更新：观测方程在第 ${\kappa }^{th}$ 次观测迭代的状态处展开到一阶。对于第 k 帧中的第 j 个点，观测方程如下

$$\begin{array}{rl}0& =h_j\left(x_k,n_j^L\right)\simeq h_j\left({\hat{x}}_k^{\kappa },0\right)+H_j^{\kappa }{\tilde{x}}_k^{\kappa }+v_j\\ & =z_j^{\kappa }+H_j^{\kappa }{\tilde{x}}_k^{\kappa }+v_j.\end{array}$$

在此背景下，$H_j^{\kappa }$ 表示在 ${\tilde{x}}_k^{\kappa }=0$ 处评估的 $h_j\left({\hat{x}}_k^{\kappa }⊞{\tilde{x}}_k^{\kappa },n_j^L\right)$ 的雅可比矩阵，其中 $v_j$ 来自原始测量噪声 $n_j^L$，分布为 $\mathcal{N}\left(0,R_j\right)$。利用最新的点云匹配关系，根据(5)计算观测残差和观测矩阵。通过整合统一运动先验，制定了以下最大后验估计(MAP)问题：

$$\underset{{\tilde{x}}_k^{\kappa }}{\min }\left({\Vert x_k\boxminus {\hat{x}}_k\Vert }_{{\hat{P}}_k^{-1}}^2+\sum _{j=1}^m{\Vert z_j^{\kappa }+H_j^{\kappa }{\tilde{x}}_k^{\kappa }\Vert }_{R_j^{-1}}^2\right),(13)$$

$$\text{令}H={\left[H_1^{{\kappa }^T},\cdots ,H_m^{{\kappa }^T}\right]}^T,R=\mathrm{diag}\left(R_1,\cdots R_m\right),P={\hat{P}}_k,$$$$\text{并且}{z}_k^{\kappa }={\left[z_1^{{\kappa }^T},\cdots ,z_m^{{\kappa }^T}\right]}^T.$$更新状态的公式如下

$${\hat{x}}_k^{\kappa +1}={\hat{x}}_k^{\kappa }⊞\left(-K{z}_k^{\kappa }-(I-KH)\left({\hat{x}}_k^{\kappa }\boxminus {\hat{x}}_k\right)\right),$$

其中

$$K={\left(H^T{R}^{-1}H+P^{-1}\right)}^{-1}{H}^T{R}^{-1}.$$

3. 过程噪声更新：在载体的实际运动中，对恒定速度假设的随时间变化。与将过程噪声视为常数的一般IEKF框架不同，$I^2$ EKF动态校正过程噪声。受到[14]的启发，滤波器的过程噪声关系如下：

$$w_k=K{d}_k,(16)$$

其中 $d_k$ 是创新序列[14]，K 是卡尔曼增益。因此，

$$Q_k=E\left[{\hat{w}}_k{\hat{w}}_k^T\right]=KE\left[d_k{d}_k^T\right]K^T.(17)$$

因此，利用观测迭代第一次收敛时获得的创新序列，我们量化了点云点平面残差观测与恒定速度假设预测之间的差异，反映了对恒定速度假设的可信度。状态向量中速度和角速度的过程噪声协方差分别定义为 $Q_v$ 和 $Q_{\omega }$。

$$\begin{array}{rl}{Q}_v& ={\mathrm{cov}}_v^{scale}\Delta t^2{I}_{3\times 3}\\ Q_{\omega }& ={\mathrm{cov}}_{\omega }^{scale}\Delta t^2{I}_{3\times 3}\end{array}$$

根据我们从大量测试中的经验，${\mathrm{cov}}_v^{\text{scale}}$ 和 ${\mathrm{cov}}_{\omega }^{\text{scale}}$ 的适当值在0.01到100之间。${\mathrm{cov}}_v^{\text{scale}}$ 和 ${\mathrm{cov}}_{\omega }^{\text{scale}}$ 的值越大，对传感器剧烈运动的敏感性就越低。因此，我们建立了以下映射关系：
$$Q_k=\left(\frac{100}{1+e^{(-\alpha ||K{d}_k||+\gamma )}}+0.01\right)\Delta t^2{I}_{3\times 3},(19)$$

其中 $\alpha$ 和 $\gamma$ 是基于运动规模的超参数。通过创新序列的变化内部校正的过程噪声，适应不同强度的运动。

完整的 $I^2$ EKF状态估计过程概述为算法1。

五、实验

实验平台使用Agilex Scout Mini作为移动底盘，特点是四轮差动转向。它配备了Livox Mid-360激光雷达，并使用Nvidia AGX Xavier作为计算平台。


图5. Urbanloco数据集中的映射结果。在激光雷达返回到原点附近时，(b) CT-ICP、© KISS-ICP和(d) F-LOAM在z轴方向上存在显著误差。而(a) I2EKF-LO在z轴方向上的误差要小得多。



图6. normal和 dance序列的映射结果如下：(a)-© 对应于正常序列，其中(a)显示了标准$I2EKF-LO$的映射结果，(b)和©分别显示了使用固定过程噪声（0.01和100）的映射结果；(d)-(f) 对应于舞蹈数据集，其中(d)显示了标准$I2EKF-LO$的映射结果，(e)和(f)分别显示了使用固定过程噪声（0.01和100）的映射结果。