4. 视觉导航基础
在无GPS环境下，视觉惯性导航系统要回答的第一个问题是：图像中的像素点，如何与真实世界中的三维点建立对应关系？这个问题的答案，构成了整个视觉导航的数学地基。本节围绕相机模型、内参外参、畸变校正、特征点与光流五个核心概念展开，它们是后续视觉里程计、SLAM与VIO算法的共同语言。
一、针孔相机模型：从三维到二维的投影
最常用的相机成像模型是针孔模型。它假设光线通过一个理想的小孔，在成像平面上形成倒立的像。设世界坐标系下一点 P = (X, Y, Z)，经过外参变换到相机坐标系，再经内参投影到像素平面，得到像素坐标 (u, v)。其核心公式为：
u = fx * (Xc / Zc) + cx
v = fy * (Yc / Zc) + cy
其中：
fx, fy —— 焦距（像素单位）
cx, cy —— 主点（光轴与像平面交点）
Xc,Yc,Zc —— 相机坐标系下的三维点写成矩阵形式即 p = K · [R|t] · P，其中 K 为内参矩阵，[R|t] 为外参矩阵。这个公式是视觉导航中所有几何计算的起点。
二、内参与外参：相机的“身份证”与“位置姿态”
内参描述相机自身的成像特性，与相机在何处无关，包括焦距 fx, fy、主点 cx, cy，以及畸变系数。内参一旦标定完成，通常固定不变。外参描述相机相对于世界坐标系（或机体坐标系）的位置和姿态，即旋转矩阵 R 与平移向量 t。在VIO中，相机与IMU之间的外参（相对旋转与平移）是必须精确标定的关键参数，它直接决定视觉与惯性信息能否正确融合。
三、畸变校正：让直线重新变直
真实镜头并非理想针孔，广角镜头尤其明显。畸变主要分为径向畸变（桶形或枕形）和切向畸变（镜头与成像面不平行）。径向畸变用 k1, k2, k3 建模，切向畸变用 p1, p2 建模。校正过程是先归一化坐标，再按畸变模型反向补偿：
x_dist = x(1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2)
y_dist = y(1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y
其中 r^2 = x^2 + y^2在无人机视觉导航中，畸变校正通常在图像进入特征提取之前完成，否则特征点位置会系统性偏移，导致位姿估计出现漂移。常用工具如OpenCV的 undistort() 或 initUndistortRectifyMap() 可完成这一步骤。
四、特征点与光流：视觉信息的两种提取方式
特征点是图像中具有显著性的局部区域，如角点、边缘交点。经典算法包括Harris、FAST、ORB、SIFT等。特征点法通过“提取—描述—匹配”三步建立帧间对应关系，精度高但计算量较大。光流则利用亮度恒定假设，直接估计像素在连续帧间的运动矢量，代表算法有Lucas-Kanade稀疏光流和Farneback稠密光流。光流法速度快、适合实时VIO，但在大运动或光照突变时容易失效。
五、案例场景：无人机室内悬停中的视觉处理
假设一架四旋翼在无GPS的室内环境中悬停。机载单目相机以30fps采集图像，IMU以200Hz输出角速度和加速度。处理流程如下：
- 读取相机内参文件，对每帧图像执行畸变校正；
- 在上一帧中提取FAST角点，用LK光流跟踪到当前帧；
- 剔除跟踪失败或外点，保留稳定特征对；
- 结合IMU预积分结果，估计相机位姿变化；
- 将视觉观测与惯性观测送入滤波器或优化器，输出无人机位置与姿态。
在这个过程中，内参错误会导致特征点位置整体偏移；外参错误会导致视觉与惯性信息“各说各话”；畸变未校正会使边缘区域特征匹配失败；光流跟踪窗口过大或过小都会影响实时性与鲁棒性。
六、常见误区与复盘要点
| 误区 | 表现 | 后果 | 纠正方法 |
|---|---|---|---|
| 内参用默认值 | 未标定直接使用理论焦距 | 投影误差大，位姿漂移 | 用棋盘格或AprilTag标定 |
| 忽略畸变校正 | 边缘特征点位置偏移 | 匹配失败，轨迹弯曲 | 先校正再提取特征 |
| 外参未标定 | 相机与IMU相对姿态未知 | 融合发散，尺度错误 | 使用Kalibr等工具联合标定 |
| 光流窗口过大 | 跟踪点跨越运动边界 | 跟踪失败，外点增多 | 根据运动速度自适应调整 |
| 特征点分布集中 | 所有点集中在纹理丰富区 | 位姿估计退化 | 网格化均匀提取特征 |
掌握相机模型、内参外参、畸变校正、特征点与光流这五个基础概念，就具备了理解视觉惯性导航前端处理的能力。后续的视觉里程计、滑动窗口优化、边缘化等算法，都是在这个几何与观测模型之上逐步构建起来的。