1. 从一个被忽略的安装细节说起
做组合导航算法的人，几乎都经历过这样一个阶段：仿真跑得漂漂亮亮，曲线平滑得像教科书，一到实车或者实物平台就发现姿态角有周期性波动，位置误差像滚雪球一样越滚越大。排查IMU零偏、标定误差、时间同步，折腾一圈下来，最后发现问题出在一个特别朴素的地方——传感器根本没装在同一个点上。
这个现象在业内有个专门的术语，叫杆臂效应，对应的参数就是杆臂。而把不同传感器坐标系之间的空间关系描述清楚的那组参数，就是外参。这两个概念是组合导航从“能跑”到“跑得准”之间必须跨过的一道坎。很多入门教程讲卡尔曼滤波讲得头头是道，却对杆臂和外参一笔带过，导致大量工程问题在后期才暴露出来，返工成本极高。
这篇内容适合已经了解组合导航基本框架、正在做多传感器融合的工程师和研究生阅读。我会从杆臂的物理本质讲起，把外参标定的完整流程、参数计算、实操踩坑经验全部摊开来讲。读完你应该能做到：理解杆臂效应为什么会产生导航误差、知道外参标定该怎么做、能自己动手完成一套可用的标定流程。
2. 杆臂效应到底是怎么产生的
2.1 用生活场景理解杆臂
先抛开公式，用一个日常场景来理解。你坐在一辆正在转弯的公交车上，如果你坐在车厢正中间，转弯时你感受到的离心力是一个方向；如果你坐在车尾靠窗的位置，转弯时你感受到的力和正中间的人是不一样的，因为你的位置离旋转中心更远，切向速度更大。
IMU和GNSS的关系就是这样。IMU测量的是它自己所在位置的加速度和角速度，而GNSS天线测量的是天线相位中心的位置。这两个点物理上不重合，中间有一段距离，这段距离就是杆臂。当载体做旋转运动时，IMU和GNSS天线处的运动状态是不同的——天线处的速度等于IMU处的速度加上角速度叉乘杆臂。
如果算法里忽略了这一项，把GNSS测到的位置直接当作IMU位置来用，就会引入一个与角速度相关的误差。这个误差在直线行驶时几乎为零，但一旦转弯、颠簸、做机动动作，就会立刻显现出来。
2.2 杆臂效应的数学表达
设IMU到GNSS天线的杆臂向量在载体坐标系下为 ( l^b )，载体角速度为 ( \omega_{ib}^b )，则GNSS天线相对于IMU的速度差为：
[ \Delta v^b = \omega_{ib}^b \times l^b ]
转换到导航坐标系下，还需要考虑姿态矩阵 ( C_b^n )：
[ \Delta v^n = C_b^n (\omega_{ib}^b \times l^b) ]
位置层面的影响则是速度误差的积分。在组合导航的观测方程中，如果不补偿这一项，GNSS位置观测值就和IMU推算的位置之间存在系统性偏差。这个偏差的量级可以估算一下：假设杆臂长度为1米，载体以30度/秒的角速度转弯，那么速度误差约为 ( 1 \times 0.52 \approx 0.52 ) 米/秒。这个速度误差持续几秒钟，位置误差就能达到米级。对于追求分米级甚至厘米级精度的组合导航系统来说，这是不可接受的。
2.3 为什么外参不只是杆臂
杆臂描述的是位置偏移，但传感器之间还有姿态失准角。IMU的坐标轴和GNSS天线坐标系、相机坐标系、激光雷达坐标系之间，不可能做到完全对齐。安装时哪怕只有0.5度的角度偏差，在远距离观测时也会被放大。
举个例子，激光雷达和IMU之间的姿态偏差如果为1度，在50米距离上产生的横向位置误差约为 ( 50 \times \tan(1°) \approx 0.87 ) 米。这个量级对于建图和定位来说已经非常显著了。所以完整的外参包括两部分：平移外参（杆臂）和旋转外参（安装角）。两者缺一不可。
3. 外参标定的完整方案设计
3.1 标定思路的整体框架
外参标定的核心思路是：找到一个约束条件，使得在这个约束下，不同传感器的观测值能够一致。对于IMU和GNSS的组合，常用的约束是速度一致性和位置一致性。对于IMU和视觉/激光的组合，约束则来自运动一致性或特征对应关系。
我一般把标定分为三个层次：
- 粗略测量：用卷尺和角度尺直接量，精度差但能给出初值
- 离线标定：采集一段数据，用优化方法估计外参
- 在线估计：把外参作为状态量加入滤波器，实时估计
对于大多数工程应用，离线标定加上合理的初值就足够了。在线估计适合外参可能发生变化或者对精度要求极高的场景。
3.2 标定方法选型对比
不同的标定方法适用于不同的传感器组合和精度要求。下面这张表是我在实际项目中总结的选型参考：
| 标定方法 | 适用场景 | 精度水平 | 实现难度 | 是否需要专门设备 |
|---|---|---|---|---|
| 直接测量法 | 快速验证、初值获取 | 厘米级/度级 | 极低 | 卷尺、角度尺 |
| 速度一致性法 | IMU+GNSS | 毫米级/0.1度级 | 中等 | 无 |
| 位置一致性法 | IMU+GNSS | 厘米级 | 中等 | 无 |
| 手眼标定法 | IMU+相机/激光 | 毫米级/0.1度级 | 较高 | 标定板 |
| 在线滤波估计 | 所有组合 | 取决于观测条件 | 高 | 无 |
选型的核心考量是：你手头有什么数据，能接受多长的标定时间，精度要求是多少。如果只是做课程项目或者初步验证，直接测量法加上速度一致性法就够用了。如果是产品级开发，建议用离线优化加在线估计的组合方案。
3.3 标定数据的采集要求
不管用哪种方法，数据采集的质量直接决定标定结果的上限。我踩过的坑里，至少一半是因为数据采集不规范导致的。几个硬性要求：
- 充分激励：载体必须做包含三个轴向旋转和加速的机动动作。如果只是直线行驶，杆臂和安装角都不可观，标定结果会发散或者收敛到错误值。
- 速度适中：太快会导致运动模糊和同步误差放大，太慢则激励不足。一般建议角速度在10到60度/秒之间，线加速度在0.5到2米/秒²之间。
- 时间同步：IMU和GNSS的时间戳必须对齐到同一时间基准。如果时间同步有偏差，标定结果会把时间误差错误地归因到杆臂上。
- 数据时长：建议至少采集3到5分钟的有效激励数据，包含多次不同方向的转弯和加减速。
注意：采集数据时一定要记录原始数据，不要只存滤波后的结果。原始数据可以反复用来调试标定算法，滤波后的数据一旦参数设错就废了。
4. 速度一致性标定法的实操细节
4.1 方法原理与观测方程
速度一致性法的逻辑很直接：GNSS测出的速度是天线处的速度，IMU积分出的速度是IMU处的速度，两者之间的差异就包含了杆臂信息。在载体做旋转运动时，这个差异会变得明显。
观测方程可以写成：
[ v_{GNSS}^n = v_{IMU}^n + C_b^n (\omega_{ib}^b \times l^b) ]
其中 ( v_{GNSS}^n ) 是GNSS在导航系下的速度，( v_{IMU}^n ) 是IMU推算的导航系速度，( l^b ) 是待估计的杆臂。把 ( l^b ) 当作未知量，用最小二乘法或者卡尔曼滤波就能解出来。
实际操作中，我通常把杆臂作为状态量加入一个简化的卡尔曼滤波器，状态向量包括速度误差、姿态误差和杆臂。这样可以在估计杆臂的同时补偿其他误差，结果更稳健。
4.2 参数初始化与计算过程
杆臂的初值可以用卷尺直接量。以IMU为原点，测量GNSS天线在载体坐标系下的三个方向偏移。假设载体坐标系定义为：x轴指向右，y轴指向前，z轴指向上。量出来天线在IMU前方0.8米、右方0.3米、上方0.5米，那么杆臂初值就是 ( l^b = [0.3, 0.8, 0.5]^T ) 米。
这个初值的精度不需要很高，因为后续优化会修正它。但初值不能太离谱，否则优化可能收敛到局部极小值。我的经验是初值误差控制在0.2米以内比较安全。
姿态失准角的初值一般设为零，因为安装时通常会尽量对齐。如果安装面有明显的角度偏差，可以用角度尺量一个粗略值作为初值。
4.3 完整标定流程的代码实现
下面是一个基于Python的简化标定流程，用最小二乘法估计杆臂。实际工程中可以用C++实现并嵌入到组合导航系统里。
import numpy as np from scipy.optimize import least_squares def calibrate_lever_arm(gnss_vel, imu_vel, omega_b, C_bn): """ gnss_vel: GNSS速度序列, shape (N, 3), 导航系 imu_vel: IMU推算速度序列, shape (N, 3), 导航系 omega_b: 载体角速度序列, shape (N, 3), 载体系 C_bn: 姿态矩阵序列, shape (N, 3, 3) """ def residual(l): res = [] for i in range(len(gnss_vel)): # 杆臂引起的速度差 delta_v_b = np.cross(omega_b[i], l) delta_v_n = C_bn[i] @ delta_v_b # 预测的GNSS速度 pred = imu_vel[i] + delta_v_n res.append(pred - gnss_vel[i]) return np.array(res).flatten() # 初值：假设天线在IMU前方0.5米 l0 = np.array([0.0, 0.5, 0.0]) result = least_squares(residual, l0, method='lm') return result.x # 使用示例 # lever_arm = calibrate_lever_arm(gnss_vel_data, imu_vel_data, omega_data, Cbn_data) # print(f"标定得到的杆臂: {lever_arm}")这段代码的核心是构造残差函数，让预测的GNSS速度和实际GNSS速度的差异最小化。least_squares会自动调整杆臂向量使得残差平方和最小。
实际使用时需要注意几点：角速度数据要先做滤波，去掉高频噪声；姿态矩阵的精度直接影响标定结果，所以姿态解算要尽量准确；如果数据量很大，可以分段标定再取平均，提高鲁棒性。
4.4 标定结果的验证方法
标定完之后必须验证，不能直接拿来用。我常用的验证方法有两种：
方法一：残差检查。把标定得到的杆臂代回观测方程，计算预测速度和实际GNSS速度的残差。如果残差的均方根值明显下降，说明标定有效。如果残差没有明显改善，可能是数据激励不足或者时间同步有问题。
方法二：交叉验证。把数据分成两段，一段用来标定，另一段用来验证。如果两段数据标定出的杆臂差异很大，说明标定结果不稳定，需要检查数据质量。
方法三：实际跑车验证。把标定后的参数写入组合导航系统，做一次实际跑车，看转弯时的位置误差是否明显减小。这是最直接的验证方式，但成本也最高。
5. 旋转外参标定的特殊处理
5.1 旋转外参与杆臂的耦合问题
旋转外参和杆臂在观测方程中是耦合的。杆臂的估计依赖于姿态矩阵，而姿态矩阵又包含了旋转外参。如果同时估计两者，可能会出现不可观或者收敛慢的问题。
我的处理策略是分步标定：先假设旋转外参为零，标定杆臂；然后用标定出的杆臂作为已知量，标定旋转外参；最后把两者放在一起做联合优化。这样每一步的初值都比较接近真值，优化更容易收敛。
5.2 基于运动一致性的旋转外参标定
旋转外参的标定思路是：同一个运动，用不同传感器观测到的角速度应该是一致的（经过外参旋转之后）。对于IMU和另一个角速度传感器（比如视觉里程计或者另一个IMU），可以构造如下约束：
[ \omega_{A} = C_{AB} \omega_{B} ]
其中 ( C_{AB} ) 是两个传感器之间的旋转矩阵。采集多组不同姿态下的角速度数据，用最小二乘法或者SVD分解就能解出 ( C_{AB} )。
实际操作中，我会让载体做绕三个轴分别旋转的动作，每个轴至少转90度，采集足够的角速度数据。然后用下面的代码求解：
def calibrate_rotation(omega_A, omega_B): """ omega_A, omega_B: 两个传感器的角速度序列, shape (N, 3) 求解 C_AB 使得 omega_A = C_AB @ omega_B """ # 构造互相关矩阵 H = np.zeros((3, 3)) for i in range(len(omega_A)): H += np.outer(omega_A[i], omega_B[i]) # SVD分解 U, S, Vt = np.linalg.svd(H) # 计算旋转矩阵 R = U @ Vt # 处理反射情况 if np.linalg.det(R) < 0: U[:, -1] *= -1 R = U @ Vt return R这个方法的精度取决于角速度数据的质量和激励的充分性。如果角速度噪声大，可以先做低通滤波再标定。
5.3 旋转外参的精度影响评估
旋转外参的误差对导航精度的影响是距离相关的。角度误差 ( \delta \theta ) 在距离 ( d ) 处产生的横向误差约为 ( d \times \delta \theta )。对于组合导航来说，这个距离通常是杆臂长度，一般在米级，所以0.1度的角度误差产生的横向误差约为1.7毫米，影响不大。
但对于视觉和激光雷达来说，观测距离可能是几十米甚至上百米，角度误差的影响就被放大了。所以如果你的系统包含视觉或激光传感器，旋转外参的标定精度要求要比纯IMU+GNSS系统高一个数量级。
6. 常见问题与排查技巧实录
6.1 标定结果不收敛怎么办
这是最常见的问题。可能的原因和排查思路：
| 现象 | 可能原因 | 排查方法 | 解决措施 |
|---|---|---|---|
| 残差始终很大 | 时间同步有偏差 | 检查时间戳对齐情况 | 重新做时间同步 |
| 杆臂估计值发散 | 激励不足 | 检查角速度幅值 | 增加机动动作 |
| 结果每次都不一样 | 数据噪声太大 | 检查传感器噪声水平 | 滤波或更换传感器 |
| 收敛到明显错误值 | 初值太离谱 | 对比卷尺测量值 | 重新设置初值 |
| 杆臂和角度互相干扰 | 耦合问题 | 分步标定 | 先标杆臂再标角度 |
我遇到最多的情况是时间同步问题。IMU和GNSS的时间戳如果差了10毫秒，在转弯时就会产生一个虚假的杆臂估计。排查方法是：把时间偏差也作为一个待估参数加入优化，如果估计出的时间偏差明显不为零，说明确实存在同步问题。
6.2 杆臂标定后精度反而变差
这种情况通常是因为标定数据和使用场景不一致。比如标定时用的是低速转弯数据，实际使用时是高速机动，杆臂效应被放大了，但标定出的杆臂在高速下不准确。
另一个可能的原因是杆臂在标定后发生了变化。传感器安装如果不够牢固，经过振动后位置可能偏移。我的做法是定期复检杆臂，特别是在经历剧烈振动或者拆装之后。
还有一种情况是符号搞反了。杆臂向量的方向定义必须和姿态矩阵、角速度的定义一致。我建议在代码里加一个注释，明确写出坐标系定义和符号约定，避免后期维护时搞混。
6.3 多传感器系统的外参标定顺序
当系统里有IMU、GNSS、相机、激光雷达多个传感器时，标定顺序很重要。我的建议是：
- 先标IMU和GNSS之间的杆臂，因为这两个是组合导航的核心，精度要求最高
- 再标IMU和相机之间的外参，用标定板或者手眼标定法
- 最后标IMU和激光雷达之间的外参，可以用运动一致性法或者特征匹配法
- 做一次全局联合优化，把所有外参放在一起微调
这个顺序的逻辑是：先标精度要求高且容易标的，再标难度大的，最后用全局优化消除累积误差。
6.4 在线估计外参的注意事项
如果你的系统需要在线估计外参，有几个坑必须注意：
- 可观性分析：不是所有运动条件下外参都是可观的。直线行驶时杆臂不可观，静止时旋转外参不可观。滤波器需要检测可观性，在不可观时冻结外参估计。
- 估计速度：外参的收敛速度通常比状态量慢，需要合理设置过程噪声。过程噪声太大导致估计抖动，太小导致收敛慢。
- 异常值处理：GNSS失锁或者多路径时，外参估计会被带偏。需要加卡方检验或者鲁棒估计。
- 初始收敛阶段：刚开始的几秒到几十秒，外参估计可能波动很大，这段时间的导航结果要谨慎使用。
提示：在线估计外参时，建议设置一个收敛判据，比如连续N次估计值的方差小于阈值，才认为收敛。收敛前可以用标定值作为固定参数，收敛后再切换到在线估计。
7. 一些实操中的个人体会
杆臂和外参这个问题，说起来原理不复杂，但真正做好需要不少经验积累。我刚开始做组合导航的时候，也觉得这点小偏移能有多大影响，结果被现实教育了好几次。
最深刻的一次教训是做一个低成本组合导航项目，IMU和GNSS天线装在同一块板子上，间距不到10厘米。我当时觉得这么近的距离，杆臂效应可以忽略。结果在动态测试中，航向角总是有周期性波动，排查了很久才发现，虽然杆臂只有几厘米，但角速度大的时候产生的速度误差仍然达到了0.1米/秒量级，足以影响滤波器的观测更新。
后来我养成了一个习惯：不管杆臂多小，都要标定，都要补偿。标定的成本很低，但不标定的代价可能很高。
另一个体会是，外参标定不是一劳永逸的。传感器安装结构会随着时间、温度、振动发生变化，外参也会漂移。对于高精度应用，定期复检是必要的。我现在做的项目里，一般会在每次长时间运行前做一次快速标定检查，确认外参没有明显变化。
还有一点，不要迷信自动标定。自动标定算法在数据质量好的时候确实方便，但数据质量差的时候，自动标定的结果可能比直接测量还差。我的做法是：自动标定给出结果，人工检查合理性，必要时用直接测量值做交叉验证。
最后分享一个小技巧：标定杆臂时，可以故意让载体做“8字”形运动。这种运动包含了三个轴的旋转和不同方向的加速度，激励非常充分，标定结果通常比单一方向的机动更准确。我试过很多种机动方式，“8字”形是性价比最高的。
