多传感器融合定位这件事真正上手做过的人都知道最难的从来不是把公式推一遍而是把三路完全不同脾气的数据——LiDAR、GNSS、IMU——捏到一起还能稳定输出一条不飘的轨迹。ES-EKFError-State Extended Kalman Filter误差状态扩展卡尔曼滤波是目前工程界做这件事最主流、也最经得起考验的框架之一。它把状态拆成名义状态和误差状态两部分用误差状态去做滤波更新好处是姿态那块的非线性被绕开了线性化点始终在零点附近数值上稳得多。这篇内容面向的是已经有一点SLAM或组合导航基础、准备把LiDAR/GNSS/IMU三路数据真正跑通融合的工程师和研究生。我会从数据准备、时间同步、ES-EKF的状态定义、各传感器观测模型的构建一直讲到代码实现和实测调参把中间那些文档里不会写、但一跑就报错的坑都摊开讲。整套流程我在实际项目里反复打磨过下面给出的参数和结构可以直接拿去改。1. 为什么选ES-EKF而不是直接上EKF或优化1.1 误差状态滤波到底解决了什么问题普通EKF做姿态估计时状态里直接放四元数或旋转矩阵问题在于姿态流形不是欧氏空间你没法直接对四元数做加法更新。很多人第一次写EKF的时候会犯一个错把四元数当成普通向量去加一个增量结果姿态很快就发散了。误差状态滤波的思路是名义状态用四元数维护误差状态用三维旋转向量李代数so(3)表示滤波只估计这个小误差更新完再把它注入回名义状态。因为误差始终很小线性化误差可以忽略协方差也不会因为姿态的大范围变化而失真。我打个比方名义状态就像你手机里的地图App显示的当前位置误差状态是你实际位置和它之间的偏差。你不需要每次都重新算整个地图只需要估计这个偏差然后修正显示位置就行。偏差小线性近似就准这就是ES-EKF比直接EKF稳的根本原因。1.2 和基于优化的方案如BA的取舍现在LiDAR建图里BABundle Adjustment很火LIO-SAM、BALM这类方法都在用因子图优化。那为什么还要用ES-EKF核心区别在实时性和计算量。因子图优化是批量的每次要重新线性化整个历史窗口窗口一大计算量就上去了虽然可以用滑窗和边缘化控制但工程实现复杂。ES-EKF是递推的每来一帧数据只做一次预测和更新计算量恒定非常适合嵌入式平台或者对实时性要求高的场景。实际项目里我的经验是如果只是要一条平滑的实时轨迹ES-EKF足够如果要做高精度离线建图那BA更合适。两者不是替代关系很多系统是ES-EKF做前端里程计BA做后端优化。下面这张表是我总结的选型参考维度ES-EKF因子图优化(BA)计算模式递推恒定计算量批量随窗口增长实时性好适合嵌入式一般需滑窗控制精度中高高实现复杂度中高回环处理需额外机制天然支持适用场景实时定位、组合导航离线建图、高精地图1.3 三路传感器各自的角色定位在融合系统里三个传感器不是平权的各自补对方的短板。IMU提供高频通常100-200Hz的角速度和加速度负责短时间内的运动预测但积分会漂移LiDAR提供低频10Hz左右但高精度的相对位姿观测负责抑制漂移GNSS提供绝对位置观测负责消除长期累积误差但频率低1-10Hz且在城市峡谷里容易跳变。ES-EKF的预测步主要靠IMU更新步则分别用LiDAR和GNSS的观测来修正。理解这个分工后面设计观测模型时思路就清晰了。2. 数据准备与时间同步融合成败的第一道关2.1 三路数据的采集与格式统一先说数据格式。IMU一般输出角速度rad/s和线加速度m/s²注意单位很多消费级IMU给的是g和度每秒必须换算。LiDAR如果是机械式输出的是点云你需要先跑一个LiDAR里程计比如LOAM系列或直接点云配准得到帧间位姿再把这个位姿作为观测喂给ES-EKF。GNSS输出的是经纬高和协方差需要先转到局部ENU坐标系。我建议统一用一个数据结构来承载所有传感器数据每条消息带上时间戳、传感器类型和测量值。下面是我常用的一个简化定义struct SensorMsg { double timestamp; // 秒统一用同一时钟源 int sensor_type; // 0IMU, 1LiDAR, 2GNSS Eigen::Vector3d meas; // 测量值 Eigen::Matrix3d cov; // 测量协方差 };把所有数据先落盘成这种统一格式后面调试的时候可以反复回放不用每次都重新采集这一点非常重要。2.2 时间同步的硬骨头硬件触发还是软件对齐时间同步是融合里最容易翻车的地方。我踩过最惨的一次坑是IMU和LiDAR差了整整50ms结果轨迹在转弯处直接画出一个大弧线怎么调参都救不回来。同步方案分两档硬件同步和软件同步。硬件同步最靠谱用同一个PPS秒脉冲信号触发所有传感器时间戳误差能压到微秒级。如果传感器不支持硬件触发那就只能软件同步靠时间戳插值对齐。软件同步的做法是以IMU的高频时间轴为基准对LiDAR和GNSS的时间戳做线性插值找到最接近的IMU时刻。注意插值只对位置和速度这类缓变量有效姿态插值要用球面插值SLERP否则会引入误差。提示如果你的GNSS时间戳是UTC整秒对齐的而IMU是本地时钟一定要先估计两者的时钟偏移否则融合出来的轨迹会有系统性偏差。可以用一段静止数据估计偏移量。2.3 外参标定LiDAR-IMU和GNSS-IMU外参就是各传感器之间的相对位姿。LiDAR和IMU之间的外参旋转和平移如果标不准融合轨迹会整体歪掉。标定方法有基于手眼标定的也有基于运动激励的。我一般用后者手持设备做充分的旋转和平移激励采集一段数据然后用优化方法同时估计外参和时间偏移。开源工具里LI-Init这类方法就是干这个的效果不错。GNSS和IMU的外参主要是杆臂lever arm也就是GNSS天线相位中心到IMU中心的位移。这个位移在载体做旋转运动时会产生额外的速度观测误差公式是 v_gnss v_imu ω × r其中r就是杆臂。杆臂如果有个几十厘米在快速转弯时误差能到米级绝对不能忽略。测量方法很简单拿尺子量就行但方向要对准IMU的坐标系。3. ES-EKF的状态定义与预测步实现3.1 状态向量的构成ES-EKF的状态分名义状态和误差状态。名义状态我一般取18维位置p、速度v、姿态q、加速度计零偏ba、陀螺仪零偏bg再加上重力向量g如果要做在线重力估计。误差状态是17维姿态误差是3维其余各3维。具体定义如下// 名义状态 struct NominalState { Eigen::Vector3d p; // 位置 Eigen::Vector3d v; // 速度 Eigen::Quaterniond q; // 姿态 Eigen::Vector3d ba; // 加速度零偏 Eigen::Vector3d bg; // 陀螺零偏 Eigen::Vector3d g; // 重力 }; // 误差状态维度p(3) v(3) theta(3) ba(3) bg(3) g(3) 18 // 注意姿态误差用3维旋转向量表示误差状态的协方差矩阵是18×18的初始值要根据传感器精度设。IMU零偏的初始协方差可以设大一点让滤波器自己去收敛。3.2 IMU预测的离散化推导预测步的核心是把IMU的连续运动学方程离散化。连续方程是p_dot vv_dot R(a_m - ba) gq_dot 0.5 * q ⊗ (ω_m - bg)ba_dot 0, bg_dot 0, g_dot 0离散化用中值积分或者一阶欧拉都行我一般用中值积分精度更好。关键是误差状态的转移矩阵F的推导这个矩阵决定了协方差怎么传播。F矩阵的推导比较繁琐但结构是固定的我把它写成代码里的一个函数每次预测时调用Eigen::Matrixdouble, 18, 18 computeF(const NominalState s, const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt) { Eigen::Matrixdouble, 18, 18 F Eigen::Matrixdouble, 18, 18::Identity(); // 位置对速度的偏导 F.block3,3(0, 3) Eigen::Matrix3d::Identity() * dt; // 速度对姿态、零偏、重力的偏导 Eigen::Matrix3d R s.q.toRotationMatrix(); F.block3,3(3, 6) -R * skew(acc - s.ba) * dt; F.block3,3(3, 9) -R * dt; F.block3,3(3, 15) Eigen::Matrix3d::Identity() * dt; // 姿态对陀螺零偏的偏导 F.block3,3(6, 12) -Eigen::Matrix3d::Identity() * dt; return F; }这里的skew()是反对称矩阵函数。协方差传播就是 P F * P * F^T QQ是过程噪声由IMU的噪声密度和零偏随机游走决定。3.3 过程噪声Q的设定经验Q矩阵设不好滤波器要么太信任IMU导致漂移要么太不信任IMU导致轨迹抖动。我的经验是加速度计噪声密度和陀螺噪声密度直接查IMU数据手册零偏随机游走取一个保守值。实际调的时候如果发现轨迹在静止时还在缓慢漂移说明零偏随机游走设小了如果轨迹抖动厉害说明噪声密度设大了。这个过程需要反复试没有一劳永逸的参数。注意Q矩阵的量纲要和状态对应位置是米速度是米每秒姿态是弧度零偏是米每二次方秒和弧度每秒。量纲搞错是新手最常见的错误之一。4. 观测模型LiDAR与GNSS如何修正轨迹4.1 LiDAR观测模型相对位姿约束LiDAR里程计给出的通常是相邻两帧之间的相对位姿或者相对于某个局部地图的位姿。在ES-EKF里我把它当作位置和姿态的观测。观测方程是z_lidar [p_lidar, q_lidar]其中p_lidar是LiDAR在局部坐标系下的位置q_lidar是姿态。观测矩阵H就是选择状态里的位置和姿态部分。这里要注意坐标系转换LiDAR的观测要转到IMU坐标系下用外参做变换。残差计算时位置残差直接相减姿态残差用四元数的误差定义。姿态残差我一般用旋转向量表示即 r_q Log(q_obs^-1 * q_pred)这样残差是三维的和误差状态维度一致。Eigen::Vector3d computePoseResidual(const Eigen::Quaterniond q_obs, const Eigen::Quaterniond q_pred) { Eigen::Quaterniond dq q_obs.inverse() * q_pred; Eigen::AngleAxisd aa(dq); return aa.angle() * aa.axis(); }4.2 GNSS观测模型绝对位置与杆臂补偿GNSS给的是绝对位置观测方程简单z_gnss p_imu R * r_lever其中r_lever是杆臂。观测矩阵H对位置部分是单位阵对姿态部分有杆臂带来的耦合项。如果杆臂很小可以忽略姿态耦合直接当位置观测处理。GNSS的协方差要如实设置不能全用一个固定值。开阔环境下水平精度可能到1米以内城市峡谷里可能到十几米。如果协方差设得太乐观GNSS跳变时会把轨迹带偏。我的做法是用GNSS接收机输出的精度因子DOP来动态调整协方差DOP大的时候把协方差放大。4.3 观测更新的顺序与卡方检验当LiDAR和GNSS的观测几乎同时到达时更新顺序会影响结果。我的做法是按时间戳排序谁先到先更新谁。如果时间戳非常接近比如差几毫秒可以合并成一次更新把两个观测拼成一个大观测向量。另外GNSS在城市里经常出现野值必须做卡方检验Chi-square test剔除。做法是算残差的马氏距离如果超过阈值就拒绝这次更新。阈值一般取卡方分布的95%或99%分位数自由度是观测维度。这个机制能极大提升鲁棒性我实测下来能挡掉大部分GNSS跳变。double mahalanobis residual.transpose() * S.inverse() * residual; if (mahalanobis chi2_threshold) { // 拒绝该观测 return; }5. 代码实现从零搭一个可跑的融合节点5.1 工程结构设计我把整个融合系统拆成几个模块数据读取模块、时间同步模块、ES-EKF核心模块、可视化模块。核心模块只依赖Eigen不依赖ROS这样方便移植。数据读取模块负责把统一格式的数据喂给核心模块。可视化用简单的轨迹对比图就行不用搞太复杂。核心类的接口设计如下class ESEKF { public: void init(const NominalState init_state, const Eigen::Matrixdouble,18,18 init_cov); void predict(const Eigen::Vector3d acc, const Eigen::Vector3d gyro, double dt); void updateLidar(const Eigen::Vector3d p_obs, const Eigen::Quaterniond q_obs, const Eigen::Matrixdouble,6,6 cov); void updateGnss(const Eigen::Vector3d p_obs, const Eigen::Matrix3d cov); NominalState getState() const; private: NominalState state_; Eigen::Matrixdouble,18,18 cov_; // 内部方法误差注入、协方差传播等 };5.2 误差注入与协方差重置每次更新完要把误差状态注入到名义状态里然后把误差状态清零协方差也要做相应的重置。姿态的注入用四元数乘法void injectError(const Eigen::Matrixdouble,18,1 dx) { state_.p dx.segment3(0); state_.v dx.segment3(3); Eigen::Vector3d dtheta dx.segment3(6); Eigen::Quaterniond dq(1, dtheta.x()/2, dtheta.y()/2, dtheta.z()/2); state_.q state_.q * dq; state_.q.normalize(); state_.ba dx.segment3(9); state_.bg dx.segment3(12); state_.g dx.segment3(15); }注意这里用的是小角度近似dq的实部取1虚部取半角。因为误差很小这个近似完全够用。注入完之后误差状态归零协方差矩阵保持不变因为误差状态的定义就是相对于当前名义状态的。5.3 实测中的数值稳定性处理跑长轨迹的时候协方差矩阵可能会失去正定性导致滤波发散。我的处理办法有两个一是每次更新后对协方差做对称化P (P P^T) / 2二是定期做Cholesky分解检查如果失败就重置协方差。另外四元数每次更新后都要归一化否则数值误差累积会让它不再是单位四元数。还有一个坑是重力向量。如果重力也放在状态里估计初始值要设准否则滤波器会花很长时间收敛。我一般先用静止段数据估计一个初始重力再放开让它在线微调。6. 实测调参与常见问题排查6.1 轨迹漂移的三种典型表现与对策实测中轨迹漂移主要有三种表现。第一种是缓慢单向漂移通常是IMU零偏没估准检查零偏的收敛曲线如果一直不收敛可能是观测信息不足需要增加GNSS或LiDAR的更新频率。第二种是转弯处漂移多半是时间同步或外参有问题重点查LiDAR和IMU的时间偏移。第三种是上下颠簸一般是重力方向估计错了检查重力向量的收敛情况。排查的时候我习惯把预测轨迹和观测轨迹画在一起看残差的时间序列。残差如果是零均值白噪声说明滤波器工作正常如果有系统性偏差说明模型有问题。6.2 参数调试的优先级调参要有优先级不要一上来就乱调。我的顺序是先调时间同步和外参这两个是基础错了后面全白搭再调过程噪声Q让预测和观测的权重合理最后调观测噪声R根据实际传感器精度设。每次只调一个参数观察轨迹变化这样才能定位问题。问题现象可能原因排查方向缓慢单向漂移IMU零偏未收敛检查零偏曲线增加观测转弯处漂移时间同步/外参错误重标定检查时间戳上下颠簸重力估计错误检查重力向量收敛轨迹抖动Q设太大减小过程噪声轨迹滞后Q设太小增大过程噪声6.3 一段真实数据的复盘我拿一段园区里的数据复盘一下。这段数据里设备绕楼走了一圈GNSS在楼边有明显的多路径跳变。没做卡方检验之前轨迹在楼边被拽出去好几米加上卡方检验后跳变被挡掉轨迹平滑了很多。LiDAR里程计在长廊里表现很好但在开阔地带因为特征少偶尔会有小的位姿跳变这时候GNSS的绝对约束就起了作用。整个融合轨迹和真值比水平误差在1米以内满足园区级定位需求。这段数据让我印象最深的是时间同步的重要性。一开始LiDAR和IMU差了20ms转弯处轨迹画出一个明显的弧线后来重新做了时间对齐弧线就消失了。所以如果你发现轨迹在动态段有规律性的形变第一反应就应该是查时间同步。7. 几个容易被忽略的工程细节7.1 坐标系一致性检查融合系统里坐标系特别多IMU系、LiDAR系、GNSS系、世界系、ENU系。每引入一个新传感器都要确认它的坐标系定义和转换关系。我一般会写一个坐标检查的小工具把各传感器的观测都转到世界系下画出来如果方向对不上一眼就能看出来。这个习惯帮我省了很多调试时间。7.2 数据回放与单元测试融合算法调试离不开数据回放。我建议把数据读取和算法核心解耦这样可以用同一段数据反复测试不同的参数。另外给核心模块写单元测试比如单独测试预测步的协方差传播是否正确单独测试观测更新的残差计算是否正确。这些测试在后期改代码时能防止引入回归错误。7.3 实时性优化如果要在嵌入式平台跑实时性是硬指标。ES-EKF本身计算量不大18维的矩阵运算在ARM上也能轻松跑到100Hz以上。瓶颈往往在LiDAR里程计和点云处理上。我的做法是把LiDAR里程计放在单独的线程融合节点只接收位姿结果这样融合频率可以保持很高。另外矩阵运算尽量用固定大小的Eigen类型避免动态内存分配。我在实际项目里最深的一点体会是多传感器融合的难点从来不在算法公式而在工程细节。时间同步差几毫秒、外参差几厘米、协方差设得离谱一点轨迹就会给你颜色看。把上面这些环节一个个抠扎实ES-EKF跑出来的轨迹稳定性会让你很踏实。后面如果要做更高精度的建图可以在这个融合结果基础上再叠一层BA优化两者配合起来效果更好。
