31.2.1 SLAM同时定位与建图：视觉惯性VIO

可视化演示

演示视觉惯性里程计(VIO)工作原理：相机视觉特征追踪、IMU高频积分预测、以及多传感器紧耦合融合估计位姿的过程。

什么是视觉惯性VIO

小明站在黑暗博物馆的入口，手里的手电筒只能照亮七八米，而内耳却能在闭眼时感受到每一次迈步。如果只靠手电筒，他走到手电筒照不到的角落就彻底迷路；如果只靠内耳，走上一百米后他就说不清自己偏到了哪个展厅。于是他想：能不能把眼睛看到的和身体感觉到的合起来？这就是视觉惯性VIO的出发点。

VIO全称Visual-Inertial Odometry，翻译过来是「视觉惯性里程计」。它把摄像头（眼睛）和惯性测量单元IMU（内耳）作为两大传感器：摄像头负责在图像里认出稳定的环境特征，IMU负责以几百赫兹的高频率测量加速度和角速度。VIO要做的事，就是一边看地标、一边记步伐，实时估计出机器人每一毫秒的位置和姿态。

如果把这个问题写成数学语言，VIO本质上是在求解一个概率估计问题：给定所有相机观测和IMU读数，找出最可能的那条运动轨迹。这条轨迹不仅是“我在哪”，还包括“我朝哪个方向、走了多快、加速度计有没有偏”——它们共同构成了机器人的状态。

所以，第一篇内容的核心是建立直觉：视觉惯性VIO不是某个神秘黑盒，而是一个「眼睛+内耳+大脑地图」的协作系统。小明的背包里，这三样东西缺一不可。

视觉里程计：眼睛怎么看路

小明举起手电筒，照到一面墙上。墙上有一幅画，画框的四个角在黑暗中闪着微光。他往前走两步，再照一次，发现同一个画框角出现在画面里偏右的位置。他立刻意识到：我刚才往左移了。这就是视觉里程计（VO）最朴素的工作方式——通过追踪图像中的特征点来反推自身运动。

所谓特征点，就是图像里那些“角角落落”：画框角、门把手边缘、地砖缝的交点。它们就像黑暗中的荧光标记，在光照变化、视角变化下依然能被认出来。VIO前端通常用ORB、SIFT这类算法提取特征点，然后在相邻两帧图像中匹配同一批特征点——认出来“这是我刚见过的那个画框角”。

一旦得到足够多匹配点，视觉里程计就要回答一个几何问题：相机发生了怎样的旋转和平移，才能让这些匹配点满足同一套投影关系？这个关系可以用对极几何描述。小明虽然不懂数学，但他的大脑在无意识中做的正是这件事：比较两次看到的画框角位置，推算出自己脑袋转了多少、平移了多少。

视觉里程计的优势是“每次看到地标都能获得绝对参照”，短板也很明显：如果手电筒照到一面白墙，没有画框也没有门把手，视觉里程计就会当场失明。这正是下一节内耳登场的原因。

惯性测量单元：内耳怎么算步

小明闭上眼睛，原地转了个圈，又向前走了两步——即使黑暗中什么也看不见，他也能清楚说出自己转了多少度、走了多远。这份能力来自内耳前庭，而在机器人身上，它叫惯性测量单元（IMU）。

IMU内部有两个核心传感器：加速度计测量三轴加速度，陀螺仪测量三轴角速度。它们以200Hz到1000Hz的高频率持续输出数据，远高于摄像头通常的30帧每秒。这意味着两次拍照之间的空隙里，IMU还在不断“汇报”身体的微小运动。

但IMU有一个致命弱点：它的测量里藏着零偏和噪声。零偏就像一把永远少称一两的秤——明明没有加速度，它却报告有一个小而恒定的值；噪声则是每次读数里随机抖动的误差。如果我们把小明内耳的读数写成公式，就是真实加速度加上这两个“污染项”：

为了让IMU数据能被高效使用，现代VIO普遍采用IMU预积分技术：把一段时间内的加速度计和陀螺仪读数预先压缩成一个“相对运动摘要”，这样在后续优化中就不需要反复从零开始积分，大幅节省计算量。这就像小明不再回忆每一步的感觉，而是在脑子里把“最近两秒内耳感觉”打包成一条简短结论：“我向右偏了3度，前进了0.4米。”

传感器融合：两个世界如何握手

手电筒看得清但会失明，内耳永远在线但会漂移。小明的大脑必须回答一个关键问题：这两个信源，到底该信谁？答案是——让它们互相制约、联合求解，这就是传感器融合。

融合方式分两种。松耦合就像小明先分别问手电筒“我大概在哪”、问内耳“我大概在哪”，再把两个答案做加权平均；简单但浪费了信息。紧耦合则让手电筒看到的特征点和内耳感受到的加速度进入同一个优化方程，彼此互相校正——就像大脑同时处理视觉和平衡信号，而不是分开计算。现代VIO几乎都是紧耦合。

紧耦合最经典的数学工具之一是扩展卡尔曼滤波（EKF）。它的思想非常直观：先用IMU做一步预测，得到“感觉位置”；到达新一帧图像后，用手电筒的观测去矫正这个位置。矫正幅度由卡尔曼增益决定——谁更可信，就多信谁一点。

除了滤波，另一种主流路线是图优化：把所有历史状态和观测关系画成一张“线索关系网”，然后用最小二乘让整张网络互相牵制。EKF像边走边改，图优化像隔一段时间回头通盘整理，两者各有千秋，但核心理念都是——让视觉和惯性在同一个模型中互相校验。

主流算法：VINS-Mono与OKVIS

小明逛过博物馆后，把寻路经验整理成了一套完整装备。VIO领域也有几套“明星装备”，其中最著名的是香港科技大学开源的VINS-Mono。

VINS-Mono是一套完整的单目视觉惯性系统，它的流程像一条流水线：前端提取并跟踪特征点；中端对IMU做预积分；然后通过严格初始化恢复尺度、重力方向；之后进入滑动窗口优化——只维护最近若干帧的状态，把旧的边缘化掉；最后用库位检测实现闭环，修正整个历史轨迹。它是目前工业界和学术界引用最多的VIO方案之一。

另一套经典是OKVIS，它率先把视觉和惯性统一放进基于关键帧的图优化框架。与VINS-Mono偏重工程完整性相比，OKVIS的强项在于多相机-IMU的紧耦合建模。此外，ORB-SLAM3在视觉SLAM基础上也加入了IMU因子，成为多传感器融合的新标杆。

这些算法的核心代价函数通常写成残差和的形式：视觉残差负责“地标投影得对不对”，惯性残差负责“IMU运动和状态一不一致”，两者被加权求和后交给非线性优化器迭代求解。

这些主流算法的差异本质上是工程取舍：窗口多大、边缘化怎么做、闭环怎么触发、初始化怎么激励。对小明的寻路背包来说，VINS-Mono是一套功能齐全的商用装备，OKVIS是更轻更精致的定制工具，而ORB-SLAM3则把视觉、惯性、闭环打包成了全家桶。

误差与鲁棒性：当手电筒熄灭时

任何寻路系统都会遇到坏天气。小明的博物馆之旅最危险的路段是白墙走廊：手电筒照上去，墙面一片光滑，没有任何画框和柱脚可认；同时他跑得太快时，手电筒画面还会拖影模糊。这些场景统称为视觉退化。

在视觉退化的路段，VIO只能依赖IMU推演。由于IMU零偏和噪声的存在，纯惯性位置估计的误差会随时间二次增长——也就是说，走得越久，不确定性的雪球滚得越快。这解释了动画里紫色光圈的膨胀现象。

另一个工程难点是初始化。VIO在启动时必须估计重力方向、初始速度和IMU零偏，这些量在设备静止或匀速直线运动时是不可观测的。所以VINS-Mono要求用户在开始时让设备做几次明显的转动和加减速——小明出发前总要原地跺两下、转个圈，让自己建立“一米有多长、地心在哪”的尺度感。如果初始化激励不足，后续所有优化都将在错误的尺度上运行，且很难自愈。

工程实践：标定、同步与调参

小明的装备再先进，也有一件必须做的事：出发前把头盔上的手电筒固定好，并对好手表。在VIO工程中，这件事叫相机-IMU联合标定和时间戳同步。

手电筒固定在头盔的哪个角度、离眼睛多远，对应外参；镜头焦距、主点位置，对应内参。如果内外参不准，手电筒照到的地标会被投影到错误的位置。而时间同步问题更隐蔽：相机曝光时刻和IMU采样时刻必须对齐到同一个时钟。试想，手电筒看到的画面是0.1秒前的，内耳报告的却是现在的运动，大脑就会产生幻觉——两路信息根本对不上。

标定之后，VIO还依赖一个核心的几何模型：相机投影模型。它把地图中的3D点经过外参变换到相机坐标系，再通过内参投影到像素平面。所有“残差”都建立在这个模型之上，模型错了，融合就错。

实践中的调参还包括：滑动窗口大小（记忆多长）、IMU与视觉噪声协方差（信任谁更多）、特征点数量上限（计算量与精度平衡）。这些参数没有统一魔值，需要针对具体平台调试，就像小明要根据自己的步幅、手电筒亮度调整走路节奏一样。

应用与趋势：从无人机到口袋

VIO的价值，在于它让机器在没有GPS的室内或遮挡环境中依然保有“位置感”。无人机在桥洞下穿行时GPS失效，手机在室内商场里没有卫星信号，停车机器人在昏暗地库里导航——这些场景都是VIO的主场。

在手机AR中，VIO是核心引擎。你用手机测量桌子的长度、把虚拟家具“摆”进客厅，靠的正是摄像头看见纹理特征、IMU感知手机晃动，两者融合出毫米级的位置追踪，不需要任何外部基站。在无人机上，VIO让机器能在GPS拒止的桥底稳定悬停、穿越；在自动驾驶的地库场景里，VIO为车辆提供高频运动先验，与轮速计、地图匹配互相校验。

VIO的未来正在与深度学习深度融合。传统特征点提取可以被可学习的特征网络替代，端到端的深度VIO甚至可以直接从图像序列回归出姿态变化。这类方法用大规模数据训练网络，损失函数通常是最朴素的轨迹误差累积：

从黑暗博物馆的小明，到未来能自行学习寻路的智能体，VIO的故事始终没有变：用可靠的物理模型守住下限，用数据与算法逼近上限。手电筒和内耳的配合，正在走进每一台无人机、每一部手机、每一辆自动驾驶汽车。

AI 应用场景

Python 演示

import numpy as np def simulate():    dt = 0.05    N = 1000    t = np.arange(N) * dt     # 真实运动：前4秒匀速走，后4秒停下，交替进行    true_vel = np.zeros(N)    for k in range(N):        if int(t[k] // 4) % 2 == 0:            true_vel[k] = 0.8    true_acc = np.diff(true_vel, prepend=0.0) / dt    true_pos = np.cumsum(true_vel) * dt     # 传感器测量    bias = 0.2    acc_noise = 0.15    obs_noise = 0.08    acc_meas = true_acc + bias + np.random.normal(0, acc_noise, N)    pos_obs = np.full(N, np.nan)    for k in range(0, N, 20):        pos_obs[k] = true_pos[k] + np.random.normal(0, obs_noise)     # 纯IMU积分（只预测不更新）    imu_only = np.zeros(N)    v = 0.0    for k in range(N):        v += acc_meas[k] * dt        imu_only[k] = imu_only[k - 1] + v * dt if k > 0 else v * dt     # VIO融合：一维卡尔曼滤波    class OneDimVio:        def __init__(self):            self.state = np.zeros(2)   # [位置, 速度]            self.cov = np.eye(2) * 0.5         def predict(self, acc, dt):            F = np.array([[1.0, dt], [0.0, 1.0]])            G = np.array([[0.5 * dt * dt], [dt]])            self.state = F @ self.state + G.ravel() * (acc - bias)  # 在线补偿零偏            Q = np.array([[0.001, 0.0], [0.0, 0.02]])            self.cov = F @ self.cov @ F.T + Q         def update(self, z, r=0.08):            H = np.array([[1.0, 0.0]])            y = z - H @ self.state            S = H @ self.cov @ H.T + r            K = self.cov @ H.T / S            self.state = self.state + K.ravel() * y            self.cov = (np.eye(2) - K @ H) @ self.cov     vio = OneDimVio()    vio_pos = np.zeros(N)    for k in range(N):        vio.predict(acc_meas[k], dt)        if not np.isnan(pos_obs[k]):            vio.update(pos_obs[k])        vio_pos[k] = vio.state[0]     rmse_imu = np.sqrt(np.mean((imu_only - true_pos) ** 2))    rmse_vio = np.sqrt(np.mean((vio_pos - true_pos) ** 2))    print("纯IMU积分 RMSE: %.4f m" % rmse_imu)    print("VIO融合   RMSE: %.4f m" % rmse_vio) if __name__ == "__main__":    np.random.seed(42)    simulate()

1. true_vel[k] = 0.8定义真实的走停运动模式：走4秒、停4秒。
2. acc_meas = true_acc + bias + np.random.normal(...)生成带零偏和噪声的IMU读数，这是传感器模型的体现。
3. v += acc_meas[k] * dt纯IMU方案直接积分加速度，零偏会在这里被无限放大。
4. G = np.array([[0.5 * dt * dt], [dt]])控制输入矩阵：把加速度转成对位置和速度的影响。
5. self.state = self.state + K.ravel() * y卡尔曼更新：用视觉观测残差修正预测状态。

常见误区

面试题