（DLIO）: 具有连续时间运动修正的轻量级LIO算法
直接激光雷达-惯性里程计（DLIO）: 具有连续时间运动修正的轻量级LIO算法
作者: Kenny Chen, Ryan Nemiroff, Brett T. Lopez
摘要
在高速飞行或穿越不规则地形的过程中，激光雷达扫描容易因运动引起失真，从而影响状态估计和地图构建。为了解决这一问题，本文提出了一种轻量级的激光雷达-惯性里程计算法（DLIO），该算法采用一种由粗到精的方法来构建连续时间轨迹进行精确的运动修正。我们的方法的关键在于构建一组仅由时间参数化的解析方程，从而实现快速并行的逐点去畸变。这种方法之所以可行，是因为我们的非线性几何观察器具有强大的收敛性，它为初始化敏感的IMU积分步骤提供了精确的状态估计。此外，通过同时进行运动修正和先验生成，并且跳过扫描到扫描匹配，直接将每次扫描注册到地图上，DLIO的简化架构比当前最先进的算法在计算上效率提升了近20%，精度提高了12%。我们通过使用多个公共基准测试和自采数据集，验证了DLIO在定位精度、地图质量和较低计算开销方面的优越性。
介绍
准确的实时状态估计和地图构建是移动机器人在未知环境中感知、规划和导航所需的基本能力。激光雷达（LiDAR）定位技术由于传感器更为紧凑且精确，近年来已经成为多种移动平台（如无人机）的可行选项。因此，研究人员开发了多种新的激光雷达里程计（LO）和激光雷达-惯性里程计（LIO）算法，这些算法通常由于激光雷达的卓越测距和深度测量精度，而优于基于视觉的方法。然而，在开发可靠且精确的以激光雷达为中心的算法时仍然存在一些根本性挑战，特别是对于执行灵活动作或穿越不平坦地形的机器人来说。这些激烈的运动会在点云中引入显著的失真，从而破坏扫描匹配过程，导致严重的定位误差和地图变形。
现有的一些算法试图补偿这种失真效应，在结构化环境中表现良好，尤其适用于非全向系统（如自动驾驶）。但它们在不规则条件下的表现可能会由于简单的运动模型、离散化导致的精度损失、或计算效率低下而恶化。例如， L O A M LOAM LOAM 等算法假设在扫描获取期间速度恒定，这在简单且可预测的轨迹中效果良好，但在剧烈加速时快速失效。另一方面， F A S T − L I O FAST-LIO FAST−LIO 和 F A S T − L I O 2 FAST-LIO2 FAST−LIO2 使用反向传播技术来减少每个点的失真，但它们的方法可能会因为随着时间推移累积的积分误差而导致精度下降。最近，连续时间方法试图在一组控制点上拟合平滑轨迹[ 7 7 7, 8 8 8]，或者通过增加自由变量[ 9 9 9]来增强扫描匹配优化，但这些方法仍然对轨迹（即平滑运动）作出较强的假设，或者计算代价太高，无法在资源受限的平台上实时运行。
为了解决这些问题，本文提出了直接激光雷达-惯性里程计（DLIO），这是一种快速且可靠的里程计算法，可提供准确的定位和详细的三维地图。图1展示了DLIO生成的详细地图及其定位过程。本文的四个主要贡献如下：
- 我们提出了一种由粗到精的连续时间轨迹构建技术，其中推导出基于常数加速度和角加速度运动模型的一组解析方程，用于快速并行化逐点运动修正。
- 提出了一个新的简化架构，将运动修正与先验构建合并为一步，直接执行扫描到地图的注册，显著减少了算法的整体计算开销。
- 我们利用了一个新的非线性几何观察器[ 10 10 10]，它具有强大的性能保证，这是实现前两项贡献的关键，可以在流水线中稳健地生成机器人完整状态的准确估计，且计算复杂度极低。
- 最后，我们通过多个数据集和与最先进的算法进行的广泛实验，验证了我们方法的有效性。
图1. 实时定位和密集地图构建。 DLIO通过实时估计机器人姿态、速度和传感器偏差来生成详细的地图。(A) 我们定制的空中平台，位于UCLA的Royce Hall旁边。(B) 由DLIO生成的Royce Hall及其周边的俯视图。© 树木的特写，展示了DLIO能够捕捉到的地图中的精细细节。颜色表示点回波强度。
相关工作
几何激光雷达里程计算法依赖于通过解决非线性最小二乘问题来对齐点云，该问题旨在最小化相应点和/或平面之间的误差。为了找到点/平面对应关系，方法如迭代最近点（ICP）算法[ 11 11 11]、[ 12 12 12] 或广义ICP（GICP）[ 13 13 13]，通过递归匹配实体直到对齐收敛到局部最小值。然而，当确定大集合点的对应关系时，通常会观察到较慢的收敛时间。因此，基于特征的方法[ 2 2 2]、[ 5 5 5]、[ 14 14 14]-[ 17 17 17] 尝试只提取扫描中的最显著数据点（例如，角点和边缘）以减少计算时间。然而，有用的点经常被丢弃，因为特征提取的效果高度依赖于具体的实现方式。相反，密集方法[ 6 6 6]、[ 18 18 18]-[ 21 21 21] 直接对齐获取的扫描，但通常严重依赖于激进的体素化（这可能会改变重要的数据对应关系）来实现实时性能。
激光雷达里程计方法还可以根据其将其他感知模态整合到估计管道中的方式进行广泛分类。松耦合方法[ 2 2 2]、[ 3 3 3]、[ 18 18 18]-[ 20 20 20] 依次处理数据。例如，IMU 测量通过提供优化的先验来增强激光雷达扫描配准。这些方法通常由于激光雷达测量的高精度而表现相当稳健，但由于仅使用了部分可用数据进行估计，因此定位结果可能不够准确。紧耦合方法[ 4 4 4]、[ 6 6 6]、[ 16 16 16]、[ 17 17 17] 则通过联合考虑所有感知模态的测量来提供更高的准确性。这些方法通常采用基于图优化[ 4 4 4]、[ 16 16 16]、[ 17 17 17]、[ 22 22 22] 或随机滤波框架，例如卡尔曼滤波[ 5 5 5]、[ 6 6 6]。然而，与几何观察器[ 23 23 23]、[ 24 24 24] 相比，这些方法即使在最理想的情况下也几乎没有收敛性保证，这可能导致从不一致的传感器融合中产生显著的定位误差，并因错误的扫描位置导致地图变形。
结合额外的传感器还可以帮助纠正运动引起的点云失真。例如，LOAM[ 2 2 2] 通过在扫描匹配和松耦合的IMU上迭代估计传感器姿态，假设恒定速度来补偿旋转失真。同样，LIO-SAM[ 4 4 4] 基于因子图构建激光雷达-惯性里程计，联合优化车体速度，并在实现中通过线性插值旋转运动来去除点的畸变。FAST-LIO[ 5 5 5] 和 FAST-LIO2[ 6 6 6] 则在IMU测量的前向传播后，对点的时间戳进行反向传播步骤，以生成相对于扫描结束时间的相对变换。然而，这些方法（以及其他方法[ 25 25 25], [ 26 26 26]）均在离散时间中操作，可能导致精度损失，从而导致人们对连续时间方法产生了高度兴趣。例如，Elastic LiDAR Fusion[ 7 7 7] 通过优化连续线性轨迹来处理扫描变形，而 Wildcat[ 8 8 8] 和[ 27 27 27] 则通过迭代拟合三次B样条来消除曲面地图中的变形。更近期的CT-ICP[ 9 9 9] 和 ElasticLiDAR++[ 28 28 28] 使用仅基于激光雷达的方法定义了由每次扫描的两个姿态参数化的连续时间轨迹，允许在优化期间进行弹性注册。然而，这些方法在处理高动态运动时建模仍过于简单，或者计算代价过高，难以在实时中可靠工作。
为了解决这些问题，DLIO 提出了一种快速的由粗到精的方法来构建每个扫描间的轨迹，从而进行精确的运动修正。首先通过对IMU测量进行数值积分计算离散的一组姿态，然后通过解析的连续时间方程构建测量样本之间的平滑轨迹，从而查询每个点的唯一去畸变变换。我们的方法快速且通过求解一组解析方程（而非优化问题，如样条拟合），其仅由点的时间戳参数化，且易于并行化。我们的方法也非常准确，因为我们使用了更高阶的运动模型来表示系统的动态，能够捕捉到高频运动，这些运动在拟合控制点的平滑轨迹时可能会丢失。该方法构建在一个简化的LIO架构中，该架构一次性执行运动修正和GICP先验构建，并直接执行扫描到地图的对齐，而无需中间的扫描到扫描匹配；所有这些都得益于我们新的几何观察器在保证状态估计正确性方面的强大收敛性。
图2. 系统架构。 DLIO的轻量级架构将运动修正与先验构建合并为一个步骤，并移除了激光雷达里程计中以前所需的扫描到扫描模块。
W
W
W 坐标系中的逐点连续时间积分确保了修正后的点云具有最大精度，并通过自定义的GICP扫描匹配器将点云注册到机器人的地图上。系统状态随后通过具有强收敛性的非线性几何观察器[
10
10
10]进行更新，估计的姿态、速度和偏差则初始化下一次迭代。
方法
系统概述
DLIO 是一种轻量级的 LIO 算法，它通过一个独特的架构生成机器人状态估计和几何地图。该架构包含两个主要组件并具有三项创新（如图2所示）。第一个组件是一个快速扫描匹配器，它通过与提取的局部子图执行对齐，将密集的、经过运动修正的点云注册到机器人的地图上。在 W W W 坐标系中的逐点连续时间积分保证了修正点云的最大图像保真度，同时为 GICP 优化构建了先验。在第二个组件中，非线性几何观察器[ 10 10 10] 使用第一个组件输出的姿态更新系统状态，以提供高频率且具有全局收敛性保证的姿态、速度和传感器偏差的精确估计。这些估计值会初始化下一次的运动修正、扫描匹配和状态更新迭代。
符号约定
设时间 t k t_k tk 开始的单次激光雷达扫描的点云 P k P_k Pk 由点 p n k ∈ R 3 p_{n_k} \in \mathbb{R}^3 pnk∈R3 组成，这些点相对于扫描开始时间 Δ t n k \Delta t_{n_k} Δtnk 被测量，点云中的点按索引 n = 1 , … , N n = 1, \ldots, N n=1,…,N 标记， N N N 为扫描中的总点数。世界坐标系用 W W W 表示，机器人坐标系用 R R R 表示，位于其重心处， x x x 轴向前， y y y 轴向左， z z z 轴向上。IMU的坐标系用 B B B 表示，激光雷达的坐标系用 L L L 表示，机器人在索引 k k k 处的状态向量 X k X_k Xk 定义为元组：
X k = [ p k W , q k W , v k W , b k a , b k ω ] T X_k = \left[ p^W_k, q^W_k, v^W_k, b^a_k, b^{\omega}_k \right]^T Xk=[pkW,qkW,vkW,bka,bkω]T
其中， p W ∈ R 3 p^W \in \mathbb{R}^3 pW∈R3 表示机器人的位置， q W q^W qW 表示使用哈密顿四元数表示的姿态， v W ∈ R 3 v^W \in \mathbb{R}^3 vW∈R3 表示机器人的速度， b a ∈ R 3 b^a \in \mathbb{R}^3 ba∈R3 表示加速度计的偏差， b ω ∈ R 3 b^{\omega} \in \mathbb{R}^3 bω∈R3 表示陀螺仪的偏差。IMU的测量 a i a_i ai 和 ω i \omega_i ωi 被建模为：
a i = ( a i − g ) + b i a + n i a a_i = (a_i - g) + b^a_i + n^a_i ai=(ai−g)+bia+nia
ω i = ω i + b i ω + n i ω \omega_i = \omega_i + b^{\omega}_i + n^{\omega}_i ωi=ωi+biω+niω
其中， i = 1 , … , M i = 1, \ldots, M i=1,…,M 表示从时间 t k − 1 t_{k-1} tk−1 到 t k t_k tk 之间的 M M M 次测量。传感器的原始测量 a i a_i ai 和 ω i \omega_i ωi 包含偏差 b i b_i bi 和白噪声 n i n_i ni， g g g 表示旋转后的重力矢量。在本工作中，我们解决的问题是：给定激光雷达累积点云 P k P_k Pk 和在接收每次扫描之间由 IMU 采样的测量值 a i a_i ai 和 ω i \omega_i ωi，估计机器人的状态 X ^ i W \hat{X}^W_i X^iW 和几何地图 M ^ k W \hat{M}^W_k M^kW。
预处理
DLIO的输入是由现代360度机械激光雷达（如 Ouster 或 Velodyne，以 10-20Hz 的速率）采集的密集3D点云，外加以更高速率（100-500Hz）同步的6轴IMU的线性加速度和角速度测量。在进入下游任务之前，所有传感器数据都通过外参标定转换到以机器人的重心为中心的 R R R 坐标系中。对于IMU，如果传感器不与重心重合，则必须考虑刚体上位移线性加速度测量的影响；这是通过考虑角速度和IMU偏移的叉乘来完成的。为了尽量减少信息丢失，我们不对点云进行预处理，除了在原点周围应用 1 m 3 1m^3 1m3 的盒滤波器，以去除可能来自机器人的点，及对高分辨率点云进行轻微的体素滤波。这使得我们的方法区别于其他方法，后者要么尝试检测特征（例如角点、边缘或表面），要么通过体素滤波器严重下采样点云。
具有联合先验的连续时间运动修正
由于旋转激光雷达传感器在采集过程中各个点的采集时间不同，导致在运动过程中点云会出现运动失真。与其假设扫描期间的简单运动（例如恒定速度），这可能无法准确捕捉到细微的运动变化，我们使用了一种更为精确的常加速度和角加速度模型来计算每个点的唯一变换，采用的是两步的由粗到精的传播方案。该策略的目的是减少由于IMU的采样率和IMU与激光雷达点测量之间的时间差异所引起的误差。通过IMU的数值积分，我们首先粗略地构建出扫描期间的轨迹，并随后通过解析的连续时间方程对其进行细化，最终得到每个点的去畸变变换。
设 t k t_k tk 为接收到的点云 P k R P^R_k PkR 的时间戳，其中累积了 N N N 个点。点 p n k p_{n_k} pnk 的时间戳为 t k + Δ t n k t_k + \Delta t_{n_k} tk+Δtnk。为了估算每个点在世界坐标系 W W W 中的位置，我们首先通过以下公式对IMU测量在 t k − 1 t_{k-1} tk−1 到 t k + Δ t N k t_k + \Delta t_{N_k} tk+ΔtNk 的时间段进行积分：
p i = p i − 1 + v i − 1 Δ t i + 1 2 R ( q i − 1 ) a i − 1 Δ t i 2 + 1 6 j i Δ t i 3 p_i = p_{i-1} + v_{i-1} \Delta t_i + \frac{1}{2} R(q_{i-1}) a_{i-1} \Delta t_i^2 + \frac{1}{6} j_i \Delta t_i^3 pi=pi−1+vi−1Δti+21R(qi−1)ai−1Δti2+61jiΔti3
v i = v i − 1 + R ( q i − 1 ) a i − 1 Δ t i v_i = v_{i-1} + R(q_{i-1}) a_{i-1} \Delta t_i vi=vi−1+R(qi−1)ai−1Δti
q i = q i − 1 + 1 2 ( q i − 1 ⊗ ω i − 1 ) Δ t i + 1 4 ( q i − 1 ⊗ α i ) Δ t i 2 q_i = q_{i-1} + \frac{1}{2} (q_{i-1} \otimes \omega_{i-1}) \Delta t_i + \frac{1}{4} (q_{i-1} \otimes \alpha_i) \Delta t_i^2 qi=qi−1+21(qi−1⊗ωi−1)Δti+41(qi−1⊗αi)Δti2
其中， i = 1 , … , M i = 1, \dots, M i=1,…,M 是IMU测量的索引， j i = 1 Δ t i ( R ( q i ) a i − R ( q i − 1 ) a i − 1 ) j_i = \frac{1}{\Delta t_i} \left( R(q_i) a_i - R(q_{i-1}) a_{i-1} \right) ji=Δti1(R(qi)ai−R(qi−1)ai−1) 是估算的线性加加速度， α i = 1 Δ t i ( ω i − ω i − 1 ) \alpha_i = \frac{1}{\Delta t_i} (\omega_i - \omega_{i-1}) αi=Δti1(ωi−ωi−1) 是估算的角加速度。齐次变换矩阵 T i W ∈ S E ( 3 ) T^W_i \in SE(3) TiW∈SE(3) 对应于 p i p_i pi 和 q i q_i qi，定义了扫描期间的粗略离散时间轨迹。然后，基于最接近的前一个变换，通过解析的连续时间解法对每个点 p n k p_{n_k} pnk 恢复出点的去畸变变换 T n W T^W_n TnW，公式如下：
p ∗ ( t ) = p i − 1 + v i − 1 t + 1 2 R ( q i − 1 ) a i − 1 t 2 + 1 6 j i t 3 p^*(t) = p_{i-1} + v_{i-1} t + \frac{1}{2} R(q_{i-1}) a_{i-1} t^2 + \frac{1}{6} j_i t^3 p∗(t)=pi−1+vi−1t+21R(qi−1)ai−1t2+61jit3
q ∗ ( t ) = q i − 1 + 1 2 ( q i − 1 ⊗ ω i − 1 ) t + 1 4 ( q i − 1 ⊗ α i ) t 2 q^*(t) = q_{i-1} + \frac{1}{2} (q_{i-1} \otimes \omega_{i-1}) t + \frac{1}{4} (q_{i-1} \otimes \alpha_i) t^2 q∗(t)=qi−1+21(qi−1⊗ωi−1)t+41(qi−1⊗αi)t2
其中，
i
−
1
i-1
i−1 和
i
i
i 分别表示最接近的前后IMU测量，
t
t
t 是点
p
n
k
p_{n_k}
pnk 与最近的前一IMU测量之间的时间，
T
n
W
T^W_n
TnW 表示
p
∗
p^*
p∗ 和
q
∗
q^*
q∗ 对应的变换。通过这种两步流程，得出的点云不仅经过了运动修正，还基本上与地图对齐，因此固有地融合了GICP优化的先验（详见第III-E节）。重要的是，公式(4)和(5)的准确性取决于初始速度
v
0
W
v^W_0
v0W、IMU偏差
b
k
a
b^a_k
bka 和
b
k
ω
b^\omega_k
bkω 以及运动修正时的初始姿态
q
0
q_0
q0 的准确估计。我们特别强调，确保这些参数的可靠性是通过利用DLIO的非线性几何观察器[
10
10
10]的全局收敛性来实现的，前提是扫描匹配返回了准确的解。
图3. 由粗到精的点云去畸变。 一个失真的点
p
0
L
p^L_0
p0L (A) 通过两步处理进行去畸变，首先在扫描之间对IMU测量进行积分，然后在连续时间内（C）为原始点求解唯一的变换，最终将
p
0
L
p^L_0
p0L 去畸变为
p
∗
p^*
p∗ (B)。
图4. 连续时间运动修正。 对于点云中的每个点，通过求解一组闭合形式的运动方程计算唯一的变换，该变换在最近的IMU测量初始化。此过程提供了准确且可并行化的连续时间运动修正。
扫描到地图的配准
通过同时进行运动失真修正并将GICP优化先验结合到点云中，DLIO可以直接执行扫描到地图的配准，而不需要之前方法中必需的扫描到扫描配准。这个配准被表述为一个非线性优化问题，其目标是最小化当前扫描与提取的子地图之间对应点/面的距离。设 P k W P^W_k PkW 是经过修正的点云， S k W S^W_k SkW 是通过[ 20 20 20]提取的关键帧子地图。然后，扫描到地图的优化目标是找到一个变换 Δ T k \Delta T_k ΔTk，使点云对齐得更好：
Δ T k = arg min Δ T k E ( Δ T k P k W , S k W ) \Delta T_k = \arg \min_{\Delta T_k} E(\Delta T_k P^W_k, S^W_k) ΔTk=argΔTkminE(ΔTkPkW,SkW)
其中，GICP残差误差 E E E 定义为：
E ( Δ T k P k W , S k W ) = ∑ c ∈ C d c ⊤ ( C S k , c + Δ T k C P k , c Δ T k ⊤ ) − 1 d c E(\Delta T_k P^W_k, S^W_k) = \sum_{c \in C} d_c^\top (C_{S_k, c} + \Delta T_k C_{P_k, c} \Delta T_k^\top)^{-1} d_c E(ΔTkPkW,SkW)=c∈C∑dc⊤(CSk,c+ΔTkCPk,cΔTk⊤)−1dc
对于点云 P k W P^W_k PkW 和子地图 S k W S^W_k SkW 在时间步 k k k 的一组对应点 C C C， d c = s k c − Δ T k p k c d_c = s^c_k - \Delta T_k p^c_k dc=skc−ΔTkpkc，其中 p k c ∈ P k W p^c_k \in P^W_k pkc∈PkW， s k c ∈ S k W s^c_k \in S^W_k skc∈SkW， ∀ c ∈ C \forall c \in C ∀c∈C。 C k , c P C^P_{k,c} Ck,cP 和 C k , c S C^S_{k,c} Ck,cS 分别是点云 P k W P^W_k PkW 和子地图 S k W S^W_k SkW 的协方差矩阵。然后，按照[ 13 13 13]的方法，将这种点到平面的公式转换为平面到平面的优化，通过对协方差矩阵 C k , c P C^P_{k,c} Ck,cP 和 C k , c S C^S_{k,c} Ck,cS 进行正则化，使其具有 ( 1 , 1 , ϵ ) (1, 1, \epsilon) (1,1,ϵ) 的特征值，其中 ϵ \epsilon ϵ 表示表面法线方向的低不确定性。结果的 Δ T k \Delta T_k ΔTk 表示一个最佳修正变换，它可以更好地全局对齐先前变换后的扫描 P k W P^W_k PkW 和子地图 S k W S^W_k SkW，因此 T k W = Δ T k T M W T^W_k = \Delta T_k T^W_M TkW=ΔTkTMW（其中 T M W T^W_M TMW 是最后一个点的IMU积分）是全局修正的机器人姿态，用于地图构建并作为非线性几何观察器的更新信号。
几何观察器
由扫描到地图配准生成的变换 T k W T^W_k TkW 与IMU的测量数据进行融合，以通过一种新的分层非线性几何观察器[ 10 10 10]生成完整的状态估计。观察器的完整分析可以在[ 10 10 10]中找到，简而言之，可以证明在确定性条件下，估计的状态 X ^ \hat{X} X^ 将全局收敛到真实状态 X X X，且计算复杂度极低。证明使用了收缩理论，首先证明四元数的估计会指数收敛到真实四元数的邻域内。然后，姿态估计作为输入，传递给另一个收缩观察器来估计平移状态。这种架构形成了一个收缩层次结构，保证估计值收敛到真实值。这种强收敛特性是该方法相较于其他融合方法（例如滤波或位姿图优化）的主要优势，后者即使在最理想的情况下，也缺乏明确的收敛保证。此外，观察器状态估计的固有平滑性使其适合用于控制。在本文中，使用的观察器是[ 10 10 10]的一个特殊案例。
设 γ ℓ ∈ { 1 , … , 5 } \gamma_{\ell} \in \{1, \dots, 5\} γℓ∈{1,…,5} 为正数， Δ t k + \Delta t^+_k Δtk+ 为GICP估算的位姿之间的时间间隔。如果姿态误差 q e : = ( q e ∘ , q ⃗ e ) = q i ∗ ⊗ q k q_e := (q^\circ_e, \vec{q}_e) = q^*_i \otimes q_k qe:=(qe∘,qe)=qi∗⊗qk 和平移误差 p e = p k − p i p_e = p_k - p_i pe=pk−pi，则状态修正的形式如下：
q i ← q i + Δ t k + γ 1 q i ⊗ [ 1 − ∣ q e ∘ ∣ sgn ( q e ∘ ) q ⃗ e ] q_i \leftarrow q_i + \Delta t^+_k \gamma_1 q_i \otimes \left[ \frac{1 - |q^\circ_e|}{\text{sgn}(q^\circ_e)} \vec{q}_e \right] qi←qi+Δtk+γ1qi⊗[sgn(qe∘)1−∣qe∘∣qe]
b i ω ← b i ω − Δ t k + γ 2 q e ∘ q ⃗ e b^\omega_i \leftarrow b^\omega_i - \Delta t^+_k \gamma_2 q^\circ_e \vec{q}_e biω←biω−Δtk+γ2qe∘qe
p i ← p i + Δ t k + γ 3 p e p_i \leftarrow p_i + \Delta t^+_k \gamma_3 p_e pi←pi+Δtk+γ3pe
v i ← v i + Δ t k + γ 4 p e v_i \leftarrow v_i + \Delta t^+_k \gamma_4 p_e vi←vi+Δtk+γ4pe
b i a ← b i a − Δ t k + γ 5 R ( q i ) ⊤ p e b^a_i \leftarrow b^a_i - \Delta t^+_k \gamma_5 R(q_i)^\top p_e bia←bia−Δtk+γ5R(qi)⊤pe
注意该公式是分层的，姿态更新（前两项）与平移更新（后三项）完全解耦。此外，这些修正是完全非线性的，可以确保状态估计足够准确，能够仅依赖IMU的先验数据，直接执行扫描到地图的配准，而无需依赖扫描到扫描配准。
结果
消融研究和运动修正的比较
为了研究我们提出的运动修正方案的影响，首先我们对DLIO进行了消融研究，在使用Newer College数据集[
30
30
30]时应用了不同程度的去畸变修正。这项研究从无运动修正（None）、仅使用最近的IMU积分进行修正（Discrete），到使用完整的连续时间运动修正（Continuous）（表I）。特别值得注意的是，在动态数据集中，包含最高可达3.5 rad/s的旋转速度的剧烈运动。当没有修正时，误差是所有算法中最高的，达到0.1959 RMSE。当使用部分修正时，误差显著降低，因为扫描匹配的点云更准确、更具代表性；然而，使用我们提出的完整修正方案时，我们观察到误差仅为0.0612 RMSE，是所有测试算法中最低的。其他数据集也表现出类似的趋势，表明更好的运动修正带来了卓越的轨迹跟踪精度：通过构建连续时间内的唯一变换，生成了比之前方法更真实的点云，最终影响了扫描匹配和轨迹精度。图6展示了这一现象的实验结果：DLIO能够捕捉到细微的细节，而简单修正或无修正会丢失这些信息。
图5. 长实验的轨迹。 DLIO为Newer College - Long Experiment生成的轨迹。颜色表示绝对位姿误差。
图6. 去畸变比较。 在无（A）和有（B）我们的运动修正方法的情况下，生成的地图展示了在激烈机动中捕获的细节。
基准测试结果
1. Newer College 数据集
使用原始的Newer College基准数据集[ 30 30 30]，我们对所有算法的轨迹精度和每次扫描的平均时间进行了比较，采用了evo工具[ 31 31 31]。在这些测试中，我们使用了Ouster的IMU（100Hz）以及激光雷达数据（10Hz），以确保传感器之间的时间同步性。对于某些Newer College数据集，为了公平比较，我们在计算FAST-LIO2的RMSE时排除了前100个位姿，因为在起始阶段存在滑动现象。我们还使用了Newer College数据集的最新扩展版进行了比较，得到了类似的结果，但由于篇幅限制，未在本文中展示。结果如表I所示，DLIO在所有五个数据集中的轨迹RMSE最低，并且扫描的平均计算时间最短。图5展示了DLIO在Newer College - Long Experiment数据集上的轨迹，与真实值相比，DLIO即使在行驶了超过三公里后，轨迹误差依然很小。
2. UCLA 校园数据集
为了进一步比较，我们在UCLA校园内收集了四个大规模数据集（图7）。这些数据集是通过手持我们的空中平台（图1）沿2261.37米的总轨迹采集的。我们的传感器包括Ouster OS1（10Hz，32个通道，水平分辨率为512），以及位于其下方约0.1米处的6轴InvenSense MPU-6050 IMU。需要指出的是，该IMU的购买价格约为10美元，表明LIO算法不需要之前工作中使用的高精度IMU传感器。由于缺少地面真实值，无法比较绝对轨迹误差，因此我们计算了端到端平移误差作为替代指标（表II）。在这些实验中，DLIO在端到端平移误差和每次扫描效率上均优于其他算法。DLIO生成的地图能够捕捉环境中的细节，这为自动移动机器人（如地形可通行性）提供了更多精确的环境信息。
图7. UCLA校园。 DLIO生成的洛杉矶UCLA校园各个位置的详细地图，包括(A) Dickson Court中的Royce Hall，(B) 科学法庭，© Bruin Plaza，以及(D) Franklin D. Murphy雕塑花园，展示了(1) 俯视图和(2) 特写，显示DLIO能够生成的精细细节。生成这些地图的轨迹以黄色表示。
结论
本文提出了直接激光雷达-惯性里程计（DLIO），这是一种高度可靠的LIO算法，能够在资源受限的移动机器人上实时生成精确的状态估计和详细的地图。DLIO的关键创新在于采用了一种快速且并行化的由粗到精的连续时间轨迹构建方法，用于逐点的运动修正。该方法被集成到一个简化的LIO架构中，该架构能够在一次性进行运动修正和先验生成的同时，直接执行扫描到地图的对齐，从而减少计算开销。这一切得以实现的关键在于我们几何观察器的强收敛性保证，它能够可靠地初始化姿态、速度和偏差，以实现精确的IMU积分。我们的实验结果表明，DLIO在定位精度、地图清晰度和算法效率方面，相比最先进的方法具有显著的提升。未来的工作包括进行闭环飞行测试以及添加回环检测功能。
致谢
作者要感谢Helene Levy和David Thorne在数据采集方面的帮助。
原文地址：https://blog.csdn.net/qq_40301351/article/details/142769020
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！