2. 无人机导航基础
无人机要在三维空间中自主飞行，首先必须回答三个问题：“我在哪”“我朝哪”“我怎么动”。视觉里程计与航迹推算正是围绕这三个问题展开的。本节从坐标系、姿态表示、运动学模型、传感器分类与导航系统组成五个方面，搭建无人机导航的基础框架，为后续视觉里程计与多传感器融合打下概念地基。
一、坐标系定义：导航的“语言”
无人机导航中，同一个物理点在不同坐标系下有不同的数值表达。若坐标系定义不统一，后续的位姿估计、坐标变换和地图构建都会出现系统性错误。常用的坐标系包括：
- 世界坐标系（World/ENU）：以起飞点或局部原点为基准，东-北-天（ENU）或北-东-地（NED）方向固定，用于表达无人机全局位置与航迹。
- 机体坐标系（Body）：原点位于无人机质心，x 轴指向机头，y 轴指向右侧，z 轴指向下方（或上方，取决于约定），用于表达机载传感器测量值。
- 相机坐标系（Camera）：原点位于光心，z 轴沿光轴向前，x、y 轴与图像平面平行，是视觉里程计的核心坐标系。
- IMU 坐标系：与机体坐标系通常存在固定安装偏差，需通过外参标定确定。
坐标系之间的转换依靠旋转矩阵 R 与平移向量 t 描述。一个点从机体坐标系到世界坐标系的变换可写为：
p_world = R_wb * p_body + t_wb
其中 R_wb 为机体到世界的旋转矩阵，t_wb 为平移向量二、姿态表示：从欧拉角到四元数
姿态描述无人机相对于世界坐标系的朝向，常用三种表示方法：
| 表示方法 | 参数个数 | 优点 | 缺点 |
|---|---|---|---|
| 欧拉角（Roll/Pitch/Yaw） | 3 | 直观易理解，便于人机交互 | 存在万向节死锁，插值不连续 |
| 旋转矩阵 | 9 | 无奇异，便于坐标变换 | 冗余，需正交约束，数值易漂移 |
| 四元数 | 4 | 无死锁，插值平滑，计算高效 | 物理意义不直观，需归一化 |
在视觉里程计与航迹推算中，四元数是最常用的姿态表示，因为它在优化和滤波中能保持单位模约束，且避免万向节死锁。欧拉角通常只在显示和调试时使用。
三、运动学模型：描述“怎么动”
无人机运动学模型描述位置、速度、姿态与角速度之间的微分关系。对于常见的六自由度刚体模型，状态量包括位置 p、速度 v、姿态四元数 q 与角速度 ω。其连续时间运动学方程为：
p_dot = v
v_dot = R(q) * a_body + g
q_dot = 0.5 * q ⊗ [0, ω]
其中 a_body 为机体加速度，g 为重力向量，⊗ 为四元数乘法视觉里程计通过相机图像估计相邻帧之间的相对运动，本质上是在求解离散时间下的运动学约束；而 IMU 则直接测量角速度与比力，通过积分得到姿态与速度。两者结合，正是视觉惯性里程计（VIO）的核心思想。
四、传感器分类：导航系统的“感官”
无人机导航传感器可按测量对象分为以下几类：
- 惯性传感器：IMU（加速度计+陀螺仪），高频输出，短时精度高，但存在零偏与积分漂移。
- 视觉传感器：单目/双目/RGB-D 相机，提供丰富环境信息，可估计位姿与稀疏/稠密地图，但受光照、纹理和运动模糊影响。
- 测距传感器：超声波、激光雷达、气压计，提供高度或距离约束，辅助尺度恢复与高度保持。
- 全局定位传感器：GNSS、UWB、磁力计，提供绝对位置或航向，但在室内或遮挡环境下易失效。
五、导航系统组成：从传感器到状态估计
一个完整的无人机导航系统通常包含四个层次：感知层（传感器采集）、预处理层（时间同步、标定、去畸变）、状态估计层（滤波或优化）、输出层（位姿、速度、地图）。下图展示了各模块之间的数据流与信号关系：
六、案例与场景：室内悬停中的坐标系与姿态
假设无人机在室内执行定点悬停任务。IMU 以 200 Hz 输出角速度与加速度，相机以 30 Hz 输出图像。系统首先通过时间同步将两路数据对齐到同一时间戳；然后利用相机特征点估计相邻帧的相对旋转与平移，同时用 IMU 预积分提供高频姿态约束；最后在滑动窗口优化中联合估计位姿、速度与 IMU 零偏。若坐标系定义错误，例如将相机 z 轴方向弄反，则视觉估计的平移方向会整体反向，导致无人机“越飞越远”。
七、复盘与要点清单
- 坐标系定义必须统一，明确轴向与原点，并在代码中显式记录。
- 姿态表示优先使用四元数，欧拉角仅用于显示与调试。
- 运动学模型是视觉里程计与 IMU 融合的理论基础，理解微分关系有助于排查漂移问题。
- 传感器分类帮助选择互补组合：IMU 高频但漂移，相机低频但绝对，二者融合可扬长避短。
- 导航系统组成遵循“感知—预处理—状态估计—输出”的数据流，反馈校正保证长期稳定性。