2. 坐标系与刚体运动：世界坐标系、机体坐标系、相机坐标系，四元数与欧拉角
各位同学，欢迎来到第二章。这一章的内容，说白了就是整个视觉SLAM与飞控融合的“地基”。你想想看，无人机在天上飞，它怎么知道自己在哪？怎么知道自己在朝哪个方向转？这些问题的答案，都藏在坐标系和刚体运动里。
我个人习惯，在开始任何融合算法之前，先把坐标系理清楚。否则后面算出来的位置和姿态，全是错的。我在项目中遇到过好几次，就是因为坐标系定义不一致，导致无人机在空中乱飘，差点炸机。嗯，这里要特别注意。
2.1 三大核心坐标系
做自主飞行，你至少得跟三个坐标系打交道。它们分别是：世界坐标系、机体坐标系、相机坐标系。我习惯把它们比作“地图”、“自己”和“眼睛”。
- 世界坐标系 (W)：也叫全局坐标系。它是固定不动的，用来描述无人机在真实世界中的位置。通常我们取起飞点为原点，x轴指向北或东，z轴指向天。
- 机体坐标系 (B)：固定在无人机上。原点在无人机重心，x轴指向机头，y轴指向右翼，z轴指向下（符合右手定则）。飞控里的加速度计、陀螺仪，测量的都是这个坐标系下的数据。
- 相机坐标系 (C)：固定在相机上。原点在相机光心，z轴指向相机前方（拍摄方向），x轴向右，y轴向下。视觉SLAM算法算出来的特征点位置，都是相对于这个坐标系的。
核心要点：所有传感器数据，最终都要统一到同一个坐标系下才能使用。飞控用机体坐标系，SLAM用相机坐标系，而路径规划必须用世界坐标系。
为什么会这样？因为每个传感器都有自己的“视角”。你想想看，IMU（惯性测量单元）装在飞机上，它只知道飞机自己在怎么动，不知道飞机在世界的哪个位置。而相机只能看到前方的东西，不知道机头朝哪。所以，我们必须通过数学变换，把它们串起来。
2.2 刚体运动与坐标变换
无人机在空中飞，就是一个典型的刚体运动。刚体运动包含两部分：旋转和平移。旋转描述的是“朝向”，平移描述的是“位置”。
从世界坐标系到机体坐标系的变换，可以写成：
P_B = R * P_W + t其中，R是旋转矩阵，t是平移向量。P_W是点在世界坐标系下的坐标，P_B是同一个点在机体坐标系下的坐标。
我记得刚开始做飞控的时候，总搞混R和t的方向。后来我总结了一个口诀：“从哪到哪，矩阵在前”。比如从世界到机体，R和t就是把世界坐标“搬”到机体坐标的变换。
个人经验：在实际代码中，我建议把所有变换都写成齐次坐标形式。也就是用一个4x4的变换矩阵T，把旋转和平移打包在一起。这样写代码更简洁，也不容易出错。
2.3 旋转的表示方法：欧拉角与四元数
旋转是刚体运动里最让人头疼的部分。表示旋转的方法有很多，但最常用的就是欧拉角和四元数。我敢说，90%的无人机事故，都跟这两种表示方法的混用有关。
2.3.1 欧拉角
欧拉角用三个角度来描述旋转：偏航角 (Yaw)、俯仰角 (Pitch)、横滚角 (Roll)。通俗点说，就是“摇头”、“点头”和“歪头”。
| 角度 | 轴 | 描述 |
|---|---|---|
| Yaw (ψ) | z轴 | 机头绕z轴旋转，决定朝向 |
| Pitch (θ) | y轴 | 机头绕y轴旋转，决定抬头低头 |
| Roll (φ) | x轴 | 机身绕x轴旋转，决定左右倾斜 |
欧拉角的好处是直观，一看就懂。但它的缺点也很致命：万向锁 (Gimbal Lock)。当俯仰角达到±90度时，偏航和横滚会失去一个自由度，导致姿态解算出错。
避坑指南：我曾经在PX4飞控里调试一个姿态控制算法，用了欧拉角做插值。结果飞机在做大机动时，姿态突然跳变，差点失控。后来我查了三天，才发现是万向锁的问题。从那以后，我在飞控内部全部改用四元数。
2.3.2 四元数
四元数是一个超复数，用一个实部和三个虚部表示旋转。它的形式是：
q = w + xi + yj + zk其中，w是实部，x、y、z是虚部。四元数没有万向锁问题，而且插值平滑，非常适合用在飞控和SLAM里。
四元数和欧拉角可以互相转换。比如从四元数到欧拉角：
roll = atan2(2*(w*x + y*z), 1 - 2*(x^2 + y^2))
pitch = asin(2*(w*y - z*x))
yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))嗯，这个公式看起来有点复杂，但别怕。实际开发中，我们直接用Eigen或ROS里的现成函数就行。我建议你手写一遍，加深理解。
我的建议：在飞控里，姿态控制用四元数；在SLAM里，位姿优化用四元数；只有在给用户显示或者做日志记录时，才转成欧拉角。这样既避免了万向锁，又保留了直观性。
2.4 坐标系与旋转的SVG结构图
下面这张图，是我自己总结的坐标系与旋转知识体系。你可以把它当作本章的“地图”。
2.5 实战中的坐标系对齐
理论说完了，咱们聊聊实战。在视觉SLAM与飞控融合时，最常遇到的一个问题就是：相机坐标系和机体坐标系不重合。
相机通常装在机头下方，或者云台上。它和机体之间有一个固定的旋转和平移。这个变换叫做外参 (Extrinsics)。你必须事先标定好这个外参，否则SLAM算出来的位置，映射到机体上就是错的。
我记得有一次，我在一个四旋翼上装了一个前视相机。我忘了标定相机和机体的相对角度，结果SLAM输出的位置和飞控的GPS位置差了半米。飞机一飞就偏，后来我重新标定了一次，问题就解决了。
标定小技巧：你可以用棋盘格或者ArUco码，让无人机在不同姿态下拍摄，然后通过PnP算法解算出相机相对于机体的外参。这个步骤，我建议每次换相机或者换安装位置后都做一次。
2.6 本章小结
这一章的内容，说白了就是三件事：
- 搞清楚世界、机体、相机三个坐标系的关系
- 理解刚体运动就是旋转加平移
- 掌握欧拉角和四元数的使用场景和转换方法
嗯，这些概念看起来简单，但实际用起来很容易出错。我建议你动手写一个小程序，把四元数和欧拉角互相转换一下，再验证一下万向锁的问题。只有亲手踩过坑，才能真正记住。
好了，坐标系和刚体运动就讲到这里。下一章我们会进入更核心的内容——IMU数据预处理与视觉特征提取。到时候，这些坐标系的知识会反复用到。