企业官网建设流程全解析

1. 姿态解算输出结果

1.1 欧拉角（Euler Angle）

就是大家最容易理解的：三次依次绕坐标轴旋转的三个角度

• Roll（横滚）：绕 X 轴，左右倾斜
• Pitch（俯仰）：绕 Y 轴，前后抬头低头
• Yaw（航向）：绕 Z 轴，车头朝哪个方向（偏航）

优点：直观，人看得懂，调试的时候直接看角度，单位是度

致命缺点：万向锁（Gimbal Lock）当其中一个角度转到 ±90° 附近时，另外两个旋转轴会重合，丢失一个自由度，没法正常旋转，计算会崩。 另外欧拉角做旋转叠加、坐标变换，计算量大，还有角度跳变（比如 359°→0° 跳变）。

举例：飞机抬头到 90 度朝上，航向和横滚轴就叠在一起，就是万向锁。

还有一个坑：旋转顺序很重要，ZYX、XYZ 结果完全不一样，AHRS 里常用 Z-Y-X（先航向、再俯仰、再横滚）。

1.2 四元数（Quaternion）

形式：q = [q_0, q_1, q_2, q_3]

几何含义：任意一个三维旋转，等价于：绕某一根固定轴，转一个角度四元数就是把【旋转轴 + 旋转角度】打包成 4 个数。

q_0 和旋转角度有关；q_1,q_2,q_3代表旋转轴方向。

优点：

1. 没有万向锁（最大优势）
2. 旋转叠加只用乘法，计算速度快，适合单片机实时解算
3. 插值平滑，不会出现角度跳变

缺点：不直观，你没法一眼看懂 q0~q3 对应车头朝哪边，必须转换成欧拉角给人看

2. 姿态解算算法

2.1 一阶互补滤波

2.2 Mahony 非线性互补滤波器

优点：

1. 四元数运算，无万向锁；
2. 算力很低，STM32、TC264、甚至 F1 都轻松跑；
3. 积分项 K_i 对陀螺温漂、静态零偏抑制效果很好，这是它碾压 Madgwick 的关键点；
4. 原生支持 6 轴 / 9 轴切换，9 轴可以稳住 Yaw 航向。

缺点：

1. 不区分传感器噪声大小，没有概率模型；
2. 加速度存在运动分量（急加速、刹车）时，误差向量错误，姿态会被带偏（原理局限）；
3. \(K_i\) 调大容易震荡、超调。

适合场景

智能车两轮直立组第一选择（比赛主流）；电赛平衡类作品；

6 轴直立车、9 轴带航向控制车。

2.3 Madgwick非线性互补滤波器

原理（向量观测 + 梯度下降优化）

同样是向量观测 AHRS，输入输出和 Mahony 一致，底层优化思路不一样：

1. 陀螺积分得到姿态预测；
2. 构造代价函数：当前姿态下观测向量（重力、地磁）与理想向量的误差；
3. 使用梯度下降法迭代，沿着误差梯度方向更新四元数，最小化代价；
4. 参数只有一个：\(\beta\)（梯度步长），没有积分项。

优点：

1. 参数只有一个，调参简单；
2. 数学上是优化框架，对重力 + 地磁联合观测一致性好；
3. 四元数，无万向锁；6/9 轴都支持。

缺点：

1. 没有积分项，不能专门估计陀螺零偏，静态长期漂移抑制弱于 Mahony；
2. 梯度迭代有额外运算，计算量略高于 Mahony；
3. 同样无法区分重力和运动加速度，加速时姿态失真。

适合场景

无人机、航模；智能直立车用的少于 Mahony；电赛飞行器类项目更常见。

重点区分 Mahony / Madgwick： Mahony：修正角速度（PI 补偿陀螺 bias）；

Madgwick：直接迭代修正四元数本身，不单独补偿陀螺偏置。

2.4 扩展卡尔曼滤波 EKF

原理（概率、最小方差估计）

卡尔曼家族核心思想：状态是随机变量，同时维护状态 + 状态不确定性（协方差矩阵 P）两步循环：

1. 预测步：状态传播（陀螺积分），协方差变大（不确定性增加）；
2. 更新步：拿到加速度 / 地磁观测，根据「过程噪声 Q、观测噪声 R」，计算卡尔曼增益，修正姿态、协方差。

状态向量可以扩展：姿态四元数 + 陀螺零偏 + 加速度偏置，同时估计多个误差源。

优点：

1. 理论是线性最小方差估计；可以人为设置 Q/R，表达 “我有多相信陀螺、多相信加速度”；
2. 可以在线估计陀螺 bias，鲁棒性上限更高；
3. 框架通用，后续很容易扩展 GPS、视觉、激光融合（组合导航）。

缺点：

1. 计算量大：矩阵乘法、矩阵求逆；对弱 MCU 压力大；
2. 调参地狱：Q、R 矩阵很难调，调不好直接发散、姿态飞掉；
3. EKF 是一阶泰勒近似，强非线性场景有线性化误差；
4. 四元数 EKF 还要额外处理四元数归一化约束，代码复杂很多。

适合场景

电赛惯性导航、室内定位、带 GPS 组合导航作品；普通直立智能车基本不使用 EKF—— 性价比太低，Mahony 足够稳，代码简单很多。

延伸：UKF 无迹卡尔曼，不用雅可比矩阵，处理非线性更好，但算力和调参难度更高，竞赛极少。