机械式 LiDAR 扫完一帧往往要几十到一百毫秒。这段时间里机体在动，把「一帧点云」当成刚体做 ICP，等于在用弯曲的墙去对齐地图。FAST-LIO / FAST-LIO2 的核心贡献，不是又一个点到面公式，而是把去畸变 + 紧耦合 IMU 状态估计 + 增量地图结构做成可实时、可分析的闭环。

问题：帧内运动破坏点云刚性

设一帧扫描起止时间为 [t_b, t_e]，扫描时刻 t ∈ [t_b, t_e] 的点在雷达系为 p_L(t)。若忽略运动，直接用结束时刻位姿 T(t_e) 变换全部点：

p_W ← T(t_e) · p_L(t)     # 错误：T 用错了时刻

正确模型应是：

p_W = T(t) · p_L(t)

其中 T(t) 由 IMU 高频积分（或匀速/匀加速模型）提供。去畸变的目标：把所有点变换到同一参考时刻（常取 t_e 或关键帧时刻）的坐标系，使后续点到面残差近似成立。

去畸变：用 IMU 填补帧内轨迹

2.1 相对运动补偿

对点时间戳 t，计算相对结束时刻的位姿增量：

T_e←t = T(t_e)⁻¹ T(t)
p_L^(e) = T_e←t · p_L(t)

这样整帧点都落在 t_e 的雷达/机体系，再统一用 T(t_e) 投到世界系。

2.2 为什么必须和 IMU 紧耦合

轮速或假设匀速在急转、振动时误差大。IMU 提供：

• 高频角速度 → 旋转畸变主导项
• 线加速度 → 平移畸变（高速平台更明显）

工程检查：

1. 每点是否有可靠时间戳（相对帧头的 offset）
2. IMU 是否覆盖 [t_b, t_e] 且无大段丢包
3. LiDAR-IMU 外参与时延是否标定

时延 t_d 会直接把「用于去畸变的 T(t)」整体错位；高速旋转时表现为墙壁系统性弯曲或重影。

观测模型：点到面，而不是点到点

FAST-LIO 系在地图中找邻近点拟合局部平面 (q, n)，残差：

r = n^T ( R p + t - q )

p 为去畸变后的体体系点。相对点到点 ICP：

• 对采样密度变化更稳
• 雅可比 sparseness 更好处理
• 仍会在几何退化方向失去约束（法向几乎共线）

地图侧需要高效最近邻：这就是 iKD-Tree / ikd-Tree 的动机。

iKD-Tree：增量地图为什么是性能关键

全局重建 KD-Tree 每帧 O(N log N) 不可接受。增量结构支持：

• 插入新点 / 体素降采样后的代表点
• 删除视野外或过旧点（可选）
• 盒搜 / KNN 查询摊销到接近对数级

FAST-LIO2 强调的「直接法 + 增量地图」组合，本质是：把计算预算从地图重建挪给状态迭代。

工程含义：

• 体素大小决定地图密度与匹配噪声
• 过密：查询变慢、平面拟合被噪声点污染
• 过疏：结构细节丢失、走廊更易退化

迭代扩展卡尔曼（IEKF）更新直觉

松耦合常是：IMU 里程计预测 → 激光里程计当黑盒观测。FAST-LIO 把点到面残差直接用于更新同一状态：

x = [R, p, v, b_g, b_a, g, ...]

预测步：IMU 传播均值与协方差。
更新步：对当前线性化点计算一批点到面残差与雅可比，做卡尔曼更新；因残差对位姿强非线性，多次迭代（IEKF）重新去畸变/重新找面元，直到残差收敛。

这与优化型 LIO（如 LIO-SAM 因子图）不同：

选型不是谁更「高级」，而是你的延迟预算与是否需要强回环。

退化：LIO 最危险的「看起来很稳」

当环境沿某一方向缺乏平面约束（长直隧道、开阔广场、单侧墙），信息矩阵在该方向接近奇异。表现：

• 轨迹在隧道轴向「滑行」
• 协方差在该方向未如实放大（若噪声模型过于自信）
• 回环前 ATE 尚可，一回环整图拧巴

应对：

1. 监测有效约束方向的特征值比
2. 退化时增大过程噪声或降低激光更新权重
3. 引入轮速 / 视觉纹理 / UWB 填补不可观方向

没有退化检测的「高精度 LIO」，在仓库长廊里会静默失败。

时间与外参：比点云算法更致命

清单（上真机前）：

• [ ] LiDAR 与 IMU 时钟源是否同源或可估计偏移
• [ ] 点时间戳单位（秒/毫秒/雷达内部 tick）是否弄错
• [ ] 外参旋转 1° 误差在 10 m 处的横向偏差量级是否可接受
• [ ] 振动是否导致外参慢漂（结构件）
• [ ] 去畸变参考时刻与状态定义是否一致（body / LiDAR / IMU 系）

建议：先在静止转台或慢速 8 字上验证「墙是否直」，再谈公里级漂移。

和视觉 / 导航系统如何衔接

• 定位输出：发布高频 odom→base_link（平滑）与低频 map→odom（若有回环/多会话）
• 导航：2D Nav2 需要投影占据栅格；3D LIO 地图不等于可通行代价地图
• 融合：视觉可在退化走廊提供互补约束，但要避免双重计数同一几何墙面

调试协议

1. 静止 30 s：位姿抖动 vs 协方差是否匹配
2. 纯旋转：去畸变后点云墙面是否仍弯曲
3. 往返 50 m：开环漂移与 RPE
4. 长廊：是否触发退化标志
5. 拔掉 IMU 数秒（若安全）：系统是降级还是炸轨迹

保存：配置哈希、外参、bag、特征值日志。没有日志的调参等于玄学。

小结

FAST-LIO 系的深度理解应落在三句话：

1. 帧内运动必须被显式补偿，否则点到面建立在错误刚体假设上
2. 增量地图结构决定实时性上限
3. IEKF 迭代把去畸变与状态更新绑在同一非线性闭环里

读源码时按这个顺序：时间戳 → 去畸变 → 近邻/平面 → 残差雅可比 → 协方差更新 → 地图插入。

参考

尺度、外参、时间其中一项不对，后端再漂亮也像在拟合噪声。

• Xu & Zhang, FAST-LIO (RAL / IROS)
• Xu et al., FAST-LIO2
• Zhang & Singh, LOAM（特征与畸变问题的历史源头）
• Geneva et al. / Forster et al. 对 IMU 传播与可观性的一般性讨论