3、姿态控制核心算法：四元数基础、欧拉角与旋转矩阵、PID控制器原理、角速度环与角度环
姿态控制，说白了就是让飞行器知道「自己朝哪」以及「该怎么转」。
我刚开始接触PX4时，觉得这部分就是一堆数学公式堆砌。后来踩过几次坑才明白——不理解这些基础，调参就是瞎蒙。今天咱们把这几个核心概念拆开揉碎，讲清楚。
3.1 旋转的三种数学语言
描述一个物体在三维空间里的朝向，有三种主流方式。它们各有优缺点，PX4里全用上了。
3.1.1 欧拉角：直观但危险
欧拉角就是俯仰（Pitch）、横滚（Roll）、偏航（Yaw）。
优点很明显：一眼就能看懂。你看到Pitch=30°，就知道机头抬起来了。
但有个致命问题——万向锁。当Pitch接近±90°时，Roll和Yaw会耦合，丢失一个自由度。我在做固定翼垂直起降过渡时遇到过这问题，飞机在90°俯仰附近姿态解算直接炸了。
3.1.2 旋转矩阵：通用但笨重
旋转矩阵是一个3×3的正交矩阵，能把一个向量从一个坐标系转到另一个坐标系。
它没有万向锁问题，但缺点也很明显：9个参数，计算量大，而且必须保证正交性。每次更新后还得重新正交化，否则误差会累积。
PX4里主要在坐标变换时用旋转矩阵，比如把机体坐标系下的加速度转换到世界坐标系。但姿态控制核心环路上，它太沉了。
3.1.3 四元数：轻量且优雅
四元数只有4个参数，没有奇点，计算效率高。它是PX4姿态控制的核心数据结构。
一个四元数可以写成：
q = w + xi + yj + zk其中w是实部，x、y、z是虚部。它本质上描述了一个绕某个轴旋转一定角度的变换。
我个人习惯把四元数想象成「旋转轴+旋转角」的紧凑表示。你不需要记住所有运算规则，但必须理解两个操作：
- 四元数乘法：对应旋转的复合。先转A再转B，等于q_B * q_A。
- 四元数共轭：对应逆旋转。
3.2 三种表示方式的对比
| 特性 | 欧拉角 | 旋转矩阵 | 四元数 |
|---|---|---|---|
| 参数数量 | 3 | 9 | 4 |
| 奇点问题 | 有（万向锁） | 无 | 无 |
| 计算效率 | 高 | 低 | 高 |
| 直观性 | 高 | 低 | 低 |
| 插值平滑性 | 差 | 一般 | 好（球面插值） |
| PX4使用场景 | 日志/遥控 | 坐标变换 | 核心姿态 |
3.3 PID控制器原理
PID是姿态控制的心脏。不管算法多花哨，最后落到执行器上的，基本都是PID的输出。
公式很简单：
u(t) = Kp * e(t) + Ki * ∫e(t)dt + Kd * de(t)/dt但实际工程里，每个环节都有坑。
3.3.1 比例项：响应速度的担当
Kp越大，响应越快。但太大就会震荡。我调过一架大四轴，Kp给到0.15时手感刚好，再大0.02就开始高频抖动。
3.3.2 积分项：消除静差
积分项能消除稳态误差。比如飞机重心偏了，比例项拉不回来，积分项慢慢累积就能补上。
但积分项也是万恶之源——积分饱和。当执行器已经饱和（比如油门100%），积分还在累积，等误差反向时，积分需要先退回来，造成超调。
3.3.3 微分项：抑制超调
微分项能预测误差变化趋势，提前施加反向控制。但它对噪声极其敏感。
你想想看，如果传感器噪声大，微分项会把噪声放大，导致电机高频抖动。我曾在室内用光流+IMU做悬停，微分项没加滤波，飞机抖得像筛子。
3.4 角速度环与角度环
PX4的姿态控制采用串级PID结构。外环是角度环，内环是角速度环。
为什么这么设计？
- 角速度环：响应快，直接对抗外部扰动（比如阵风）。它控制的是「转得多快」。
- 角度环：响应慢，负责跟踪目标姿态。它控制的是「转到哪」。
串级的好处是：内环可以先把扰动吃掉，外环就不用那么累。我做过对比测试，单环PID在阵风下角度偏差5°，串级PID只有1.5°。
3.4.1 角速度环的实现
角速度环的输入是目标角速度（来自角度环输出）和实际角速度（来自陀螺仪）。输出是期望力矩，最终映射到电机油门。
PX4里角速度环的PID参数通常这样调：
- 先调Kp，让响应快但不震荡
- 加Kd抑制超调
- 最后加Ki消除静差
3.4.2 角度环的实现
角度环的输入是目标角度和实际角度（来自姿态估计）。输出是目标角速度，送给内环。
角度环一般只用P，甚至P+少量D。因为内环已经处理了大部分动态，外环不需要太激进。
3.5 核心逻辑框架
下面这张图展示了PX4姿态控制的核心数据流。我建议你仔细看，它把今天讲的所有概念串起来了。
3.6 实践中的几个坑
讲完理论，说几个我实际踩过的坑。
3.6.1 四元数归一化
四元数必须保持单位长度。每次更新后都要归一化，否则误差会累积。我见过有人忘了这步，飞了3分钟后姿态直接漂移了20°。
3.6.2 角速度限幅
角度环输出的目标角速度一定要限幅。如果不限，内环会收到一个根本追不上的目标，导致积分饱和。我一般把最大角速度限制在200°/s~300°/s。
3.6.3 积分项初始化
解锁时积分项一定要清零。否则上次飞行的积分残留会让飞机一解锁就猛转。这个坑我栽过一次，飞机差点砸到人。
3.7 小结
这一章内容不少，但核心就三句话：
- 四元数是PX4姿态控制的基石，理解它比背公式重要
- 串级PID结构让内环先处理扰动，外环只管跟踪
- 调参顺序永远是先内后外，先P后ID
嗯，把这些基础打牢了，后面讲具体调参和部署时，你才能知道每一步在干什么。