2. 预备知识
在无人机视觉惯性里程计（VIO）中，所有状态估计问题最终都归结为“在哪个坐标系下、用什么姿态描述、如何对姿态求导”。如果这三件事没有理清，后续的 IMU 预积分、视觉重投影误差、滑窗优化都会变成一堆无法解释的公式。本节把 VIO 工程中最常用的三维几何工具串成一条线：坐标系变换 → 旋转表示 → 李群李代数。
一、坐标系与刚体变换：VIO 的“空间语言”
VIO 系统通常涉及四个坐标系：世界系 W、机体系 B、相机系 C、IMU 系 I。世界系是惯性参考，机体系随无人机运动，相机系和 IMU 系通过外参固定安装在机体上。一个空间点 p 从相机系到世界系的变换可以写成：
p_W = T_WB · T_BC · p_C
T = [ R t ]
[ 0 1 ]其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。齐次变换矩阵 T 把旋转和平移统一在一个 4×4 矩阵中，使连续变换可以写成矩阵连乘。工程上要特别注意：T_AB 表示“把 B 系下的点变换到 A 系”，而不是“从 A 到 B”。这个方向约定一旦写反，标定和优化结果会整体错位。
T_WB 就是把 B 系表达变换到 W 系表达。二、旋转的三种表示：欧拉角、旋转矩阵、四元数
旋转矩阵 R 是最直观的表示：列向量分别对应机体轴在世界系中的方向，满足 R^T R = I、det(R)=1。它没有奇异点，但用 9 个参数表达 3 个自由度，存在冗余，优化时不能直接对 9 个数做无约束更新。
欧拉角用三个角（如 roll、pitch、yaw）描述姿态，物理意义清晰，适合人机交互和调试输出。但它存在万向锁：当 pitch 接近 ±90° 时，roll 和 yaw 耦合，失去一个自由度。因此欧拉角适合“看”，不适合“算”。
四元数用 4 个参数 q = [w, x, y, z] 表示旋转，单位四元数满足 ||q||=1。它没有万向锁，插值平滑，是 VIO 内部最常用的姿态存储形式。四元数乘法对应旋转复合，但要注意左乘和右乘的区别：左乘对应世界系下的旋转，右乘对应机体系下的旋转。
| 表示 | 参数个数 | 优点 | 缺点 | VIO 中的典型用途 |
|---|---|---|---|---|
| 欧拉角 | 3 | 直观、易读 | 万向锁、非线性强 | 日志输出、调试显示 |
| 旋转矩阵 | 9 | 无奇异、变换直接 | 冗余、需正交约束 | 外参标定、坐标变换 |
| 四元数 | 4 | 无奇异、插值平滑 | 有单位约束、符号二义 | 状态存储、姿态传播 |
| 旋转向量/李代数 | 3 | 无冗余、可求导 | 需指数/对数映射 | 优化更新、误差状态 |
三、李群与李代数：为什么优化离不开它
旋转矩阵集合 SO(3) 和刚体变换集合 SE(3) 是李群：它们对乘法封闭，但不对加法封闭。也就是说，两个旋转矩阵相加不再是一个旋转矩阵。因此不能像优化普通向量那样直接做 R + ΔR。
解决办法是引入李代数 so(3) 和 se(3)。李代数是一个向量空间，可以对它做加法。通过指数映射 Exp，把李代数中的小量映射回李群；通过对数映射 Log，把李群元素映射回李代数。于是姿态更新写成：
R_new = R · Exp(δθ) // 右乘扰动，机体系
R_new = Exp(δθ) · R // 左乘扰动，世界系
T_new = T · Exp(δξ) // SE(3) 上的更新在 VIO 的滑动窗口优化中，待优化变量通常是 δθ 或 δξ，而不是旋转矩阵本身。这样既保证了更新后的 R 仍然正交，又让雅可比矩阵可以正常求导。理解“在切空间上做加法，再映射回流形”是读懂 VIO 后端代码的关键。
四、工程案例：从一帧视觉观测到状态更新
假设无人机在某一时刻，相机观测到路标点 p_C，IMU 给出角速度和加速度。VIO 前端的处理链路如下：
- 用外参
T_CI把路标点从相机系变换到 IMU 系：p_I = T_CI · p_C。 - 用当前位姿估计
T_WI把点变换到世界系：p_W = T_WI · p_I。 - 计算重投影误差，对位姿
T_WI求雅可比。 - 把位姿更新量写成李代数扰动
δξ，在切空间求解增量方程。 - 用
T_WI ← T_WI · Exp(δξ)更新位姿，进入下一轮迭代。
这条链路里，T_CI 是标定得到的常量，T_WI 是待优化状态，Exp(δξ) 是李代数到李群的映射。三者缺一不可。
五、复盘与检查清单
- 变换方向是否写对？
T_AB是把 B 系点变到 A 系，不是反过来。 - 旋转矩阵是否保持正交？优化后要检查
R^T R ≈ I。 - 四元数是否单位化？长时间传播后要重新归一化。
- 扰动是左乘还是右乘？世界系扰动左乘，机体系扰动右乘，二者雅可比不同。
- 欧拉角是否接近万向锁？pitch 接近 ±90° 时避免用欧拉角做优化变量。
把坐标系、旋转表示和李群李代数这三层关系理顺，后续的 IMU 预积分、视觉重投影、滑窗边缘化才有统一的数学基础。VIO 工程中大量“看起来是代码 bug”的问题，根源往往就在这几个基础约定上。