2. 坐标系与姿态表示

在视觉惯性组合导航中，几乎所有误差最终都会以“坐标系不一致”或“姿态表示不统一”的形式暴露出来。同一架飞行器，IMU 输出的是机体系下的角速度和比力，相机观测到的是相机系下的特征点方向，而导航解算需要的是导航系下的位置、速度和姿态。如果坐标系定义含糊，或者旋转矩阵、四元数、欧拉角之间转换时符号约定不统一，就会出现“数据都对、结果却发散”的典型问题。因此，本节先把坐标系和姿态表示这条链路讲清楚。

一、导航中常用的四个坐标系

飞行器视觉惯性系统通常涉及以下坐标系，它们各自承担不同职责：

• 惯性系（i 系）：原点在地心，坐标轴指向恒星，不随地球自转。它只用于理论推导和重力模型，实际组合导航很少直接使用。
• 地球系（e 系）：原点在地心，随地球自转，常用于描述经纬高和地球自转补偿。
• 导航系（n 系）：原点在飞行器所在位置，常用“东北天（ENU）”或“北东地（NED）”定义。本文统一采用 ENU：x 轴指东、y 轴指北、z 轴指天。导航解算输出的位置、速度、姿态都在 n 系下表达。
• 机体系（b 系）：原点在飞行器质心，x 轴指机头、y 轴指右翼、z 轴指机腹下方（前右下）。IMU 的陀螺和加速度计固连于 b 系。

视觉系统还引入相机系（c 系）：通常 z 轴沿光轴指向前方，x 轴向右，y 轴向下。相机与 IMU 之间的安装关系由外参 R_bc 和 t_bc 描述，这是视觉惯性标定的核心输出之一。

二、旋转矩阵：最直观也最容易出错

旋转矩阵 R 是 3×3 正交矩阵，满足 RᵀR = I、det(R) = +1。它把一个向量从源坐标系旋转到目标坐标系。例如 v_n = R_nb · v_b 表示把机体系下的向量 v_b 转换到导航系。注意下标读法：R_nb 是“从 b 到 n”。

旋转矩阵的优点是可直接与向量相乘、可级联（R_nc = R_nb · R_bc），缺点是 9 个参数只有 3 个自由度，数值积分后需要重新正交化，否则会漂移。在视觉惯性系统中，它常用于表达外参、重力对齐和局部地图到导航系的变换。

三、四元数：姿态递推的主力

单位四元数 q = [qw, qx, qy, qz] 用 4 个参数表示旋转，无奇异点，递推效率高，是 IMU 姿态积分和 VIO 后端优化的首选。四元数乘法对应旋转复合，其微分方程为：

q̇ = 0.5 * q ⊗ [0, ω_b]

其中 ω_b 是机体系下的角速度。离散实现时常用一阶或中值积分：

// 示例：一阶四元数积分（伪代码）
// q: 当前姿态四元数, w: 机体系角速度, dt: 采样间隔
dq = quat_from_omega(w * dt);
q_next = quat_normalize(quat_multiply(q, dq));

四元数与旋转矩阵的转换关系为：

R(q) =
[1-2(qy²+qz²)   2(qx qy - qw qz)   2(qx qz + qw qy)]
[2(qx qy + qw qz) 1-2(qx²+qz²)     2(qy qz - qw qx)]
[2(qx qz - qw qy) 2(qy qz + qw qx) 1-2(qx²+qy²)  ]

在 VIO 中，优化变量通常就是四元数，但为了求导方便，常使用“误差状态”形式：在标称四元数上左乘或右乘一个小角度扰动 δθ，从而把 4 维约束优化转化为 3 维无约束优化。

四、欧拉角：便于人理解，不便于递推

欧拉角用三个角度描述姿态，常见的有“偏航-俯仰-横滚（yaw-pitch-roll）”。在 ENU 导航系下，若采用 Z-Y-X 旋转顺序（先绕 z 转 yaw，再绕 y 转 pitch，最后绕 x 转 roll），则：

• 偏航角 yaw：机头相对东向的夹角，向右为正；
• 俯仰角 pitch：机头相对水平面的夹角，抬头为正；
• 横滚角 roll：绕机头轴的滚转，右滚为正。

欧拉角直观，但存在万向锁：当 pitch 接近 ±90° 时，yaw 和 roll 耦合，无法唯一确定。因此它适合显示和人工检查，不适合作为滤波器的内部状态。实际系统中，内部用四元数或旋转矩阵递推，仅在输出时转换为欧拉角。

五、转换关系与视觉惯性系统中的实际链路

视觉惯性系统的典型姿态链路可以概括为：IMU 在 b 系测量角速度 → 四元数积分得到 q_nb → 转换为 R_nb → 与相机外参 R_bc 级联得到 R_nc = R_nb · R_bc → 将相机观测到的特征方向投影到导航系，与 IMU 预积分结果对齐。反过来，当需要显示或控制时，再把 q_nb 转成欧拉角。

下面这张图给出了四个坐标系、三种姿态表示以及它们之间的转换关系，是本节最需要建立的整体图景。

六、一个可复现的转换示例

假设某时刻 IMU 积分得到机体系到导航系的四元数 q_nb = [0.9914, 0.0175, 0.0872, 0.0872]（已归一化），相机外参为 R_bc，表示相机系到机体系的旋转。要得到相机在导航系下的姿态，按以下顺序计算：

1. 由四元数构造 R_nb，代入上面的矩阵公式；
2. 读取标定文件中的 R_bc，例如绕 y 轴 -90° 的安装角；
3. 计算 R_nc = R_nb · R_bc；
4. 若需要显示，再将 R_nb 转为欧拉角：yaw = atan2(R(1,0), R(0,0))，pitch = asin(-R(2,0))，roll = atan2(R(2,1), R(2,2))（ENU 与 Z-Y-X 约定下）。

判断转换是否正确，可以用两个简单检查：一是把导航系下的重力矢量 [0,0,-9.81] 用 R_nbᵀ 转到机体系，静止时加速度计输出应接近 [0,0,+9.81]（前右下机体系，z 向下）；二是把同一特征点在相机系和导航系下的方向做点积，夹角应与几何关系一致。若符号相反，多半是旋转方向或下标顺序写反了。

掌握这四个坐标系和三种姿态表示之间的转换，是后续 IMU 预积分、视觉重投影误差构建以及外参在线估计的基础。实际工程中，建议把坐标系定义、旋转顺序、四元数分量顺序（w 在前还是后）写成一份简短的约定文档，并在代码中统一封装转换函数，避免在多个模块中重复实现导致不一致。