第2章坐标系与姿态表示

坐标系与姿态表示：地理坐标系与机体坐标系

做飞控调参，第一个绕不开的坎就是坐标系。说白了，你得先搞清楚无人机在「谁的世界里」运动。

我个人习惯把地理坐标系想象成「大地妈妈的眼睛」。它固定在地面上，原点通常选在起飞点，X轴指向北，Y轴指向东，Z轴指向地心。这个坐标系是绝对的，你的GPS坐标、高度数据，全是在这个框架下算的。

而机体坐标系呢？它是「无人机自己的眼睛」。原点在飞机重心，X轴指向机头，Y轴指向右翼，Z轴指向机腹。你想想看，飞机在空中翻跟头时，它自己的前后左右一直在变，但大地坐标系纹丝不动。

嗯，这里要注意：所有传感器测量的原始数据，都是在机体坐标系下的。陀螺仪测的是绕机体三轴的角速度，加速度计测的是机体三轴上的加速度。但你要控制飞机平飞，得把这些数据转换到地理坐标系下去理解。

核心要点：控制器的误差计算在地理坐标系，执行器的输出在机体坐标系。中间缺的那个桥梁，就是姿态表示。

欧拉角是最直观的姿态表示法。我刚开始学飞控时，觉得这玩意儿太友好了——不就是航向、俯仰、横滚嘛。

但欧拉角有个大坑——万向锁。我在项目中遇到过一架固定翼，俯仰拉到90°时，偏航和横滚突然分不清了，飞机直接螺旋下坠。为什么会这样？因为当Pitch=90°时，Yaw和Roll的旋转轴重合了，丢失了一个自由度。

避坑指南：我曾经在PX4的EKF2配置里，因为欧拉角初始化顺序写错，导致飞机起飞就翻。记住：旋转顺序必须是Z-Y-X（偏航→俯仰→横滚），顺序错了，姿态全乱。

欧拉角有缺陷，那就上旋转矩阵。它用3×3的矩阵，把机体坐标系下的向量，映射到地理坐标系。

公式长这样（绕Z轴旋转ψ角为例）：

Rz(ψ) = [cosψ  -sinψ   0]
        [sinψ   cosψ   0]
        [0       0      1]

完整的旋转矩阵是三个基本旋转的乘积：R = Rz(ψ) · Ry(θ) · Rx(φ)。

我个人习惯在代码里直接用这个矩阵做向量变换。比如加速度计测到的机体加速度a_body，要转成地理加速度a_earth：

a_earth = R * a_body

就这么简单。但旋转矩阵有9个元素，每次更新都要算一堆三角函数，计算量不小。而且它容易受数值误差影响，时间长了矩阵会「漂移」，不再满足正交性。

小技巧：每次更新完旋转矩阵后，做一次正交化修正。我一般用Gram-Schmidt方法，几行代码就能搞定，能省掉后面很多麻烦。

四元数，说白了就是旋转矩阵的「轻量版」。它用4个数字表示旋转，没有万向锁，计算量小，插值平滑。

一个四元数写成：q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。约束条件是w² + x² + y² + z² = 1。

为什么用四元数？我举个例子。你在做航点飞行时，飞机要从当前姿态平滑转到目标姿态。用欧拉角插值，搞不好就绕大圈；用四元数插值（球面线性插值），路径最短最自然。

四元数乘法表示旋转叠加：

q_result = q2 * q1

注意顺序！先q1旋转，再q2旋转。顺序反了，姿态就错了。我刚开始写代码时，这个坑踩了不下三次。

四元数 vs 欧拉角 vs 旋转矩阵：

现在问题来了：陀螺仪测角速度，积分得角度，但积分会漂移。加速度计测角度，不漂移但噪声大。怎么办？把两者融合起来。

互补滤波的思路很简单：陀螺仪负责高频姿态，加速度计负责低频修正。

公式核心：

angle = α * (angle + gyro * dt) + (1-α) * acc_angle

其中α是权重系数，通常取0.98左右。α越大，越信任陀螺仪；α越小，越信任加速度计。

我个人习惯把α设成0.985，然后根据飞行状态动态调整。比如飞机在做剧烈机动时，加速度计受振动干扰大，我就把α调高到0.995，暂时更依赖陀螺仪。悬停时，再把α降回0.98。

注意：互补滤波只适用于小角度情况。如果飞机倒飞，加速度计没法区分重力加速度和运动加速度，这时候互补滤波会失效。我建议在倾斜超过45°时，切换到四元数+卡尔曼滤波的方案。

嗯，这里还要提一句：互补滤波的截止频率怎么设？我一般把截止频率设在0.5~2Hz之间。低于这个频率的，交给加速度计；高于这个频率的，交给陀螺仪。说白了，就是让陀螺仪管「快变」，加速度计管「慢变」。

下面这张图，是我自己总结的坐标系与姿态表示的核心逻辑。你照着这个思路去理解，调参时就不会迷路。

这张图把整个流程串起来了。你从传感器拿到数据，先搞清楚它在哪个坐标系下，然后选择合适的姿态表示方法，最后用互补滤波把数据融合好，输出给控制器。每一步都有坑，但每一步也都有成熟的解法。

我的建议：刚开始调参时，先用欧拉角理解姿态变化规律。等飞控稳定了，再切到四元数做高阶优化。互补滤波的α系数，从0.98开始调，根据飞行日志里的姿态抖动幅度微调。别一上来就搞卡尔曼，那个复杂度够你喝一壶的。

公众号:蓝海资料掘金营，微信deep3321