4. 位置控制器的数学基础：回顾四元数、旋转矩阵、坐标系变换
各位同学，欢迎来到第四章。这一章咱们不写代码，但它是后面所有修改工作的地基。说白了，数学基础不牢，后面调参你会调得怀疑人生。我个人习惯是，每次改位置控制器代码之前，都会先在脑子里过一遍这几个数学工具，确保自己没搞混坐标系。
今天要聊的三个东西——四元数、旋转矩阵、坐标系变换——其实是同一件事的不同表达方式。它们都在描述同一个问题：怎么把一个向量从一个坐标系转到另一个坐标系。
核心思想： 无人机的位置控制，本质上就是在地球坐标系（NED）里算出一个期望的力，然后把这个力转换到机体坐标系下，让电机去执行。这个转换过程，全靠我们今天讲的数学工具。
4.1 坐标系：NED 与 机体坐标系
先搞清楚我们在哪两个坐标系之间来回折腾。
- NED坐标系（北东地）：这是地球固连坐标系。X轴指向北，Y轴指向东，Z轴指向地心。说白了，这就是我们常说的「世界坐标系」。PX4里所有导航数据、位置设定值，默认都在这个坐标系下。
- 机体坐标系（Body Frame）：这是跟着飞机走的坐标系。X轴指向机头，Y轴指向右翼，Z轴指向机腹。传感器（IMU、磁力计）测量的原始数据，都在机体坐标系下。
嗯，这里要注意：NED坐标系是右手系，机体坐标系也是右手系。但它们的Z轴方向不同——NED的Z轴向下，机体的Z轴也向下（因为飞机肚子朝下）。所以旋转顺序和角度定义要特别小心。
我的经验： 我在项目中遇到过好几次，因为搞混了NED和机体坐标系的定义，导致位置控制反了——飞机明明应该往前飞，结果它往后倒。后来我养成了一个习惯：每次写坐标变换代码之前，先在纸上画一下两个坐标系的轴方向，确认无误再动手。
4.2 旋转矩阵：最直观的坐标变换工具
旋转矩阵，说白了就是一个3x3的矩阵。它能把一个向量从一个坐标系旋转到另一个坐标系。比如，把机体坐标系下的加速度转换到NED坐标系下：
// 伪代码：旋转矩阵应用
Vector3f accel_body = imu.getAccel(); // 机体坐标系下的加速度
Vector3f accel_ned = R * accel_body; // 转换到NED坐标系
// 其中 R 是旋转矩阵，由欧拉角或四元数计算得到旋转矩阵有三个重要的性质：
- 正交性：R的逆等于R的转置。这意味着从NED转到机体，只需要转置一下矩阵就行。
- 行列式为1：纯旋转，不改变向量长度。
- 可组合：多个旋转可以连乘成一个矩阵。
你想想看，为什么PX4里不直接用欧拉角做坐标变换？因为欧拉角有万向锁问题。当俯仰角接近±90度时，偏航和滚转就分不清了。旋转矩阵没有这个问题，但它有9个元素，计算量稍大。
4.3 四元数：轻量级、无奇点的旋转表达
四元数，我刚开始学的时候觉得这东西很玄乎。其实它就是一个四维复数，用来表示三维空间的旋转。形式是：
q = w + x*i + y*j + z*k
其中 w 是实部，x,y,z 是虚部。
满足：w² + x² + y² + z² = 1四元数最大的好处是：没有万向锁，计算量小，插值平滑。PX4的姿态估计（EKF）内部用的就是四元数。你去看PX4源码里的 Quaternion 类，它封装了所有四元数运算。
关键公式： 用四元数旋转一个向量 v：
v' = q * v * q_conj
其中 q_conj 是 q 的共轭四元数。
v 要写成纯四元数形式 (0, vx, vy, vz)。我曾经在调试一个无人机时，发现姿态估计偶尔会跳变。排查了半天，发现是四元数归一化没做好。四元数如果不归一化，旋转出来的向量长度会变，导致位置控制出现偏差。所以记住：每次更新四元数后，一定要归一化。
4.4 四元数与旋转矩阵的互转
在实际代码里，我们经常需要在四元数和旋转矩阵之间来回转换。比如，EKF输出的是四元数，但位置控制器需要旋转矩阵来计算期望推力方向。
转换公式如下：
// 四元数 -> 旋转矩阵
// 假设 q = (w, x, y, z)
R = [
[1-2(y²+z²), 2(xy-wz), 2(xz+wy) ],
[2(xy+wz), 1-2(x²+z²), 2(yz-wx) ],
[2(xz-wy), 2(yz+wx), 1-2(x²+y²) ]
]
// 旋转矩阵 -> 四元数
// 从R的迹(trace)计算
t = trace(R)
if t > 0:
s = 0.5 / sqrt(t+1)
w = 0.25 / s
x = (R[2][1] - R[1][2]) * s
y = (R[0][2] - R[2][0]) * s
z = (R[1][0] - R[0][1]) * s
else:
// 处理特殊情况...避坑指南： 我曾经在从旋转矩阵转四元数时，忘记处理迹为负的情况，结果在特定姿态下四元数符号反了，导致位置控制器输出反向推力。嗯，从那以后我每次写这个转换都会加上所有分支判断。
4.5 知识体系结构图
下面这张图展示了本章的知识体系，以及它们之间的逻辑关系：
4.6 在PX4源码中的实际应用
说了这么多理论，我们看看在PX4位置控制器代码里，这些东西是怎么用的。打开 mc_pos_control_main.cpp，你会看到：
// 从姿态估计获取当前四元数
Quaternionf q_att = _attitude.quat;
// 将四元数转换为旋转矩阵
Matrix3f R = q_att.toRotationMatrix();
// 用旋转矩阵将期望加速度从NED转换到机体坐标系
Vector3f accel_sp_body = R.transpose() * accel_sp_ned;
// 计算期望推力（在机体坐标系下）
float thrust = accel_sp_body(2); // Z轴推力你看，实际代码就是这么简洁。但背后支撑它的，就是我们今天讲的这些数学基础。我个人建议，如果你要修改位置控制器，一定要先理解这段代码里的坐标变换逻辑。否则你改出来的代码，很可能在某个边界条件下出问题。
小技巧： 调试坐标变换时，可以打印出四元数和旋转矩阵的值，然后手动验证一下。比如，让飞机机头朝北，俯仰角为0，此时旋转矩阵应该近似为单位矩阵。如果不对，说明姿态估计或者坐标变换有问题。
好了，这一章的内容就到这里。数学基础虽然枯燥，但它是后面所有修改工作的基石。下一章我们会进入实战，看看怎么修改位置控制器的核心代码。