第二章：坐标系与刚体运动

各位同学，欢迎来到第二章。

上一章我们聊了VIO的整体框架，今天要啃的这块骨头，是整个定位系统的地基——坐标系与刚体运动。说白了，就是搞清楚「无人机在哪儿」、「摄像头朝哪儿看」、「飞机怎么转的」这三个问题。

我刚开始做飞控的时候，觉得坐标系这东西太简单了，不就是XYZ嘛。结果第一次实飞，IMU数据全乱套，飞机在天上画八字。后来才发现，是坐标系定义搞反了。嗯，从那以后，我再也不敢小看这一章了。

2.1 三个核心坐标系

在VIO系统里，我们打交道最多的就是三个坐标系。我习惯用W、B、C来简称它们。

2.1.1 世界坐标系 (World Frame)

世界坐标系，也叫全局坐标系。它是我们描述无人机位置的「绝对参考」。通常我们取起飞点作为原点，Z轴指向上（重力反方向），X轴指向北或东。

为什么这么定？因为IMU里的加速度计测的是重力方向，对齐Z轴能省很多事。我在项目里吃过亏——有一次把Z轴朝下了，结果高度估计直接反了，飞机越飞越低还以为是高度保持住了。你想想看，这要是真飞出去，后果不堪设想。

2.1.2 机体坐标系 (Body Frame)

机体坐标系固定在无人机上。原点在质心，X轴指向机头，Y轴指向右翼，Z轴指向下（符合右手定则）。

IMU测量的加速度和角速度，就是在机体坐标系下给出的。所以当我们说「飞机在加速」，其实是「机体坐标系下的加速度」。要换算到世界坐标系，就得做旋转。

2.1.3 相机坐标系 (Camera Frame)

相机坐标系的原点在相机光心，Z轴指向镜头前方（即拍摄方向），X轴向右，Y轴向下。

这里有个容易搞混的地方：相机坐标系的Z轴是朝前的，而机体坐标系的Z轴是朝下的。所以从机体到相机的旋转，不是简单的平移，还包含一个90度的旋转。

我曾经调试一个双目VIO，怎么都对不准。折腾了两天，最后发现是相机坐标系定义反了——我把Z轴朝后了。嗯，这种低级错误，犯过一次就记住了。

2.2 旋转的数学表示

好，坐标系定义清楚了。接下来是重头戏：怎么描述一个坐标系到另一个坐标系的旋转？

有三种主流方法：旋转矩阵、四元数、欧拉角。我个人最常用的是四元数，但咱们一个一个说。

2.2.1 旋转矩阵

旋转矩阵 R 是一个 3×3 的正交矩阵，满足 R^T R = I，det(R) = 1。它属于 SO(3) 群。

假设你在机体坐标系下有一个点 P_B，想转换到世界坐标系：

P_W = R_WB · P_B

这里的 R_WB 表示「从机体到世界的旋转」。

旋转矩阵的好处是直观，可以直接做矩阵乘法。但坏处也很明显——9个参数，6个约束，冗余。而且连续旋转时容易产生万向锁。

2.2.2 四元数

四元数是我在VIO里的首选。它用4个参数表示旋转：q = [w, x, y, z]，其中 w 是实部，x,y,z 是虚部。

单位四元数满足 w² + x² + y² + z² = 1。它没有奇点，插值方便，计算效率高。

用四元数旋转一个向量：

P_W = q ⊗ P_B ⊗ q*

其中 ⊗ 是四元数乘法，q* 是共轭四元数。

在实际代码里，我一般这样用：

// C++ 伪代码
Eigen::Quaterniond q_WB;  // 机体到世界的旋转
Eigen::Vector3d p_B;      // 机体坐标系下的点
Eigen::Vector3d p_W = q_WB * p_B;  // 重载了乘法运算符

2.2.3 欧拉角

欧拉角用三个角度表示旋转：滚转角(Roll)、俯仰角(Pitch)、偏航角(Yaw)。

虽然直观，但我不推荐在VIO里直接用欧拉角。原因有二：

• 万向锁：当俯仰角接近±90°时，滚转和偏航会耦合，丢失一个自由度
• 非线性：欧拉角的微分方程高度非线性，不适合做状态估计

我一般只在可视化调试时用欧拉角，比如在Rviz里显示飞机的姿态。内部计算一律用四元数。

2.3 坐标系变换实战

好了，理论说完了。咱们来点实际的。

在VIO系统里，最核心的变换就是：把相机观测到的特征点，转换到世界坐标系下。

整个链条是这样的：

1. 相机拍到特征点，得到像素坐标 (u, v)
2. 通过相机内参，反投影到相机坐标系下的归一化平面 (Xc, Yc, Zc)
3. 用外参 T_CB（相机到机体的变换），转到机体坐标系
4. 用状态估计出的 T_WB（机体到世界的变换），转到世界坐标系

写成公式就是：

P_W = T_WB · T_BC · P_C

其中 T 是 4×4 的齐次变换矩阵，包含旋转和平移。

我在实际项目中，会把所有变换矩阵都预先算好，存成Eigen::Isometry3d类型。这样每次只需要做一次矩阵乘法，效率高，也不容易出错。

2.4 常见陷阱与经验

最后，分享几个我踩过的坑：

• 坐标系定义不一致：不同传感器厂商用的坐标系可能不同。比如有的IMU把Z轴朝上，有的朝下。一定要仔细看手册。
• 四元数归一化：每次更新四元数后，记得归一化。否则误差会累积，导致旋转矩阵不正交。
• 左右手系混淆：OpenCV用的是右手系，ROS也是右手系。但有些数据集用的是左手系，转换时要小心。

嗯，坐标系和刚体运动这部分，看似基础，但真的是VIO的命根子。你想想看，如果坐标系都搞错了，后面再好的算法也是白搭。所以，我建议各位同学在写代码之前，先在纸上把坐标系画清楚，把变换链条写明白。这一步省不得。

---