title: 自动驾驶多传感器融合建图：一次 IEKF-GNSS-LiDAR 排障实验的工程记录
slug: iekf-gnss-lidar-mapping-debug

写在前面：这不是一篇 SLAM 教程

这篇文章不会从头解释什么是 IEKF，也不会推导 point-to-plane 的雅可比。网上已经有足够多的教材和论文做这些事了。

这里记录的是一次真实的排障过程。一个 LiDAR-IMU-GNSS 融合建图系统，轨迹跟 GNSS 对不上，建图效果差。花了一天逐层排查，有些问题修成了，有些没有。我觉得把整个过程写下来，比只写"修好了"更有用。

轨迹和 GNSS 对不上

需要接手排障的是一个建图仓库，系统叫 Lightning-LM，基于 IEKF（Iterated Extended Kalman Filter，迭代扩展卡尔曼滤波）做 LiDAR-IMU-GNSS 融合。管线大致是：LiDAR 点云预处理 → IMU 去畸变 → 0.5m 体素降采样 → IEKF 更新 → 增量建图 → 回环检测。

IEKF 维护一个 23 维状态向量：位置、姿态（SO3）、LiDAR 外参、速度、陀螺/加计零偏、重力方向。其中 pos_ 是 IMU 原点在世界系下的位置——不是 LiDAR 原点。这个细节后面会变得很重要。

传感器：128 线 Rayz W100 激光雷达、内置 IMU、一个输出 geometry_msgs/PoseStamped 的 GNSS 接收机（数据已预先转为局部笛卡尔坐标）。GNSS 天线装在 LiDAR 后方约 1.4 米，沿车体 x 轴负方向。

实际跑车数据里，红色 LiDAR-IEKF 轨迹和紫色 GNSS 轨迹各走各的。路口漂移，过路口后误差持续累积，点云重影严重。

GNSS 数据根本没进滤波器

第一个发现说起来有点尴尬：ProcessGNSS 和 GetClosestGNSS 都写好了，数据也正常收进来了，时间戳能对上，buffer 里有数据——但观测回调没注册。GNSS 从头到尾没有进入 IEKF 的状态估计。滤波器完全靠 LiDAR 和 IMU 在跑。

修复三件事：

1. 在 Init 里把 gps_obs_func_ 回调绑定到 GNSSObsModel。
2. 实现 6-DOF 版本的 GNSS 观测：position 残差 = GNSS 位置 - IMU 位置，rotation 残差 = log(R_imu⁻¹ · R_gnss)，雅可比取单位阵。
3. GPS 更新放在 LiDAR 之前。这个顺序很关键——LiDAR ICP 跑完会把协方差 P 压到毫米级，GPS 再更新就基本没效果了。趁 P 还在 cm-dm 量级先让 GPS 拉一把，LiDAR 再精配。

改完轨迹就对齐了，效果立竿见影，直接 git 提交。

轨迹对齐了，建图却变差了

轨迹在全局尺度上确实跟 GNSS 对上了。但建图效果不行——点云发散、重影、局部结构模糊。轨迹的局部精度反而变差了。

轨迹好不等于建图好。建图要的是每一帧位姿都准，不是整条轨迹在全局尺度上"看着对"。GNSS 给的是低频全局约束，LiDAR 给的是高频局部约束。GNSS 的约束方式不对的话，它会在每个更新点把轨迹拽偏，破坏 LiDAR 帧间配准的一致性。

GNSS 的姿态靠不住

回头看上午的实现，我做了个很粗暴的假设：把 GNSS 当成完整的 6-DOF 位姿观测——既约束位置，也约束姿态。

但 GNSS 的姿态是哪来的？天线定位解算或者内部 IMU/磁力计融合出来的。这个姿态的精度，跟 LiDAR-IMU 纯惯性递推出来的比，差了一个数量级。强行拿 GNSS 姿态当观测，每帧都在把 IMU 的姿态往不准的方向拉。yaw 角被拉歪 → 点云投影错位 → ICP 配准发散。

我把 GNSSObsModel 从 6-DOF 改成 3-DOF position-only。残差从 [dx,dy,dz, drx,dry,drz] 缩减为 [dx,dy,dz]，雅可比只保留位置偏导。GNSS 只管"你在哪"，不管"你朝哪"。

直线段表现正常。这成了当前的 stable baseline。

天线离 IMU 有 1.4 米

Position-only 跑通之后，我开始想另一件事。GNSS 测量的是天线相位中心的位置，IEKF 的 pos_ 是 IMU 原点。IMU 和 LiDAR 可以近似看作一处，天线装在 LiDAR 后方约 1.4 米，两者近似处于同一水平面。不做补偿的话，滤波器每帧都在把 IMU 原点往天线位置拉，存在约 1.4 米的系统偏置。

观测模型需要加杆臂补偿：

p_pred = p_imu + R_imu * t_imu_to_gnss   （从 IMU 原点推算天线位置）
residual = p_gnss_meas - p_pred           （3-DOF 位置残差）
∂h/∂pos = I₃
∂h/∂rot = -R * [t_imu_to_gnss]×          （杆臂对姿态的耦合）

[t]× 是叉乘矩阵（skew-symmetric）。ESKF 的 rot error state 定义为 R_new = R * exp([δθ]×)，δθ 在 body 系表达，所以雅可比里要乘 -R * [t]×。

YAML 里加了 gnss_extrinsic_T: [-1.4, 0.0, 0.0]（车体 x=前，GNSS 在后方取负），LoadParamsFromYAML 里读这组参数。

加了杆臂，反而更差

加上杆臂之后，效果比不加差得多。

初始位姿直接偏了。排查发现 T_gnss_to_lio_（GNSS 全局帧到 LIO 世界系的变换）初始化有问题。原始代码：

T_gnss_to_lio_ = state_point_.GetPose() * first_gnss_pose_.inverse();

这行把 IMU 原点对齐到了 GNSS 天线位置。但 GNSSObsModel 里预测的是天线位置。两边语义不一致——转换后的 GNSS 测量被隐式当成了 IMU 位置，实际是天线的位置。

把锚点从 IMU 原点改成预测的天线位置：

Vec3d p_gnss_anchor = state_point_.pos_ + state_point_.rot_ * t_imu_to_gnss_;
SE3 gnss_anchor_pose(state_point_.rot_, p_gnss_anchor);
T_gnss_to_lio_ = gnss_anchor_pose * first_gnss_pose_.inverse();

起始位置正常了。但新问题马上出现：车辆沿 GNSS 轨迹前进，姿态开始水平顺时针转，建图直接炸掉。

我怀疑是杆臂的 rotation Jacobian（∂h/∂rot = -R*[t_lg]×）在持续修 yaw。做了一个控制变量：保留位置预测里的杆臂补偿（p_pred = pos + R*t_lg），但把雅可比 rotation block 置零。滤波器知道天线和 IMU 不在一起，但不通过杆臂修正姿态。

结果还是顺时针转。问题不在 ∂h/∂rot 直接修 yaw。是 GNSS 测量本身的坐标系或者对齐有毛病。

试了几种坐标系对齐

接下来盯着 T_gnss_to_lio_ 的对齐方式做了三组实验。

纯平移对齐——把变换从完整 SE3 改成纯平移（旋转取 Identity）。车辆开局应该直走，但 GNSS 把轨迹拉向左侧。raw GNSS 的坐标轴和 LIO 世界系之间有旋转偏差，纯平移不够。

yaw-only 对齐——取 GNSS 初始运动方向和 LIO 初始前向，只做绕 z 轴的旋转对齐。还是方向异常。

回到 baseline——position-only + 无杆臂。当前唯一稳定的配置。

到这一步我意识到，坐标系对齐、初始 yaw、杆臂、观测权重这四件事是耦合的。没搞清 GNSS 坐标语义（ENU？NED？自定义局部坐标？）和初始 yaw 偏差之前，在一个参数上盲调只会把问题推到另一个参数上。

本次工程总结

小结

这次排障让我最深的感受是：理论基础仍不够扎实。多传感器融合和 SLAM 的数学深度真的很高，很多问题不是简单调参数就能解决的。上午修复 GNSS 轨迹对齐问题是有效的，但下午针对建图质量的排查，虽然做了多轮实验和分析，却仍未找到最终原因。

整个过程让我体会到工程排障的真实状态：每改一个参数，都可能冒出新的症状，效率很低。这也是为什么我把这篇文章定位为"排障实验的工程记录"，而不是"解决方案"。我觉得把排障过程、分析思路和实验观察记录下来，比只展示最终结果更有价值。

总的来说，这次经历让我对多传感器融合建图的复杂性有了更直观的认识，也明确了自己对 SLAM 理论掌握的局限性。