Part IV · 第 15 章状态估计与建图 · 15.1

视觉 / 惯性里程计

IMU 每秒报告几百次加速度和角速度，但积分起来误差按 t² 甚至 t³ 增长；单目相机能看出自己怎样移动，却不知道一米有多长，尺度还会慢慢漂。把两者紧耦合起来，就得到无人机、AR 头显和手机里都在跑的视觉惯性里程计（VIO）。本节在一个二维世界里把 VIO 完整跑一遍：捷联积分与漂移、单目 VO 的尺度模糊、滑动窗口优化与边缘化、IMU 预积分、尺度与重力的初始化，以及永远测不出来的那几个方向。

约 45 分钟前置：9.4 执行器与传感器（IMU） · 4.8 卡尔曼滤波与 EKF · 5.7 扰动与流形上的求导 · 6.9 非线性最小二乘与因子图 · 11.4 PnP · 11.6 光束法平差 6 个交互

1为什么机器人需要它

一架无人机在室内飞，没有 GPS；一副 AR 眼镜要把虚拟物体钉在桌面上，头一转，画面必须在十几毫秒内跟上；一台火星直升机要在一到三分钟的飞行里知道自己飞了多远。它们都需要里程计：不依赖外部基础设施，只靠自带的传感器，连续地估计“我相对起点移动了多少、现在朝向哪里”。

两种最便宜、最常见的传感器各能解决一半。IMU（9.4 节）每秒 200 到 1000 次报告角速度和“比力”（加速度减去重力），响应快、不受光照影响，可把它积分两次求位置，零偏会让误差无界地增长，几十秒后就没法用。相机（第 10、11 章）看着世界里的固定特征，用 PnP、三角化和光束法平差就能算出自己的运动，可单目相机测不出绝对尺度，快速运动时图像会模糊，看向白墙时什么也算不出来。

两者的缺点恰好互补：IMU 提供公制尺度、重力方向和高频的运动预测，相机提供不漂的几何约束，并把 IMU 的零偏“校准”出来。把它们放进同一个估计问题，就是视觉惯性里程计（VIO）。本节先分别看两个“半传感器”怎样失败（第 2 节），再跑一个完整的紧耦合滑动窗口 VIO（第 3 节），然后拆开它的三块关键零件：IMU 预积分、初始化和可观性（第 4 节）。

为了能在浏览器里实时跑、并且看得清楚，本节的世界是一个竖直的平面：$x$ 向右，$z$ 向上，重力 $\mathbf g=(0,-9.81)$ 在平面内。机器人（一架“二维无人机”）的状态是位置 $\mathbf p$、速度 $\mathbf v$ 和俯仰角 $\theta$；IMU 有一个陀螺和两个加速度计；相机朝下，是一台“一维相机”：每个地面特征只给出一个像素坐标 $u$。所有公式在第 5 节都会写回三维。

2直觉：两个各缺一半的传感器

IMU 单独积分：捷联惯导与漂移

“捷联”（strapdown）的意思是 IMU 直接固定在机体上，跟着机体一起转。要从它的读数得到位置，每个采样周期做三件事：用陀螺的角速度更新姿态 $\theta$；用姿态把加速度计测得的比力 $\mathbf f$（机体系）转到世界系，再加回重力，得到真正的加速度 $\mathbf a=R(\theta)\mathbf f+\mathbf g$；把加速度积分一次得速度，再积分一次得位置。比力这个名字很关键：静止的加速度计读到的不是 0，而是向上的 $9.81$ m/s²（支撑力），自由落体时它才读 0。

9.4 节已经看到，陀螺零偏 $b_g$ 积分一次变成线性增长的角度误差。到了位置，事情更糟。加速度计零偏 $b_a$ 被积分两次：位置误差 $\tfrac12b_at^2$。陀螺零偏的危害更隐蔽：姿态错了 $b_gt$，你就把重力的一部分（约 $g\,b_gt$）当成了水平加速度，再积分两次，位置误差 $\tfrac16g\,b_gt^3$。下图让一架二维无人机沿 8 字形飞 60 秒，IMU 以 200 Hz 采样，实时做捷联积分。

FIG 1捷联积分：零偏怎样让位置漂走

左：竖直平面里的真实轨迹（紫色）和只用 IMU 积分得到的轨迹（琥珀色），视野随两者自动缩放，左下角是比例尺。加速度计零偏加在机体 z 轴上（主要造成高度误差），陀螺零偏造成水平误差，所以两者的误差互相垂直、按平方和合成。右：位置误差随时间变化的双对数图。琥珀色是实际误差，红色虚线是 $\tfrac12b_at^2$，蓝色虚线是 $\tfrac16g\,b_gt^3$，打开噪声后青色虚线是白噪声造成的均方根误差。在双对数坐标里幂律是直线，斜率就是指数：红线斜率 2，蓝线斜率 3。噪声取 EuRoC 数据集所用 MEMS IMU 的标称值（陀螺 $1.7\times10^{-4}$ rad/s/√Hz，加速度计 $2\times10^{-3}$ m/s²/√Hz）。零偏和噪声都为 0 时，60 秒后的数值积分误差小于 0.3 mm。

试一试

噪声关闭，把陀螺零偏设为 0、加速度计零偏设为至少 2 mg，运行到 20 秒以后：琥珀色曲线贴着红色的 $t^2$ 线（读数里“实测 / 理论”在 0.9 到 1.1 之间）。时间翻倍，误差变成 4 倍。
噪声关闭，把加速度计零偏设为 0、陀螺零偏设为至少 0.02 °/s，运行到 20 秒以后：误差沿蓝色的 $t^3$ 线增长（比值在 0.9 到 1.1 之间）。时间翻倍，误差变成 8 倍：一个看起来很小的陀螺零偏，长时间后比加速度计零偏危险得多。

相机单独估计：单目视觉里程计与尺度

视觉里程计（VO）把第 11 章的工具串成一条流水线：初始化用最初几帧求出相机运动和一批三维点（11.1 节的对极几何、11.3 节的三角化）；之后每来一帧，先用恒速模型预测位姿，再对已有的地图点做 PnP（11.4 节）；视差足够的新特征被三角化加入地图；最后对最近若干帧做一次局部 BA（11.6 节）把误差压下去。下图就是这样一条流水线，在下方的“一维图像”里你能看到每帧跟踪到的特征。

有一个细节是二维世界特有的：一维相机看一个点只得到一个数，两帧图像对相机运动没有任何约束（三维里 11.1 节的五点法 / 八点法可以做到），所以这里的初始化是对最初 1 秒的 11 帧做一次小的 BA（从一个粗略的猜测出发）。初始化时必须人为规定一个尺度，这里规定“第 0 帧到第 10 帧的基线长 1 个单位”。从此以后，VO 的整条轨迹和地图都以这个单位计量：你不知道它对应多少米。拖动“显示尺度”滑块，把整个 VO 结果放大，可以让它和真实轨迹重合；可是重投影误差一点也不变，因为图像本来就不含尺度（11.6 节的规范自由度）。更糟的是，每一次三角化和 PnP 都会带进一点尺度误差，它们一帧一帧地累积：尺度会漂。要说明的是，本图的一维相机每帧只有很少、很弱的约束，所以尺度漂得比真实的单目 VO 快得多：真实系统在二维图像上跟踪几百个特征，同样会漂，但慢得多。漂移的存在是真实的，它的速度被这个玩具世界夸大了。

FIG 2单目视觉里程计：只知道形状，不知道大小

上：无人机以约 2 m/s 在 3 m 高度飞越起伏地面，30 秒飞 60 m，相机每秒 10 帧，视场 ±50°，焦距 400 px。紫色是真实轨迹和真实地面特征（空心圆），琥珀色是 VO 的在线输出和它的地图点，都乘了滑块给出的尺度 $s$（绕起点缩放）。青色楔形是相机视场，细线连到当前帧看到的特征。底部条带是这一帧的一维图像：青色刻度是已在地图里、参与 PnP 的特征，琥珀色刻度是还在等足够视差、尚未三角化的特征。下：“局部尺度”：最近 2 秒里真实位移与 VO 位移之比（米 / VO 单位），青色虚线是滑块的 $s$。像素噪声设为 0 时，这条曲线在每个随机种子下都是一条水平线（2.626 m/单位）。

运行过 5 秒后，调“显示尺度”滑块，让琥珀色的前 10 m 与紫色轨迹重合（读数里“前 5 秒的最佳尺度”与你的 $s$ 相差小于 3%）。注意重投影误差在你拖动时一点不变。
像素噪声至少 1 px，运行到 30 秒结束：最后 5 秒的最佳尺度与最初 5 秒的相差 10% 以上，没有哪一个 $s$ 能让整条轨迹都对齐。（若这次漂得少，按“换一组噪声”再试。）

核心洞见

两种失败是互补的。IMU 的误差在短时间内很小（FIG 1 里 1 秒后只有几厘米），但随时间按幂律爆炸；视觉的误差不随时间爆炸（看着同一批特征时位置是被“钉住”的），却缺一个尺度，而且尺度和偏航会慢慢漂。IMU 测的是公制的加速度，所以它知道尺度；它还感受得到重力，所以知道哪边是下。相机每一帧都在纠正 IMU 的积分，顺便把 IMU 的零偏估计出来。这就是 VIO。

3动手：一个紧耦合的滑动窗口 VIO

下图运行的是一个真实的 VIO 估计器，结构和 VINS-Mono、OKVIS 这类系统相同，只是降到了二维。窗口里保存最近 $W$ 个关键帧（这里每帧都是关键帧，10 Hz），每个关键帧的状态是 $\mathbf x_k=(\mathbf p_k,\mathbf v_k,\theta_k)$，再加上一组共享的 IMU 零偏 $\mathbf b=(b_{a,x},b_{a,z},b_g)$ 和窗口里看到的地面特征位置 $\mathbf l_m$。每来一帧，估计器最小化三类残差平方和（6.9 节的因子图）：

视觉残差：每个特征在每个关键帧里的预测像素减去观测像素（权重 1/σ²，σ = 1 px）。直接用像素而不是先算出一个 VO 位姿，这叫紧耦合。
IMU 残差：相邻两个关键帧之间，IMU 积分出来的位移、速度和转角应当与两帧状态之差一致（权重来自 IMU 噪声的传播，第 4、5 节的“预积分”）。
先验残差：窗口滑走的那些旧状态留下的信息。

最后一项是本图的重点。窗口满了，最老的关键帧必须离开，否则问题会越来越大。有两种做法：边缘化，把最老的状态用 Schur 补（11.6 节）消去，它通过各个因子提供的信息变成一个作用在剩余变量上的高斯先验；直接丢弃，把它连同它的观测和 IMU 因子一起扔掉，为了固定规范，再把新的最老关键帧的位置和俯仰角钉在当前估计上。两个估计器吃的是完全相同的数据。

FIG 3滑动窗口 VIO：边缘化还是丢弃

同一次飞行（与 FIG 2 相同的轨迹和地面），IMU 200 Hz，真实零偏 $b_a=(0.08,-0.05)$ m/s²、$b_g=0.40$ °/s，估计器从零偏 0 出发（先验 σ 为 0.1 m/s² 和 1 °/s），初始位姿和速度取真值（如同已经完成了 FIG 5 的初始化）。上：视图跟随无人机。紫色是真实轨迹，琥珀色是“边缘化”估计器的关键帧轨迹，蓝色是“丢弃”估计器的；琥珀色空心圆是边缘化估计器当前窗口里的关键帧，琥珀色小点是它窗口里的地图点。下：两者的位置误差（对数坐标），开启“相机遮挡”时，青色阴影标出 12 到 16 秒相机什么也看不到的时段（比如飞过水面）。窗口大小在重新开始时生效。

保持窗口 $W=8$、不开遮挡，按“运行”看到 30 秒结束：琥珀色（边缘化）飞了 60 m 后误差小于 0.5 m，蓝色（丢弃）的误差是它的 5 倍以上，而且蓝色的零偏估计离真值很远。
打开“相机遮挡”，运行过 16 秒：在相机全盲的 4 秒里，边缘化估计器只靠 IMU 积分，误差增长不到 0.5 m，因为零偏在遮挡前已被估计出来；和 FIG 1 里 4 秒的纯惯导比一比。

有意思的细节

(1) 默认设置（$W=8$，第 1 组噪声）下，边缘化估计器 30 秒后误差 0.21 m（航程的 0.35%），陀螺零偏估计 0.400 °/s；丢弃估计器的误差 11.5 m，零偏估计跑到了 2 °/s。丢弃并不只是“少用了一点数据”：速度和零偏只能靠跨越很多帧的 IMU 约束才估得准，每次只看 0.8 秒的窗口，它们永远估不准。 (2) 边缘化估计器对窗口大小不敏感：$W=3$ 时误差也在同一量级；丢弃估计器则随 $W$ 增大明显变好。窗口的作用是让最近的状态能被反复重新线性化，信息的保存靠的是先验。 (3) 两个估计器的俯仰角误差都保持有界（边缘化的约 0.2°），位置误差却会慢慢增长：第 4 节会说明，俯仰角有重力在“看着”，水平和高度位置却没有任何传感器能测到，它们是不可观的。

4另外三个视角：预积分、初始化、可观性

IMU 预积分：只积分一次

FIG 3 的优化器每一帧要迭代好几次，每次迭代都会改动窗口里每个关键帧的速度和姿态。如果 IMU 残差是“从关键帧 $i$ 的当前估计出发，把中间 20 个采样（或 1 秒的 200 个）重新积分一遍，看是否到达关键帧 $j$”，每次迭代都要把整个窗口的 IMU 数据重新积分。预积分（Lupton 与 Sukkarieh，2012；Forster 等，2017）的观察是：位移可以拆成三部分，

$$ \mathbf p_j=\mathbf p_i+\hl{vdt}{\ck{\mathbf v_i\Delta t}}+\hl{grav}{\cj{\tfrac12\mathbf g\Delta t^2}}+\hl{dp}{\cv{R_i\,\Delta\mathbf p_{ij}}},\qquad \Delta\mathbf p_{ij}=\sum_{k=i}^{j-1}\Big(\Delta\mathbf v_{ik}\,\delta t+\tfrac12\,\Delta R_{ik}\,(\mathbf f_k-\mathbf b_a)\,\delta t^2\Big) $$

前两项只用到起点的状态，第三项 $\Delta\mathbf p_{ij}$ 只用到 IMU 读数和零偏，它是“在关键帧 $i$ 的机体系里，假装没有重力、初速度为零时，比力把你推了多远”。起点的速度和姿态怎样变，$\Delta\mathbf p_{ij}$ 都不用重算。下图把这条链画出来：拖动蓝色箭头的端点改变 $\mathbf v_i$，拖动青色机体轴的端点改变 $\theta_i$，预积分结果与从头重积分的结果始终重合。

唯一的麻烦是零偏：$\Delta\mathbf p_{ij}$ 依赖零偏，而零偏也是优化变量。解决办法是对零偏做一阶修正：预积分时顺便算出 $\partial\Delta\mathbf p/\partial\mathbf b$，零偏估计变化 $\delta\mathbf b$ 时用 $\Delta\mathbf p+\tfrac{\partial\Delta\mathbf p}{\partial\mathbf b}\delta\mathbf b$ 代替重积分。拖动零偏滑块看这个近似有多好。

FIG 4预积分：一条与起点状态无关的增量

一段 1 秒的飞行（FIG 2 那次飞行的第 4 到 5 秒，200 个 IMU 采样，无噪声）。从关键帧 $i$ 出发：蓝色箭头是 $\mathbf v_i\Delta t$，绿色箭头是 $\tfrac12\mathbf g\Delta t^2$（4.9 m，向下），琥珀色箭头是 $R_i\Delta\mathbf p_{ij}$：它向上约 5.6 m，因为加速度计感受到的是托住无人机的力，正好抵消了绿色箭头的大部分。三者首尾相接到达预积分给出的 $\mathbf p_j$（琥珀色圆点）。紫色曲线和空心圆是用当前的起点状态和零偏从头重积分 200 个采样的结果。零偏不为 0 时，红色叉是“预积分了却不修正零偏”的终点。

把 $\mathbf v_i$ 拖动 1 m/s 以上，或把 $\theta_i$ 转动 20° 以上：重积分计数器飞涨，而“预积分 − 重积分”始终在 $10^{-9}$ m 以下（只剩舍入误差）。
把陀螺零偏变化 $\delta b_g$ 调到 ±2 °/s 或更大（加速度计的 $\delta b_a$ 保持 0）：一阶修正的误差仍小于 1 mm，不修正则偏差超过 3 cm。再推到 10 °/s，看一阶误差怎样按平方增长到 14 mm。

初始化：尺度从加速度里来

FIG 3 的估计器是从真值出发的。真实系统开机时，它既不知道速度，也不知道重力朝哪（俯仰角），更不知道 VO 的一个单位是多少米。常见的办法（VINS-Mono 的“视觉惯性对齐”）是先让纯视觉跑一小段，得到按未知尺度 $s$ 计量的位置 $\bar{\mathbf p}_k$ 和姿态，再把上面的预积分关系当作关于速度、重力和尺度的线性方程来解：

$$ \hl{scale}{\cv{s}}\,(\bar{\mathbf p}_j-\bar{\mathbf p}_i)-\mathbf v_i\Delta t-\tfrac12\,\mathbf g\,\Delta t^2=R_i\Delta\mathbf p_{ij},\qquad \mathbf v_j-\mathbf v_i-\mathbf g\,\Delta t=R_i\Delta\mathbf v_{ij} $$

未知数是每个关键帧的速度、重力向量 $\mathbf g$ 和尺度 $s$。有一种情形下它注定失败：如果真实运动是匀速直线，那么把 $s$ 和所有速度同时放大同一个倍数，两组方程都照样成立，尺度不可观。加速度计只有在你真的加速时，才“知道”一米有多长。

FIG 5视觉惯性初始化：激励越足，尺度越准

无人机以 1 m/s 前进，叠加上下前后的晃动，“激励”滑块缩放晃动的幅度（0 = 匀速直线）。关键帧 4 Hz；VO 位置噪声 3 mm，姿态噪声 0.1°；IMU 噪声同 FIG 1，零偏为 0。VO 的真实单位是 2 m。左：紫色是真实的关键帧位置，琥珀色是 VO 位置乘以估计的尺度 $\hat s$；右上角的两个箭头是真实重力（紫）和估计重力（琥珀）。右：尺度的“剖面代价”：固定 $s$、对其余未知数求最优后，代价比最小值高出多少（以 $\chi^2$ 计，$\Delta\chi^2=1$ 处是 1σ，青色虚线），横轴是 $s/s_{\text{true}}$，紫色竖线是真值。剖面越平，尺度越不确定。

把激励调到 0（匀速直线）：尺度的标准差超过 10%，估计值塌向 0、离真值很远：此时方程里关于尺度的信息只剩噪声，剖面又宽，最低点又落在错误的地方。无论窗口多长都救不回来。
加大激励、拉长窗口，让尺度标准差小于 2%，并且估计值与真值相差小于 2%（若这组噪声不走运，按“换一组噪声”）。

可观性：有些方向永远测不出来

FIG 3 里位置误差慢慢增长，俯仰角误差却有界。这背后是可观性：如果把整个解（所有关键帧、速度、地图点）沿某个方向一起移动，所有残差都不变，那么这个方向上的信息量为零，估计误差只能像随机游走一样累积。用 6.9 节的话说，信息矩阵 $H=J^\top WJ$ 在这个方向上有零特征值（11.6 节里 BA 的 7 维规范自由度就是这样的方向）。下图在真值处为一个 7 个关键帧的小批量问题算出 $H$，画出最小的特征值，并检查四个“整体动作”：沿 $x$ 平移、沿 $z$ 平移、整体旋转、整体缩放。

FIG 6信息矩阵的零空间：哪些整体动作不改变任何残差

7 个关键帧（间隔 0.25 s）和被至少两帧看到的地面特征，无噪声，在真值处线性化。左：紫色是真值（关键帧画成机体，特征画成空心圆），琥珀色是沿所选整体动作来回摆动的解；读数给出这个动作让代价增加多少（瑞利商 $\mathbf d^\top H\mathbf d/\lambda_{\max}$）。右：$H$ 最小的 12 个特征值（相对最大值，对数坐标），小于 $10^{-12}$ 的数值零画成红色，其余为蓝色。“仅相机”时状态里没有速度，重力选项不起作用。“水平平面”表示把同一个平面放平：重力垂直于平面，加速度计在平面内感受不到它。

默认的“仅相机”有 4 个零特征值（两个平移、旋转、尺度）。加上 IMU（竖直平面，有加速）：只剩 2 个，就是两个平移；旋转和尺度都变成可观的了。
保持相机 + IMU，分别找出让旋转重新不可观的设置和让尺度重新不可观的设置（各有 3 个零特征值）。

一个想法，几种看法

几何：重力是平面里唯一的“绝对方向”，所以能测出俯仰；没有任何东西标记“原点在哪”，所以位置测不出。把平面放平，重力从平面里消失，旋转也就丢了：这就是三维 VIO 里偏航角不可观的原因（重力只能告诉你横滚和俯仰）。代数：零特征值对应的特征向量就是这些整体动作；任何正确的估计器都不应该在这些方向上“得到信息”。概率：不可观方向上的协方差只增不减，所以长时间运行的 VIO 一定会漂，只有回环（15.2 节）或外部参考（GPS、已知地图）能把它拉回来。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮：FIG 1 的红线和蓝线、FIG 3 窗口里承载先验的最老关键帧、FIG 4 的三支箭头、FIG 5 的尺度剖面、FIG 6 的零特征值。

捷联惯导与误差增长

三维里，机体姿态 $R={}^{W}R_{B}\in\SO$，IMU 测得角速度 $\boldsymbol\omega_m$ 和比力 $\mathbf f_m$（都在机体系），模型是 $\boldsymbol\omega_m=\boldsymbol\omega+\mathbf b_g+\mathbf n_g$，$\mathbf f_m=R^\top(\mathbf a-\mathbf g)+\mathbf b_a+\mathbf n_a$。捷联方程：

$$ \dot R=R\,\skew{\boldsymbol\omega_m-\mathbf b_g-\mathbf n_g},\qquad \dot{\mathbf v}=R\,(\mathbf f_m-\mathbf b_a-\mathbf n_a)+\mathbf g,\qquad \dot{\mathbf p}=\mathbf v $$

离散实现时姿态用指数映射更新，$R_{k+1}=R_k\Exp\big((\boldsymbol\omega_k-\mathbf b_g)\delta t\big)$（5.6 节），不能把三个角速度当欧拉角的导数去积。在悬停附近做小角度分析，零偏造成的误差为：

$$ \delta\mathbf p_{b_a}(t)\approx\tfrac12\,\hl{ba}{\ci{b_a}}\,t^2,\qquad \delta\theta(t)=b_g t\ \Rightarrow\ \delta a\approx g\,\delta\theta\ \Rightarrow\ \delta p_{b_g}(t)\approx\tfrac16\,g\,\hl{bg}{\ck{b_g}}\,t^3 $$ $$ \sigma_{p}(t)\approx\sqrt{\tfrac13\sigma_a^2t^3+\tfrac1{20}g^2\sigma_g^2t^5}\qquad(\sigma_a,\ \sigma_g:\ \text{noise densities}) $$

第二行是白噪声的均方根：加速度噪声积分两次是“速度随机游走”的积分，方差 $\sigma_a^2t^3/3$；陀螺噪声先变成角度随机游走 $\sigma_g\sqrt t$，乘以 $g$ 再积分两次，方差 $g^2\sigma_g^2t^5/20$。FIG 1 里实测与这三条公式的比值在 20 秒后都在 1% 以内（5 mg、0.05 °/s：10 秒后分别是 2.45 m 和 1.44 m）。代价最高的是 $t^3$ 项：所以惯导系统最贵的部件是陀螺，导航级陀螺的零偏稳定性在 0.01 °/h 量级，而手机里的 MEMS 陀螺是它的几百到几千倍。

预积分的完整形式

在关键帧 $i$ 与 $j$ 之间，以 $i$ 的机体系为参照累积 IMU 采样（Forster 等 2017 的记号）：

$$ \Delta R_{ij}=\prod_{k=i}^{j-1}\Exp\big((\boldsymbol\omega_k-\mathbf b_g)\delta t\big),\quad \Delta\mathbf v_{ij}=\sum_{k=i}^{j-1}\Delta R_{ik}(\mathbf f_k-\mathbf b_a)\delta t,\quad \Delta\mathbf p_{ij}=\sum_{k=i}^{j-1}\Big(\Delta\mathbf v_{ik}\delta t+\tfrac12\Delta R_{ik}(\mathbf f_k-\mathbf b_a)\delta t^2\Big) $$ $$ \mathbf r_{\Delta R}=\Log\!\Big(\big(\Delta R_{ij}\Exp(J^{R}_{g}\delta\mathbf b_g)\big)^{\!\top}R_i^\top R_j\Big),\quad \mathbf r_{\Delta v}=R_i^\top(\mathbf v_j-\mathbf v_i-\mathbf g\Delta t)-\big(\Delta\mathbf v_{ij}+J^{v}_{a}\delta\mathbf b_a+J^{v}_{g}\delta\mathbf b_g\big) $$ $$ \mathbf r_{\Delta p}=R_i^\top\big(\mathbf p_j-\mathbf p_i-\mathbf v_i\Delta t-\tfrac12\mathbf g\Delta t^2\big)-\big(\Delta\mathbf p_{ij}+J^{p}_{a}\delta\mathbf b_a+J^{p}_{g}\delta\mathbf b_g\big) $$

$J^{\cdot}_{\cdot}$ 是预积分量对零偏的雅可比，在累加时顺带递推出来（旋转部分用 5.7 节的右雅可比）。残差的协方差 $\Sigma_{ij}$ 也递推：把误差 $\boldsymbol\eta=(\delta\boldsymbol\phi,\delta\mathbf v,\delta\mathbf p)$ 写成 $\boldsymbol\eta_{k+1}=A_k\boldsymbol\eta_k+B_k\mathbf n_k$，就有 $\Sigma_{k+1}=A_k\Sigma_kA_k^\top+B_k\Sigma_nB_k^\top$（4.5 节的不确定性传播，$\Sigma_n$ 由噪声密度除以 $\delta t$ 得到）。FIG 3 的 IMU 因子就以 $\Sigma_{ij}^{-1}$ 为权重。二维里旋转只是一个角度，$\Delta\theta_{ij}=\sum(\omega_k-b_g)\delta t$ 对 $b_g$ 是精确线性的；非线性只来自 $\Delta\mathbf p$ 和 $\Delta\mathbf v$ 里的 $R(\Delta\theta)$，所以 FIG 4 里一阶修正的误差按 $\delta b_g^2$ 增长：1 °/s 时 0.14 mm，10 °/s 时 14 mm。实际系统在 $\delta\mathbf b$ 大到一定程度时才重新预积分。

紧耦合代价、松耦合与边缘化

滑动窗口估计器在每一帧求解一个最大后验问题（4.6 节）：

$$ \min_{\{\mathbf x_k\},\,\mathbf b,\,\{\mathbf l_m\}}\ \big\|\mathbf r_{\text{prior}}\big\|^2+\sum_{k}\big\|\mathbf r_{\text{IMU},k,k+1}\big\|^2_{\Sigma_{k,k+1}}+\sum_{(k,m)}\rho\Big(\big\|\pi(\mathbf x_k,\mathbf l_m)-\mathbf u_{km}\big\|^2_{\sigma^2}\Big) $$

$\pi$ 是投影（11.4、11.6 节），$\rho$ 是鲁棒核（11.5 节）。这里视觉残差直接是像素，所以叫紧耦合。松耦合则让一个独立的 VO 先算出位姿，再把这个位姿当作测量交给 IMU 滤波器；它更简单、模块化，但丢掉了两样东西：每个特征各自的不确定度和它们之间的相关性（VO 位姿的协方差往往只是粗略估计），以及 VO 失败时仍然有用的那一两个特征。图像只剩两三个特征时，紧耦合系统还能用它们约束 IMU，松耦合系统已经拿不到位姿了。

边缘化。把要移走的变量记为 $\mathbf m$，留下的记为 $\mathbf r$。在当前估计处，把所有连到 $\mathbf m$ 的因子（加上旧先验）线性化成 $H\boldsymbol\delta=-\mathbf g$，再像 11.6 节消去 BA 中的点那样做 Schur 补：

$$ \begin{bmatrix}H_{mm}&H_{mr}\\H_{rm}&H_{rr}\end{bmatrix}\begin{bmatrix}\boldsymbol\delta_m\\\boldsymbol\delta_r\end{bmatrix}=-\begin{bmatrix}\mathbf g_m\\\mathbf g_r\end{bmatrix}\ \Longrightarrow\ \hl{prior}{\cv{H^\star}}=H_{rr}-H_{rm}H_{mm}^{-1}H_{mr},\qquad \mathbf g^\star=\mathbf g_r-H_{rm}H_{mm}^{-1}\mathbf g_m $$

新先验的能量是 $\mathbf g^{\star\top}\boldsymbol\delta_r+\tfrac12\boldsymbol\delta_r^\top H^\star\boldsymbol\delta_r$，其中 $\boldsymbol\delta_r$ 是相对边缘化时线性化点的偏差：先验从此固定在那个线性化点上。这带来两个工程问题。第一，填充：被边缘化的关键帧看到过的所有地图点、下一个关键帧和零偏，在 $H^\star$ 里两两相连，先验是稠密的；FIG 3 的实现保留这些点，VINS-Mono 等系统还会丢掉一部分观测来保持稀疏。第二，一致性：如果之后同一个变量的雅可比在新的线性化点上计算，先验和新因子“看到”的零空间不一致，估计器会在不可观方向（三维里的偏航）上凭空获得信息，变得过于自信。First-Estimates Jacobian（FEJ）让同一变量的雅可比始终在第一次估计处计算，以此保持零空间（Huang、Mourikis 与 Roumeliotis）。FIG 3 的二维世界里不可观方向只有两个平移，雅可比与平移无关，所以这个问题不会出现。“丢弃”相当于令 $H^\star=0$，再人为钉住一个位姿。

滤波：MSCKF。多状态约束卡尔曼滤波（Mourikis 与 Roumeliotis，2007）走另一条路：EKF（4.8 节）的状态是当前 IMU 状态加最近 $N$ 个相机位姿的“克隆”，地图点不进状态。一条特征轨迹结束时，先用克隆的位姿三角化出它的位置 $\hat{\mathbf l}$，把这条轨迹的所有残差堆起来线性化：$\mathbf r=H_x\delta\mathbf x+H_l\delta\mathbf l+\mathbf n$；再左乘 $H_l$ 左零空间的基 $A^\top$（$A^\top H_l=0$），得到 $\mathbf r_o=A^\top H_x\delta\mathbf x+A^\top\mathbf n$，与特征位置无关，直接做标准 EKF 更新。代价随特征数线性增长。它和滑动窗口优化的本质区别是：滤波器每个测量只线性化一次（相当于一次 Gauss-Newton 迭代），优化器则在窗口内反复重新线性化，精度更高、算得也更多（Strasdat、Montiel 与 Davison 2012 在单目 SLAM 中做过这样的比较）。OpenVINS 走滤波路线，VINS-Mono、OKVIS 走优化路线。

可观性与初始化

三维 VIO 的整体动作：取绕重力方向（世界 $z$ 轴）的旋转 $R_z(\psi)$ 和任意平移 $\mathbf t$，令 $R_k'=R_z R_k$，$\mathbf p_k'=R_z\mathbf p_k+\mathbf t$，$\mathbf v_k'=R_z\mathbf v_k$，$\mathbf l_m'=R_z\mathbf l_m+\mathbf t$。因为 $R_z\mathbf g=\mathbf g$，

$$ R_i'^{\top}\big(\mathbf p_j'-\mathbf p_i'-\mathbf v_i'\Delta t-\tfrac12\mathbf g\Delta t^2\big)=R_i^\top R_z^\top R_z\big(\mathbf p_j-\mathbf p_i-\mathbf v_i\Delta t-\tfrac12\mathbf g\Delta t^2\big),\qquad H\,\hl{null}{\cx{N}}=0,\quad N\in\R^{n\times4} $$

IMU 残差不变，相机残差显然也不变（相机系里的点坐标没变），所以 $H$ 有一个 4 维零空间 $N$：3 维位置加偏航（Jones 与 Soatto 2011；Kelly 与 Sukhatme 2011；Hesch 等 2014）。绕其他轴的旋转会改变重力在机体系里的方向，因而可观；尺度在有加速度时可观，匀速直线时不可观（FIG 5、FIG 6）。另外还有一些退化运动会让更多方向不可观，例如没有转动时加速度计零偏与姿态（重力方向）分不开。FIG 6 的二维结果是它的缩影：重力在平面内时零空间 2 维，重力垂直平面时 3 维（多了“偏航”），匀速时再加上尺度。

初始化就是在这些可观方向上给出一个足够好的初值。FIG 5 的线性方程组未知数为 $\mathbf y=(\mathbf v_0,\dots,\mathbf v_{K-1},\mathbf g,s)$，按噪声加权求最小二乘 $\hat{\mathbf y}=(A^\top WA)^{-1}A^\top W\mathbf b$，$(A^\top WA)^{-1}$ 的最后一个对角元就是 $\sigma_s^2$。匀速时 $A$ 有零空间 $\big(\delta s,\ \delta\mathbf v_k=\delta s\,\mathbf v/s,\ \delta\mathbf g=0\big)$，只有噪声让它不完全奇异。VINS-Mono 还会在得到 $\mathbf g$ 后把它的模长约束为 9.81 再细化方向，并先用纯旋转关系估计陀螺零偏；ORB-SLAM3 则把初始化写成一个只含惯性量的 MAP 问题（Campos 等，2020）。静止起步是常见的“第 0 步”：静止几秒，加速度计的平均值给出重力方向，陀螺的平均值给出陀螺零偏，然后再运动起来提供尺度。

6机器人中的它

真实系统里

开源 VIO：MSCKF（Mourikis 与 Roumeliotis，2007）和在它基础上发展的 OpenVINS（Geneva 等，2020）是滤波路线；ROVIO（Bloesch 等，2015）是直接用图像块光度误差的 EKF。OKVIS（Leutenegger 等，2015）是关键帧滑动窗口优化加边缘化；VINS-Mono（Qin、Li 与 Shen，2018）是滑动窗口优化加预积分，用 Ceres 求解，并在线估计相机–IMU 外参和时间偏移；Basalt、Kimera-VIO（基于 GTSAM 的固定滞后平滑器）也属于优化路线；ORB-SLAM3（Campos 等，2021）在视觉惯性模式下把 BA 扩展到 IMU 因子。 数据与指标：EuRoC MAV 数据集（Burri 等，2016）的 IMU 200 Hz、双目相机 20 Hz，是最常用的基准；Delmerico 与 Scaramuzza（2018）在它上面系统地比较了主流单目 VIO 的精度与计算量。EuRoC 的序列长约 40 到 130 m，好的 VIO 在上面的绝对轨迹误差通常是几厘米到几分米，即航程的百分之一以下（FIG 3 结束时 0.35% 的误差是同一量级，虽然两种指标并不完全相同）。产品：苹果说明 ARKit 的世界跟踪用的是视觉惯性里程计，谷歌 ARCore 的运动跟踪同样把相机特征点与 IMU 读数融合；VR 头显的“由内向外”跟踪用头显上的相机和 IMU；NASA 的“机智号”火星直升机用一台朝下的导航相机、IMU 和激光测高计估计自己的运动，正是 FIG 2、FIG 3 的“朝下的相机 + IMU”结构。标定：Kalibr（Furgale、Rehder 与 Siegwart，2013）同时标定相机–IMU 外参和时间偏移，它本身也是一个连续时间的大型最小二乘问题。之后：VIO 只是里程计，不可观方向会一直漂；15.2 节的回环与位姿图把漂移拉回来，15.3 节讨论它建出的地图该怎样表示。

7常见误区

误区 1：忘了加速度计测的是比力

静止的 IMU 读到的是向上 9.81 m/s²。如果直接把读数当加速度积分，或者把重力加错了符号、加在了错误的坐标系里（重力在世界系，读数在机体系），一秒钟就是 5 m 的误差（FIG 4 的绿色箭头）。同类的错误还有坐标系约定：IMU 的轴向、四元数的 Hamilton 与 JPL 约定（5.2 节）、姿态是“机体到世界”还是“世界到机体”。拿到一个新 IMU，先让它静止放平，检查读数是不是 $(0,0,+9.81)$。

误区 2：相机–IMU 外参和时间偏移随便填

紧耦合的 VIO 把像素和 IMU 放在同一个方程里，两者之间的旋转、平移和时间差都必须准确。一个 5 ms 的时间偏移，在头部以 2 rad/s 转动时就是 0.01 rad 的姿态差，焦距 400 px 的相机上约 4 px，远大于特征跟踪的噪声。用 Kalibr 一类工具离线标定，或者像 VINS-Mono 那样把外参和时间偏移作为状态在线估计；卷帘快门相机还要对每一行的曝光时间建模。

误区 3：在退化的运动下初始化

匀速直线（FIG 5 的激励 0）时尺度不可观；纯旋转时没有视差，深度和尺度都没有（11.1 节）；在地面上平稳行驶的车辆常常长时间处在这类运动里。解算出来的尺度看起来是一个确定的数，其实可能毫无意义：一定要看它的协方差，或者像 FIG 5 那样看剖面是否宽、最低点是否可信。可行的做法：静止起步估计重力和陀螺零偏，再用一段有加速度的运动去初始化尺度；对地面车辆加入轮速计提供尺度。

误区 4：丢弃旧信息，或让估计器过于自信

FIG 3 说明直接丢掉滑出窗口的状态代价很大：速度和零偏失去长期约束。反过来，边缘化做得不小心也会出错：在新的线性化点重新计算旧变量的雅可比，会让先验和新因子的零空间对不上，估计器在偏航方向上“凭空”获得信息，协方差变得太小，一个过于自信的滤波器会拒绝正确的测量（4.8 节 FIG 5 的“说谎的滤波器”）。用 FEJ 或可观性约束的 EKF，并用 NEES 之类的一致性检验看估计器是否诚实。

误区 5：相信数据手册上的噪声参数

数据手册给的是静止、恒温条件下的噪声密度和零偏稳定性。装在电机和螺旋桨旁边，振动会让加速度计噪声大上好几倍；剧烈运动会让 IMU 饱和；温度变化会让零偏缓慢漂移；运动模糊、动态物体、反光和重复纹理会产生错误的视觉匹配。实际系统通常把 IMU 噪声参数放大几倍，对零偏建模为随机游走，并对视觉残差使用鲁棒核和外点剔除（11.5 节）。

8小练习

1. 一个未补偿的加速度计零偏 10 mg（约 0.098 m/s²），姿态完全准确，纯惯导 10 秒后位置误差大约是多少？

$\tfrac12b_at^2=\tfrac12\times0.098\times10^2\approx4.9$ m。零偏被积分了两次，所以误差随 $t^2$ 增长：1 秒后只有 5 cm（选项 A 是 1 秒时的答案），100 秒后约 490 m。

2. 一架装了单目相机和 IMU 的无人机，起飞后以恒定速度沿直线平飞（姿态也不变）。这段时间里 VIO 能确定尺度吗？

此时加速度计只测到重力的反作用，它与“走了多远”无关。初始化方程 $s(\bar{\mathbf p}_j-\bar{\mathbf p}_i)-\mathbf v_i\Delta t=\dots$ 在 $s\to\lambda s$、$\mathbf v\to\lambda\mathbf v$ 下不变（FIG 5 的激励 0，FIG 6 的“匀速”）。飞得再久也没用（选项 C）。重力方向反而是可观的（选项 D 错）：加速度计正是在测它。

3. IMU 预积分在优化式 VIO 里的主要好处是什么？

预积分不改变信息量（噪声仍在，选项 A 错；零偏是否可观由运动决定，选项 B 错），只改变计算方式：两帧之间的几百个采样被压缩成一个因子，状态怎么变都可以直接复用（FIG 4 里重积分计数器一直涨，预积分结果却不用重算）。

4. 三维单目 VIO（相机 + IMU，运动有加速和转动）有几个不可观的方向？

单目 BA 的 7 维规范（选项 B）里，尺度因加速度计而可观，横滚和俯仰因重力而可观，剩下 3 维平移和偏航。这正是 FIG 6 的三维版本：竖直平面里有重力，零空间只有两个平移。

5. 滑动窗口里，“边缘化最老的关键帧”和“直接丢弃它”的区别是什么？

两者都让变量离开窗口，区别在信息：$H^\star=H_{rr}-H_{rm}H_{mm}^{-1}H_{mr}$ 记住了它对速度、零偏、地图点和下一帧的约束；丢弃相当于 $H^\star=0$。FIG 3 里前者 0.21 m，后者 11.5 m。先验一般是稠密的（选项 D 错），状态值本身并不是要保留的东西（选项 B）。

9延伸阅读

D. Titterton & J. Weston, Strapdown Inertial Navigation Technology, 2nd ed., IET, 2004 — 捷联惯导的标准教材：误差方程与 IMU 等级
D. Scaramuzza & F. Fraundorfer, Visual Odometry, Part I: The First 30 Years and Fundamentals, IEEE Robotics & Automation Magazine, 2011 — 视觉里程计的入门教程
A. I. Mourikis & S. I. Roumeliotis, A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation, ICRA 2007 — MSCKF：零空间投影，地图点不进状态
T. Lupton & S. Sukkarieh, Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions, IEEE Transactions on Robotics, 2012 — IMU 预积分的提出
C. Forster, L. Carlone, F. Dellaert & D. Scaramuzza, On-Manifold Preintegration for Real-Time Visual-Inertial Odometry, IEEE Transactions on Robotics, 2017 — 在 SO(3) 上的预积分、零偏雅可比与协方差
S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart & P. Furgale, Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization, International Journal of Robotics Research, 2015 — OKVIS：关键帧滑动窗口与边缘化
T. Qin, P. Li & S. Shen, VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, IEEE Transactions on Robotics, 2018 — 初始化（视觉惯性对齐）、滑动窗口优化、回环
J. A. Hesch, D. G. Kottas, S. L. Bowman & S. I. Roumeliotis, Consistency Analysis and Improvement of Vision-aided Inertial Navigation, IEEE Transactions on Robotics, 2014 — VIO 的 4 维不可观子空间与 EKF 的不一致性
P. Geneva, K. Eckenhoff, W. Lee, Y. Yang & G. Huang, OpenVINS: A Research Platform for Visual-Inertial Estimation, ICRA 2020 — 现代 MSCKF 的完整开源实现与文档
J. Delmerico & D. Scaramuzza, A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots, ICRA 2018 — 在 EuRoC 上比较主流 VIO 的精度、效率和鲁棒性