2. 激光惯性里程计基础
FAST-LIO 能在高速运动、剧烈旋转和复杂环境中保持稳定定位，根本原因在于它把 LiDAR 与 IMU 两类传感器的优势做了紧耦合：LiDAR 提供高精度的三维几何观测，IMU 提供高频的运动先验。要真正读懂 FAST-LIO 的代码与公式，必须先理解这两类传感器的物理原理、它们各自的坐标系、时间戳体系，以及标定与时间同步为什么是系统能否收敛的前提。
一、LiDAR 与 IMU 的传感器原理
机械式或固态 LiDAR 的核心是测距：通过发射激光并测量回波时间，得到目标点到传感器的距离 r，同时结合激光发射时的水平方位角 θ 与垂直俯仰角 φ，把一次测量转换为一个三维点：
x = r · cos(φ) · cos(θ)
y = r · cos(φ) · sin(θ)
z = r · sin(φ)一帧点云就是成千上万个这样的点，按扫描顺序排列。LiDAR 的优点是几何精度高、量程远、不受光照影响；缺点是单帧频率通常只有 10 Hz 左右，且每个点都带有自己的采样时刻，帧内存在运动畸变。
IMU 则由三轴陀螺仪和三轴加速度计组成。陀螺仪测量角速度 ω，加速度计测量比力 a（包含重力反作用）。IMU 的频率通常为 100～1000 Hz，能在极短时间间隔内提供连续的运动信息，但存在零偏（bias）和随机游走噪声，单独积分会快速漂移。FAST-LIO 正是用 LiDAR 的几何观测去约束 IMU 的积分漂移，用 IMU 的高频预测去补偿 LiDAR 的低频与运动畸变。
二、坐标系定义：从传感器到世界
激光惯性里程计中至少涉及四个坐标系，理解它们之间的变换关系是阅读 FAST-LIO 状态定义的基础：
- LiDAR 坐标系（L 系）：原点在激光雷达光学中心，点云原始坐标即在此系下。
- IMU 坐标系（I 系）：原点在 IMU 测量中心，是 FAST-LIO 状态估计的机体参考系。
- 机体坐标系（B 系）：常与 I 系重合或固定安装，用于描述载体姿态。
- 世界坐标系（W 系）：也称地图系，通常取系统启动时刻的 IMU 位姿为原点，重力方向对齐 -Z。
外参 T_IL 描述 LiDAR 到 IMU 的刚体变换，包含旋转 R_IL 与平移 t_IL。FAST-LIO 的优化状态中，位姿、速度、重力、陀螺零偏和加计零偏都在 I 系与 W 系之间定义，点云则需先经外参变换到 I 系，再投影到 W 系参与残差计算。
三、时间同步：帧内畸变与帧间对齐
LiDAR 一帧并不是“瞬间拍照”，而是持续约 100 ms 的扫描过程。如果载体在这 100 ms 内发生旋转或平移，同一帧内不同时刻采集的点就会落在不同位姿下，直接拼在一起会形成“运动畸变”。FAST-LIO 的做法是：对每个点，根据其采样时间在 IMU 积分轨迹上插值出对应位姿，把点统一变换到帧起始时刻的 IMU 系，再去参与迭代卡尔曼滤波的残差计算。
时间同步包含两个层面：一是硬件层面的时钟对齐，即 LiDAR 与 IMU 是否共用同一时间基准；二是软件层面的时间偏移估计，即 t_d。若 t_d 未标定，高速运动时点云会整体“错位”，表现为地图重影、墙面变厚、闭环漂移。工程上常用两种手段：
- 硬件触发同步：用 PPS 或 GPS 秒脉冲同时触发 LiDAR 与 IMU，时间戳直接对齐。
- 软件时间偏移标定：在离线阶段用连续时间轨迹优化，联合估计
t_d与外参。
time_offset_lidar_to_imu 就是 t_d 的入口。若使用 Livox 雷达，通常还需注意其点时间戳是相对帧起始还是绝对时间。四、标定概念与常见误区
标定分为内参标定与外参标定。LiDAR 内参包括各激光束的垂直角、方位角修正和距离修正；IMU 内参包括零偏、尺度因子和非正交误差。外参标定则确定 LiDAR 与 IMU 之间的 R_IL、t_IL。FAST-LIO 对初始外参有一定鲁棒性，但外参误差过大会导致点云配准残差系统性偏大，甚至滤波发散。
| 标定项 | 符号 | 典型获取方式 | 误差影响 |
|---|---|---|---|
| LiDAR–IMU 旋转外参 | R_IL | CAD 模型 / 手眼标定 / 离线优化 | 点云旋转错位，地图扭曲 |
| LiDAR–IMU 平移外参 | t_IL | 卷尺测量 / 标定板 / 联合优化 | 旋转时产生杠杆臂误差 |
| 时间偏移 | t_d | 硬件同步 / 连续时间标定 | 高速运动重影、漂移 |
| 陀螺零偏 | b_g | 静止初始化 / 在线估计 | 姿态缓慢漂移 |
| 加计零偏 | b_a | 静止初始化 / 在线估计 | 速度与位置漂移 |
t_d。五、工程场景：一次外参错误导致的建图失败
某次室外测试中，FAST-LIO 在低速直行时地图正常，但一进入转弯就出现墙面分层、轨迹跳变。排查发现：LiDAR 与 IMU 的平移外参 t_IL 被误填为 0，而实际安装中 LiDAR 位于 IMU 前方约 0.15 m。直行时杠杆臂影响小，转弯时角速度与杠杆臂耦合，产生明显的速度误差，最终导致点云配准失败。重新标定 t_IL 并填入正确值后，转弯建图恢复正常。
这个案例说明：外参与时间同步不是“可选项”，而是激光惯性里程计能否工作的基础条件。理解 LiDAR 与 IMU 的测量原理、坐标系定义、时间戳体系与标定参数，才能在 FAST-LIO 出现异常时快速定位是数据问题、标定问题还是算法问题。