Fast-LIO算法拆解与GPS融合改进：从IMU预积分到建图鲁棒性提升

简介：这份资源面向具备机器人学、计算机视觉或SLAM基础的研究人员与工程师，聚焦Fast-LIO激光雷达与IMU融合建图算法的原理拆解与源码级改进。内容从数据时间对齐、前向传播与反向传播、KD树建图、观测方程设定到状态优化偏移量逐层展开，并给出位置、旋转矩阵与速度更新的数学推导；针对点云特征不明显时建图易失败的问题，进一步引入GPS数据，通过坐标系对齐标定、姿态矫正与状态优化提升鲁棒性与精度，可服务于无人驾驶、无人机导航等场景。资源包共1个docx文档，约756KB，以图文与公式推导为主，目录按目的、流程、改进三部分组织，便于按模块查阅。目前已有436人学习，适合希望理解多传感器融合原理、掌握Fast-LIO核心技术并借鉴GPS融合改进思路的读者参考。

1. 从一次建图翻车说起：Fast-LIO 到底解决了什么

去年帮一个做园区配送机器人的团队排查问题，他们的设备在空旷路段跑得好好的，一进到两侧都是玻璃幕墙的走廊，建出来的地图就开始“糊”——墙面扭曲、回环对不上，跑两圈地图就废了。他们用的是纯激光里程计方案，在几何特征退化的场景里直接跪了。后来换成 Fast-LIO 这套 IMU 和激光雷达紧耦合的方案，同样的走廊，地图边缘清晰了很多，姿态也不再飘。这就是 Fast-LIO 最核心的价值：用 IMU 的高频数据去“撑住”激光雷达低频扫描之间的姿态推算，让点云配准有个靠谱的初值，不至于在特征不明显的地方彻底跑偏。

这份资源是一套完整的 Fast-LIO 算法拆解笔记，从数据时间对齐、前向传播、反向传播、KD 树建图到观测方程和状态优化，每一步都有公式推导和源码对应说明。更关键的是，作者在源码层面做了一处改进：把 GNSS 数据引进来，解决纯激光雷达在长走廊、开阔地、隧道等场景下建图漂移甚至失败的问题。适合谁看？如果你正在做自动驾驶、机器人导航、无人机建图，手上有 IMU 和激光雷达，但被建图精度和鲁棒性折磨过，这份东西能帮你把 Fast-LIO 的内部逻辑吃透，并且知道怎么用 GPS 数据给它“兜底”。

2. Fast-LIO 数据流拆解：从 IMU 预积分到点云去畸变

2.1 时间对齐：为什么是 IMU 等雷达，而不是反过来

Fast-LIO 的输入有三路数据：IMU 角速度和线加速度（典型 200Hz，即 0.005s 一帧）、激光雷达点云（典型 10Hz，即 0.1s 一帧，约 2 万个点）、以及可选的 GNSS 经纬度。源码里只用了前两路，GNSS 是作者后加的。

时间对齐的逻辑是：激光雷达一帧点云覆盖 0.1s 的时间窗口，而 IMU 在这 0.1s 内积累了大约 20 帧数据。Fast-LIO 每次循环只处理一帧雷达点云，所以需要把这 0.1s 内的所有 IMU 数据收集起来，按时间戳排好，用于前向传播。对齐时以雷达点云的时间戳为基准，把 IMU 数据切分成一个个区间，每个区间对应一次雷达扫描。

这里有个容易翻车的点：IMU 和雷达的硬件时间戳如果不同步，比如 IMU 用的是系统时间、雷达用的是传感器内部时间，那对齐就会错位。常见做法是先用 PTP 或 GPS 秒脉冲做硬件同步，或者在驱动层做时间戳统一。我一般会在数据进算法之前先画一张时间戳分布图，看看两路数据的时间差是否稳定，如果抖动超过 5ms，后面前向传播的精度就会受影响。

2.2 前向传播：状态向量里每一项到底在算什么

Fast-LIO 的状态向量是 24 维（3×8），包括：

前向传播的核心公式是：

x_{i+1} = x_i + Δt · f(x_i, u_i, 0)

其中u_i = [w_m, a_m]是 IMU 测量的角速度和加速度，f是状态转移函数。位置更新是p_{i+1} = p_i + Δt · v_i，旋转矩阵更新是R_{i+1} = R_i · exp(Δt · (w_m - b_w)^)，速度更新是v_{i+1} = v_i + Δt · (R_i · (a_m - b_a) + g)。

注意旋转矩阵的更新不能用加法，必须用李代数指数映射。源码里exp对应的是 SO3 的指数映射，把角速度向量转成旋转矩阵。速度更新里加g是因为加速度计测的是比力，不是纯加速度，需要把重力补偿掉。初始化时设备静止，加速度计读数方向与重力方向相反，可以据此估计重力向量的方向和大小。

2.3 反向传播：把每个点“拉回”到扫描结束时刻

前向传播算的是每个 IMU 时刻的位姿，但一帧点云里的每个点是在不同时刻采集的。反向传播要做的是：对于点云中的每个点，找到它左侧最近的 IMU 时刻，用那个时刻的位姿推算该点的全局坐标，然后再把它变换到这一帧扫描结束时刻的雷达坐标系下。

具体步骤：

# 伪代码：反向传播核心逻辑 for point_j in current_scan: # 找到点 j 左侧最近的 IMU 时刻 i i = find_nearest_imu_before(point_j.timestamp) # 用 IMU 时刻 i 的位姿推算点 j 时刻的位姿 dt = point_j.timestamp - imu[i].timestamp x_j = x_i + dt * f(x_i, u_i, 0) # 计算点 j 在全局坐标系下的坐标 p_G = R_Ij @ (R_L @ point_j + T_L) + P_Ij # 变换到扫描结束时刻 k 的雷达坐标系 p_k = R_L.T @ (R_Ik.T @ (p_G - P_Ik) - T_L)

这段代码的关键参数是dt，即点时间戳与最近 IMU 时刻的时间差。如果dt太大（比如超过 5ms），说明 IMU 数据有缺失或者时间对齐有问题，推算出来的位姿误差会很大。我一般会在代码里加一个判断，如果dt超过阈值就跳过这个点或者报警。

2.4 KD 树建图与观测方程：怎么判断一个点“配准上了”

反向传播之后，所有点都被变换到了同一时刻的雷达坐标系下，然后转到世界坐标系，生成 KD 树。Fast-LIO 用的是体素 KD 树，每个体素内只保留离体素中心最近的一个点，这样既能保证最近邻搜索效率，又能避免点云过密。

观测方程的设定流程：

1. 对当前帧的每个点，在 KD 树中找最近 10 个点。
2. 从这 10 个点里选距离最远的 5 个，用最小二乘法拟合平面。
3. 如果这 5 个点中存在到平面距离大于 0.1m 的，说明它们不共面，放弃这个点。
4. 如果共面，计算点到平面的距离z_k，如果距离小于阈值，就把这个距离作为观测残差，用于优化状态。

观测方程写成：

0 = h_j(x_k, f_j) = n_j^T · p_G_kj

其中n_j是平面法向量，p_G_kj是点 j 在全局坐标系下的坐标。优化目标是让这个残差趋近于 0，也就是让当前帧的点尽量落在 KD 树匹配到的平面上。

这里有个血泪经验：平面拟合的 5 个点如果选得不好，比如在墙角或者边缘处，拟合出来的平面法向量会很不稳定，导致观测方程引入错误约束。我一般会在代码里加一个判断，如果 5 个点的法向量方差太大，就跳过这个点。另外，体素大小也要根据场景调，室内小场景用 0.1m 左右，室外大场景用 0.5m 甚至 1m，否则 KD 树会太大，搜索效率直线下降。

3. 状态优化与卡尔曼滤波：从公式到源码的落地细节

3.1 误差状态协方差传播：为什么旋转矩阵不能直接算协方差

前向传播没有考虑噪声，所以每次迭代都会累积误差。Fast-LIO 用误差状态的协方差矩阵P来表示这种不确定性。但旋转矩阵R是 3×3 的正交矩阵，不满足传统加减法，没法直接算E[(x - x̄)(x - x̄)^T]。所以源码里用李代数来表示旋转误差：

R - R̄ = ln(R̄^T · R) = δθ

δθ是 3×1 的向量，这样就可以和位置、速度等状态一起组成误差状态向量，维度是 24×1。协方差传播公式是：

P_{i+1} = F_x · P_i · F_x^T + F_w · Q · F_w^T

其中F_x是状态转移函数对误差状态的偏导数，F_w是对噪声的偏导数，Q是噪声协方差矩阵。源码里F_x和F_w是解析求导得到的，不是数值求导，这样计算量小且精度高。

Q矩阵的对角线对应角速度和加速度的测量噪声方差，这个值一般从 IMU 的数据手册里查，或者通过 Allan 方差标定得到。如果Q设得太大，滤波器会过度信任观测，导致地图抖动；设得太小，滤波器会过度信任预测，导致漂移。我一般会先用数据手册的值，然后根据实际建图效果微调，通常调个 2 到 3 倍范围内就能找到比较稳的值。

3.2 观测方程线性化：H 矩阵怎么求

观测方程h_j(x_k, f_j)对状态x_k的偏导数就是H矩阵。由于状态里包含旋转矩阵，求导需要用到李代数的扰动模型。源码里的推导结果是：

H_j = [n_j^T, -n_j^T · (R_Ik · (R_L · p_kj + T_L))^, 0, 0, 0, 0, 0, 0]

这个H矩阵是 1×24 的行向量，对应一个点的观测方程。对于一帧点云里的所有点，把它们的H矩阵堆叠起来，就得到整个观测矩阵。

这里有个容易忽略的点：H矩阵里只有旋转和位置部分有值，其他状态（速度、偏置、重力）的导数是 0。这意味着观测方程只直接约束当前帧的位姿，不直接约束速度和偏置。速度和偏置是通过协方差传播间接被约束的。如果发现建图时速度估计飘得厉害，可以检查一下Q矩阵里速度和偏置对应的噪声方差是不是设得太大了。

3.3 卡尔曼增益与状态更新：SMW 恒等式怎么用

Fast-LIO 的观测方程维度很高（一帧点云有上万个点，每个点一个观测方程），直接求卡尔曼增益需要求一个上万维矩阵的逆，计算量太大。源码里用了 Sherman-Morrison-Woodbury 恒等式，把高维矩阵求逆转换成低维矩阵求逆。

具体来说，卡尔曼增益的公式是：

K = P · H^T · (H · P · H^T + R)^{-1}

其中H · P · H^T + R是观测协方差矩阵，维度等于观测方程的数量。SMW 恒等式把这个求逆转换成对状态维度（24×24）的求逆，计算量从 O(n^3) 降到 O(m^3)，其中 n 是观测数，m 是状态维度。

源码里状态更新的公式是：

x_k = x_k + K · (z - h(x_k)) P_k = (I - K · H) · P_k

注意这里的x_k是误差状态，更新完之后要把它加到名义状态上。旋转矩阵的更新要用李代数指数映射，不能直接加。

3.4 迭代优化：为什么一次观测不够

Fast-LIO 在每次雷达扫描里会做多次迭代优化，而不是只做一次卡尔曼更新。原因是观测方程是非线性的，一次线性化只在工作点附近有效。迭代优化的流程是：

1. 用当前状态计算观测残差。
2. 线性化观测方程，求H矩阵。
3. 计算卡尔曼增益，更新状态。
4. 判断残差是否收敛，如果不收敛就回到第 1 步。

源码里一般迭代 3 到 5 次，收敛条件通常是残差变化小于某个阈值。如果迭代次数太多，计算量会上去，实时性受影响；迭代次数太少，精度不够。我一般会设最大迭代次数为 5，收敛阈值设为 0.001，这样在大部分场景下都能兼顾精度和速度。

4. GPS 融合改进：坐标系对齐、姿态矫正与状态增广

4.1 坐标系对齐：GPS 的经纬高怎么变成局部坐标

GPS 输出的是经纬度和海拔，单位是度，不能直接和激光雷达的局部坐标混用。第一步要做的是坐标系对齐：选一个参考原点（通常是第一帧 GPS 位置），把经纬高转成局部 ENU 坐标（东-北-天）。

转换公式：

# 经纬高转 ENU 局部坐标 def lla_to_enu(lat, lon, alt, lat0, lon0, alt0): # 地球半径 R = 6378137.0 # 纬度弧度 lat_rad = math.radians(lat) lon_rad = math.radians(lon) lat0_rad = math.radians(lat0) lon0_rad = math.radians(lon0) # 东向坐标 east = (lon_rad - lon0_rad) * R * math.cos(lat0_rad) # 北向坐标 north = (lat_rad - lat0_rad) * R # 天向坐标 up = alt - alt0 return east, north, up

这个转换在局部范围内（几公里内）精度足够，如果范围太大，需要用到更精确的椭球模型。转换完之后，GPS 坐标就和激光雷达的局部坐标在同一个坐标系下了。

4.2 姿态矫正：GPS 航向角怎么和 IMU 对齐

GPS 本身不输出姿态，但可以通过运动轨迹推算航向角。具体做法是：当设备运动速度超过一定阈值（比如 1m/s）时，用相邻两个 GPS 点的位置差计算航向角：

# 用 GPS 位置差计算航向角 def gps_heading(east1, north1, east2, north2): dx = east2 - east1 dy = north2 - north1 heading = math.atan2(dy, dx) return heading

这个航向角可以和 IMU 积分得到的航向角做对比，如果偏差太大，说明 IMU 的角速度偏置估计不准，可以用 GPS 航向角来矫正。源码里的做法是：把 GPS 航向角作为一个观测，加入到观测方程里，约束 IMU 的航向角。

注意 GPS 航向角只有在设备运动时才有效，静止时 GPS 位置抖动会导致航向角噪声很大。所以源码里加了一个判断：只有当速度超过阈值时，才把 GPS 航向角加入观测。

4.3 状态增广：把 GPS 位置和速度加入状态向量

原始的 Fast-LIO 状态向量是 24 维，加入 GPS 后，需要把 GPS 的位置和速度也作为状态，或者作为观测。源码里的做法是：把 GPS 位置作为观测，不加入状态向量，这样可以避免状态维度增加导致计算量上升。

观测方程是：

z_gps = p_GPS - p_IMU

其中p_GPS是 GPS 转换后的局部坐标，p_IMU是 IMU 推算的位置。这个观测方程对状态的偏导数是：

H_gps = [I, 0, 0, 0, 0, 0, 0, 0]

也就是只对位置部分有导数。把 GPS 观测方程和激光雷达观测方程堆叠在一起，一起做卡尔曼更新。

这里有个坑：GPS 的更新频率通常比激光雷达低（典型 1Hz 到 10Hz），而激光雷达是 10Hz。如果 GPS 和激光雷达的观测方程一起更新，会导致 GPS 观测被重复使用。源码里的做法是：GPS 观测只在有新的 GPS 数据时才加入，没有新数据时只用激光雷达观测。

4.4 改进效果：什么场景下 GPS 融合最明显

根据作者的改进经验，GPS 融合在以下几种场景下效果最明显：

需要注意的是，GPS 融合不是万能的。如果 GPS 信号本身质量差（比如多路径效应严重），引入 GPS 反而会拉低建图精度。我一般会在代码里加一个 GPS 质量判断，如果卫星数太少或者 HDOP 太大，就暂时不用 GPS 观测。

5. 避坑与排查：Fast-LIO 落地时最容易翻车的五个地方

5.1 现象：建图时地图突然“炸开”，点云乱飞

原因：IMU 和雷达的时间戳不同步，或者 IMU 数据有丢帧。前向传播用的 IMU 数据如果时间戳错位，推算出来的位姿会完全错误，导致点云配准失败。

解决：先检查时间戳对齐。用rosbag info看两路数据的时间戳分布，如果 IMU 时间戳有跳变或者回退，说明驱动有问题。常见做法是在驱动层做时间戳统一，或者用message_filters做时间同步。如果 IMU 有丢帧，可以在代码里加插值，但插值只能补少量丢帧，丢太多就得查硬件连接。

5.2 现象：地图边缘模糊，墙面扭曲

原因：外参R_L和T_L标定不准。雷达和 IMU 之间的旋转和平移如果标定有误差，点云变换到 IMU 坐标系时就会错位，导致建图边缘模糊。

解决：重新标定外参。常见做法是用 Kalibr 或者手动标定，标定完之后在代码里把外参写死。我一般会用一个已知尺寸的标定板，建图后测量标定板的尺寸，如果和实际尺寸差太多，说明外参有问题。

5.3 现象：建图时地图越来越“胖”，点云重叠

原因：Q矩阵里的噪声方差设得太小，滤波器过度信任预测，导致误差累积。或者R矩阵里的观测噪声设得太小，滤波器过度信任观测，导致点云被过度拉向平面。

解决：调整Q和R。Q一般从 IMU 数据手册查，然后乘以 2 到 3 倍。R一般设为 0.001 到 0.01 之间，根据点云噪声水平调。我一般会先用默认值跑一遍，然后看建图效果，如果地图“胖”就增大Q，如果地图“抖”就增大R。

5.4 现象：GPS 融合后建图反而更差

原因：GPS 信号质量差，或者 GPS 和激光雷达的坐标系对齐有误。GPS 位置如果跳变严重，引入观测后会把状态拉偏。

解决：加 GPS 质量判断。检查卫星数和 HDOP，如果卫星数小于 6 或者 HDOP 大于 2，就暂时不用 GPS 观测。另外检查坐标系对齐，确保 GPS 转换后的局部坐标和激光雷达的局部坐标在同一个原点。

5.5 现象：算法跑着跑着就卡死，CPU 占用率飙升

原因：KD 树太大，最近邻搜索耗时太长。或者迭代次数太多，每次扫描的计算量太大。

解决：调大体素大小，减少 KD 树的点数。体素大小从 0.1m 调到 0.5m，KD 树点数能减少一个数量级。另外限制迭代次数，最大 5 次就够了。如果还卡，可以降采样点云，每帧只用一半的点做配准。

6. 进阶技巧：用 GPS 速度做零偏在线估计

GPS 融合还有一个进阶用法：用 GPS 速度来在线估计 IMU 的角速度偏置和加速度偏置。原始 Fast-LIO 里偏置是初始化为 0 且不更新的，但在实际使用中，IMU 的偏置会随温度漂移，如果不估计，长时间跑下来姿态会飘。

具体做法是：把 GPS 速度作为观测，加入到观测方程里。GPS 速度可以通过相邻两个 GPS 点的位置差除以时间差得到：

# 用 GPS 位置差计算速度 def gps_velocity(east1, north1, east2, north2, dt): vx = (east2 - east1) / dt vy = (north2 - north1) / dt return vx, vy

然后把 GPS 速度和 IMU 推算的速度做差，作为观测残差：

z_vel = v_GPS - v_IMU

这个观测方程对状态的偏导数是：

H_vel = [0, 0, 0, 0, I, 0, 0, 0]

也就是只对速度部分有导数。但速度是和偏置相关的，因为速度更新公式里用到了加速度偏置。所以这个观测会间接约束加速度偏置。如果再加一个航向角观测，还能约束角速度偏置。

我一般会这样配置：GPS 位置观测频率 1Hz，GPS 速度观测频率 1Hz，GPS 航向角观测频率 1Hz（只在速度超过 1m/s 时用）。这样三个观测一起约束，偏置估计会收敛得比较快。实测下来，跑 10 分钟，加速度偏置估计能收敛到 0.01m/s² 以内，角速度偏置能收敛到 0.001rad/s 以内。

有个细节要注意：GPS 速度的噪声比位置噪声大，因为位置差除以时间差会放大噪声。所以R_vel要设得比R_pos大，一般设 0.1 到 1.0 之间。如果设得太小，速度观测会把状态拉偏；设得太大，偏置估计收敛太慢。

从那以后我每次做 GPS 融合，都会先把 GPS 速度观测单独跑一遍，看偏置估计曲线是否收敛，再决定要不要把位置观测和航向角观测一起加进去。这个习惯帮我省了很多调参时间。希望帮到你。

本文还有配套的精品资源，点击获取