# FAST-LIVGO：一种抗退化激光雷达-惯性-视觉-GNSS融合里程计
陈志宇 $^{1*}$、郑春然 $^{2*}$、温佳宇 $^{1}$、张晓雷 $^{1}$、徐佳明 $^{1}$、潘峰 $^{3}$、崔玉康 $^{1†}$
摘要—在长期、大规模且高度动态的环境中实现鲁棒的状态估计与建图，仍是机器人领域的关键挑战。现有的激光雷达-惯性-视觉里程计（LIVO）系统虽然具备较高的局部精度，但存在长距离行驶时的累积漂移问题，且在几何特征退化或无纹理的场景中可能无法正常工作。同时，依赖GNSS辅助的融合框架往往需借助激光雷达或视觉里程计来进行状态预测与异常值剔除，一旦里程计出现退化现象，这类系统便会失效。为克服上述局限，我们提出了一种基于误差状态迭代卡尔曼滤波的紧耦合激光雷达-惯性-视觉-GNSS融合框架。针对高度动态工况，我们引入了基于动态时间规整算法的在线时空对齐模块。为充分利用GNSS的高精度特性，我们构建了基于多普勒频移和固定锚点载波相位的时间差观测模型，无需扩充历史锚点状态即可提供毫米级的相对约束。我们还设计了感知退化情况的双模式异常值剔除策略：根据LIVO的退化程度，在“以LIVO先验为导向的剔除方式”与“GNSS辅助的恢复方式”之间切换。在公开的M3DGR数据集以及自制的20米/秒固定翼无人机数据集上的实验表明，该系统能有效减少累积漂移与地图重影现象，在精度和鲁棒性方面均优于现有最先进的方法。
## 一、引言
在复杂、大规模且高度动态的环境中实现鲁棒的状态估计与环境建图，仍是自主系统的核心挑战 [^1]。为克服单传感器存在的局限性，多传感器融合技术，尤其是激光雷达-惯性-视觉里程计（LIVO），因能结合激光雷达的几何精度优势与摄像头在纹理丰富区域的鲁棒性而被广泛应用 [^2] [^3] [^4] [^5] [^6]；而近期出现的RGB-热成像融合技术更是进一步提升了低光照环境下的建图鲁棒性 [^7]。然而，在大规模野外任务或高速无人机飞行场景中，纯LIVO系统仍面临关键局限：缺乏全局约束会导致随时间、距离推移而不断累积的漂移现象，这种漂移往往沿Z轴方向发散；此外，在感知退化的场景下，如无纹理的开阔广场、无特征墙面或高速运动模糊环境中，LIVO可能无法正常工作。
尽管全球导航卫星系统（GNSS）可提供绝对定位信息以抑制LIVO的漂移现象，但将GNSS紧密集成仍存在挑战。在城市峡谷或林荫道等复杂环境中，非视距（NLOS）信号与多径效应会产生严重的异常噪声，直接融合此类受污染的测量数据会降低轨迹平滑度 [^8]，[^9]。此外，高速运动下低成本传感器的时间同步误差会被显著放大，进而导致估计滞后。现有的松耦合方案未能充分利用GNSS载波相位的高精度特性，而传统的紧耦合框架在LIVO与GNSS性能交替退化的场景下缺乏自适应的完整性监测机制。
为解决这些问题，我们提出了一种基于误差状态迭代卡尔曼滤波（ESIKF）的紧耦合多传感器融合系统，旨在实现高动态场景下的全局一致性建图，并在环境严重退化时仍能稳健运行。该系统利用传感器的互补性：采用高频FAST-LIVO2里程计作为GNSS信号评估与在线时空对齐的几何先验；同时利用GNSS多普勒频移和差分载波相位（TDCP）观测值来提供绝对速度和相对位移约束。此外，我们还设计了分层完整性监测与自适应融合策略，以排除GNSS多路径效应异常，并支持在LIVO几何或纹理特征退化情况下的无缝室内外过渡。
主要贡献总结如下：
- 一种基于ESIKF的紧耦合激光雷达-惯性-视觉-GNSS融合框架。该框架利用动态时间规整（DTW）和两步式外部参数优化，可在高动态条件下处理低成本传感器的时空同步问题。此外，还构建了精确的多普勒和固定锚点TDCP观测模型，无需扩展状态即可引入亚毫米级的相对约束，从而提升全局一致性。
- 一种依赖退化程度的双模式异常值剔除策略。通过分析LIVO海森矩阵的特征值来实现实时环境退化检测，系统可自适应地在“基于LIVO先验的剔除”与“GNSS辅助恢复”之间切换，确保在特征稀疏或卫星遮挡环境中仍能稳健运行。
---
- 在公开的M3DGR数据集[^10]以及我们自行采集的20 m/s高速无人机和手持设备数据集上进行了大量验证。结果表明，所提系统在定位精度和建图一致性方面均优于现有最先进方法。这些高动态数据集将被开源，以支持后续研究。
### II. 相关工作
### A. 激光雷达-惯性-视觉里程计（LIVO）
多传感器融合是鲁棒状态估计的基础。早期的LIVO系统主要采用松散融合方式，而R2LIVE [^12]通过将在ESIKF内融合的LIO状态与并行优化线程中的视觉重投影误差相结合，实现了紧耦合。R3LIVE [^4]进一步将视觉光度误差和激光雷达几何残差整合到统一的ESIKF更新中，提升了系统的一致性，同时还引入了RGB彩色点云地图以增强在纹理丰富环境中的性能。
受直接视觉里程计的启发，尤其是SVO [^13]，FAST-LIVO [^14]通过最小化像素级光度误差来减少视觉前端的计算量。在激光雷达分支中，VoxelMap [^15]通过概率体素级平面拟合提高了配准效率和噪声适应性，后来被扩展为完整的激光雷达-惯性SLAM系统Voxel-SLAM [^16]。这些思路被整合到FAST-LIVO2 [^3]中，它结合了SVO风格的前端和VoxelMap风格的后端，达到了最先进的局部精度。然而，由于缺乏全局约束，FAST-LIVO2在长距离任务中仍会出现漂移现象，且在视觉和几何特征同时退化时可能会失效。
为了抑制累积漂移并确保全局一致性，GNSS集成技术已从松散耦合发展到利用原始载波相位观测值的紧耦合方案[^17]、[^18]。
### B. GNSS辅助融合系统
早期方法通常采用松耦合方式。例如，LIO-SAM [^8]直接将GPS位置信息引入因子图，以约束长期存在的LIO漂移问题。不过这类方法依赖于接收机层面的解算结果，且忽略了原始GNSS观测值之间的相关性，因此极易受到严重多路径效应的影响。为了更好地利用原始测量数据，研究人员开发了紧耦合框架。GVINS [^19]在非线性优化框架中融合了原始伪距和多普勒测量值，提升了系统的鲁棒性；而GLIO [^20]则将这一思路拓展到激光雷达系统中，以实现在GNSS信号缺失环境下的平滑过渡。不过，米级精度的伪距噪声仍然限制了局部定位的精确度。
基于载波相位的融合技术近年来在高精度估计领域受到了越来越多的关注。为了避免在移动平台上解决整数模糊度问题，人们采用了时间差分载波相位（TDCP）技术。LIGO [^21]直接将TDCP约束条件引入LIO框架，利用毫米级的相对位移观测值来抑制里程计漂移。尽管取得了这些进展，现有方法仍存在两方面局限：其一，它们对几何约束的依赖使其在几何条件较差的场景下表现不佳；其二，在混合高动态和多路径环境下，它们缺乏针对不同传感器故障模式的自适应鲁棒机制。我们的框架正是基于这些紧耦合思路，通过智能融合LIVO和GNSS来实现鲁棒的地图构建。
### III. 方法论
### A. 系统概述
我们提出的紧耦合多传感器融合框架是基于误差状态迭代卡尔曼滤波器（ESIKF）构建的。如图1所示，该框架接收来自3D激光雷达、RGB相机、惯性测量单元（IMU）以及GNSS接收机的原始数据。为了实现空间参考的统一，在预处理阶段会将GNSS星历和观测值（如位置、速度）从地心地球固定（ECEF）坐标系转换到本地东-北-天（ENU）坐标系，该坐标系也作为后续所有GNSS状态的默认参考系。
该架构充分利用了传感器的互补性：高频且鲁棒性强的LIVO里程计可为GNSS提供在线时空同步支持，填补信号中断期间的空白，并协助进行质量评估。与此同时，GNSS载波相位和多普勒观测值能够消除LIVO的长期漂移，从而确保全局一致性。整个处理流程包含两个主要模块：
LIVO更新模块：采用FAST-LIVO2策略，该模块通过ESIKF依次利用相机光度残差和激光雷达点面几何残差来更新系统状态。由此得到的高频、亚毫米级精度的局部里程计数据，可作为后续GNSS完整性监测和融合的初始条件以及可靠的几何先验。
GNSS更新模块：接收到GNSS数据后，由LIVO辅助的完整性监测模块会首先剔除异常值。随后，利用DTW算法对有效的测量值进行处理，以实现LIVO和GNSS速度序列的对齐。完成时间同步后，采用最小二乘法估算IMU坐标系下的本地到ENU的外参以及GNSS杆臂长度。初始化完成后，便利用两类残差来更新ESIKF的状态：
- 多普勒残差：直接用于约束瞬时速度和接收机时钟漂移（$\dot{\delta}t$）。
- 时间差分载波相位（TDCP）残差：为了获得高精度的相对约束，我们采用固定锚点策略。具体而言，将先前更新的状态作为固定锚点来构建TDCP残差。这种轻量化的方法在充分利用载波相位观测值亚毫米级相对精度的同时，成功避免了状态增广。
### B. 符号说明与状态转移模型
我们将连续时间下的完整系统状态向量x定义为流形$\mathcal{M}$上的元素。激光雷达、相机和惯性测量单元之间的外部参数被假定为已预先标定且固定不变，而惯性测量单元与GNSS天线之间的杆臂长度则被建模为刚性变换。
---
Fig. 1. System overview of our proposed framework.
为了实现与原始GNSS测量数据的紧耦合融合，状态向量会被增广，以包含惯性导航状态和GNSS接收机时钟参数，具体如表I所示：
TABLE I
IMPORTANT NOTATIONS
| Notations | Meaning |
| $^{E}(\cdot)$ | The vector ( $\cdot$) in the ENU frame. |
| $^{G}(\cdot)$ | The vector ( $\cdot$) in the local global frame. |
| $^{C}(\cdot)$ | The vector ( $\cdot$) in the RGB camera frame. |
| $\mathbf{x}, \mathbf{\widehat{x}}, \mathbf{\bar{x}}$ | The ground-truth, predicted, and updated estimation of $\mathbf{x}$. |
| $\mathbf{\delta x}$ | The error state between ground-truth $\mathbf{x}$ and its estimation. |
| $^{I}T_{L}$ | The extrinsic of LiDAR frame w.r.t. IMU frame. |
| $^{I}T_{C}$ | The extrinsic of camera frame w.r.t. IMU frame. |
| $^{I}t_{r}$ | The extrinsic (lever-arm) of GNSS Receiver w.r.t. IMU. |
在紧耦合框架下，GNSS观测量不仅取决于平台姿态和速度，还依赖于接收机时钟状态以及系统间偏差。因此，我们将接收机时钟偏移/漂移和多星座时间偏差向量添加到惯性导航状态中，并在流形M上定义连续时间系统状态。
$$ \mathbf{x}\triangleq\left[\begin{matrix}{^{G}\mathbf{R}_{I}^{\top}\mathbf{\nabla}^{G}\mathbf{p}_{I}^{\top}\mathbf{\nabla}^{G}\mathbf{v}_{I}^{\top}\mathbf{\nabla}\mathbf{b}_{\mathbf{g}}^{\top}\mathbf{\nabla}\mathbf{b}_{\mathbf{a}}^{\top}\mathbf{\nabla}^{G}\mathbf{g}^{\top}\mathbf{\nabla}\gamma c\delta\mathbf{t}_{r}^{\top}\delta\mathbf{t}_{r}^{\top}}\\ \end{matrix}\right]^{\top}, $$
其中，$^G_{\mathbf{R}_I}$、$^G_{\mathbf{p}_I}$和$^G_{\mathbf{v}_I}$分别描述了惯性测量单元在局部全局坐标系中的姿态、位置和速度，$^G_{\mathbf{g}}$表示重力矢量。$\delta\mathbf{t}_r = [\delta t_{gr}, \delta t_{br}, \delta t_{ar}, \delta t_{or}]^\top \in \mathbb{R}^4$是代表四个独立卫星系统（gr：GPS、br：北斗、ar：伽利略、or：格洛纳斯）接收机时间偏差的向量。
### C. 预处理
1) 基于DTW的时间校准：在进行GNSS对齐之前，LIV子系统会独立初始化：激光雷达、相机和惯性测量单元的时间戳实现硬件同步，而激光雷达与相机的空间外部参数则通过FAST-Calib算法进行标定[^22]，之后保持固定不变。由于GNSS接收机和LIVO系统运行在独立的时钟下，两个数据流之间会存在时间偏移$\delta_{trI}$。鉴于速度大小不受旋转影响，我们对LIVO和GNSS的速度序列进行对齐，其中$v_{R} = \{\|\mathbf{v}_{GNSS_k}\|\}$表示由GNSS多普勒效应推导出的速度序列。我们采用动态时间规整（DTW）算法[^23]来寻找最优对齐路径。与互相关方法或在线时间校准技术[^24]相比，DTW对非线性采样抖动和时钟漂移具有更强的鲁棒性。最终得到的最优时间滞后$\delta_{trI}$会在使用GNSS观测数据时予以补偿。
2) 空间对齐：完成时间戳对齐且LIV的外部参数固定后，我们利用最小二乘法（LS）联合优化从局部全局坐标系$\{G\}$到ENU坐标系$\{E\}$的变换关系，以及GNSS杆臂长度$^I\mathbf{t}_r$。对于每一个同步时刻k，ENU坐标系下的GNSS位置观测值$^E\mathbf{p}_{r_k}$与LIVO预测的惯性测量单元位置$^G\mathbf{p}_{I_k}$之间的几何关系为：
$$ \mathbf{\Sigma}^{E}\mathbf{p}_{r_{k}}=^{E}\mathbf{R}_{G}\left(^{G}\mathbf{p}_{I_{k}}+^{G}\mathbf{R}_{I_{k}}{}^{I}\mathbf{t}_{r}\right)+^{E}\mathbf{p}_{G}, $$
其中，$^E\mathbf{R}_G \in SO(3)$ 和 $^E\mathbf{p}_G \in \mathbb{R}^3$ 分别表示从局部坐标系到ENU坐标系的旋转和平移。我们构建如下目标函数来求解：
---
参数集 $\mathcal{X}_{init} = \{^{E}\mathbf{R}_{G}, ^{E}\mathbf{p}_{G}, ^{I}\mathbf{t}_{r}\}$：
$$ \min_{\mathcal{X}_{init}}\sum_{k=1}^{N}\left\|\mathbb{E}_{\mathbf{p}_{r_k}}-\left({^E\mathbf{R}_G}{^G\mathbf{p}_{I_k}}+{^G\mathbf{R}_{I_k}}{^I\mathbf{t}_{r}}\right)+{^E\mathbf{p}_G}\right\|^2. $$
这个最小二乘问题能够给出精确的初始全局位姿，并优化粗略的杆臂外部参数。
### IV. 状态估计
### A. 状态传播
在ESIKF框架中，传播步骤充当预测阶段，用于填补离散的激光雷达/相机/GNSS更新之间的空白。前向传播基于连续时间运动模型 $\mathbf{u}_i$，整合从 $t_{k-1}$ 到 $t_k$ 期间的IMU测量数据 [^25]，并假设该阶段的过程噪声为零 $\mathbf{w}_i$。预测得到的状态与协方差分别记为 $\widehat{\mathbf{x}}_k$ 和 $\widehat{\mathbf{P}}_k$，它们将作为观测更新的先验信息。需要注意的是，GNSS时钟偏差项 $\delta t_r$ 遵循恒定漂移率模型。此外，为了补偿激光雷达扫描中的运动失真，我们采用反向传播方法，确保扫描内的所有点都能被统一投影到更新时刻 $t_k$。为简洁起见，后续的状态定义中除非必要，否则会省略下标k。
### B. 激光雷达更新
对于激光雷达更新，我们采用点到平面观测模型。设$^L p_j$表示当前扫描中未失真的点。利用预测的先验状态$\widehat{x}$，可将每个点$^L p_j$从激光雷达坐标系$\{L\}$投影到局部全局坐标系$\{G\}$：
$$ {^{G}}\hat{\mathbf{p}}_{j}={^{G}}\hat{\mathbf{T}}_{I}{^{I}}\mathbf{T}_{L}{^{L}}\mathbf{p}_{j}, $$
其中$^G\hat{\mathbf{T}}_I$和$^I\mathbf{T}_L$分别表示估计的位姿以及激光雷达到IMU的外参变换。
我们采用高效的VoxelMap[^15]来维护局部地图。对于每个投影后的点$G\hat{\mathbf{p}}_j$，我们会搜索包含几何平面的最近体素。若找到有效的平面（由法线$\mathbf{n}_j$和中心点$\mathbf{q}_j$描述），便会构建点到平面的残差。观测残差$r_{L,j}$定义为查询点到对应地图平面的正交距离：
$$ 0=r_{\mathcal{L},j}(\mathbf{x}_{k},{}^{L}\mathbf{p}_{j})=\mathbf{n}_{j}^{\top}\mathbf{\Gamma}{}^{G}\mathbf{T}_{I}{}^{I}\mathbf{T}_{L}{}^{L}\mathbf{p}_{j}-\mathbf{q}_{j}\mathbf{\Gamma}{}. $$
对于观测函数$h_l(\hat{\mathbf{x}}, v_l)$，其噪声项$v_l = (\delta^L \mathbf{p}_j, \delta \mathbf{n}_j, \delta \mathbf{q}_j)$分别由原始激光雷达观测噪声、法线拟合噪声以及平面中心点噪声组成。
### C. 视觉更新
我们采用直接法作为视觉测量模型。直接法通过最小化对应图像块之间的光度误差（强度差异）来实现亚像素级精度，且在无纹理环境中具有更好的鲁棒性[^13]，[^14]。
设$^G\mathbf{p}_i$表示视觉地图中与参考图像块$I_r$相关联的点。当在时间k采集到新图像$I_k$时，我们会利用系统状态将该点投影到当前图像平面上：
$$ \mathbf{u}_{i,k}=\pi\left({^{C}\mathbf{T}_{I}}\left({^{G}\hat{\mathbf{T}}_{I}}\right)^{-1}{^{G}\mathbf{p}_{i}}\right), $$
其中$\pi(\cdot)$表示相机投影模型，$^{C}\mathbf{T}_{I}$为IMU到相机的外参。我们将映射到以投影像素为中心的小图像块$r_{\mathcal{V},i}$上的光度残差$\mathcal{N}$定义为需最小化的强度差异：
$$ \begin{align*}0&=r_{\mathcal{V},i}(\mathbf{x}_{k},{}^{G}\mathbf{p}_{i})\\&=\sum_{\Delta\mathbf{u}\in\mathcal{N}}\Big\|\gamma_{k}I_{k}(\mathbf{u}_{i,k}+\Delta\mathbf{u})-\gamma_{r}I_{r}(\mathbf{u}_{i,r}+\mathbf{A}_{i}^{r}\Delta\mathbf{u})\Big\|,\end{align*} $$
其中$\Delta u$为图像块内的像素偏移量，$A_i^r$为用于补偿缩放和透视失真的仿射变换矩阵，$\gamma$为曝光补偿参数。
### D. GNSS更新
1) 单差载波相位：载波相位测量可提供毫米级的精度。设载波相位观测值为 $\Phi_{k}^{m,j}$，其中 m 表示星座系统，j 为卫星编号，k 代表历元。它代表了基于系统 m、波长为 $\lambda_{j}$ 的卫星 j 的传输周期数。绝对伪距测量关系可建模为：
$$ \begin{aligned}\lambda_{j}\Phi_{k}^{m,j}=&\|\mathbf{\Pi}^{E}\mathbf{p}_{k}^{j}-\mathbf{\Pi}^{E}\mathbf{p}_{k}^{r}\|+\mathcal{T}_{k}^{j}-\mathcal{I}_{k}^{j}+c(\delta t_{k}^{r}-\delta t_{k}^{j})\\&+\lambda_{j}(\kappa_{j}-N_{j})+\mathcal{M}_{k}^{j}+\epsilon_{\Phi},\end{aligned} $$
其中 $^E\mathbf{p}_k^j$ 和 $^E\mathbf{p}_k^r$ 分别表示卫星 j 和接收机天线 r 的ENU坐标。$\mathcal{T}_k^j$ 和 $\mathcal{I}_k^j$ 为对流层和电离层误差。$\delta t_k^r$ 和 $\delta t_k^j$ 分别为接收机和卫星钟差。$\kappa_j$ 源于相位缠绕效应。$N_j$ 是未知的整数模糊度，$\mathcal{M}_k^j$ 代表多路径效应，而 $\epsilon_\Phi$ 则是测量噪声。
与伪距不同，由于整数模糊度的存在，载波相位无法直接用作绝对距离测量。因此，我们构建了时间差分载波相位（TDCP）模型，通过跨历元单差消除时钟跳变和共享的大气误差 [^26]。假设未发生周跳，恒定的整数模糊度会被完全抵消。
单差观测方程可推导为：
$$ \begin{aligned}\lambda_{j}(\Phi_{k+1}^{m,j}-\Phi_{k}^{m,j})=&\|\mathbf{\Pi}^{E}\mathbf{p}_{k+1}^{j}-\mathbf{\Pi}^{E}\mathbf{p}_{k+1}^{r}\|-\|\mathbf{\Pi}^{E}\mathbf{p}_{k}^{j}-\mathbf{\Pi}^{E}\mathbf{p}_{k}^{r}\|\\&+(\mathcal{T}_{k+1}^{j}-\mathcal{T}_{k}^{j})-(\mathcal{I}_{k+1}^{j}-\mathcal{I}_{k}^{j})\\&+c(\delta t_{k+1}^{r}-\delta t_{k}^{r})-c(\delta t_{k+1}^{j}-\delta t_{k}^{j})\\&+(\mathcal{M}_{k+1}^{j}-\mathcal{M}_{k}^{j})+\Delta\epsilon_{\Phi}.\tag{9}\end{aligned} $$
对于高采样率的GNSS而言，在短时间间隔内 $\Delta t$，电离层和对流层延迟的空间变化相对于载波相位噪声可忽略不计。因此，我们假设：
$$ \Delta\mathcal{T}_{k}^{j}\approx0,\quad\Delta\mathcal{I}_{k}^{j}\approx0. $$
---
TDCP残差的具体计算机制在图2中进行了直观总结：
Fig. 2. Schematic of the Time-Differenced Carrier Phase (TDCP) model. Consecutive epoch differencing provides precise relative position constraints ( $\Delta\rho$) by eliminating integer ambiguities and mitigating atmospheric delays.
与大气误差不同，复杂场景（如城市峡谷）中的多路径误差 $\Delta\mathcal{M}$ 变化剧烈且难以预测。将未建模的多路径效应和噪声归入残差后，用于ESIKF更新的实际TDCP残差方程可推导为：
$$ \begin{align*}r^{m,j}_{\Delta\Phi}&=\Big(\lambda_{j}(\Phi^{m,j}_{k+1}-\Phi^{m,j}_{k})+c(\delta t^{j}_{k+1}-\delta t^{j}_{k})\Big)\\&\quad-\Big(\rho^{r,j}_{k+1}-\rho^{r,j}_{k}+c(\delta t^{r}_{k+1}-\delta t^{r}_{k})\Big),\end{align*} $$
其中 $\rho_{\tau}^{r,j}$ 被定义为时间 $\tau \in \{k, k+1\}$ 下卫星相位中心与接收机天线之间的几何距离：
$$ \begin{array}{r}{\rho_{\tau}^{r,j}=\left\|{{}^{E}\mathbf{p}_{\tau}^{j}}-\left({^{E}\mathbf{R}_{G}}\left({^{G}\mathbf{p}_{I_{\tau}}}+{^{G}\mathbf{R}_{I_{\tau}}{}^{I}}\mathbf{t}_{r}\right)+{^{E}\mathbf{p}_{G}}\right)\right\|.}\end{array} $$
为了在不增加状态变量的情况下约束相对运动，我们采用“基于锚点”的方法，将前一时刻的后验状态 $\hat{X}_k = \{G\hat{\mathbf{p}}_k, G\hat{\mathbf{R}}_k, \delta t_k^r\}$ 视为固定锚点。于是，公式(11)中的历元k项（$\rho_k^{r,j}$、$\delta t_k^r$）便成为了常数。通过仅针对当前误差状态 $\delta\mathbf{x}_{k+1}$ 对观测模型进行线性化，TDCP实际上起到了高精度里程计约束的作用。此外，未被消除的多路径误差 $\Delta\mathcal{M}$ 会在 $r_{\Delta\Phi}^{m,j}$ 中引发异常峰值，从而便于基于LIVO先验的异常值剔除。
2) 多普勒频率测量模型：原始多普勒频移观测值 $\mathcal{D}_{k}^{j}$（Hz）提供了卫星与接收机之间相对速度的瞬时测量结果，其建模公式为：
$$ \begin{aligned}\mathcal{D}_{k}^{j}=&-\frac{1}{\lambda_{j}}(\mathbf{u}_{k}^{j})^{\top}\begin{pmatrix}E_{\mathbf{v}_{k}^{j}}-E_{\mathbf{v}_{k}^{r}}\end{pmatrix}\\&-\frac{c}{\lambda_{j}}(\dot{\delta}\boldsymbol{t}_{k}^{r}-\dot{\delta}\boldsymbol{t}_{k}^{j})+\epsilon_{\mathcal{D}},\end{aligned} $$
其中 $\mathbf{u}_k^j$ 为视线方向单位向量。$^E\mathbf{v}_k^j$ 和 $^E\mathbf{v}_k^r$ 分别代表卫星和接收机在ENU坐标系下的速度矢量。$\dot{\delta}t_k^r$ 和 $\dot{\delta}t_k^j$ 为时钟漂移率。
利用 $^ER_G$ 将估计的速度投影到ENU坐标系后，多普勒残差 $r_{\mathcal{D}}^{m,j}$ 可定义为：
$$ \begin{align*}r_{\mathcal{D}}^{m,j}=&\left(-\lambda_{j}\mathcal{D}_{k}^{j}+c\cdot\dot{\delta}\boldsymbol{t}_{k}^{j}\right)\\&-\left[(\mathbf{u}_{k}^{j})^{\top}\left(^{E}\mathbf{v}_{k}^{j}-^{E}\mathbf{R}_{G}{}^{G}\hat{\mathbf{v}}_{k}^{r}\right)+c\cdot\dot{\delta}\boldsymbol{t}_{k}^{r}\right].\end{align*} $$
考虑到从IMU到接收机天线的杆臂效应，天线速度需按如下方式补偿：
$$ {^{G}}\hat{\mathbf{v}}_{k}^{r}={^{G}}\hat{\mathbf{v}}_{I_{k}}+{^{G}}\hat{\mathbf{R}}_{I_{k}}\left({^{I}}\hat{\boldsymbol{\omega}}_{k}\times{^{I}}\mathbf{t}_{r}\right). $$
### E. 分层完整性监测与自适应融合
由于ESIKF对异常观测值极为敏感，因此严格的异常值剔除策略是确保全局一致性的前提。该策略分为两个层级：
1) GNSS内部完整性检查（RAIM）：在引入GNSS观测值之前，会先仅依据接收机内部信息进行一次一致性检查。信号载波噪声密度较低（C/N0 <35 dB-Hz）或仰角较小（< 15°）的信号会被直接过滤掉。随后，利用剩余的正常卫星数据进行基于最小二乘法的单点定位（SPP）。任何伪距残差明显偏离整体分布的卫星都会被强制排除。
2) 依赖退化程度的自适应剔除机制：我们在LIVO更新过程中提取海森矩阵的最小特征值$\lambda_{min}$，将其作为局部几何/纹理约束强度的可靠度量。为避免传感器噪声引发的高频模式切换（“乒乓效应”），系统会采用滑动窗口来计算平滑后的均值$\bar{\lambda}_{min}$。系统会根据退化阈值$\lambda_{th}$自适应地在两种模式间切换。
模式A：LIVO条件良好（$\bar{\lambda}_{min} \geq \lambda_{th}$）。此时LIVO先验的可靠性很高。我们会对通过RAIM检测的GNSS观测值$\mathbf{z}_j$执行严格的卡方检验：
$$ \mathbf{r}_{j}^{\top}(\mathbf{H}_{j}\widehat{\mathbf{P}}_{k}\mathbf{H}_{j}^{\top}+\mathbf{V}_{j})^{-1}\mathbf{r}_{j}<\chi_{t h}^{2}, $$
其中$\mathbf{r}_j$表示GNSS测量值与LIVO预测值之间的残差；$\widehat{\mathbf{P}}_k$和$\mathbf{V}_j$分别为先验协方差和观测协方差。该模式利用高置信度的先验来有效消除细微的多路径偏差，确保轨迹平滑性。
模式B：LIVO条件退化（$\lambda_{min} < \lambda_{th}$）。当遇到特征退化情况（如无纹理的长走廊）时，先验协方差$\widehat{\mathbf{P}}_k$会增大。系统会自适应地中止基于严格先验的检验，直接采纳所有通过RAIM检测的GNSS观测值。在缺乏局部约束的极端条件下，该机制会完全信任GNSS的绝对定位能力，从而有效防止灾难性漂移。
### V. 实验结果
### A. 基准数据集上的评估（M3DGR）
我们在公开的M3DGR数据集[^10]上进行了标准化的定量对比，该数据集提供了多种城市及森林场景下的RTK真实值数据。
---
TABLE II
QUANTITATIVE COMPARISON OF ATE RMSE (M) ON THE M3DGR DATASET ACROSS DIFFERENT EVALUATED ALGORITHMS.
| Algorithm | Open Sky Scenarios | GNSS Denied Scenarios |
| Varying-illu03 | Varying-illu04 | Varying-illu05 | Outdoor01 | Outdoor04 | Dark01 | Dark02 |
| RTKLIB | 7.50 | 7.97 | 5.62 | 2.08 | 3.45 | 16.39 | 40.88 |
| LIO-SAM-GPS | 1.93 | 1.01 | 4.83 | 0.45 | 0.58 | 1.85 | 3.62 |
| FAST-LIVO2 | 1.44 | 0.35 | 0.16 | 0.27 | 0.25 | 0.31 | 0.18 |
| LIGO | 0.98 | 0.26 | 0.28 | 0.33 | 0.36 | 2.45 | 4.12 |
| Ours w/o Rejection | 1.05 | 0.28 | 0.13 | 0.18 | 0.20 | - | - |
| Ours | 0.23 | 0.19 | 0.09 | 0.11 | 0.13 | 0.34 | 0.27 |
尽管MARS-LVIG [^11]提供了大规模航空激光雷达-视觉-惯性-GNSS数据，但由于LIO-SAM无法在其上可靠初始化，导致无法进行公平对比，因此未纳入评估。我们将所提方法与SOTA开源系统进行了对比，包括LIO-SAM [^8]、FAST-LIVO2 [^3]和LIGO [^21]。我们还评估了一种消融变体“Ours w/o Rejection”，即通过禁用自适应鲁棒模块并融合所有预筛选后的GNSS观测值来进行测试。
表II总结了定量结果，可得出以下结论：
1) 开阔环境下的性能表现：在条件良好的GNSS序列中（如Outdoor_01、Outdoor_04），我们的完整方法（Ours）能够实现最高的定位精度。得益于全球GNSS信号与本地LIVO数据的紧密耦合，该系统能有效抑制长距离累积漂移现象。值得注意的是，尽管移除异常值剔除模块后的版本（Ours w/o Rejection）因融合了GNSS数据而优于纯FAST-LIVO2算法，但其精度仍略逊于完整版本。这表明即便在开阔天空下，偶尔出现的微弱多路径噪声仍会影响高精度估计结果；而我们设计的鲁棒机制能有效过滤这些隐藏误差，从而提取出极致定位精度。
2) GNSS信号受干扰环境下的性能表现：在GNSS信号受干扰的环境中（如Dark01、Dark02），卫星信号会受到严重遮挡，进而引发极端的多路径误差。
在此类恶劣条件下，一个关键发现是：移除异常值剔除模块后的版本（Ours w/o Rejection）会直接出现轨迹发散与系统崩溃现象（表中标记为“-”）。由于我们的系统采用了高度紧密耦合的ESIKF架构，未经过隔离的大规模GNSS误差会直接污染误差状态与协方差矩阵，最终导致滤波器不可逆地发散。相比之下，LIO-SAM和LIGO这类松散耦合的系统虽会产生米级误差，但并未完全失效。这一对比充分证明：在深度紧密耦合的框架中，我们设计的具备退化感知能力的双模式异常值剔除机制，正是保障系统生存能力与鲁棒性的根本基石。
与此同时，我们的完整系统（Ours）在此类环境中展现出极强的鲁棒性。其定位精度仅略低于纯FAST-LIVO2算法（厘米级差异）——而后者并不受外部信号影响——却远超LIO-SAM和LIGO系统。这种微小的精度损失正是多传感器融合机制所付出的必然代价。通过在信号受限环境中牺牲极少量的精度，我们的系统既能在开阔区域获得显著的全局精度提升，又能避免紧密耦合系统的崩溃，最终在所有场景下实现最优的综合性能。
### B. 私有数据集上的评估
1) 实验平台：我们搭建了一个模块化多传感器数据采集平台，如图3所示，集成了Livox Avia固态激光雷达、工业级全局快门RGB相机以及Ublox ZED-F9P GNSS接收机。为确保时间一致性，通过微控制器生成PPS信号来实现硬件触发同步。该传感器套件有两种配置形式：手持式（用于室内外过渡场景）以及搭载于高速固定翼无人机上（用于大规模动态测绘）。
Fig. 3. Our platform with hardware synchronization for data acquisition. (a) Handheld system, (b) Fixed-wing drone setup, (c) Drone top view, (d) Drone bottom view.
2) 大规模高动态环境中的高精度测绘：为验证系统的精度与一致性，我们开展了长距离飞行实验。无人机在75米高度以20米/秒的高速巡航，这一条件对系统提出了严峻挑战（纹理稀疏且存在运动模糊）。如图4所示，我们的系统在长时间直线飞行及急转弯过程中均保持了稳定性。
图5展示了生成的全局点云的定性对比结果。具体而言，FAST-LIVO2算法
---
Fig. 4. Trajectory overview of the large-scale fixed-wing UAV experiment overlaid on Google Maps. The path segments (a), (b), and (c) correspond to challenging maneuvering phases.
会出现严重的累积漂移现象，导致结构对齐严重偏差（图5 (a2)）。相比之下，我们的方法通过紧密耦合的GNSS约束有效消除了这种漂移，保持了严格的几何对齐效果（图5 (a1)）。此外，虽然FAST-LIVO2在复杂的重叠区域会出现明显的地图重影与分层现象（图5 (b2)），但我们的系统成功抑制了这些误差，最终生成了清晰且全局一致的点云（图5 (b1)）。
Fig. 5. Qualitative comparison of global point cloud maps. (a1, b1) Our method; (a2, b2) FAST-LIVO2.
为了进一步验证这一改进效果，图6对估计出的轨迹进行了评估。结果表明，我们的方法能与RTK真实值紧密对齐，有效避免了纯LIVO算法中出现的严重Z轴漂移现象。
3) 手持混合场景下的鲁棒性：为了评估系统在复杂环境中的鲁棒性，我们采集了一组包含室内外过渡场景的手持数据集（见图7）。
1) 室内GNSS信号受干扰场景：在长长的室内走廊中（见图7中的区域(d)，视图2），GNSS信号受到严重遮挡。
Fig. 6. Trajectory evaluation in the large-scale flight. (a) and (b) show ENU position estimates across different phases. Our method (green) tightly aligns with the RTK ground truth (dashed), outperforming FAST-LIVO2 (grey).
Fig. 7. Mapping results generated in complex LIDAR, Camera, and GNSS degenerated scenes.
传统方案在此类场景下很容易出现偏差，而我们的严格异常值剔除策略能够隔离不可靠的GNSS信号。最终生成的地图与最优纯里程计算法FAST-LIVO2生成的结果一样平整清晰。
2) 室外LIVO信号受干扰场景：在视图1的区域(a)-(c)（见图7）中，激光雷达缺乏几何约束，摄像头则面临无纹理墙面和突发光照变化的问题。FAST-LIVO2会出现累积漂移现象，导致地图出现重影。相比之下，我们的系统能够无缝融合室外可用的正常GNSS信号，修正里程计漂移并生成清晰的边界，性能显著优于纯LIVO算法。
---
4) 实时性能：得益于资源受限平台上高效的FAST-LIVO2算法[^28]以及我们轻量级的GNSS观测模型，该系统具备出色的实时处理能力。表III列出了每帧数据的平均处理时间。
TABLE III
AVERAGE TIME CONSUMPTION OF EACH MODULE
| Module / Operation | Time (ms) |
| Base LIVO LiDAR Update | 23.36 |
| Base LIVO Visual Update | 2.64 |
| Base LIVO Subtotal | 25.99 |
| GNSS Preprocessing (DTW & Alignment) | 4.15 |
| Integrity Monitoring (RAIM & $\chi^{{2}}$ Test) | 3.42 |
| GNSS State Update (Doppler & TDCP) | 4.21 |
| Total GNSS Overhead | 11.78 |
| Total System Processing Time | 37.77 |
如表III所示，作为基准的FAST-LIVO2-RC算法仅进行LIVO处理时，每帧所需时间为25.99毫秒。包含DTW对齐、完整性监测和ESIKF更新在内的完整GNSS集成过程，由于采用了固定锚点TDCP设计，仅会增加11.78毫秒的开销。37.77毫秒的总处理时间远低于标准10Hz激光雷达100毫秒的处理时限要求，既能满足实时性需求，还能为后续的规划与控制预留计算余量。
### 六、结论
本文针对高度动态的混合场景，提出了一种紧耦合的激光雷达-惯性-视觉-GNSS融合系统。通过结合基于DTW的对齐算法、TDCP/多普勒观测以及依赖退化程度的异常值剔除机制，该系统能够抑制长期漂移现象，提升在信号受干扰环境下的鲁棒性。在无人机和手持数据集上的实验验证了该系统的鲁棒性与全局一致性。
## 参考文献
[^1]: Y. Cui, C. Sheng, B. Min, B. Zhu, and M. Li, "Resilient Distributed Event-Triggered Joint Localization and Target Tracking for Cooperative Perception," Automatica, vol. 191, pp. 113114, 2026.
[^2]: Y. Zhu, C. Zheng, C. Yuan, X. Huang, and X. Hong, “CamVox: A Low-cost and Accurate Lidar-assisted Visual SLAM System,” in IEEE International Conference on Robotics and Automation (ICRA), 2021, pp. 5049–5055.
[^3]: 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.
[^4]: J. Lin and F. Zhang, "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package," in IEEE International Conference on Robotics and Automation (ICRA), 2022, pp. 10672-10678.
[^5]: T. Shan, B. Englot, C. Ratti, and D. Rus, “LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping,” in IEEE International Conference on Robotics and Automation (ICRA), 2021, pp. 5692-5698.
[^6]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator,” IEEE Transactions on Robotics, vol. 34, no. 2, pp. 1004-1020, 2018.
[^7]: J. Xu, T. Ye, S. Shi, M. He, C. Zheng, T. Huang, and Y. Cui, "FAST-RTFM: Fast RGB-Thermal Fusion for Point Cloud Mapping in Low-Light Environments," IEEE/ASME Transactions on Mechatronics, pp. 1–11, 2026, doi: 10.1109/TMECH.2026.3681437.
[^8]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIOSAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping," in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020, pp. 5135-5142.
[^9]: T. Qin et al., “A General Optimisation-Based Framework for Global Pose Estimation With Multiple Sensors,” IET Cyber-Syst. Robot., vol. 7, no. 1, Art. no. e70023, 2025.
[^10]: D. Zhang et al., “Towards Robust Sensor-Fusion Ground SLAM: A Comprehensive Benchmark and a Resilient Framework,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), pp. 8894–8901, 2025.
[^11]: H. Li, Y. Zou, N. Chen, J. Lin, X. Liu, W. Xu, C. Zheng, R. Li, D. He, F. Kong, Y. Cai, Z. Liu, S. Zhou, K. Xue, and F. Zhang, “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.
[^12]: J. Lin, C. Zheng, W. Xu, and F. Zhang, "R2LIVE: A Robust, Real-Time, LiDAR-Inertial-Visual Tightly-Coupled State Estimator and Mapping," IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 7469-7476, 2021.
[^13]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in IEEE International Conference on Robotics and Automation (ICRA), 2014, pp. 15-22.
[^14]: C. Zheng, Q. Zhu, W. Xu, X. Liu, G. Guo, and F. Zhang, "FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry," in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2022.
[^15]: C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “VoxelMap: A Voxel-based Efficient and Accurate LiDAR Odometry,” in IEEE International Conference on Robotics and Automation (ICRA), 2022.
[^16]: Z. Liu, H. Li, C. Yuan, X. Liu, J. Lin, R. Li, C. Zheng, B. Zhou, W. Liu, and F. Zhang, "Voxel-SLAM: A Complete, Accurate, and Versatile LiDAR-Inertial SLAM System," Advanced Intelligent Systems, p. e202501081, 2026.
[^17]: W. Wen, T. Pfeifer, X. Bai, and L. -T. Hsu, "Factor Graph Optimization for GNSS/INS Integration: A Comparison With the Extended Kalman Filter," IEEE Transactions on Intelligent Transportation Systems, vol. 22, no. 5, pp. 3156-3171, 2021.
[^18]: T. Suzuki and N. Kubo, “Graph-based sensor fusion of GNSS Doppler and carrier phase observations for precise vehicle localization,” GPS Solutions, vol. 24, no. 4, pp. 1-13, 2020.
[^19]: S. Cao, X. Lu, and S. Shen, "GVINS: Tightly Coupled GNSS–Visual–Inertial Fusion for Smooth and Consistent State Estimation," IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2004-2021, 2022.
[^20]: H. Ye, Y. Chen, and M. Liu, "Tightly Coupled 3D Lidar Inertial Odometry and Mapping with GNSS," in IEEE International Conference on Robotics and Automation (ICRA), 2020.
[^21]: D. He, H. Li, and J. Yin, "LIGO: A Tightly Coupled LiDAR-Inertial-GNSS Odometry Based on a Hierarchy Fusion Framework for Global Localization With Real-Time Mapping," IEEE Transactions on Robotics, vol. 41, pp. 1224–1244, 2025.
[^22]: C. Zheng and F. Zhang, "FAST-Calib: LiDAR-Camera Extrinsic Calibration in One Second," IEEE Robotics and Automation Letter, pp. 1–5, 2026.
[^23]: M. Müller, Information Retrieval for Music and Motion. Springer, 2007.
[^24]: T. Qin and S. Shen, “Online Temporal Calibration for Monocular Visual-Inertial Systems,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018, pp. 3662-3669.
[^25]: N. Trawny and S. I. Roumeliotis, “Indirect Kalman Filter for 3D Attitude Estimation,” University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep. 2005-002, 2005.
[^26]: T. Takasu and A. Yasuda, “Development of the low-cost RTK-GPS receiver with an open source program package RTKLIB,” in International Symposium on GPS/GNSS, 2009.
[^27]: J. Zhang and S. Singh, “LOAM: Lidar Odometry and Mapping in Real-time,” in Robotics: Science and Systems, vol. 2, no. 9, 2014.
[^28]: B. Zhou, C. Zheng, Z. Wang, F. Zhu, Y. Cai, and F. Zhang, "FAST-LIVO2 on Resource-Constrained Platforms: LiDAR-Inertial-Visual Odometry With Efficient Memory and Computation," IEEE Robotics and Automation Letters, vol. 10, no. 8, pp. 7931–7938, 2025.