企业官网建设流程全解析

做多传感器融合的人应该都体会过这种痛苦：轨迹越跑越偏，滤波参数调了一轮又一轮，卡尔曼增益改来改去，问题纹丝不动。最后排查到底，才发现根本不是算法问题，而是相机和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^k，INS对应时间间隔内的相对运动为T_i^k，外参为T_ci。如果外参标定正确，那么下面这个共轭关系应该成立：

T_ci * T_v^k * T_ci^{-1} = T_i^k

这个等式其实就是经典手眼标定AX=XB的等价写法。它非常巧妙：不管两个传感器各自的绝对参考系差了多少，只要相对运动是同一个刚体运动，经过外参共轭变换后就应该一致。

把尺度写进相对运动里，就是把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是30Hz，INS是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{k+1}; relIns{k} = Tins{k} \ Tins{k+1}; 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 一组典型对标结果参考

以下是我在类似工作中看到的典型指标变化趋势，可以作为你自己标定结果的参考水平：

注意尺度估计结果接近1并不奇怪，有些VO实现里已经对尺度做了粗略归一，标定要做的其实是精确对齐。平移偏差0.12米对严苛的视觉惯性融合来说仍偏大，现实中往往还需要再加一层“在线微调”，也就是前面说的滤波器弱先验方案。

最后再说两句

标定流程本身不难，难的是把每一环都做到位。我个人的体会是，时间戳和运动激励是决定成败的两个前置条件，它们比优化器本身重要得多。算法再优秀，喂进来的数据没有激励出足够的几何约束，或者时间对不上，结果就是一堆噪声里找真理。

另一个很值得养成的习惯是：每次标定完，把原始数据、脚本、参数配置、结果文件全部归档。换传感器、换载体、甚至换了一套实验环境后，这套流程可以原封不动复跑一遍。我最早就是因为没存脚本，第二次做标定时又从零开始攒代码，白白浪费了不少时间。

如果你的融合系统正好是VO和INS的紧耦合结构，这篇里的外参标定结果可以直接作为滤波器状态量的先验初值。让滤波器自己从零开始收敛外参，不是不行，但速度慢、方差大、容易跑飞。先把静态外参标定扎实，再让滤波跟踪接手动态微调，这套组合是我目前用过最稳的路子。