0. 简介

一般来说，当系统经过不规则的地形时候，机器人自身会存在激烈运动会导致激光雷达扫描中的运动畸变，从而可能降低状态估计和建图的精度。虽然已经有一些方法用于缓解这种影响，但它们仍然过于简单或计算成本过高，难以应用于资源受限的移动机器人。之前这个团队开发了《经典文献阅读之—DLO》这套方法。该团队在23年又提出了《Direct LiDAR-Inertial Odometry: Lightweight LIO with Continuous-Time Motion Correction》一文。该轻量级激光雷达惯性导航系统算法，采用一种新的粗到细的方法构建连续时间轨迹以进行精确的运动校正。该方法的关键在于构建一组仅由时间参数化的解析方程，使得逐点去畸变的过程可以快速且可并行化。这种方法之所以可行，是因为我们的非线性几何观察器具有强大的收敛性质，可以为IMU的敏感积分步骤提供可靠的状态估计初始化。相关代码可以在GIthub上找到。

1. 主要贡献

本文提出了一种名为Direct LiDAR-Inertial Odometry (DLIO)的快速可靠的里程计算法，它提供了精确的定位和详细的三维建图（见图1），具有四个主要贡献。

1. 首先，我们提出了一种新的粗到细的技术，用于构建连续时间轨迹，其中导出了一组具有恒定加加速度和角加速度运动模型的解析方程，以进行快速和可并行化的逐点运动校正。
2. 其次，提出了一种新的简化结构，将运动校正和先验构建合并为一步，并直接执行扫描到地图的配准，大大降低了算法的总体计算负载。（Point-LIO也在做这个工作）
3. 第三，我们利用了一种新的非线性几何观察器[10]，它具有强大的性能保证，对于实现前两个贡献至关重要，在管道中以最小的计算复杂度鲁棒地生成机器人完整状态的准确估计。
4. 最后，我们通过使用多个数据集对最先进的技术进行广泛的实验验证，证明了我们方法的有效性。

2. 系统概述

DLIO是一种轻量级的LIO算法，通过一个包含两个主要组件和三个创新的独特架构生成机器人状态估计和几何地图（见图2）。第一个组件是一个快速的扫描匹配器，通过与提取的局部子地图进行对齐，将密集的、经过运动校正的点云注册到机器人的地图上。在W中逐点进行连续时间积分，确保修正后的点云具有最大的图像保真度，同时为GICP优化建立先验。在第二个组件中，一个非线性几何观察器[10]使用第一个组件的姿态输出更新系统状态，提供高速和可靠的姿态、速度和传感器偏差估计，这些估计具有全局收敛性。然后，这些估计将初始化下一个运动校正、扫描匹配和状态更新的迭代。

图2. 系统架构。DLIO的轻量级架构将运动校正和先验构建合并为一步，并去除了以前用于基于激光雷达的里程计所需的扫描对扫描模块。在W中逐点进行连续时间积分，确保修正后的点云具有最大的保真度，并通过自定义的基于GICP的扫描匹配器注册到机器人的地图上。随后，系统的状态由一个具有强大收敛性质的非线性几何观察器[10]进行更新，这些姿态、速度和偏差的估计然后初始化下一次迭代。

3. 符号表示

假设在时间t_k开始的单次激光雷达扫描的点云表示为P_k，并用k进行索引。点云P_k由相对于扫描开始时间的时间差∆t^n_k 测量的点p^n_k∈\mathbb{R}^3组成，其中n = 1，…，N，N是扫描中的总点数。世界坐标系表示为W，机器人坐标系表示为R，位于其重心，其中x指向前方，y指向左侧，z指向上方。IMU的坐标系表示为B，激光雷达的坐标系表示为L，机器人在索引k处的状态向量X_k定义为元组。

其中p^W ∈ \mathbb{R}^3是机器人的位置，q^W是四元数编码的方向，采用Hamilton符号表示，位于\mathbb{S}^3，v^W ∈\mathbb{R}^3是机器人的速度，b^a ∈ \mathbb{R}^3是加速度计的偏差，b^ω ∈ \mathbb{R}^3是陀螺仪的偏差。IMU的测量值\hat{a}和\hat{ω}被建模为

并用i = 1，…，M进行索引，表示在时钟时间t_{k-1}和t_k之间的M次测量。为了简单起见，除非另有说明，否则索引k 和 i 分别出现在激光雷达和IMU速率上，并以这种方式书写。原始传感器测量值a_i和ω_i包含偏差b_i和白噪声n_i，g是旋转后的重力向量。在本文中，我们解决以下问题：给定来自激光雷达的累积点云P_k 和由IMU在接收到每个扫描之间采样的测量值a_i 和 ω_i，估计机器人的状态\hat{X}^W_i和几何地图\hat{M}^W_k。

4. 预处理

DLIO的输入是由360度机械LiDAR（如Ouster或Velodyne（10-20Hz））收集的稠密的3D点云，以及来自6轴IMU的时间同步的线性加速度和角速度测量，IMU速率要比雷达高得多（100-500Hz）。在下游任务之前，所有传感器数据都通过外部校准转换为位于机器人重心处的R。对于IMU，必须考虑将刚体上的线性加速度测量位移的影响，如果该传感器与重心不重合，则通过考虑所有线性加速度在R处通过角速度和IMU偏移之间的叉积来完成。为了最小化信息丢失，除了在原点周围使用1m^3的框滤波器以删除可能来自机器人本身的点，并使用轻量级体素滤波器进行更高分辨率的点云处理外，我们不对点云进行预处理。这使我们的工作与其他试图检测特征（例如角落、边缘或surfels）或通过体素滤波器大量下采样云的工作区分开来。

5. 带联合先验的连续时间运动校正(重点内容)

旋转LiDAR传感器收集的点云在移动过程中会因旋转激光阵列在扫描过程中的不同时刻收集点而受到运动失真的影响。我们不再假设简单的运动（例如，恒定速度）来捕捉精细的运动，而是使用更准确的恒定加速度和角加速度模型通过两步粗到细的传播方案为每个点计算唯一的变换。此策略旨在最小化由于IMU的采样率和IMU与LiDAR点测量之间的时间偏移而引起的误差。在W中通过数值IMU积分[29]首先粗略构建扫描的轨迹，随后通过解决一组解析连续时间方程来进行细化（图3）。

图3. 粗到细的点云去畸变。通过两步过程去畸变扭曲的点p^{L_0}(A)，首先在扫描之间积分IMU测量，然后在连续时间中求解独特的变换（C）以将原始点p^{L_0}去畸变为p^∗（B）

假设t_k是接收到具有N个累积点的点云P^R_k的时钟时间，并且t_k + ∆t^n_k是云中点p^n_k的时间戳。为了近似每个点在W中的位置，我们首先通过在t_{k-1}和t_k + ∆t^N_k之间积分IMU测量来获得运动估计：

对于i = 1，…，M，其中M是两个扫描之间IMU测量的数量，其中\hat{j}_i = \frac{1}{∆t_i}(\hat{R}(\hat{q}_i)\hat{a}_i - \hat{R}(\hat{q}_{i-1})\hat{a}_{i-1})，\hat{α}_i = \frac{1}{∆t_i}(\hat{ω}_i - \hat{ω}_{i-1})分别是估计的线性jerk和角加速度。与\hat{p}_i和\hat{q}_i相对应的一组齐次变换\hat{T}^W_i∈\mathbb{SE}（3）然后定义了扫描期间粗略的离散时间轨迹。然后，从最近的前置变换到每个点p^n_k的解析连续时间解决方案恢复特定于点的去畸变变换\hat{T}^{W∗}_n，使得

其中，i-1和i分别对应于最近的前一个和后一个IMU测量，t是点p^n_k和最近的前一个IMU之间的时间戳，\hat{T}^{W∗}_n是对于p^n_k所对应的p^∗和q^∗的变换（图4）。请注意，（5）仅由t参数化，因此可以查询任何所需时间的变换以构建连续时间轨迹。

图4. 连续时间运动校正。对于点云中的每个点，通过解决一组闭合形式的运动方程，初始化最接近的IMU测量，计算出一个唯一的变换。这提供了准确且可并行化的连续时间运动校正

这个两步过程的结果是一个运动校正的点云，同时也与W 中的地图大致对齐，因此固有地包含了用于GICP（第6节）的优化先验。重要的是，（4）和（5）除了准确的初始姿态\hat{q}_0（为了正确补偿重力向量）外，还依赖于\hat{v}^W_0，速度的初始估计，b^a_k和b^ω_k，IMU偏差的估计，在运动校正时。因此，我们强调，我们方法可靠的关键在于利用DLIO的非线性几何观察器[10]保证这些项的全局收敛性，前提是扫描匹配返回准确的解决方案。

6. 帧与地图匹配(重点内容)

通过同时校正运动畸变并将GICP优化先验纳入点云中，DLIO可以直接执行扫描到地图的配准，绕过了以前方法中需要的扫描到扫描过程。这个配准被构建为一个非线性优化问题，最小化当前扫描和提取的子图之间对应点/平面的距离。让\hat{P}^W_k是W中经过校正的云，\hat{S}^W_k是通过[20]提取的基于关键帧的子图。然后，扫描到地图优化的目标是找到一个变换∆\hat{T}^k，更好地对齐点云，使得

GICP残差误差E被定义为

其中C是\hat{P}^W_k 和\hat{S}^W_k 之间一组对应点集，d_c = \hat{s}^c_k−∆T_k\hat{p}^c_k ，\hat{p}^c_k ∈ \hat{P}^W_k ，\hat{s}^c_k ∈ \hat{S}^W_k ，∀c ∈ C。C^P_{k,c}和C^S_{k,c} 是点云\hat{P}^W_k 和子图\hat{S}^W_k 的估计协方差矩阵。然后，根据[13]的方法，这个点对平面的配准公式被转换为平面对平面的优化，通过对协方差矩阵C^P_{k,c}和C^S_{k,c}进行正则化，使其具有(1, 1, \epsilon)的特征值，其中\epsilon代表表面法向方向的低不确定性。得到的∆\hat{T}_k代表一个最优的校正变换，可以更好地全局对齐\hat{P}^W_k 到\hat{S}^W_k ，因此\hat{T}^W_k = \hat{∆T}_k\hat{T}^W_M（其中\hat{T}^W_M$$是最后一个点的IMU积分）是全局优化的机器人姿态，用于地图构建，并作为非线性几何观测器的更新信号

7. 几何观测

通过扫描到地图的对齐计算出的变换\hat{T}^W_k与IMU测量融合，通过一种新的分层非线性几何观测器生成完整的状态估计\hat{X}_k。观测器的完整分析可以在[10]中找到，但总结一下，可以证明在最小计算量的情况下，\hat{X} 将在确定性环境中全局收敛到X。证明利用收缩理论首先证明四元数估计以指数方式收敛到接近真实四元数的区域。然后，方向估计作为另一个收缩观测器的输入，估计平移状态。这种架构形成了一个收缩层次结构，保证估计收敛到它们的真实值。这种强收敛结果是与其他融合方案（例如过滤或姿态图优化）相比的主要优势，后者即使在最理想的情况下也具有最小的收敛保证。此外，观测器的状态估计的固有平滑性使其适用于控制。本文中使用的观测器是[10]中的特殊情况。

令γ_l∈{1,…,5}为正常数，∆t^+_k为GICP姿态之间的时间。如果q_e := (q^◦_e , \tilde{q}_e) = \hat{q}^∗_i ⊗ \hat{q}_k 和p_e = \hat{p}_k−\hat{p}_i （传播和测量姿态之间的误差），则状态校正的形式为：

注意，式（7）是分层的，因为方向更新（前两个方程）完全与平移更新（最后三个方程）解耦。此外，（7）是一个完全非线性的更新，可以保证状态估计足够准确，以便仅使用IMU先验进行扫描到地图的配准，而不需要扫描到扫描的匹配。