做多传感器融合的人应该都体会过这种痛苦轨迹越跑越偏滤波参数调了一轮又一轮卡尔曼增益改来改去问题纹丝不动。最后排查到底才发现根本不是算法问题而是相机和INS惯性导航系统之间的外参标错了。两个传感器各自的数据都正常一旦放进同一个融合框架坐标对不上后面再怎么优化都是白搭。外参标定这件事听起来就是“求一个旋转和平移”但真落在工程里牵扯到时间戳对齐、坐标系基准、尺度恢复、运动激励设计还有一堆“看起来标好了、实际换个场景就现原形”的坑。这篇文章我想把一套在MATLAB下做VO与INS外参标定的完整思路讲透如何把相机到INS的坐标变换旋转、平移、尺度用优化方式求解出来以及这套流程在实际联调中容易在哪里翻车。无论你是在做视觉惯性里程计、移动机器人底盘融合还是拿着VO和INS数据做预集成、做滤波跟踪这篇文章都适合当一份可以直接上手的操作手册。我会尽量少讲废话多给原理和可复现的步骤最后再把我踩过比较多的几个坑单独拎出来说。1. 为什么外参标定是VO与INS融合前绕不开的一步1.1 VO和INS的数据本质上不在一个“坐标系”里视觉里程计输出的是相机坐标系下的位姿估计。相机固定在载体上每一帧图像都在描述“相机当前相对初始帧的位置和姿态”。而INS输出的是导航坐标系下的位置、速度和姿态通常还带着经纬度、高度或者本地北东地坐标。这两个坐标系之间差一个刚体变换就是相机与INS之间的外参。外参包括两部分旋转矩阵R表示相机坐标轴相对INS坐标轴的旋转关系平移向量t表示相机光学中心相对IMU测量中心的杆臂lever arm。这个杆臂和旋转如果算不准在静止状态下可能看不出来但载体一旦运动起来特别是做旋转运动时位置解算就会产生肉眼可见的误差。在经典的表征方式里外参写成一个4×4的齐次变换矩阵T_ci含义是把相机坐标系下的某个点变换到INS坐标系下。多传感器融合的本质就是先把所有传感器观测统一到同一个坐标系再进滤波器。外参就是这层“翻译官”。1.2 单目VO额外多一个尺度这是很多人会忽略的如果使用的是单目VO情况还要再复杂一层。单目视觉从像素中恢复出的轨迹天生没有绝对尺度也就是说轨迹的“形状”可能是对的但整体被放大或缩小了一个未知比例。这个比例通常记作s。这也是为什么标题里强调“平移、旋转、尺度”三者都要优化。双INSS双目视觉惯性系统或者RGB-D可以直接从深度信息里拿到尺度但单目VO不行。做融合前如果不把尺度校准掉哪怕旋转和平移标得再准轨迹放进INS坐标系一样会整体膨胀或者收缩融合结果照样崩。1.3 外参错误在融合端有哪些典型表现外参不准的融合输出特征很典型静止起步时没问题转弯时位置估计出现明显的横向偏移直线行驶基本能对上但一旦有俯仰或横滚动作姿态就开始波动融合后的轨迹看起来平滑但和真实路径相比有整体旋转像个“拧着”的路径VO的短时精度很好INS的短时精度也不错融合后反而比任何一个单独传感器都差。这些现象在联调现场非常迷惑。我的建议是遇到类似症状先做一件事把VO原始轨迹和INS原始轨迹按时间戳对齐后画在同一张图里。如果两条轨迹的形状相似但存在明显的整体旋转、平移或者缩放差异那基本就是外参问题先不要碰滤波器参数。2. 数学建模sim(3)外参、相对位姿约束与目标函数2.1 外参的完整参数化融合前需要求解的外参是一个相似变换sim(3)而不是纯刚体变换SE(3)。参数包括旋转3个自由度用轴角、四元数或旋转矩阵表示平移3个自由度即杆臂向量尺度1个自由度单目VO的缩放比例。组合起来一共7个自由度。如果忽略尺度把问题当纯SE(3)标定来解对单目VO系统来说会在优化结果上留下系统偏差。所以做单目VO和INS标定必须把s作为优化变量。这7个自由度和相机内参标定不太一样。相机内参是把像素坐标投影到相机坐标系外参是把相机坐标系变换到另一个传感器坐标系。一个是“传感器内部”一个是“传感器之间”。2.2 用相对位姿约束绕开“参考基准不一致”直接把VO轨迹和INS轨迹做绝对对齐是不可行的做法。原因在于两个传感器各自建立参考基准的时刻不同初始航向完全不同初始位置也不同。如果直接用绝对轨迹做差共享初值误差也会被当作观测误差写进目标函数里。更干净的做法是用相邻帧之间的相对位姿约束。设VO在相邻两帧之间的相对运动为T_v^kINS对应时间间隔内的相对运动为T_i^k外参为T_ci。如果外参标定正确那么下面这个共轭关系应该成立T_ci * T_v^k * T_ci^{-1} T_i^k这个等式其实就是经典手眼标定AXXB的等价写法。它非常巧妙不管两个传感器各自的绝对参考系差了多少只要相对运动是同一个刚体运动经过外参共轭变换后就应该一致。把尺度写进相对运动里就是把T_v^k的平移部分乘以s旋转部分保持不变。优化中需要惩罚的就是这个等式左右两侧的偏差。2.3 目标函数与残差的李代数表达直接对矩阵元素做差既难约束矩阵的正交性也会让问题变得病态。标准做法是把残差投影到李代数se(3)上用6维向量表示误差3个旋转分量3个平移分量。残差定义为e_k log( inv(T_i^k) * T_ci * T_v^k * inv(T_ci) )这里的log是矩阵对数把4×4齐次变换映射成一个6维李代数向量。优化目标就是让所有残差的二范数之和最小min_{R,t,s} Σ || e_k ||^2状态向量x有7个元素前3个是旋转的轴角表示中间3个是平移最后1个是尺度对数。用轴角而不是欧拉角是为了避免万向锁和角度周期性问题。尺度取对数是为了保证优化过程中s始终为正同时让尺度参数和旋转、平移的数值量级接近优化器更容易收敛。2.4 为什么不能只用轨迹点集对齐法有一种看起来更简单的方法取VO和INS轨迹的逐帧坐标点套用经典的点集相似变换对齐算法一次性求出旋转、平移、尺度。这类方法作为初值估计非常合适但直接当最终结果用精度不够。原因有两点。第一点集对齐只用到了位置信息姿态信息被完全忽略了而相机和IMU之间的旋转关系恰恰主要体现在姿态变化上。第二VO轨迹存在累积漂移点集对齐会把漂移也“拟合”进外参里造成外参实际上吸收了轨迹误差。相对位姿约束基于短时间间隔的增量受漂移影响小得多所以优化解完的精度明显更高。3. MATLAB标定流程从原始轨迹到输出外参3.1 数据采集与运动激励设计标定结果好不好有一半取决于数据采得怎么样。采集时要注意几个基本事项相机和INS必须刚性固定标定期间不能有任何松动运动要覆盖三个轴的旋转只做平面运动或者只做直线运动很多方向上的参数无法激励出来建议运动路径呈“8字形”中间穿插俯仰和横滚动作单次采集时长在30秒到60秒为宜太长会把VO漂移引入太短约束不够尽量选择纹理丰富的环境避免VO特征跟踪掉线。实际测试中一组质量好的标定数据应该看到VO轨迹和INS轨迹在形状上非常接近只是有整体旋转和平移差异。如果两条轨迹形状都对不上说明VO在这里已经跟踪失败这组数据不能用。3.2 时间戳对齐与位姿插值外参标定对时间同步的要求比很多人想象中高。如果VO是30HzINS是100Hz不能直接把最近一帧INS数据拿来配对时间差会造成不小的残差。常规做法是以VO时间戳为基准在INS时间序列上做插值。平移可以线性插值旋转不能对矩阵做线性插值需要对四元数做球面线性插值slerp或者对旋转矩阵做指数坐标插值。MATLAB里可以直接构造四元数对象后用slerp方法。时间戳处理完还要画一条曲线检查两个传感器对应轨迹的峰谷是否对齐。如果发现整体偏移说明可能存在固定延迟需要先在优化前单独估计这个时间偏差否则后面所有残差都会被滞后污染。3.3 构建相对位姿序列数据读入后先把每个时间戳下的VO位姿和INS位姿整理成4×4齐次变换矩阵序列然后计算相邻帧的相对位姿。这一步的核心代码如下% Tvo: 视觉轨迹位姿序列cell数组每个元素为4x4矩阵 % Tins: INS轨迹位姿序列与Tvo时间对齐后cell数组 N numel(Tvo); relVo cell(N-1, 1); relIns cell(N-1, 1); for k 1:N-1 relVo{k} Tvo{k} \ Tvo{k1}; relIns{k} Tins{k} \ Tins{k1}; end这里的\在MATLAB里是左除等价于inv(A)*B。用\比显式求逆数值稳定性好一点。注意VO的平移部分在这里还没有尺度尺度要通过优化变量s去乘不要在这一步自作主张归一化。3.4 用点集对齐法计算初值有个工程经验是永远不要拿一组全零或者“目测”的外参直接开始非线性优化。初始值如果旋转差了十几度优化很容易掉进局部极值。初值来源有两个比较可靠一是通过CAD模型或者安装图纸直接测量二是用点集相似变换对齐法从轨迹坐标直接估算。后者实现很简洁把VO轨迹平移序列和INS轨迹平移序列作为两组同名点在MATLAB中可以用最小二乘方式计算最优的旋转、平移、缩放。这一步算出的外参虽然精度有限但方向基本正确作为后续优化的起点足够好。优化前还可以做一次人工校验把VO轨迹按初值外参变换后和INS轨迹画在一起视觉上应该大致重合。3.5 非线性优化求解lsqnonlin与残差函数MATLAB的lsqnonlin是非线性最小二乘问题的首选。残差函数需要满足一个约定输出是向量优化器内部自动求平方和。残差函数的核心逻辑如下function f calibrateResidual(x, relVo, relIns) % x(1:3) 旋转轴角x(4:6) 平移x(7) 尺度对数 so3_vec x(1:3); R expm(vec2so3(so3_vec)); % 轴角转旋转矩阵 t x(4:6); s exp(x(7)); Tci [R, t; 0 0 0 1]; f []; for k 1:numel(relVo) Tv relVo{k}; Tv_s Tv; Tv_s(1:3, 4) s * Tv(1:3, 4); % 视觉尺度恢复 pred Tci * Tv_s / Tci; % 共轭变换预测的INS相对位姿 err se3Log(relIns{k} \ pred); % 残差映射到6维李代数 f [f; err]; end end调用时只需要给定x0设置opts optimoptions(lsqnonlin,Display,iter)然后跑迭代就行。se3Log是一个自定义函数把4×4矩阵映射到6维向量。我这里没有展开全部实现原理是取出旋转部分求对数映射取出平移部分直接保留旋转和平移的权重可以在实际项目中根据噪声水平调整。优化收敛后把所有残差画出来应该呈零均值的高斯分布。如果残差有明显的结构比如在转弯处突然变大基本可以确定是时间戳或者数据分割的问题。3.6 结果导出与模块对接标定输出建议直接存成结构体包含旋转矩阵、平移向量、尺度、协方差矩阵以及每次优化后的残差RMSE。我在实际项目里会额外把两组校验数据一并导出一组是参与标定的数据一组是完全独立采集的验证数据。这样后面谁质疑标定结果都可以拿验证数据快速复现。导出格式尽量和融合模块的读取格式一致。有的模块读四元数有的读旋转矩阵导出时两种都存一份省得下游同学再转换一次转换过程中很容易再引入新的坐标系约定错误。4. 标定调试中最大的几个坑4.1 时间戳偏移残差全部变成噪声我见过很多次的典型场景是优化结果发散了或者收敛到一个旋转误差很大的解看起来完全不可用。排查半天发现问题是VO帧和INS数据之间的时间戳存在一个固定滞后。这个滞后可能来自采集软件多个线程各自打时间戳也可能来自传感器驱动内的缓冲区延迟。时间滞后在残差上的表现很有迷惑性它不会让残差均匀变大而是会在运动速度变化剧烈的时间段产生大残差运动平缓时反而接近零看起来像是数据里混入了异常值。处理方法有两种。简单粗暴的方式是先把轨迹画在一起观察峰谷错位量手动补偿一个固定时间偏移。更工程化的方式是把时间偏移直接加入优化变量在优化外参的同时估计时间偏差。两个方式都不难关键是要有这个意识。时间同步是外参标定的前置条件不是可选项。4.2 运动激励不足几何退化没有唯一解如果采集数据时只做平面运动优化会陷入退化状态。平面运动中垂直于运动平面的平移分量无法被观测绕重力轴的旋转也无法准确估计。优化器可能会收敛到一个数值上很小的残差但解出来的外参和真实值差得很远。判断退化有一个比较简单的方法优化完成后把各个参数的估计协方差打印出来。如果某个方向协方差特别大说明该方向的约束不足。另外也可以在优化后把外参微扰比如绕某个轴加5度重新计算残差如果残差几乎不变说明这个方向没有约束。一劳永逸的办法还是采集数据时做足三个轴的旋转激励。我第一次标定吃的亏就是偷懒只在室内平地上推了一圈结果优化惊叹号不断后来加了俯仰和横滚动作问题才消失。4.3 初值太差局部极值被人误判为“算法无效”非线性优化对初值敏感。初值错得离谱时优化器会卡在某个局部极值点明明残差已经不小但梯度接近零迭代就停了。解决思路是“从粗到精”地解耦优化。我习惯先固定旋转只优化平移和尺度等平移和尺度差不多稳定了再放开旋转做联合优化。这么做牺牲了一点便利但换来了很高的稳定性。另一个办法是给旋转、平移、尺度分别设置不同规模的正则化让小范围的迭代优先调整尺度和平移避免一开始旋转就乱转。4.4 尺度与平移的纠缠尺度和平移之间有天然的耦合。缩放视觉轨迹的同时如果把平移向量往反方向调整残差可能不会有太大变化。这就导致优化过程中尺度和平移经常“打架”收敛速度慢结果也不稳定。处理方法是在优化初期给尺度加一个较强的先验比如认为单目VO的尺度应该在0.5到2之间把优化步长限制在这个区间。等轨迹大致对齐了再逐步放开先验权重让尺度和平移一起精调。4.5 VO漂移对标定的影响短时间内的VO漂移很小但随着序列变长漂移会积累。如果把整个长轨迹喂给优化器漂移会污染后面所有帧的相对位姿约束。解决方法是把长轨迹切成若干段每段只取相邻帧约束段与段之间不跨段连接。或者干脆只使用每段的前半部分因为漂移在短时间内还处于可控范围。我实际使用下来30到60秒的短序列足够提供充分约束没必要用几分钟的长轨迹。5. 标定结果验证与滤波跟踪阶段的衔接5.1 标定前后的轨迹误差评估优化完成不等于标定完成。验证方法很简单把VO轨迹的平移逐个乘以尺度再按外参旋转平移变换到INS坐标系下与INS轨迹做逐帧位置差计算RMSE。这个指标和SLAM领域常用的绝对轨迹误差ATE类似但这里参照的是INS轨迹不依赖外部真值设备。如果采集期间有一小段静止时段也可以把静止时段的轨迹方差单独统计用来排除VO漂移的影响。我做过的一次标定实验中标定前VO轨迹和INS轨迹之间的位置RMSE高达4米左右标定后下降到0.3米以内。从工程角度看能达到这个量级融合阶段就已经有很大把握了。如果标定后RMSE仍然超过0.5米我建议先别急着进融合重新检查数据质量和时间同步。5.2 用独立数据做泛化验证标定结果在参与标定的数据上表现好只能说明优化器在拟合不能说明外参是对的。必须另外采集一段完全独立的运动数据用同一套外参做同样的轨迹误差评估。如果泛化误差和训练误差接近说明外参确实反映了传感器之间的物理关系。如果泛化误差明显变大则怀疑过拟合了轨迹漂移或者时间同步误差。这种过拟合的原因往往很隐蔽因为外参只是“吸收”了不属于它的误差成分。我在流程里会固定留出一段“验证集”数据不参与优化只用来评估。这个习惯帮我排掉过很多次看似成功、实际不可用的标定结果。5.3 把外参作为先验接入融合滤波器标定得到的是静态最优外参但融合系统里很多滤波器会在运行过程中对“外参”做在线估计。这里有两种思路一种是直接把外参固定为常量在滤波器的状态转移和观测模型里作为已知参数写入。这种方式最稳定适合外参不会发生变化也的刚性系统。另一种是把外参的初始值设置为标定结果并将协方差设置得较小允许滤波器在运行中做微小修正。后者适合车载、机载这些长期运行后可能出现微小形变的场景。标题里提到“滤波跟踪”对应的就是这个衔接点标定给出的结果本质上是滤波器中外参状态的先验。初值准了滤波器不仅收敛快而且不容易发散。5.4 一组典型对标结果参考以下是我在类似工作中看到的典型指标变化趋势可以作为你自己标定结果的参考水平指标标定前标定后位置RMSE米4.20.31旋转相对误差度3.60.35尺度估计未标定1.04平移偏差米未标定0.12注意尺度估计结果接近1并不奇怪有些VO实现里已经对尺度做了粗略归一标定要做的其实是精确对齐。平移偏差0.12米对严苛的视觉惯性融合来说仍偏大现实中往往还需要再加一层“在线微调”也就是前面说的滤波器弱先验方案。最后再说两句标定流程本身不难难的是把每一环都做到位。我个人的体会是时间戳和运动激励是决定成败的两个前置条件它们比优化器本身重要得多。算法再优秀喂进来的数据没有激励出足够的几何约束或者时间对不上结果就是一堆噪声里找真理。另一个很值得养成的习惯是每次标定完把原始数据、脚本、参数配置、结果文件全部归档。换传感器、换载体、甚至换了一套实验环境后这套流程可以原封不动复跑一遍。我最早就是因为没存脚本第二次做标定时又从零开始攒代码白白浪费了不少时间。如果你的融合系统正好是VO和INS的紧耦合结构这篇里的外参标定结果可以直接作为滤波器状态量的先验初值。让滤波器自己从零开始收敛外参不是不行但速度慢、方差大、容易跑飞。先把静态外参标定扎实再让滤波跟踪接手动态微调这套组合是我目前用过最稳的路子。