简介这份资源面向具备机器人学、计算机视觉或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包括符号含义维度是否迭代更新P_IIMU 相对第一帧 IMU 坐标系的位置(1,3)是R_IIMU 当前坐标系到全局坐标系的旋转矩阵(3,3)是R_L雷达坐标系到 IMU 坐标系的旋转矩阵(3,3)否外参固定T_L雷达坐标系到 IMU 坐标系的位置偏移(1,3)否外参固定v_IIMU 相对全局坐标系的线速度(1,3)是b_w角速度偏置(1,3)源码中未实际使用b_a加速度偏置(1,3)源码中未实际使用g重力向量(1,3)初始化后固定前向传播的核心公式是x_{i1} x_i Δt · f(x_i, u_i, 0)其中u_i [w_m, a_m]是 IMU 测量的角速度和加速度f是状态转移函数。位置更新是p_{i1} p_i Δt · v_i旋转矩阵更新是R_{i1} R_i · exp(Δt · (w_m - b_w)^)速度更新是v_{i1} 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 树每个体素内只保留离体素中心最近的一个点这样既能保证最近邻搜索效率又能避免点云过密。观测方程的设定流程对当前帧的每个点在 KD 树中找最近 10 个点。从这 10 个点里选距离最远的 5 个用最小二乘法拟合平面。如果这 5 个点中存在到平面距离大于 0.1m 的说明它们不共面放弃这个点。如果共面计算点到平面的距离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_{i1} 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 在每次雷达扫描里会做多次迭代优化而不是只做一次卡尔曼更新。原因是观测方程是非线性的一次线性化只在工作点附近有效。迭代优化的流程是用当前状态计算观测残差。线性化观测方程求H矩阵。计算卡尔曼增益更新状态。判断残差是否收敛如果不收敛就回到第 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 信号但入口出口有入口出口 GPS 矫正中间靠 IMU回环回环检测错误地图错位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.5mKD 树点数能减少一个数量级。另外限制迭代次数最大 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 位置观测频率 1HzGPS 速度观测频率 1HzGPS 航向角观测频率 1Hz只在速度超过 1m/s 时用。这样三个观测一起约束偏置估计会收敛得比较快。实测下来跑 10 分钟加速度偏置估计能收敛到 0.01m/s² 以内角速度偏置能收敛到 0.001rad/s 以内。有个细节要注意GPS 速度的噪声比位置噪声大因为位置差除以时间差会放大噪声。所以R_vel要设得比R_pos大一般设 0.1 到 1.0 之间。如果设得太小速度观测会把状态拉偏设得太大偏置估计收敛太慢。从那以后我每次做 GPS 融合都会先把 GPS 速度观测单独跑一遍看偏置估计曲线是否收敛再决定要不要把位置观测和航向角观测一起加进去。这个习惯帮我省了很多调参时间。希望帮到你。本文还有配套的精品资源点击获取