2. 坐标系与姿态表示
在无人机VIO（Visual-Inertial Odometry）惯导融合定位中，所有测量、估计与优化都建立在统一的坐标描述之上。相机看到的是像素，IMU感受到的是比力与角速度，而我们要输出的是无人机在世界中的位姿。若坐标系定义混乱、姿态表示不统一，融合结果必然漂移甚至发散。本章围绕世界、机体、相机、IMU四类坐标系，以及旋转矩阵、四元数、欧拉角、李群李代数与坐标变换方法展开。
一、四类核心坐标系及其物理含义
VIO系统通常涉及四个坐标系，它们各自承担不同的观测与表达职责：
- 世界坐标系 W：也称全局坐标系，通常取初始时刻的机体位置为原点，Z轴指向重力反方向（或按ENU/NED约定）。它是定位输出的参考基准，要求短期稳定、长期可扩展。
- 机体坐标系 B：固连于无人机机身，原点在质心或IMU中心，X轴指向前，Y轴指向右，Z轴指向下（前右下约定）。它描述无人机自身的姿态与运动。
- 相机坐标系 C：原点在相机光心，Z轴沿光轴指向前方，X轴向右，Y轴向下。视觉特征点先在该系下表达，再投影到像素平面。
- IMU坐标系 I：原点在IMU敏感中心，轴向由器件安装决定。IMU的角速度和加速度均在该系下测量，是预积分与状态传播的输入。
四者之间通过外参（旋转与平移）建立联系。相机与IMU之间的外参 T_IC 是VIO标定的核心参数之一，其精度直接影响视觉与惯性信息对齐的质量。
二、姿态表示：旋转矩阵、四元数、欧拉角
姿态描述的是两个坐标系之间的相对旋转。常用表示方法各有优劣：
| 表示方法 | 参数个数 | 优点 | 缺点 | 典型用途 |
|---|---|---|---|---|
| 旋转矩阵 R | 9（含6约束） | 线性变换直观，可直接作用于向量 | 冗余、需正交约束、优化时易失正交性 | 坐标变换、投影计算 |
| 四元数 q | 4（单位模） | 无奇异、插值平滑、计算高效 | 物理直观性弱、双覆盖 | 姿态传播、滤波状态量 |
| 欧拉角 | 3 | 直观易读、便于人机交互 | 万向节死锁、旋转顺序敏感 | 显示、调试、控制指令 |
| 李代数 so(3) | 3 | 无约束、适合最小二乘优化 | 需指数/对数映射、概念抽象 | VIO后端优化、BA |
旋转矩阵满足 R^T R = I 且 det(R)=1，属于特殊正交群 SO(3)。四元数用单位向量加标量表示旋转，避免了欧拉角的奇异问题。欧拉角按不同旋转顺序（如ZYX、XYZ）得到不同结果，工程中必须明确约定顺序。
三、李群李代数基础与坐标变换
SO(3)与SE(3)是VIO中最重要的两个李群。SO(3)描述旋转，SE(3)描述刚体变换（旋转+平移）。李群是光滑流形，直接在其上做加法会破坏约束，因此引入李代数 so(3) 与 se(3)，它们是向量空间，可以自由求导与优化。
核心映射关系为指数映射与对数映射：
// 李代数到旋转矩阵（罗德里格斯公式）
R = exp(φ^) = I + sinθ/θ * φ^ + (1-cosθ)/θ² * (φ^)²
// 其中 φ 为旋转向量，θ = |φ|，φ^ 为反对称矩阵
// 旋转矩阵到李代数
φ = ln(R)^∨
// SE(3) 上的扰动模型（左乘）
T_perturbed = exp(δξ^) * T
在VIO后端优化中，位姿通常用SE(3)表示，残差对位姿的雅可比通过李代数扰动模型求解，这样既保持约束，又使优化变量维度最小（6维）。
四、VIO中的坐标变换流程与场景
以视觉惯性里程计为例，一次完整的坐标变换流程如下：
- IMU在I系下测量角速度ω和加速度a，通过预积分得到相邻关键帧间的相对位姿ΔT_ij。
- 相机在C系下提取特征点，通过外参T_IC将特征点转换到I系或B系。
- 利用世界系W下的位姿T_WB，将地图点投影到当前相机，计算重投影误差。
- 后端以SE(3)为状态量，联合优化IMU预积分残差与视觉重投影残差。
- 输出T_WB，即无人机在世界系下的位姿，供控制与导航使用。
典型场景：无人机室内飞行时，世界系原点设在起飞点，Z轴向上。相机前视，IMU与相机刚性连接。若外参T_IC标定偏差1°，在5米距离上会产生约8.7厘米的投影误差，足以导致特征匹配失败。因此外参在线估计或精细标定是VIO稳定运行的前提。
五、常见误区与复盘
- 坐标系约定不统一：相机Z向前、IMU Z向下、世界Z向上，若混用会导致重力方向错误，融合迅速发散。
- 欧拉角顺序混淆：ZYX与XYZ结果不同，调试时务必确认旋转顺序。
- 四元数未归一化：长时间积分后四元数模长漂移，需定期归一化。
- 李代数扰动方向错误：左乘与右乘对应不同雅可比，写错会导致优化不收敛。
- 外参忽略时间同步：即使空间外参正确，时间戳未对齐也会引入等效旋转误差。