{
  "paper_id": "9d2e05a49ef0021ff1665ca3fd9451cc",
  "paper_title": "FAST-LIVGO: A Degeneracy-Robust LiDAR-Inertial-Visual-GNSS Fusion Odometry",
  "paper_authors": [
    "Zhiyu Chen",
    "Chunran Zheng",
    "Jiayu Wen",
    "Xiaolei Zhang",
    "Jiaming Xu",
    "Feng Pan",
    "Yukang Cui"
  ],
  "publication_date": "",
  "publication_venue": "unknown",
  "abstract": "在长期、大尺度且高度动态的环境中实现鲁棒的状态估计与建图仍是机器人领域的核心挑战。现有LiDAR-惯性-视觉里程计(LIVO)系统虽具备较强的局部精度,但在长距离任务中会累积漂移,并可能在几何退化或无纹理场景中失效。同时,GNSS辅助融合框架通常依赖LiDAR或视觉里程计进行状态预测与外点剔除,当里程计退化时系统变得脆弱。为此,我们提出一种基于误差状态迭代卡尔曼滤波(ESIKF)的紧耦合LiDAR-惯性-视觉-GNSS融合框架。引入基于动态时间规整(DTW)的在线时空对齐模块以应对高度动态条件。为更好利用GNSS精度,我们构建了基于Doppler频移与固定锚点时间差分载波相位(TDCP)的观测模型,在不增广历史锚点状态的情况下提供毫米级相对约束。我们进一步设计了退化感知的双模式外点剔除策略,根据LIVO退化程度在LIVO先验引导剔除与GNSS辅助恢复之间切换。在公开M3DGR数据集与自建20 m/s固定翼UAV数据集上的实验表明,本系统减少了累积漂移与地图重影,在精度与鲁棒性上优于现有最先进方法。",
  "keywords": [
    "多传感器融合",
    "LiDAR-惯性-视觉里程计",
    "GNSS紧耦合融合",
    "误差状态迭代卡尔曼滤波",
    "时间差分载波相位",
    "Doppler观测",
    "退化感知外点剔除",
    "动态时间规整",
    "状态估计与建图"
  ],
  "paper_institutions": [],
  "output_language": "zh",
  "provider": "doubao",
  "model": "glm-5.3",
  "prompt_template": "simple",
  "extracted_at": "2026-09-17T09:27:36.713964Z",
  "template_tag": "simple",
  "summary": "### 摘要\n\n长期、大尺度、高度动态环境下的鲁棒状态估计与建图是机器人领域的核心难题:现有LiDAR-惯性-视觉里程计(LIVO)系统(如R2LIVE、R3LIVE、FAST-LIVO2)虽局部精度优异,却因缺乏全局约束而随时间与距离累积漂移(常沿Z轴发散),并在无纹理广场、特征缺失墙面、高速运动模糊等感知退化场景中失效;而LIO-SAM、GVINS、GLIO、LIGO等GNSS辅助融合框架依赖里程计进行状态预测与外点剔除,在里程计退化时同样脆弱,且松耦合方案未充分利用载波相位精度、传统紧耦合框架缺乏自适应完整性监测。为此,本文提出基于误差状态迭代卡尔曼滤波(ESIKF)的紧耦合LiDAR-惯性-视觉-GNSS融合框架FAST-LIVGO:以FAST-LIVO2高频亚毫米级局部里程计作为GNSS完整性监测与在线时空对齐的几何先验,利用动态时间规整(DTW)对齐LIVO与GNSS速度序列以补偿低成本传感器时间偏移,并以最小二乘联合优化局部全局系到ENU系的变换与GNSS杆臂;其核心机制为固定锚点时间差分载波相位(TDCP)观测模型,即将上一后验状态作为固定锚点构造残差 r^m,j_ΔΦ=(λ_j(Φ^m,j_{k+1}−Φ^m,j_k)+c(δt^j_{k+1}−δt^j_k))−(ρ^r,j_{k+1}−ρ^r,j_k+c(δt^r_{k+1}−δt^r_k)),通过跨历元单差消除整周模糊度与共模大气误差,在不增广历史锚点状态的前提下获得毫米级相对约束,辅以Doppler残差约束瞬时速度与接收机钟漂;同时设计退化感知双模式外点剔除策略,依据LIVO更新Hessian矩阵最小特征值的滑动窗口平滑均值与阈值λ_th比较,在LIVO良好时以卡方检验严格剔除GNSS多径、在LIVO退化时直接信任通过RAIM的GNSS观测以防灾难性漂移。实验选用公开M3DGR数据集(提供RTK真值,涵盖开阔与GNSS拒止场景,用于与SOTA方法公平对比)与自建20 m/s高速固定翼UAV数据集(75 m巡航高度,验证大尺度高动态下的全局一致性与建图质量)及手持室内外切换数据集(验证混合退化鲁棒性):以ATE RMSE为指标,开阔场景达0.09–0.23 m,优于LIO-SAM、FAST-LIVO2与LIGO,GNSS拒止场景为0.27–0.34 m仅略逊于纯LIVO,消融实验证明双模式剔除是紧耦合系统在GNSS污染下生存的基石;UAV实验消除累积漂移、地图重影与Z轴发散,系统总耗时37.77 ms/帧满足10 Hz LiDAR实时预算。未来将开源高动态数据集以支持后续研究,并进一步探索更复杂混合退化环境下的自适应融合;实际部署需应对城市峡谷与林荫道的NLOS多径、低成本传感器时间同步误差在高速下被放大、以及感知退化等现实约束。\n\n### 关键要点\n\n- 方法核心:基于误差状态迭代卡尔曼滤波(ESIKF)的紧耦合LiDAR-惯性-视觉-GNSS融合框架,以FAST-LIVO2高频亚毫米级局部里程计作为GNSS信号评估与在线时空对齐的几何先验,同时用GNSS载波相位与Doppler观测消除LIVO长期漂移以保证全局一致性。\n- 时空标定:利用动态时间规整(DTW)对齐LIVO与GNSS速度序列以估计时间偏移δtrI,相比互相关与在线时间标定方法更鲁棒于非线性采样抖动与时钟漂移;随后用最小二乘联合优化局部全局系到ENU系的旋转平移与GNSS杆臂。\n- 观测模型:构建Doppler残差直接约束瞬时速度与接收机钟漂,并补偿IMU到接收机天线的杆臂速度效应,即天线速度为IMU速度加上旋转引起的杆臂速度项。\n- 固定锚点TDCP:将上一后验状态作为固定锚点构造时间差分载波相位残差,通过跨历元单差消除整周模糊度与共模大气误差,在不增广历史锚点状态的情况下获得毫米级相对位移约束,且未消除的多径误差会在残差中形成异常峰值以辅助外点剔除。\n- 退化感知双模式剔除:提取LIVO更新Hessian矩阵最小特征值λ_min的滑动窗口平滑均值以避免高频模式切换(乒乓效应),当均值不低于阈值时以卡方检验严格剔除GNSS多径,当低于阈值时直接采纳通过RAIM的GNSS观测以防止灾难性漂移。\n- GNSS内部完整性检查(RAIM):先物理过滤低载噪比(C/N0<35 dB-Hz)与低仰角(<15°)卫星,再以最小二乘单点定位残差强制剔除显著偏离整体分布的卫星。\n- 数据与实验设置:在公开M3DGR数据集(提供RTK真值、开阔与GNSS拒止场景)上与LIO-SAM、FAST-LIVO2、LIGO定量对比;自建平台集成Livox Avia LiDAR、全局快门RGB相机与Ublox ZED-F9P接收机,经PPS硬件同步,采集手持与20 m/s固定翼UAV(75 m巡航高度)数据。\n- 开阔场景结果:完整方法在M3DGR开阔场景ATE RMSE为0.09–0.23 m(如Varying-illu03为0.23 m、Outdoor04为0.13 m),全面优于LIO-SAM、FAST-LIVO2、LIGO及无剔除消融版本,验证紧耦合GNSS与鲁棒剔除机制对极限精度的贡献。\n- 拒止场景结果:在GNSS严重遮挡的Dark01/Dark02场景,无剔除消融版本因未隔离的GNSS粗差污染误差状态与协方差而直接发散失效,而完整方法保持0.34/0.27 m,仅以厘米级差距略逊于不受外部信号影响的纯FAST-LIVO2,证明双模式剔除是紧耦合系统生存能力的基石。\n- 大尺度动态建图:20 m/s高速UAV实验中,系统在长直线与急机动阶段保持稳定,消除FAST-LIVO2的累积漂移、结构错位、地图重影与分层,生成与RTK真值紧密对齐的锐利全局一致点云。\n- 手持混合场景:室内长走廊GNSS严重遮挡时,严格外点剔除隔离不可靠信号,建图质量与纯里程计相当;室外LiDAR几何约束缺失、相机面对无纹理墙面与光照突变时,系统无缝融合健康GNSS信号纠正漂移,显著优于纯LIVO。\n- 实时性能:总处理时间37.77 ms/帧(LIVO子模块25.99 ms,GNSS开销仅11.78 ms,含DTW对齐4.15 ms、完整性监测3.42 ms、状态更新4.21 ms),满足10 Hz LiDAR的100 ms预算,为下游规划控制留出算力余量。"
}