# CT-VoxelMap：基于概率自适应体素映射的高效连续时间激光雷达-惯性里程计
赵磊、李星毅、邓天辰、曹川、张涵、陈伟东
摘要—对于搭载有限计算资源的移动机器人而言，在快速运动或崎岖地形上保持稳定且精确的定位仍然极具挑战性。目前，基于连续时间表示的多传感器融合方法为应对这一挑战提供了潜在且有效的解决方案。其中，基于样条曲线的方法为连续时间表示提供了一种高效且直观的途径。以往基于B样条的连续时间里程计研究要么将控制点视为待估计变量，要么在四元数空间中进行估计，这导致解析雅可比矩阵的推导较为复杂，且往往忽略了样条曲线与真实轨迹随时间变化的拟合误差。为解决这些问题，我们首先提出将矩阵李群上的控制点增量表示为待估计变量。利用B样条的累积形式特性，我们推导出了更紧凑的公式，从而无需额外考虑边界条件即可获得更简单的解析雅可比矩阵。其次，我们利用来自IMU测量的前向传播信息来在线估计拟合误差，并进一步引入了基于混合特征的体素地图管理策略，以提升系统的精度和鲁棒性。最后，我们提出了一种重估计策略，显著提高了系统的计算效率和鲁棒性。我们在多个具有挑战性的公开数据集上对所提方法进行了评估，结果显示其在大多数序列中均表现出优异的性能。我们还开展了详细的消融实验，以分析各模块对整体位姿估计系统的影响。
索引词—连续时间里程计、地图构建、传感器融合。
## I. 引言
随着各类机器人——如移动机器人、人形机器人、四足机器人、轮腿式机器人以及无人机——在自动驾驶、具身智能、巡检和搜救等领域的应用日益广泛，如何让这些机器人利用自身搭载的资源，在复杂地形引发的颠簸或自身复杂的运动模式下仍能保持稳定可靠的运动或位姿估计，已成为一项至关重要且极具挑战性的任务 [^1] [^2] [^3] [^4] [^5]。由于激光雷达传感器在测量范围、距离精度以及不同光照条件下的鲁棒性方面具有优势，因此被广泛用于位姿估计，尤其适用于复杂的现实环境。此外，融合摄像头、IMU和轮式里程计等互补传感器模态 [^6] [^7] [^8] [^9] [^10]，可进一步提升位姿估计系统的精度和鲁棒性。这种多传感器融合范式近年来受到广泛关注，极大地推动了状态估计技术的发展。
当前主流的位姿估计技术通常以固定频率在离散时间内运行，例如以下系统所示
Fig. 1: Dataset platforms and mapping performance. Despite the varying motion patterns and sensor configurations across different dataset platforms, the proposed method achieves robust and accurate pose estimation in all cases.
诸如FAST-LIO [^11]、LeGO-LOAM [^12]以及FAST-LIVO2 [^13]等方法。这类方法通常需要额外的预处理步骤，包括传感器时间同步和激光雷达点云畸变校正（运动补偿），随后才能在滤波器或非线性优化框架内进行位姿估计。在基于滤波器的方法中，迭代扩展卡尔曼滤波（IEKF）[^9]以及迭代误差状态卡尔曼滤波（IESKF）[^11]、[^13]被广泛应用。在此类框架中，首先会在恒定速度假设下（仅使用激光雷达时）或利用惯性测量单元数据（多传感器系统中包含IMU时）对状态进行预测，同时执行运动补偿以修正激光雷达扫描的畸变。之后便基于预测状态进行最大后验概率（MAP）或最大似然估计（MLE）。对于基于优化的框架，通常采用滑动窗口策略：利用一定时间范围内的测量数据构建残差项，再依据MAP或MLE准则执行非线性优化 [^14]、[^15]。总体而言，基于优化的方法往往能达到更高的精度，而基于滤波器的方法则具有更高的计算效率。不过，当涉及多传感器融合场景时——尤其是那些需要处理密集且异步的传感器数据、高保真处理要求以及不同传感器采样率差异的情况时，这些离散时间方法便会受到局限。随着位姿估计技术的不断进步，基于连续时间轨迹表示的方法逐渐受到关注，它们有望进一步提升估计精度、鲁棒性及多传感器融合效率。
近年来，连续时间位姿估计越来越受到重视。
---
与离散时间方法不同，连续时间方法采用连续函数来描述待估计的状态。其显著优势在于能够通过插值在任意时间戳查询状态，从而实现无损、密集且异步的多传感器融合。当前的连续时间方法大致可分为三类：基于分段线性轨迹的方法、基于高斯过程（GP）的方法以及基于B样条的方法（尤其是三次B样条）。
分段线性方法通常采用恒定速度假设，并用相连的线性段来表示轨迹 [^16]、[^17]。虽然这种方法较为简便，但对复杂运动的表征能力有限。相比之下，基于GP和样条的方法在建模复杂运动模式时更具灵活性 [^18]。基于GP的方法利用高斯过程来建模连续轨迹，从而能在任意时间戳推断位姿均值和协方差。不过，早期的GP方法需要求解高维稠密逆矩阵来完成插值，计算成本极高，难以满足实时SLAM的需求 [^19]。后续的研究 [^20] 引入了源自线性时变随机微分方程的运动先验，从而得到了完全稀疏的逆核矩阵，提升了查询效率。这一成果后来还被拓展到了矩阵李群领域 [^21]。
与GP表示方法相比，基于B样条的实现在姿态估计任务中更加直观且灵活。虽然可以通过De Boor-Cox公式递归构造B样条[^22]、[^23]，但实际应用中主要采用累积形式的B样条。[^24]分析了李群上的时间导数，并提出了能够提升雅可比计算效率的递归公式，从而显著推动了三次B样条在连续时间姿态估计中的应用。诸如[^14]、[^25]、[^26]等研究，均在优化框架内利用矩阵李群上的累积三次B样条来表示6自由度姿态。其特色包括：[^25]中集成了闭环机制以提高大规模建图的一致性；[^14]中融入了相机测量数据以提升精度；而[^26]则采用解析雅可比矩阵来替代Ceres Solver，从而提高计算效率。
尽管这些基于优化的框架已能达到令人满意的效率，但它们直接将控制点视为待估参数，导致雅可比矩阵的推导较为复杂，且还需额外处理边界条件，因此仍有进一步优化的空间[^27]。[^27]将连续时间B样条估计扩展至滤波框架以提升效率，不过采用的是四元数表示方式，这反而使雅可比矩阵的推导更为复杂且缺乏直观性。此外，上述B样条方法均未考虑样条曲线与实际轨迹之间的拟合误差。总而言之，虽然B样条方法具备灵活性与直观性，但缺乏不确定性描述能力，仍需进一步的理论发展。
研究动机与贡献：为了进一步提升基于B样条的连续时间姿态估计的效率与性能，本文的主要贡献概括如下：
- 我们提出了一种基于IEKF的多传感器融合里程计框架，该框架将更一致的样条表示与混合特征体素地图相结合，从而同时实现了高效率与鲁棒性。所提方法在多个具有挑战性的公开数据集上均表现出优异的性能，同时还能保持计算效率与系统鲁棒性。
- 在[^24]的基础上，我们推导出了一种更紧凑的、基于李群上的累积形式三次B样条表示。通过将控制点的增量而非控制点本身作为待估参数，我们得到了形式统一且无需显式处理边界条件的简化解析雅可比矩阵。
- 为了弥补样条拟合误差，我们利用IMU测量数据在线估计此类误差，从而增强系统的鲁棒性。此外，我们采用了包含平面特征与体素特征的混合特征体素地图结构，以提高估计精度。
- 我们引入了一种重新估计策略，该策略将m维问题分解为k个维度为m/k的子问题，从而显著提升了计算效率与系统鲁棒性。
• 我们在公开数据集上进行了大量实验，并开展了消融研究，从而对所提算法进行了全面评估。完整的代码将会开源，以回馈社区。
不同公共数据集和平台上的建图性能如图1所示，其中机器人的图像分别来源于与公共数据集对应的数据采集平台 [^28] [^29] [^30] [^31]。
### II. 相关工作
本节简要回顾了主流的基于激光雷达以及激光雷达-惯性里程计框架，重点探讨了从离散时间位姿估计范式向连续时间位姿估计范式的演变。
离散时间基于激光雷达的里程计：早期的基于激光雷达的位姿估计主要依赖2D激光雷达传感器 [^32]。随着3D激光雷达技术的出现，利用3D测量数据的方法受到了越来越多的关注。LOAM [^33] 是3D激光雷达SLAM领域的开创性工作，它从点云中提取几何特征，并通过非线性优化来实现较高的精度和效率。后续的研究如 [^12] 通过引入地面特征并加入回环闭合机制来改进特征提取，从而减轻z轴漂移问题并提升建图的一致性。[^34] 进一步将LOAM框架扩展到多传感器配置中，提升了在恶劣环境下的鲁棒性。
与基于滤波的方法相比，LOAM类方法的计算效率相对较低 [^8]。相比之下，[^8]、[^9] 基于MSCKF范式 [^35]、[^36]，利用状态增广和基于QR分解的状态压缩技术，在保持高效率的同时实现了与基于优化的方法相当的精度。为了直接利用原始点云来提取更精细的几何细节 [^11]、[^37]，IESKF框架结合了
---
点面ICP算法展现出了卓越的效率和精度。更近期的研究成果 [^13]、[^38] 采用了体素化表示方法，并利用局部地图中的平面特征来进一步提升精度和计算性能。
然而，随着自动驾驶和具身智能的快速发展，传感器套件变得越来越多样化。离散时间位姿估计方法在适配多种同构或异构传感器的异步融合方面的局限性也日益凸显。因此，连续时间位姿估计技术受到了越来越多的研究关注。
连续时间轨迹表示大致可分为参数化方法（如线性插值、样条曲线）和非参数化方法（如一维高斯过程）[^18]。早期的参数化连续时间方法通常采用线性插值或分段连续函数来描述轨迹。例如，[^17] 利用球面线性插值来实现快速运动时的运动补偿，而 [^16] 则采用分段连续的线性函数来表示机器人轨迹。尽管这些方法在相对较快的运动下能取得令人满意的位姿估计性能，但线性插值和分段线性函数对复杂运动模式的表征能力有限，且无法保证连接点的速度或加速度连续性。
基于连续时间激光雷达的里程计：与仅在离散时间点估计位姿的离散时间方法不同，连续时间方法将位姿视为位于连续轨迹上，特定时刻的位姿可通过沿该轨迹采样获得。因此，连续时间SLAM也可称为同步轨迹估计与建图（STEAM）[^20]。当来自不同时间戳的测量数据到达时，可从连续轨迹中查询对应的状态以构建残差，从而无需额外的时序同步或运动补偿[^25]。
基于高斯过程和B样条的方法解决了这些局限性。[^19]提出了一种基于高斯过程的SLAM方法，但在离散时间点之间查询位姿时需要求解高维稠密矩阵的逆，这使其无法应用于实时场景。后续研究引入了源自线性时变随机微分方程的运动先验，从而得到了稀疏的逆核矩阵，实现了实时部署[^20]。[^39]进一步将该框架扩展到滤波范式，实现了更高效的实现方式。不过，基于高斯过程的方法严重依赖于运动先验的形式。
相比之下，基于B样条的方法更具直观性和便利性。[^25]利用源自[^24]的累积形式B样条实现了连续时间激光雷达-惯性里程计，尽管其计算效率相对有限。[^26]通过引入关于控制点的解析雅可比矩阵来提升效率，以此替代Ceres Solver的自动微分功能，同时保持了较高的位姿估计精度。不过，仍有进一步改进的空间。[^27]随后将连续时间估计扩展到滤波框架，采用四元数表示旋转轨迹，实现了高效的连续时间估计，但代价是复杂度有所增加。总体而言，基于B样条的连续时间位姿估计方法仍需进一步发展。
### III. 预备知识
本节介绍了所提方法的框架，包括坐标系定义、传感器测量模型以及累积形式的连续时间轨迹表示。
### A. 系统概述、坐标系与测量模型
图2展示了所提系统的框架。激光雷达坐标系记为$^L(\cdot)$，惯性测量单元坐标系记为$^I(\cdot)$，全局坐标系记为$^G(\cdot)$。整个系统包含两种传感器：激光雷达和惯性测量单元。相应的测量模型描述如下。
Fig. 2: Schematic diagram of system overview.
## 1) 惯性测量单元测量模型
$$ {}^{I}\omega_{m}={}^{I}\omega+b_{\omega}+n_{w},{}^{I}a_{m}={}^{I}a+b_{a}+{}_{G}^{I}R^{G}g+n_{a}. $$
其中$b_{a}$表示加速度计偏差，$b_{\omega}$表示陀螺仪偏差，$n_{w}$和$n_{a}$分别代表角速度和加速度测量中的加性白噪声。
2) 激光雷达测量模型：设$\{^LP_j, j = 1, ..., m\}$表示当前扫描中的点，这些点在局部激光雷达坐标系$\{L\}$中被采样。由于激光雷达测量噪声的存在，每个测量点$^LP_j$通常会受到噪声$^Ln_j$的影响，该噪声包含测距噪声和光束指向噪声成分[^11]。
$$ ^{L}P_{j}^{g t}=^{L}P_{j}+^{L}n_{j} $$
### B. 基于B样条的插值
近年来，连续时间里程计方法取得了显著的性能提升[^17]、[^25]、[^26]、[^40]。由于样条表示相对简单，我们采用累积形式的B样条来表示连续轨迹。
---
[^18]。这种表示方法具有以下参数：节点间隔为$\Delta t$，样条阶数N（或次数$D \triangleq N-1$），以及一组控制点$\{T_m\}, m=0,...,M$，其中$T_m \in SE(3)$。每个控制点$T_m$都与一个节点$t_m$相关联，且满足$t_m = t_0 + m \Delta t$。我们采用节点间隔恒定的三次样条$(N=4)$，即$\Delta t = \tau$。除非另有说明，后续提及的样条轨迹均指均匀三次样条。三次B样条的累积形式可描述如下[^24]：
$$ \begin{array}{r l}&{R(t)=R_{i}\displaystyle\prod_{j=1}^{N-1}E x p(\tilde{\lambda}_{j}(u)L o g(R_{i+j-1}^{-1}R_{i+j})),}\\ &{p(t)=p_{i}+\displaystyle\sum_{j=1}^{N-1}\tilde{\lambda}_{j}(u)(p_{i+j}-p_{i+j-1}).}\end{array} $$
其中$u = \frac{t - t_i}{\Delta t}$、$t \in [t_i, t_{i+1})$、$t_i \in \{t_m | m = 0, ..., M\}$。$\tilde{\lambda}_i$及其一阶和二阶导数的计算方法如下：
$$\begin{align*} [\tilde{\lambda}_{0},\tilde{\lambda}_{1},...,\tilde{\lambda}_{N-1}]^{T}&=\tilde{B}^{(N)}[1,u,...,u^{N-1}]^{T}, \end{align*}$$ \dot{\tilde{\lambda}}_{0},\dot{\tilde{\lambda}}_{1},...,\dot{\tilde{\lambda}}_{N-1}]^{T}&=\tilde{B}^{(N)}[0,1,...,\dot{u}^{N-2}]^{T},\ $$\begin{align*} [\ddot{\tilde{\lambda}}_{0},\dddot{\tilde{\lambda}}_{1},...,\dddot{\tilde{\lambda}}_{N-1}]^{T}&=\tilde{B}^{(N)}[0,0,...,\ddot{u}^{N-3}]^{T}. \end{align*}$$
其中$\dot{u}^{j} = \frac{j}{\Delta t} u^{j-1}$且$\ddot{u}^{j} = \frac{j(j-1)}{\Delta t^{2}} u^{j-2}$。给定N后，混合矩阵$\tilde{B}^{(N)}$及其累积形式$\tilde{B}^{(N)}$可在[^24]、[^26]中查到。
### 四、状态估计
在采用连续时间轨迹表示的状态估计任务中，状态向量$\chi$包含IMU加速度计偏差$b_a$、陀螺仪偏差$b_w$、重力g，以及与近期连续轨迹段对应的B样条控制点增量。为了便于估计，我们考察旋转控制点。从公式(1)和公式(2)可以看出$\tilde{\lambda}_0(u) = 1$。因此，连续轨迹表示可重新表述为：
$$ R(t)=R_{i-1}\prod_{j=0}^{N-1}E x p(\tilde{\lambda}_{j}(u)d_{j}),d_{j}=L o g(R_{i+j-1}^{-1}R_{i+j}) $$
与[^27]中的方法类似，我们估计的是$d_j$而非直接估计$^G_C R^T$。关键区别在于我们的估计是在矩阵李群$SO(3)$上进行的，这种方式的表述更为直观。基于上述表示，可推导出角速度的相应递归公式：
$$ \begin{align*}\omega^{(j+1)}&=A_{j}^{T}\omega^{(j)}+\dot{\tilde{\lambda}}_{j}(u)d_{j},j=0,...,N-1.\\\omega(t)&=\omega^{(N)},\omega^{(0)}=0,\\A_{j}&=Exp(\tilde{\lambda}_{j}(u)d_{j}),j=0,...,N-1.\end{align*} $$
同样的思想也适用于位置控制点，可得：
$$ p(t)=p_{i-1}+\sum_{j=0}^{N-1}\tilde{\lambda}_{j}(u)d_{j},d_{j}=p_{i+j}-p_{i+j-1}. $$
相应的速度和加速度可按如下方式计算：
$$ \begin{align*}v(t)&=\sum_{j=0}^{N-1}\dot{\tilde{\lambda}}_{j}(u)d_{j},d_{j}=p_{i+j}-p_{i+j-1},\\a(t)&=\sum_{j=0}^{N-1}\ddot{\tilde{\lambda}}_{j}(u)d_{j},d_{j}=p_{i+j}-p_{i+j-1}.\end{align*} $$
这种表述方式具有两方面优势。首先，与控制点对应的状态变量位于欧几里得空间中，且增量为小量，从而确保状态变量保持在较小范围内，这有利于状态估计。其次，基于所提出的连续轨迹表示，在推导雅可比矩阵时边界条件保持一致，无需单独考虑j=0和j=N-1的情况，因此表述更为简洁。相应地，该系统可表示为：
$$ \begin{align*}\chi&\doteq\begin{bmatrix}\chi_{C_{r}}&\chi_{C_{p}}&\chi_{I}\end{bmatrix}^{T}\\\chi_{C_{r}}&=[d_{R,j}^{T}],\chi_{C_{p}}=[d_{p,j}^{T}],j=0,...,N-1,\end{align*} $$
$$ \chi_{I}=[b_{w}^{T},b_{a}^{T},^{G}g^{T}], $$
$$ u\doteq[\omega_{m}^{T},a_{m}^{T}]^{T},w\doteq[n_{b_{w}}^{T},n_{b a}^{T}]^{T}. $$
$d_{R,j}^{T}$、$d_{p,j}^{T}$、j = 0, ..., N - 1 分别表示与新添加的B样条段相对应的旋转和位置控制点增量，它们的具体定义分别见公式(3)和公式(5)。$b_{w}$、$b_{a}$ 和 $G_{g}$ 分别代表陀螺仪偏差、加速度计偏差和重力矢量。u表示IMU测量输入，w代表相应的噪声。
### A. 运动学模型
基于均匀B样条的特性，第k个样条段在区间$[t_{k,s}, t_{k,e}]$上表示连续的轨迹。当新的激光雷达扫描数据到达时，其点的最大时间戳记为$t_{L_e}$。如果$t_{L_e} \in [t_{k,s}, t_{k,e}]$，则在状态预测阶段控制点保持不变；而当$t_{L_e} > t_{k,e}$时，需要扩展新段及对应的控制点以表示额外的轨迹信息，这会导致状态$\chi$发生跳变，同时$b_w$、$b_a$和G保持不变。
混合系统：根据上述描述，基于连续时间表示的状态估计任务可以建模为混合系统，其中状态在离散时刻$t_k, k = 1, ..., n$发生跳变，而在区间$t_i \in [t_k, t_{k+1})$、$k = 1, ..., n$内连续演化。该系统可表示为：
$$ \left\{\begin{aligned}\dot{\chi}(t)=0\boxplus w,t\in\left[t_{k},t_{k+1}\right),k=1,...,n\\ \chi_{t^{+}}=A\chi_{t^{-}},t=t_{k},k=1,...,n.\end{aligned}\right. $$
其中w表示过程噪声，均建模为高斯白噪声。（$\boxminus$和$\boxplus$的定义遵循[^37]中的定义）。A表示状态转移矩阵，定义为$A = \mathrm{diag}(A_r, A_p, I_3, I_3, I_3)$。此处，$A_r$表示对应于旋转控制点的状态转移矩阵，
---
而$A_{p}$表示对应于位置控制点增量的状态转移矩阵。当没有添加新的控制点时，$A_{r}$和$A_{p}$均为单位矩阵；当添加新控制点时，其具体形式还需进一步讨论。
对于控制点的预测，由于B样条的局部支撑特性，新扩展的第$k+1$段对应的控制点可以从第k段的控制点初始化。相应的状态可利用时间$t_{k}^{-}$处的状态进行初始化，得到：
$$ \begin{align*}b_{w}^{t_{k}^{+}}&=b_{w}^{t_{k}^{-}},b_{a}^{t_{k}^{+}}=b_{a}^{t_{k}^{-}},{}^{G}g^{t_{k}^{+}}={}^{G}g^{t_{k}^{-}},\\d_{R,j}^{t_{k}^{+}}&=d_{R,j+1}^{t_{k}^{-}},d_{p,j}^{t_{k}^{+}}=d_{p,j+1}^{t_{k}^{-}},j=0,...,N-2.\end{align*} $$
关于$d_{R,N-1}^{t_{k}}$和$d_{p,N-1}^{t_{k}}$的初始化与初始化策略相关。本文采用的初始化策略是恒速初始化策略，即保持时间$t_{k}$和$t_{k+1}$处的线速度和角速度不变。对于旋转控制点，根据公式(4)可得：
$$ \begin{aligned}&\boldsymbol{\omega}(t_{k})=\boldsymbol{\omega}(t_{k+1})\\&\Rightarrow A_{N-1}^{T}\cdots A_{0}^{T}\boldsymbol{\omega}^{(0)}+\cdots+\dot{\tilde{\lambda}}_{N-1}(0)d_{N-1}\\&=A_{N-1}^{T}\cdots A_{0}^{T}\boldsymbol{\omega}^{(0)}+\cdots+\dot{\tilde{\lambda}}_{N-1}(1)d_{N-1}\\&\Rightarrow A_{N-1}^{T}|_{u=0}d_{N-3}+d_{N-2}=A_{N-1}^{T}|_{u=1}d_{N-2}+d_{N-1}\\&\Rightarrow d_{R,N-1}^{t_{k}^{+}}\simeq d_{R,N-2}^{t_{k}^{-}},\\ \end{aligned} $$
当控制点频率足够高时，$d_j, j = 0, ..., N - 1$为小量。对于位置控制点，根据公式(6)可得：
$$ v(t_{k+1})=v(t_{k})\Rightarrow d_{N-3}=d_{N-1}\Rightarrow d_{p,N-1}^{t_{k}^{+}}=d_{p,N-2}^{-}. $$
基于上述讨论，当添加新控制点时，$A_{r}$和$A_{p}$的定义如下：
$$ A_{r}=\begin{bmatrix}0&I_{3}&0&0\\ 0&0&I_{3}&0\\ 0&0&0&I_{3}\\ 0&0&I_{3}&0\end{bmatrix},A_{p}=\begin{bmatrix}0&I_{3}&0&0\\ 0&0&I_{3}&0\\ 0&0&0&I_{3}\\ 0&0&I_{3}&0\end{bmatrix} $$
### B. IMU观测
在第$(k+1)$次激光雷达扫描到达时，基于$[t_{s,k+1}, t_{e,k+1}]$时间段内的激光雷达点云信息和IMU测量数据，在迭代扩展卡尔曼滤波（IEKF）框架内对状态进行最大后验概率（MAP）估计。相应的观测方程表述如下：
$$ z_{k}=h(f(\chi_{k};t_{k}))+v_{k}. $$
其中，$v_k$ 表示观测噪声，h 代表相应的观测方程，而 f 是关于 $\chi$ 的函数，用于生成在时间 $t_k \in [t_{k,s}, t_{k,e}]$ 从控制点获得的内插信息，例如 $R_{t_k}, p_{t_k}$ 等。
1) 连续时间轨迹下的IMU观测：
基于预测阶段的状态信息，为了更精确地估计控制点，我们利用区间 $[t_{s,k+1}, t_{e,k+1}]$ 内的IMU测量值、偏差以及重力信息。此外，由于B样条的局部支撑特性，当前轨迹段的控制点也会影响历史轨迹段的表示。具体来说，当前段的N个控制点会影响N个样条轨迹段，总共涉及 $2N - 1$ 个控制点。在构建观测值时，会综合考虑受影响区间内的测量值与估计信息，即 $[t_{s,k-N+2}, t_{e,k}]$ 区间内的IMU测量值、估计的偏差及重力信息。每个区间具有相同的 $\Delta t$ 和 $t_{e,i} = t_{s,i+1}, i = 0, ..., n$。示意图如图3所示。
Fig. 3: The black line denotes a continuous trajectory, represented by a 4th-order spline. The solid green circle denotes the IMU measurement, while the hollow green and red circles represent the affected and unaffected historical estimated states, respectively.
根据上述描述，由区间 $[t_{s,k-N+2}, t_{e,k+1}]$ 内的IMU位姿和测量值所构成的观测方程可表示为：
$$ \begin{aligned}h_{B}(\chi,0)&=\begin{bmatrix}^{G}_{I_{k}}R(t_{k})^{T}(a(t_{k})+^{G}\hat{g})+\hat{b}_{a}-^{I_{t_{k}}}a_{m}\\\omega(t_{k})+\hat{b}_{\omega}-^{I_{t_{k}}}\omega_{m}\\\hat{b}_{\omega}-b_{\omega_{t_{k}}}\\\hat{b}_{a}-b_{a_{t_{k}}}\\\quad^{G}\hat{g}\boxminus^{G}g_{t_{k}}\end{bmatrix},\\&=\begin{bmatrix}h^{T}_{a}&h^{T}_{\omega}&h^{T}_{I}\end{bmatrix}^{T}.\end{aligned} $$
其中，$R(t)$ 和 $p(t)$ 分别由公式(3)和公式(5)计算得出。$\omega(t)$ 可由公式(4)得到，而 $a(t)$ 可由公式(6)得出。$^G g_{t_k}$ 表示时间 $t_k$ 处的重力估计值，$b_{\omega t_k}$ 和 $b_{a_{t_k}}$ 分别代表时间 $t_k$ 处的角速度和加速度偏差，$^{I_{t_k}}a_m$ 与 $^{I_{t_k}}\omega_m$ 则是时间 $t_k$ 时的IMU测量值。$^G \hat{g}$、$\hat{b}_a$ 和 $\hat{b}_\omega$ 为当前时刻的状态变量。项 $^G \hat{g} \boxminus^G g_{t_k}$ 可在 $\mathbb{S}^2$ 上计算，其计算方法如下：
$$ s\boxminus d=B_{d}^{T}\frac{\lfloor d\rfloor s}{||\lfloor d\rfloor s||}a t a n2(||\lfloor d\rfloor s||,d^{T}s),d\neq-s,s,d\in\mathbb{S}^{2}. $$
雅可比矩阵：围绕 $\hat{\chi}_{k+1|k}$ 对观测方程进行线性化可得：
$$ \begin{aligned}h_{B}(\boldsymbol{\chi},n)&=h_{B}(\hat{\boldsymbol{\chi}}\boxplus\delta\boldsymbol{\chi},n)\\&\simeq h_{B}(\hat{\boldsymbol{\chi}},0)+H_{B}\delta\boldsymbol{\chi}+n,n\sim N(0,\sum_{B})\end{aligned} $$
---
其中，$\sum_B$ 是IMU观测噪声的协方差。根据IMU测量模型，观测方程中的噪声 $h_a$ 和 $h_w$ 可用测量噪声来近似。H是在围绕 $h_B(\chi,0)$ 对 $\hat{\chi}_{k+1|k}$ 线性化时得到的雅可比矩阵：
$$ \begin{aligned}H_{B}&=\lim_{\delta\chi\rightarrow0}\frac{h_{B}(\hat{\chi}\boxplus\delta\chi,0)\boxminus h_{B}(\hat{\chi},0)}{\delta\chi}\\&=\begin{bmatrix}\frac{\partial h_{B}}{\partial\hat{d}_{R,j}}&\frac{\partial h_{B}}{\partial\hat{d}_{p,j}}&\frac{\partial h_{B}}{\partial\hat{\chi}_{I}}\end{bmatrix}\end{aligned} $$
根据观测方程 $h_{B}(\chi, n)$（公式(8)），利用链式法则计算雅可比矩阵。为了表述方便且不会产生歧义，后续推导中省略下标k：
$$ \frac{\partial h_{a}}{\partial\hat{d}_{R,j}}=\frac{\partial h_{a}}{\partial R(t)}\frac{\partial R(t)}{\partial\hat{d}_{R,j}},j=0,...,N-1. $$
其中，$\frac{\partial h_{a}}{\partial R(t)}$ 的计算方法如下：
$$ \frac{\partial h_{a}}{\partial R(t)}=\left\lfloor_{I}^{G}R(t)^{T}(a(t)+{}^{G}g)\right\rfloor_{\times} $$
根据公式(3)，$\frac{\partial R(t)}{\partial \hat{d}_{R,i}}$ 的计算方法如下：
$$ \begin{aligned}&\frac{\partial R(t)}{\partial\hat{d}_{R,j}}=\frac{\partial R(t)}{\partial d_{j}}=\tilde{\lambda}_{j}(u)P_{j}J_{r}(\tilde{\lambda}_{j}(u)d_{j}),\\&P_{j}=P_{j+1}A_{j+1}^{T},P_{N}=I,A_{N}=I,j=0,...,N-1.\\ \end{aligned} $$
其中，$A_j$ 的定义如公式(4)所示，而 $A_N = I$ 对应 $j = N$ 的定义。可以看出，与 [^24]、[^26] 中的连续轨迹表示方法相比，本文提出的公式形式更为简洁，无需额外考虑边界条件。$\frac{\partial h_w}{\partial d_{D,j}}$ 的计算方法如下：
$$ \frac{\partial h_{\omega}}{\partial\hat{d}_{R,j}}=\frac{\partial w(t)}{\partial d_{j}}=\frac{\partial w^{(j+1)}}{\partial d_{j}},j=0,...,N-1. $$
根据公式(4)，$\frac{\partial w^{(j+1)}}{\partial d_{j}}$ 的计算方法如下：
$$ \frac{\partial w^{(j+1)}}{\partial d_{j}}=P_{j}(\tilde{\lambda}_{j}(u)A_{j}^{T}\left\lfloor w^{(j)}\right\rfloor_{\times}J_{r}(-\tilde{\lambda}_{j}(u)d_{j})+\dot{\tilde{\lambda}}_{j}(u)I) $$
$b_{a}, b_{w}$和g与$d_{R,j}$无关，因此它们的导数均为零。对于$\frac{\partial h_{B}}{\partial \hat{d}_{p,j}}$而言，可基于观测方程$h_{B}(\chi, n)$以及公式(6)计算出对应的雅可比矩阵，具体如下：
$$ \frac{\partial h_{a}}{\partial\hat{d}_{p,j}}=\mathbf{\Pi}_{I}^{G}R(t)^{T}\ddot{\tilde{\lambda}}_{j}(u)I,\frac{\partial h_{w}}{\partial\hat{d}_{p,j}}=0,j=0,...,N-1. $$
$b_{a}, b_{w}$和g与$d_{p,j}$无关，因此它们的导数均为零。关于$\frac{\partial h_{B}}{\partial \hat{x}_{I}}$，其计算方法如下：
$$ \frac{\partial h_{a}}{\partial\hat{\chi}_{I}}=\left[\begin{matrix}{0}&{I}&{-_{I}^{G}R(t)^{T}\left\lfloor\hat{g}\right\rfloor_{\times}B_{\hat{g}}}\\ \end{matrix}\right],\frac{\partial h_{w}}{\partial\hat{\chi}_{I}}=\left[\begin{matrix}{I}&{0}\\ \end{matrix}\right.\quad0 $$
其中，$\frac{\partial h_{B}(\tilde{\chi},0)}{\partial d_{R,j}}$和$\frac{\partial h_{B}(\tilde{\chi},0)}{\partial d_{p,j}}$与历史状态所在的线段相关。对应的雅可比矩阵仅在待优化的控制点处取值非零，而该线段中的其余控制点属于静态点，不参与雅可比矩阵的计算。$h_{I}$仅与IMU状态相关，因此它相对于状态的雅可比矩阵为：
$$ \frac{\partial h_{I}}{\partial\hat{\chi}}=\left[\begin{matrix}{0_{3\times24}}&{I_{3}}&{0_{3}}&{0_{3\times2}}\\ {0_{3\times24}}&{0_{3}}&{I_{3}}&{0_{3\times2}}\\ {0_{2\times24}}&{0_{2\times3}}&{0_{2\times3}}&{-\frac{B_{g_{k}}^{T}\lfloor g_{k}\rfloor_{\times}\lfloor g_{k}\rfloor_{\times}B_{g_{k}}}{||g_{k}||^{2}}}\\ \end{matrix}\right] $$
拟合误差：在实际应用中，为了平衡效率与表示精度，不会采用高阶样条曲线，因此无法保证所有离散时间点$t_{i}, i=1,...,n$处的位姿恰好位于连续轨迹上。由此不可避免地会产生表示误差，如图4所示，这些误差随后会影响位姿估计的精度。值得注意的是，现有的连续时间里程计研究并未解决这一问题[^17]、[^25]、[^26]、[^27]。
控制点 | IMU测量值 | 激光雷达测量值 | 时间$t_{k}$处估计的状态 | 拟合误差
Fig. 4: Schematic diagram of representation error of continuous time spline curve.
对于受影响的区间 $[t_{k-N+2}, t_{k+1}]$，$\delta_T$ 基于以下两者之间的差异进行估计：(i) 在 $[t_{k-N+2}, t_k]$ 上的最优估计位姿与在 $[t_k, t_{k+1}]$ 上的 IMU 前向传播位姿 [^13]、[^37]，以及 (ii) 在相应时间戳处通过连续轨迹插值获得的位姿。表示误差通常较小，我们假设其服从分布 $\delta_T \sim \mathcal{N}(0, \Sigma_T)$。在实际应用中，旋转和平移是分别进行插值的，从而得出各自的假设 $\delta_R \sim \mathcal{N}(0, \Sigma_R)$ 和 $\delta_p \sim \mathcal{N}(0, \Sigma_p)$。具体计算方法如下：
$$ \begin{align*}\delta\theta_{i}&={}_{I_{i}}^{G}\bar{R}\boxminus R(t_{i}),t_{i}\in[t_{k-N+2},t_{k}],i=1,...,n.\\\delta\theta_{j}&={}_{I_{j}}^{G}\hat{R}\boxminus R(t_{j}),t_{i}\in[t_{k},t_{k+1}],j=1,...,s.\\\Sigma_{R}&=\frac{1}{N}\sum_{m=1}^{N}\delta\theta_{m}\delta\theta_{m}^{T},N=n+s.\end{align*} $$
上述公式给出了旋转表示误差的估计值。对于平移表示误差，可采用类似的方法。
$$ \begin{align*}\delta p_{i}&={}^{G}\bar{p}_{I_{i}}\boxminus p(t_{i}),t_{i}\in[t_{k-N+2},t_{k}],i=1,...,n.\\\delta p_{j}&={}^{G}\hat{p}_{I_{j}}\boxminus p(t_{j}),t_{j}\in[t_{k},t_{k+1}],j=1,...,s.\\\Sigma_{p}&=\frac{1}{N}\sum_{m=1}^{N}\delta p_{m}\delta p_{m}^{T},N=n+s.\end{align*} $$
C. 含不确定性的激光雷达观测
1) 概率性点到稳定特征投影：受[^38]的启发，我们利用点云不确定性建模[^38]，
---
[^40]: [^41] [^42] to characterize feature uncertainty, further improving registration accuracy and efficiency. However, unlike [^38] which focuses solely on planar features, we consider stable environmental structures more broadly. This is motivated by the fact that sensor placement or cluttered environments may not always provide sufficient and stable planar features, potentially degrading localization accuracy.
点不确定性：点的不确定性采用[^38]、[^43]中描述的方法进行量化。在位姿估计过程中，点会通过连续时间表示法投影到全局坐标系下：$^G p_i = {}^G R(t)({}^I R^L p_i + {}^I t_L) + {}^G p(t)$，这会带来额外的定位不确定性[^38]。$^G p_i$对应的不确定性表示为：
$$ \begin{aligned}&\sum_{G_{p_{i}}}=\sum_{I}^{G}R(t)_{L}^{I}R\sum_{L_{p_{i}}}(\sum_{I}^{G}R(t)_{L}^{I}R)^{T}+\sum_{t}\\ &+\sum_{I}^{G}R(t)\left\lfloor\sum_{L}^{I}R^{L}p_{i}+\sum_{I}t_{L}\right\rfloor_{\times}\sum_{R}\left\lfloor\sum_{L}^{I}R^{L}p_{i}+\sum_{I}t_{L}\right\rfloor_{\times}^{T}\sum_{I}^{G}R(t)^{T}\\ \end{aligned} $$
其中，$\sum_{t}$对应$^G p(t)$的不确定性，$\sum_{R}$对应$^G I R(t)$的不确定性。
平面不确定性：对于体素内的点$^G p_i, i = 1, ..., N$，我们可以得到：
$$ q=\frac{1}{N}\sum_{i=1}^{N}{}^{G}p_{i},\sum_{V}=\frac{1}{N}\sum_{i=1}^{N-1}({}^{G}p_{i}-q)({}^{G}p_{i}-q)^{T}. $$
设 q 表示体素内点云分布的平均值，$\sum_{V}$ 表示相应的协方差矩阵。对该协方差矩阵进行特征分解可得：$U\Lambda U^{T}=\sum_{V}$，其中 $\Lambda=diag(\lambda_{1},\lambda_{2},\lambda_{3})$ 和 $U=[u_{1},u_{2},u_{3}]$ 分别代表特征值和特征向量。由于同时考虑了激光雷达点测量不确定性和位姿不确定性，这些不确定性会传递到特征值、特征向量以及均值 [^38] 上，最终得到：
$$ \begin{align*}\left[\lambda^{gt},u^{gt},q^{gt}\right]^{T}&=f\big(^{G}p_{1}+\delta_{G_{p_{1}}},...,\big.,^{G}p_{N}+\delta_{G_{p_{N}}}\big)\\&\approx\left[\bar{\lambda},\bar{u},q\right]^{T}+\sum_{i=1}^{N}\frac{\partial f}{\partial^{G}p_{i}}\delta_{G_{p_{i}}}.\end{align*} $$
相应的协方差可表示为 $\sum_{\lambda,u,q} = \sum_{i=1}^{N} \frac{\partial f}{\partial G_{pi}} \sum_{G_{pi}} \left( \frac{\partial f}{\partial G_{pi}} \right)^{T}$，其中 $\frac{\partial f}{\partial G_{pi}} = \left[ \frac{\partial \lambda_{k}}{\partial G_{pi}}, \frac{\partial u_{k}}{\partial G_{pi}}, \frac{\partial q}{\partial G_{pi}} \right]^{T}$。在本研究中，并未考虑均值的不确定性。
带特征不确定性的点匹配：在点云配准过程中，若仅依赖单一特征类型，当无法可靠检测到相应特征时，可能会导致性能下降 [^38]。因此，本研究采用了多种特征类型。考虑到激光雷达点云的稀疏性，我们同时采用平面特征和体素特征，分别记为 $f_{p}$ 和 $f_{v}$。与以往主要采用 ICP 类方法的研究不同 [^37]、[^44]、[^45]，我们的不确定性建模能够精确刻画特征的可靠性，从而实现更稳健的配准性能。具体的特征表示如图 (5) 所示。
体素构建遵循类似于 [^38] 的由粗到细策略。具体而言，当点云较为稀疏时，先建立粗略的体素表示；随着点云密度增加，则采用自适应细化策略
Fig. 5: Voxel representation and decomposition diagram.
来构建更精细的体素。（注：若能从某个体素中提取出平面特征，则不再进一步细分；否则，该体素会被持续细分，直至能提取出稳定特征或达到最大细分深度为止。）作为叶节点且不包含平面特征的体素被定义为体素特征。构建完体素后，会根据特征类型制定不同类型的观测方程。
点到平面：相应的观测方程表述如下：
$$ \begin{aligned}&h_{L}(\boldsymbol{\chi},n)=u_{1}^{T}(^{G}p_{i}-q)\\&=(\hat{u}_{1}\boxplus\delta_{u_{1}})^{T}[(\hat{T}(t_{i})\boxplus\delta_{T})^{I}T_{L}(^{L}p_{i}+\delta_{L_{p_{i}}})-q-\delta_{q}]\\&\simeq\underbrace{\hat{u}_{1}^{T}(\hat{T}(t_{i})^{I}T_{L}^{L}\hat{p}_{i}-\hat{q})}_{h_{L,r}(\hat{x},0)}+H_{L}\delta\chi\\&+\underbrace{J_{u_{1}}\delta_{u_{1}}+J_{q}\delta_{q}+J_{R(t_{i})}\delta_{\theta_{R(t_{i})}}+J_{p(t_{i})}\delta_{p(t_{i})}+J_{L_{p_{i}}}\delta_{L_{p_{i}}}}_{}\\ \end{aligned} $$
其中 $h_{L,r}(\hat{\chi},0)$ 表示残差，$n_r$ 代表噪声。$\hat{T}(t_i) \in SE(3)$ 表示在时间 $L\hat{p}_i$ 处由连续轨迹计算得到的位姿，$\delta_T$ 则表示连续轨迹的拟合误差。$H_{L,i}$ 表示在 $\hat{\chi}$ 处求得的雅可比矩阵，其计算方法如下：
$$ \begin{align*}H_{L,i}&=\lim_{\delta\chi\rightarrow0}\frac{h_{L,r}(\hat{\chi}\boxplus\delta\chi,0)\boxminus h_{L,r}(\hat{\chi},0)}{\delta\chi}\\&=\left[\frac{\partial h_{L,r}}{\partial d_{R,j}}\quad\frac{\partial h_{L,r}}{\partial d_{p,j}}\quad\frac{\partial h_{L,r}}{\partial\chi_{I}}\right],j=0,...,N-1.\end{align*} $$
其中 $\frac{\partial h_{L,r}}{\partial\chi_{I}}=[0_{1\times8}]$。关于样条状态变量的雅可比矩阵可利用链式法则推导如下：
$$ \frac{\partial h_{L,r}}{\partial d_{R,j}}=\frac{\partial h_{L,r}}{\partial R(t_{i})}\frac{\partial R(t_{i})}{\partial d_{R,j}},\frac{\partial h_{L,r}}{\partial d_{p,j}}=\frac{\partial h_{L,r}}{\partial p(t_{i})}\frac{\partial p(t_{i})}{\partial d_{p,j}} $$
根据观测方程，我们可以得到：
$$ \frac{\partial h_{L,r}}{\partial R(t_{i})}=-\hat{\boldsymbol{u}}_{1}^{T}\hat{\boldsymbol{R}}(t_{i})\left\lfloor^{I}T_{L}{}^{L}p_{i}\right\rfloor_{\times},\frac{\partial h_{L,r}}{\partial p(t_{i})}=\hat{\boldsymbol{u}}_{1}^{T}. $$
$\frac{\partial R(t_{i})}{\partial d_{R,j}}$ 可通过公式 (9) 计算得出，而 $\frac{\partial p(t_{i})}{\partial d_{p,j}}$ 的计算方法如下：
$$ \frac{\partial p(t_{i})}{\partial d_{p,j}}=\tilde{\lambda}_{j}(u)I,j=0,...,N-1; $$
---
根据观测方程的表述可知，$h_{L}(\chi, n)$ 的每个维度均近似服从零均值高斯分布：
$$ \begin{aligned}&h_{L,i}(\boldsymbol{\chi},n)\sim N(h_{L,i}(\hat{\boldsymbol{\chi}},0),\sum_{n_{i}}),\sum_{n_{i}}=J_{n_{i}}\sum J_{n_{i}}^{T}\\&\sum=diag(\sum_{u_{1}},\sum_{q},\sum_{L_{p_{i}}},\sum_{R(t_{i})},\sum_{p(t_{i})})\\ \end{aligned} $$
其中 $J_{n_1}$ 的计算方法如下：
$$ \begin{align*}J_{n_{i}}&=\left[J_{u_{1}},J_{q},J_{L_{p_{i}}},J_{R(t_{i})},J_{p(t_{i})}\right]\\J_{u_{1}}&=(\hat{T}(t_{i})^{I}T_{L}{}^{L}\hat{p}_{i}-q)^{T},J_{q}=-\hat{u}_{1}^{T},J_{L_{p_{i}}}=\hat{u}_{1}^{T}\hat{R}(t_{i})_{L}^{I}R,\\J_{R(t_{i})}&=-\hat{u}_{1}^{T}\hat{R}(t_{i})\left\lfloor{}^{I}T_{L}{}^{L}\hat{p}_{i}\right\rfloor_{\times},J_{p(t_{i})}=\hat{u}_{1}^{T}.\end{align*} $$
点云到体素：当点不位于平面区域内时，会进一步利用体素特征来进行配准。体素内的局部结构由空间点云分布的三个特征值及特征向量来表征，通过将点投影到该分布上即可建立观测方程。与[^46]、[^47]、[^48]不同，本研究同时考虑了激光雷达点的不确定性和位姿不确定性，因此将这三个特征向量和特征值视为随机变量。
$$ h_{L}(\boldsymbol{\chi},n)=\bar{\Lambda}_{i}^{-\frac{1}{2}}U_{i}^{T}\binom{G}{p_{i}-q}=\begin{bmatrix}\bar{\lambda}_{1}^{-\frac{1}{2}}u_{1}^{T}\binom{G}{p_{i}-q}\\ \bar{\lambda}_{2}^{-\frac{1}{2}}u_{2}^{T}\binom{G}{p_{i}-q}\\ \bar{\lambda}_{3}^{-\frac{1}{2}}u_{3}^{T}\binom{G}{p_{i}-q}\end{bmatrix}= $$
$$ \begin{bmatrix}k_{1}(\hat{u}_{1}\boxplus\delta_{u_{1}})^{T}[(\hat{T}(t_{i})\boxplus\delta_{T_{i}})^{I}T_{L}(^{L}\hat{p}_{i}+\delta_{L_{p_{i}}})-\hat{q}-\delta_{q}]\\ k_{2}(\hat{u}_{1}\boxplus\delta_{u_{2}})^{T}[(\hat{T}(t_{i})\boxplus\delta_{T_{i}})^{I}T_{L}(^{L}\hat{p}_{i}+\delta_{L_{p_{i}}})-\hat{q}-\delta_{q}]\\ k_{3}(\hat{u}_{3}\boxplus\delta_{u_{3}})^{T}[(\hat{T}(t_{i})\boxplus\delta_{T_{i}})^{I}T_{L}(^{L}\hat{p}_{i}+\delta_{L_{p_{i}}})-\hat{q}-\delta_{q}]\end{bmatrix} $$
其中$\sum_V U = U \Lambda U^T = U \Lambda^{\frac{1}{2}} \Lambda^{\frac{1}{2}} U^T = (U \Lambda^{-\frac{1}{2}} \Lambda^{-\frac{1}{2}} U^T)^{-1}$、$k_i = \bar{\lambda}_i^{-\frac{1}{2}}$、$i = 1, 2, 3$。体素特征的雅可比矩阵和协方差计算过程与平面特征类似。主要区别在于体素特征需要分别进行三次计算，且每次计算还需以对应的特征值为系数进行加权。为简洁起见，此处不再赘述详细的推导过程。
特征过滤：考虑到激光雷达扫描的稀疏性，要准确表征体素的局部结构通常需要多帧扫描积累得到的观测数据。因此，在基于特征类型进行点投影时，会应用基于$3\sigma$原理的过滤机制。以点到平面匹配为例，引入该过滤机制后可表示为：
$$ \begin{aligned}h_{L}(\chi_{k},n)&=su_{1}^{T}({}^{G}p_{i}-q),\\s&=\begin{cases}1&if d_{i}\in U(q,3\sqrt{\sum_{n_{j}}}),\\0&otherwise\end{cases}\end{aligned} $$
其中$U(q,3\sqrt{\sum_{n_j})}$表示q的$3\sigma$邻域，$d_i$则表示到对应平面的距离。
对于体素特征而言，需对三个平面逐一进行判断。只有当所有三个平面同时满足$3\sigma$原理时，匹配才被视为有效。
2) 重新估计策略：在本研究中，状态预测是基于匀速假设进行的。当预测间隔$\delta t$较短时，单次激光雷达扫描的时间间隔$[t_{ls}, t_{le}]$内会发生多次预测，导致控制点的不确定性迅速累积。 consequently，后续依赖此进行的后验估计，
Fig. 6: The schematic diagram illustrates three prediction and estimation strategies, with the rightmost strategy being the prediction and re-estimation strategy used in this paper.
可能仅需寥寥几次迭代便无法得到理想结果。反之，若预测间隔较长，简单的运动假设便无法准确描述该区间内的运动情况，从而导致初始猜测偏差较大，最终使估计性能下降。
为解决这一问题，受[^49]的启发，我们提出了一种重新估计策略。具体而言，采用相对较短的预测间隔，并设定最大重新估计次数（也即最大采样次数）。一旦当前间隔内的点数超过预设阈值，便启动固定数量采样，随后重复上述过程，直至达到最大重新估计次数或当前间隔内的所有点均被处理完毕。该策略显著减少了每次估计中用于计算卡尔曼增益的观测数据量，进而提升了计算效率。此外，在较短的间隔内可进行多次迭代估计（以上一次估计结果为先验），从而使估计状态能更好地贴合机器人的实际运动情况。如图(6)所示，该方法展现出了卓越的性能与鲁棒性。
以预测区间$\delta t$为例，当新的激光雷达扫描数据到达时，系统状态会被更新一次。设此次更新的起始时间和结束时间分别为$t_{ps}$和$t_{pe}$。当对应区间$[t_{ps}, t_{pe}]$内的点数量超过阈值$N_{thre}$时，便会启动采样-估计-重采样流程。假设最终的采样迭代次数为k，即点云被划分为k个片段，那么就可以从计算复杂度的角度来分析效率的提升情况。
由于残差和雅可比矩阵的计算量与点的数量呈线性关系，因此这一环节的计算效率基本不受影响。主要的差异体现在卡尔曼增益的计算以及协方差矩阵的更新上。根据公式(22)中的卡尔曼增益$K \in \mathbb{R}^{m \times n}$、协方差$P \in \mathbb{R}^{n \times n}$，结合[^50]中的计算复杂度分析，我们可以得到：
$$ \begin{aligned}&S_{K}=6n^{2}m+2nm^{2}+n^{2}-4mn+O(m^{3})+O(n^{3})\\&S_{P}=2n^{3}+2n^{2}m-n^{2}\\ \end{aligned} $$
其中$S_{P}$表示协方差更新的时间复杂度，$S_{K}$表示计算卡尔曼增益的时间复杂度（这是一个保守估计；实际上，由于R
---
(a) M2UD [^29]
(b) MARS-LVIG [^28]
(c) Diter++ [^31]
(d) MCD [^30]
Fig. 7: The experiments are conducted on four public datasets with diverse platforms and environmental conditions. M2UD is collected using an all-terrain vehicle operating in staircases, forests, and playgrounds, featuring rugged terrain and low-speed motion. MARS-LVIG is captured by a quadrotor UAV flying in a downward-facing view at high speed. Diter++ is collected using a quadruped robot navigating complex terrain with significant vibrations and irregularities. MCD is acquired from a ground mobile robot operating in campus environments, characterized by relatively high-speed motion and a LiDAR sensor with a limited field of view.
是一个对角矩阵，其求逆运算可简化为$O(m)$。n代表状态维度，m代表观测维度。因此，这种重新估计策略实际上将原本m维的问题分解为k个维度为m/k的子问题，从而提升了计算效率。
### D. 状态更新
基于迭代扩展卡尔曼滤波（IEKF）框架，根据观测方程和运动学方程迭代执行最大后验概率（MAP）估计：
$$ \begin{aligned}&\delta\chi_{k,i+1}=\\ &K_{i}(z_{k}-h(\hat{\chi}_{k|k,i},0))-(I-K_{i}H_{i})(\hat{\chi}_{k|k,i}-\hat{\chi}_{k|k-1}).\\ \end{aligned} $$
其中，$K_i = (H_i^T R_k^{-1} H_i + P_{k|k}^{-1})^{-1} H_i^T R_k^{-1}$ 表示卡尔曼增益，$z_k$ 是设定为 0 的理想观测值，而 $\hat{\chi}_k|_{k,i}$ 则表示第 i 次迭代时的状态估计值。$H_i = [H_B^T, H_L^T]^T$、$R_k = \text{diag}(\sum_B, \sum_L)$。当达到最大迭代次数 N 或者满足 $\|\delta\chi_{k,i+1}\| \leq \epsilon$ 时，迭代便会终止；此处 $\epsilon$ 是一个足够小的正数阈值。迭代终止后，协方差会更新为 $P_{k|k} = (I - K_i H_i) P_{k|k-1}$。基于上述讨论，算法框架在算法 1 中进行了总结。
### V. 实验
通过在公开数据集上进行大量实验和详细的消融研究，对所提算法的性能进行了全面评估，随后还在轮腿式机器人上开展了进一步的实物验证。
本研究使用的数据集包括M2UD [^29]、MARS-LVIG [^28]、Diter++ [^31]和MCD [^30]。这些数据集涵盖了多种室内外场景，涉及各类机器人平台，如全地形车、无人机、四足机器人和地面移动机器人。详细信息见表I和图7。
### A. 实验设置
TABLE I: Dataset for real-world
| Dataset | Scenarios | Sensors(Adopted) |
| M2UD [^29] | urban, palza, parking, campus, ground robot, uneven-terrain | L(Velodyne VLP-16), I(Xsens MTi 300) |
| MARS LVIG [^28] | airport, island, and valley, large-scale, fast, drone | L(AVIA), I(built-in) |
| Diter++ [^31] | quadruped, forest, and park, large-scale, rough-terrain | L(Ouster OS1 64/128), I(built-in) |
| MCD [^30] | large-scale urban, fast, ground vehicle | L(Livox Mid 70), I(VN100) |
为了进行方法对比，选取以下算法作为基准：FAST-LIO2（F-LIO2）[^37]、FAST-LIVO2（F-LIVO2）[^13]、CLIC [^14]以及RESPLE（R-LIO）[^27]。F-LIO2是一种广泛采用的激光雷达-惯性里程计框架。F-LIVO2是VoxelMap的改进版本，其性能达到业界领先水平，作为一种多传感器融合里程计方法受到了广泛关注；比较实验中采用了其中的LIO组件。CLIC和RESPLE都是近期的连续时间里程计方法，其中CLIC采用基于优化的框架，而RESPLE则采用基于IEKF的框架，两者均展现出具有竞争力的性能。
为了量化精度，需在激光雷达帧的结束时间戳处对轨迹估计结果进行插值。随后利用evo工具[^51]计算绝对位置误差（APE）的均方根误差（RMSE）和最大值（MAX）。为降低随机误差的影响，每个数据集中的每条序列均运行三次，再对得到的RMSE和MAX指标取平均值，从而得到最终数值。
由于不同数据集所采用的平台和激光雷达传感器类型存在差异，实验参数也需相应调整。在MCD数据集中，节点频率被设定为80Hz。对于M2UD和MARS-LVIG数据集，50Hz的节点频率能够实现最佳的精度和鲁棒性。而对于Diter++数据集，节点频率则设定为35Hz。所有算法均在相同的硬件平台上进行评估，具体配置如下：CPU：i5-12400F，内存：16GB。
---
```pseudo
// Algorithm 1: CTVoxelMap: Continuous-time state estimation with adaptive voxel map
Input: $\Delta t$: the knot's interval time of cubic B-spline; $L p_{i}^{t}, i = 1, ..., n$: the k-th frame measurements of lidar; $\{v_{t}, \omega_{t}\}$: the measurements of imu, $t \in [t_{k-j}, t_{k}]$, $j = 1, ..., \hat{\chi}_{k}$: the state to be estimated;
Output: $\bar{\chi}_{k}$: the state estimated.
1: repeat
2: Propagate the state once.
3: Let $N_{l}$ denote the set of LiDAR points acquired within the propagation interval.
4: repeat
5: if $N_{l} > N_{thre}$ then
6: Devide $N_{l}$ into two parts, $N_{l1}$ and $N_{l2}$, let $N_{l} = N_{l1}$.
7: end if
8: calculate the pose in $\{G\}$ of lidar points in $N_{l}$.
9: If the voxel map is not initialized, construct the adaptive voxel map(Fig.(5)).
10: Based on the $\chi_{k}$ and imu measurements, calculate the cubic Bspline fitting error.
11: i = 0, $\chi_{k,i} = \hat{\chi}_{k}$.
12: for $||\chi_{k,i+1} \boxminus \chi_{k,i}|| > \epsilon$ or i < N do
13: Based on measurements and eq.(8), calculate the residual and jacobian with respect to $\chi_{k,i}$.
14: Based on voxel map, the fitting error and the observation formulation of features, calculate the residual and jacobian with respect to $\chi_{k,i}$.
15: Calculate the $\delta\chi_{k,i}$ according to eq.(22).
16: $\chi_{k,i+1} = \chi_{k,i} \boxplus \delta\chi_{k,i}$
17: Recompute the pose of lidar points in $\{G\}$ according to $\chi_{k,i+1}$ and update voxel map.
18: Update the cubic Bspline fitting error.
19: i = i + 1.
20: end for
21: Get the estimated state $\bar{\chi}_{k}$.
22: $N_{l} = N_{l2}$, $\hat{\chi}_{k} = \bar{\chi}_{k}$.
23: until all points in this frame is processed or the max re-estimation times is reached.
24: until all points is processed.
25: return $\bar{\chi}_{k}$.
```
### B. 真实数据集上的性能对比
公共数据集上的统计结果汇总于表II中。CT-VLO（我们的方法）代表所提算法的纯激光雷达模式，而CT-VLIO（我们的方法）则代表IMU-激光雷达融合模式。在LO模式下，连续轨迹拟合误差的协方差是通过固定超参数来设定的；而在LIO模式下，连续轨迹拟合误差的协方差则是根据公式(12)计算得出的。此处列出所提方法的MAX指标仅作说明之用。
从表中的结果可以看出，所提方法在多个公共数据集的不同序列上，就APE指标而言均取得了最佳性能。在MCD数据集的对比方法中，由于[^26]和[^27]这两篇文献未提供MAX指标，因此未报告其MAX指标。为确保对比公平，仅比较RMSE指标。
1) MCD数据集：该数据集包含校园场景，所提方法在该数据集的大多数序列上均取得了最佳性能。对于‘ntu-night13’序列，由于IMU与激光雷达测量之间存在时间延迟，对测量时间戳较为敏感的基于连续时间的方法，在加入IMU测量后系统估计性能会出现下降。
2) M2UD数据集：针对M2UD数据集，我们选取了几个具有挑战性的序列。其中，“aggressive04”序列是通过一辆全地形车在崎岖地形中行驶采集得到的，包括上下楼梯以及大幅度俯仰运动等场景。在该序列中，只有R-LIO和所提算法能够完成整个序列，且所提算法的性能更优。值得注意的是，在此序列中CT-VLO的表现优于CT-VLIO，主要是因为车辆在崎岖地形行驶时IMU测量值会出现显著突变；由于未对这类IMU数据采取特殊处理，IMU测量值的融合反而降低了整体性能。“plaza03”序列涉及购物中心内的多个楼层。由于所有方法在进入电梯后于1350秒时均失效，因此该序列的时间范围被限定为[^0] [^1350]。在这些方法中，F-LIO虽未取得最佳性能，但结果相对稳定。然而，CLIC和F-LIVO2无法完成该序列，R-LIO则表现出不稳定性，多次失效。此处报告的R-LIO在该序列中的结果是有限次成功运行中所获得的最佳结果。
3) MARS-LVIG数据集：对于MARS-LVIG数据集，我们选取了三个中等速度（8 m/s）到高速（12 m/s）的运动序列（包含GNSS信息）。在“HKairport02”序列中，飞机起飞或降落时激光雷达测量值会严重退化，所有方法均会出现一定漂移；引入IMU测量值后，漂移程度减小，系统的鲁棒性得以提升。在“AMtown03”序列中，既有平坦区域的起降过程，也有快速的大角度转弯，此时激光雷达同样会发生退化现象。在此类场景下，CLIC方法会出现显著漂移，无法完成序列。
4) Diter++数据集：Diter++数据集共使用了两个序列。“Forest-new”序列是在地形较为复杂的林区进行的短距离运行测试，所有入选方法均取得了良好性能。“Park-in-day-cam-free”序列需要穿越林区、建筑物和长走廊，属于大规模且长时间的运行测试（2160秒）。在这些方法中，CLIC仅能维持有限时间的运行，无法完成整个序列；相比之下，CT-VLO成功完成了整个序列，且APE指标最优。该序列的完整运行结果详见补充视频资料。
为了更直观地展示算法性能，我们选取了具有挑战性的序列“aggressive04”和“AM-
---
TABLE II: APE (MAX/ RMSE, meters) of methods on public dataset
| Dataset | Sequence | CLIC | F-LIO2 | R-LIO | F-LIVO2 | CT-VLO (ours) | CT-VLIO (ours) |
| MCD | ntu-day-01 | 25.194 | 0.901 | 0.549 | 3.681 / 1.276 | 2.639 / 1.100 | 2.904 / 1.444 |
| ntu-day-02 | 0.503 | 0.185 | 0.188 | 0.439 / 0.148 | 0.421 / 0.167 | 0.355 / 0.144 |
| ntu-day-10 | 22.775 | 1.975 | 1.493 | 2.127 / 0.826 | 1.549 / 0.646 | 2.295 / 0.955 |
| ntu-night-04 | 4.154 | 0.902 | 0.416 | 1.028 / 0.455 | 1.063 / 0.385 | 0.901 / 0.310 |
| ntu-night-08 | 22.275 | 1.002 | 0.940 | 2.133 / 0.868 | 1.983 / 0.789 | 2.242 / 1.132 |
| ntu-night-13 | 21.299 | 1.288 | 0.560 | 2.587 / 1.112 | 0.855 / 0.381 | 2.452 / 1.229 |
| M2UD | plaza03 | $\times$ | 0.831 / 0.436 | 1.733 / 0.308 $^{*}$ | $\times$ | 2.419 / 0.644 | 1.435 / 0.619 |
| aggressive04 | $\times$ | $\times$ | 2.507 / 0.882 | $\times$ | 2.107 / 0.690 | 2.118 / 0.715 |
| aggressive05 | $\times$ | 7.298 / 3.567 | 7.465 / 3.228 | $\times$ | 7.521 / 3.179 | 7.548 / 3.243 |
| campus06 | $\times$ | 6.601 / 2.524 | 3.642 / 1.422 | 2.392 / 1.256 | 2.292 / 1.123 | 3.011 / 1.461 |
| MARS-LVIG | HKisland02 | 3.475 / 1.973 | 3.851 / 2.241 | 4.503 / 2.177 | 3.948 / 2.028 | 4.562 / 2.065 | 4.232 / 1.909 |
| HKairport02 | 15.635 / 3.122 | 8.987 / 2.931 | 10.797 / 3.557 | 8.390 / 3.284 | 15.294 / 3.220 | 4.509 / 2.702 |
| AMtown03 | $\times$ | 6.191 / 3.216 | 20.640 / 12.630 | 5.490 / 2.685 | 3.971 / 2.299 | 3.183 / 2.033 |
| Diter++ | Park-in-day-cam-free | $\times$ | 0.939 / 0.348 | 0.825 / 0.416 | 3.194 / 1.011 | 0.581 / 0.214 | 0.573 / 0.252 |
| Forest-new | 2.704 / 1.372 | 0.438 / 0.103 | 0.371 / 0.127 | 0.505 / 0.147 | 0.479 / 0.108 | 0.466 / 0.103 |
town03 $^{4}$ 用于进一步分析。这些序列上的建图结果如图8所示。在“aggressive04”序列中，CLIC方法在穿过第一级楼梯后便开始出现位姿估计振荡，最终完全失效。F-LIO2和F-LIVO2在遇到剧烈俯仰变化时，也难以维持稳定的位姿估计，导致姿态出现突变。相比之下，RESPLE、CT-VLO（我们的算法）和CT-VLIO（我们的算法）在整个序列中均能保持稳定的位姿估计。在“AMtown03”序列中，CLIC在第一次快速大角度转弯时便丢失轨迹；其余方法成功完成了整个序列。不过，由于这些方法均未采用回环闭合技术，因此均存在累积的位姿估计误差。相比之下，CT-VLO和CT-VLIO的累积误差较小，建图结果也更清晰。
### C. 消融实验
基于APE指标，在公共数据集上的实验结果验证了CT-VLO和CT-VLIO的性能。为了更深入、更直接地了解每个所提出模块的影响，我们在搭载不同激光雷达传感器的三个序列上开展了消融实验：MCD：ntu-day01（Mid-70）；M2UD：aggressive04（Velodyne-16）；以及MARS-LVIG：AMtown03（Avia）。这些消融实验中采用了仅使用激光雷达数据的CT-VLO方法，以消除惯性测量单元（IMU）数据的影响。我们分别研究了拟合误差建模、体素特征以及重估计策略的作用。启用重估计策略时，所有场景下的最大重估计次数均设为5次。每个序列上分别独立运行10次实验，结果汇总于表III中。表中报告的与APE相关的指标包括最大偏差、平均值和均方根误差，单位均为米（m）；每帧的平均处理时间以毫秒（ms）为单位报告；成功率（S-rate）的计算方式为SC/TC，其中SC表示成功次数，TC表示总次数。消融配置的具体含义如下：wo fit-err表示不考虑三次样条拟合误差；wo re表示不采用重估计策略；wo voxel表示不使用体素特征；wo re&voxel表示既不采用重估计策略也不使用体素特征；w all则表示完整的CT-VLO方法。所报告的APE指标对应10次运行中的最佳结果。
TABLE III: Ablation Experiments of CT-VLO on datasets
| MCD | Max/Mean/RMSE | Ave-time | S-rate |
| wo fit-err | - | - | 0/10 |
| wo re | 2.278/1.072/1.168 | 27.5316 | 6/10 |
| wo voxel | 1.779/0.826/0.906 | 27.2216 | 10/10 |
| wo re&voxel | 2.504/0.885/1.009 | 27.4204 | 4/10 |
| w all | 2.635/0.878/0.994 | 27.3120 | 9/10 |
| M2UD | Max/Mean/RMSE | Ave-time | S-rate |
| wo fit-err | - | - | 0/10 |
| wo re | 2.046/0.601/0.691 | 33.796 | 8/10 |
| wo voxel | 2.081/0.593/0.686 | 33.497 | 10/10 |
| wo re&voxel | 2.097/0.598/0.688 | 33.931 | 10/10 |
| w all | 2.072/0.586/0.677 | 33.565 | 10/10 |
| MARS-LVIG | Max/Mean/RMSE | Ave-time | S-rate |
| wo fit-err | - | - | 0/10 |
| wo re | - | - | 0/10 |
| wo voxel | 18.959/2.704/2.939 | 62.2079 | 10/10 |
| wo re&voxel | - | - | 0/10 |
| w all | 3.972/2.204/2.299 | 62.498 | 10/10 |
1) 拟合误差：在算法设计中，我们明确考虑了三次B样条轨迹拟合引入的误差。根据公式(15)，该误差会传递到观测噪声中，进而增加观测的不确定性。表III中的结果表明，在当前框架下，若不将连续轨迹拟合误差纳入考量，系统将无法正常运行。
2) 体素特征：如表III所示，引入体素特征会增加系统可获取的信息量。不过MCD数据集上的结果显示，在该数据集上体素特征会导致系统稳定性和性能略有下降。从M2UD的结果来看，可以
---
(a) CLIC
(b) F-LIO2
(c) F-LIVO2
(d) RESPLE
(e) CT-VLO
(f) CT-VLIO
(g) CLIC
(h) F-LIO2
(i) F-LIVO2
(j) RESPLE
(k) CT-VLO
(1) CT-VLIO
Fig. 8: Performing results comparison of different methods on the ‘aggressive04’ and ‘AMtown03’ sequence.
---
Fig. 9: Statistical results of total and voxel residual numbers.
观察到：若不采用重估计策略，仅使用体素特征会使系统稳定性略微降低；但当同时采用重估计策略和体素特征时，系统性能会得到提升。MARS-LVIG的结果表明，引入体素特征还能进一步改善估计性能。
图9展示了运行时残差总数与对应体素特征的残差数量之间的关系。结合表III的结果可以得出结论：当体素特征所占比例较高时，系统稳定性和性能往往会下降；反之，当体素特征所占比例较低时，它们会对提升估计性能产生积极作用。
3) 重新估计策略：如MCD所示，无论是否利用体素特征，重新估计策略都能提高系统的成功率。根据M2UD中的结果，当结合体素特征时，重新估计策略能进一步提升系统的鲁棒性。MARS-LVIG中的结果表明，若没有重新估计策略，系统将无法正常运行。因此可以确定，重新估计策略显著增强了系统的鲁棒性。此外，表格数据还显示，若不采用该策略，平均每帧处理时间会增加，这进一步证明了其在提升计算效率方面的有效性。为了进一步探究重新估计策略的影响，讨论部分对最大重新估计次数进行了详细分析。
### D. 讨论
通过消融实验可以明显看出，重新估计策略对系统鲁棒性有显著影响。本节
Fig. 10: Performing results comparison of CT-VLO with different re-estimate times on 'AMtown03' sequence
TABLE IV: Re-estimate policy of CT-VLO on MARS-LVIG dataset
| AMtown03 | re-estimate times | Max/Mean/RMSE | Ave-time |
| CT-VLO | 1 | - | - |
| CT-VLO | 2 | - | - |
| CT-VLO | 3 | - | - |
| CT-VLO | 4 | - | - |
| CT-VLO | 5 | 3.971/2.204/2.299 | 62.4751 |
| CT-VLO | 6 | 10.611/2.265/2.481 | 63.6368 |
| CT-VLO | 7 | 29.316/3.186/3.989 | 66.2219 |
| CT-VLO | 8 | 41.146/2.814/3.485 | 69.2019 |
| CT-VLO | 9 | 7.416/3.459/3.721 | 71.3103 |
| CT-VLO | 10 | 3.829/2.149/2.253 | 74.809 |
| CT-VLO | 11 | 7.219/3.665/3.986 | 75.4734 |
| CT-VLO | 12 | 7.219/3.665/3.986 | 75.0008 |
进一步探讨了重新估计策略中最大重新估计次数的影响。讨论基于‘AMtown03’序列展开，结果如表IV所示。
根据表IV中的结果，当最大重新估计次数设置过低时，扫描中的大部分点会被丢弃，从而导致系统失效。随着最大重新估计次数的逐步增加，每次扫描中使用的点数以及处理时间也会相应增加。当重新估计次数达到10次时，每帧处理时间趋于稳定，此时性能表现最佳。若最大值设为11或12，处理时间并无显著变化，这表明扫描中的所有点均已被充分利用。
图IV展示了序列执行期间的实际重新估计次数，其中横轴代表
---
(a) Trajectory
(b) Plot trajectories in the x, y, and z directions
(c) Speed curve
Fig. 11: Performing results comparison of CT-VLO with different re-estimation times on 'AMtown03' sequence.
为了进一步验证该算法对不同机器人平台（运动模式）及不同激光雷达传感器的适应性，我们在校园内使用轮腿式机器人开展了实地验证。该机器人平台如图12所示。
实地实验在校园内具有挑战性的崎岖地形区域进行，包括植物园
估计索引，纵轴代表实际重新估计次数。在当前参数设置下，实际最大重新估计次数为10次。轨迹结果如图11所示，表明在激光雷达性能下降的条件下，重新估计策略的影响更为显著，此时较少的重新估计次数反而有助于系统稳定。基于这些发现，为在实际应用中平衡性能与效率，最大重新估计次数被设定为5次。
Fig. 12: Description of the Experimental Wheel-Legged Robot Platform and Hardware.
### E. 轮腿式机器人实验
花园、河岸及园林区域。实验环境与实验结果的截图分别如表V和图13所示。
基于实地机器人实验可知，搭载Livox-Mid 360激光雷达的轮腿式机器人仍能实现更优的性能。
TABLE V: APE Metric (RMSE, mster) on Our Self-Collected Botanical Dataset.
| Dataset | F-LIO2 | CT-VLO | CT-VLIO |
| botanical01 | 1.853 | 1.723 | 1.711 |
| botanical02 | 1.249 | 0.877 | 0.821 |
| botanical03 | 2.093 | 2.008 | 2.025 |
### F. 结论
通过对样条表示的深入分析，本研究对广泛采用的李群上的累积形式B样条进行了改进，从而在推导关于状态变量的雅可比矩阵时，无需再考虑额外的边界条件（即$j=0$或$j=N-1$这类情况）。通过估计控制点增量而非直接估计控制点本身，雅可比矩阵的计算过程得以大幅简化。此外，无论是通过基于惯性测量单元的在线估计还是预定义的超参数来补偿连续轨迹拟合误差，所提出的系统都能实现稳定运行。体素图中引入的体素特征使系统能够利用更多额外信息，进而提升估计性能。最后，所提出的重新估计策略在预测区间内包含多个采样-估计循环，不仅显著增强了系统的鲁棒性，还缩短了每帧处理时间并提高了计算效率。
不过该研究仍存在一些局限性。首先，论文仅探讨了在存在颠簸或快速运动场景下的性能表现，若要构建功能更完备且更鲁棒的位姿估计系统，还需进一步拓展至高度动态且存在遮挡的环境。其次，尽管重新估计策略
---
(a) Botanical01 env
(b) BotanicalO2 env
(c) BotanicalO3 env
(d) Botanical01 Trajectory
(e) Botanical02 Trajectory
(f) Botanical03 Trajectory
Fig. 13: Comparison of Trajectories Across Different Scenes in the Dataset.
能显著提升系统的鲁棒性，但在传感器性能严重退化的环境中，其有效性可能会受到影响。解决这些局限性将是未来研究的方向。
## 参考文献
[^1]: C. Xia, Y. Shen, Y. Yang, X. Deng, S. Chen, J. Xin, and N. Zheng, "Onboard sensors-based self-localization for autonomous vehicle with hierarchical map," IEEE Transactions on Cybernetics, vol. 53, no. 7, pp. 4218–4231, 2023.
[^2]: W. Liu, Y. Ren, R. Guo, V. W. Kong, A. S. Hung, F. Zhu, Y. Cai, H. Wu, Y. Zou, and F. Zhang, “Slope inspection under dense vegetation using lidar-based quadrotors,” Nature Communications, vol. 16, no. 1, p. 7411, 2025.
[^3]: N. Chen, F. Kong, W. Xu, Y. Cai, H. Li, D. He, Y. Qin, and F. Zhang, “A self-rotating, single-actuated uav with extended sensor field of view for autonomous navigation,” Science Robotics, vol. 8, no. 76, p. eade4538, 2023.
[^4]: Y. Wang, B. Ren, X. Zhang, P. Wang, C. Wang, R. Song, Y. Li, and M. Q.-H. Meng, “Rolo-slam: rotation-optimized lidar-only slam in uneven terrain with ground vehicle,” Journal of Field Robotics, vol. 42, no. 3, pp. 880–902, 2025.
[^5]: T. Deng, G. Shen, C. Xun, S. Yuan, T. Jin, H. Shen, Y. Wang, J. Wang, H. Wang, D. Wang, et al., “Mne-slam: Multi-agent neural slam for mobile robots,” in Proceedings of the Computer Vision and Pattern Recognition Conference, pp. 1485–1494, 2025.
[^6]: J. Lv, J. Xu, K. Hu, Y. Liu, and X. Zuo, "Targetless calibration of lidar-imu system based on continuous-time batch estimation," in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 9968–9975, 2020.
[^7]: Z. Yuan, F. Lang, T. Xu, and X. Yang, "Liu: Lidar-inertial-wheel odometry," in 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1481–1488, 2023.
[^8]: X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, "Lic-fusion: Lidar-inertial-camera odometry," in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 5848–5854, 2019.
[^9]: X. Zuo, Y. Yang, P. Geneva, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, "Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking," in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 5112–5119, 2020.
[^10]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, “Fast-livo: Fast and tightly-coupled sparse-direct lidar-inertial-visual odometry,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4003–4009, 2022.
[^11]: W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3317–3324, 2021.
[^12]: T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4758–4765, 2018.
[^13]: C. Zheng, W. Xu, Z. Zou, T. Hua, C. Yuan, D. He, B. Zhou, Z. Liu, J. Lin, F. Zhu, Y. Ren, R. Wang, F. Meng, and F. Zhang, "Fast-livo2: Fast, direct lidar-inertial-visual odometry," IEEE Transactions on Robotics, vol. 41, pp. 326–346, 2025.
[^14]: J. Lv, X. Lang, J. Xu, M. Wang, Y. Liu, and X. Zuo, “Continuous-time fixed-lag smoothing for lidar-inertial-camera slam,” IEEE/ASME Transactions on Mechatronics, vol. 28, no. 4, pp. 2259–2270, 2023.
[^15]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 5135–5142, 2020.
[^16]: X. Zheng and J. Zhu, “Traj-lo: In defense of lidar-only odometry using an effective continuous-time trajectory,” IEEE Robotics and Automation Letters, vol. 9, no. 2, pp. 1961–1968, 2024.
[^17]: P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, "Ct-icp: Real-time elastic lidar odometry with loop closure," in 2022 International Conference on Robotics and Automation (ICRA), pp. 5580–5586, 2022.
[^18]: W. Talbot, J. Nubert, T. Tuna, C. Cadena, F. Dümbgen, J. Tordesillas, T. D. Barfoot, and M. Hutter, “Continuous-time state estimation methods in robotics: A survey,” IEEE Transactions on Robotics, pp. 1–20, 2025.
## [^19]: C. H. Tong, P. Furgale, and T. D. Barfoot, “Gaussian process gauss-newton for non-parametric simultaneous localization and mapping,” The
---
《国际机器人研究杂志》，第32卷第5期，第507–525页，2013年。
[^20]: T. D. Barfoot, C. H. Tong, and S. Särkkä, “Batch continuous-time trajectory estimation as exactly sparse gaussian process regression,” in Robotics: Science and Systems, vol. 10, pp. 1–10, Citeseer, 2014.
[^21]: J. Dong, B. Boots, and F. Dellaert, “Sparse gaussian processes for continuous-time trajectory estimation on matrix lie groups,” arXiv preprint arXiv:1705.06020, 2017.
[^22]: M. G. Cox, “The numerical evaluation of b-splines,” IMA Journal of Applied Mathematics, vol. 10, no. 2, pp. 134–149, 1972.
[^23]: C. De Boor, “On calculating with b-splines,” Journal of Approximation theory, vol. 6, no. 1, pp. 50–62, 1972.
[^24]: C. Sommer, V. Usenko, D. Schubert, N. Demmel, and D. Cremers, “Efficient derivative computation for cumulative b-splines on lie groups,” in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), June 2020.
[^25]: J. Lv, K. Hu, J. Xu, Y. Liu, X. Ma, and X. Zuo, "Clins: Continuous-time trajectory estimation for lidar-inertial system," in 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 6657–6663, 2021.
[^26]: T.-M. Nguyen, X. Xu, T. Jin, Y. Yang, J. Li, S. Yuan, and L. Xie, “Eigen is all you need: Efficient lidar-inertial continuous-time odometry with internal association,” IEEE Robotics and Automation Letters, 2024.
[^27]: Z. Cao, W. Talbot, and K. Li, "Resple: Recursive spline estimation for lidar-based odometry," IEEE Robotics and Automation Letters, vol. 10, no. 10, pp. 10666–10673, 2025.
[^28]: H. Li, Y. Zou, N. Chen, J. Lin, X. Liu, W. Xu, C. Zheng, R. Li, D. He, F. Kong, et al., “Mars-lvig dataset: A multi-sensor aerial robots slam dataset for lidar-visual-inertial-gnss fusion,” The International Journal of Robotics Research, vol. 43, no. 8, pp. 1114–1127, 2024.
[^29]: Y. Jia, S. Wang, S. Shao, Y. Wang, F. Zhang, and T. Wang, "M2ud: A multi-model, multi-scenario, uneven-terrain dataset for ground robot with localization and mapping evaluation," The International Journal of Robotics Research, p. 02783649251401224, 2025.
[^30]: T.-M. Nguyen, S. Yuan, T. H. Nguyen, P. Yin, H. Cao, L. Xie, M. Wozniak, P. Jensfelt, M. Thiel, J. Ziegenbein, et al., “Mcd: Diverse large-scale multi-campus dataset for robot perception,” in Proceedings of the IEEE/CVF conference on computer vision and pattern recognition, pp. 22304–22313, 2024.
[^31]: J. Kim, H. Kim, S. Jeong, Y. Shin, and Y. Cho, "Diter++: Diverse terrain and multi-modal dataset for multi-robot slam in multi-session environments," in 2025 IEEE International Conference on Robotics and Automation (ICRA), pp. 12187–12193, 2025.
[^32]: S. Kohlbrecher, O. Von Stryk, J. Meyer, and U. Klingauf, “A flexible and scalable slam system with full 3d motion estimation,” in 2011 IEEE international symposium on safety, security, and rescue robotics, pp. 155–160, IEEE, 2011.
[^33]: J. Zhang, S. Singh, et al., “Loam: Lidar odometry and mapping in real-time,” in Robotics: Science and systems, vol. 2, pp. 1–9, Berkeley, CA, 2014.
[^34]: G. P. C. Júnior, A. M. C. Rezende, V. R. F. Miranda, R. Fernandes, H. Azpúruas, A. A. Neto, G. Pessin, and G. M. Freitas, “Ekf-loam: An adaptive fusion of lidar slam with wheel odometry and inertial data for confined spaces with few geometric features,” IEEE Transactions on Automation Science and Engineering, vol. 19, no. 3, pp. 1458–1471, 2022.
[^35]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE International Conference on Robotics and Automation, pp. 3565–3572, 2007.
[^36]: M. Li and A. I. Mourikis, “High-precision, consistent ekf-based visual-inertial odometry,” The International Journal of Robotics Research, vol. 32, no. 6, pp. 690–711, 2013.
[^37]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, "Fast-lio2: Fast direct lidar-inertial odometry," IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2053–2073, 2022.
[^38]: C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry,” IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 8518–8525, 2022.
[^39]: H. Shen, Z. Wu, Y. Hui, W. Wang, Q. Lyu, T. Deng, Y. Zhu, B. Tian, and D. Wang, "Cte-mlo: Continuous-time and efficient multi-lidar odometry with localizability-aware point cloud sampling," IEEE Transactions on Field Robotics, 2025.
[^40]: Y. Lv, Y. Zhang, X. Zhao, W. Li, J. Ning, and Y. Jin, “Cta-lo: Accurate and robust lidar odometry using continuous-time adaptive estimation,”
发表于2024年IEEE机器人与自动化国际会议（ICRA），第12034–12040页，2024年。
[^41]: B. Jiang and S. Shen, “A lidar-inertial odometry with principled uncertainty modeling,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 13292–13299, 2022.
[^42]: K. Huang, J. Zhao, J. Lin, Z. Zhu, S. Song, C. Ye, and T. Feng, “Loglio2: A lidar-inertial odometry with efficient uncertainty analysis,” IEEE Robotics and Automation Letters, vol. 9, no. 10, pp. 8226–8233, 2024.
[^43]: C. Yuan, X. Liu, X. Hong, and F. Zhang, “Pixel-level extrinsic self calibration of high resolution lidar and camera in targetless environments,” IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 7517–7524, 2021.
[^44]: T. Tuna, J. Nubert, Y. Nava, S. Khattak, and M. Hutter, “X-icp: Localizability-aware lidar registration for robust localization in extreme environments,” IEEE Transactions on Robotics, vol. 40, pp. 452–471, 2024.
[^45]: Z. Liu and F. Zhang, “Balm: Bundle adjustment for lidar mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3184–3191, 2021.
[^46]: C. Wang, Z. Cao, J. Li, J. Yu, and S. Wang, "Hierarchical distribution-based tightly-coupled lidar inertial odometry," IEEE Transactions on Intelligent Vehicles, vol. 9, no. 1, pp. 1423–1435, 2024.
[^47]: J. Zhu, J. Mu, C.-B. Yan, D. Wang, and Z. Li, "3dmndt: 3d multi-view registration method based on the normal distributions transform," IEEE Transactions on Automation Science and Engineering, vol. 21, no. 1, pp. 488–501, 2024.
[^48]: P. Biber and W. Strasser, “The normal distributions transform: a new approach to laser scan matching,” in Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453), vol. 3, pp. 2743–2748 vol.3, 2003.
[^49]: D. He, W. Xu, N. Chen, F. Kong, C. Yuan, and F. Zhang, “Point-lio: robust high-bandwidth light detection and ranging inertial odometry,” Advanced Intelligent Systems, vol. 5, no. 7, p. 2200459, 2023.
[^50]: B. Chen, X. Liu, H. Zhao, and J. C. Principe, “Maximum correntropy kalman filter,” Automatica, vol. 76, pp. 70–77, 2017.
[^51] M. Grupp著，《evo：用于评估里程计与SLAM的Python软件包》。，2017年。