4. 姿态估计分析：从日志中提取姿态四元数、欧拉角，分析姿态解算的收敛性与震荡

姿态估计，说白了就是飞控怎么知道自己「脑袋朝哪」的。我见过不少开发者，日志里姿态数据哗哗地刷，但根本看不出问题。嗯，这一章我们就来聊聊，怎么从日志里把姿态四元数和欧拉角挖出来，然后判断解算到底稳不稳。

4.1 姿态数据在日志中的存储格式

飞控日志里，姿态数据通常以两种形式出现：四元数和欧拉角。我个人习惯先看四元数，因为它没有万向锁问题，而且能直接反映旋转的连续性。

4.2 提取与可视化：从原始数据到曲线

拿到日志后，我一般会写个Python脚本，把四元数和欧拉角提出来，然后画成时间序列图。下面是我常用的提取代码片段：

import pandas as pd
import numpy as np

# 假设日志已解析为DataFrame
log = pd.read_csv('flight_log.csv')

# 提取四元数
q = log[['ATT.Q0', 'ATT.Q1', 'ATT.Q2', 'ATT.Q3']].values

# 检查归一化
norm = np.sqrt(np.sum(q**2, axis=1))
print(f"四元数模长范围: {norm.min():.6f} ~ {norm.max():.6f}")

# 提取欧拉角
euler = log[['ATT.Roll', 'ATT.Pitch', 'ATT.Yaw']].values

画图的时候，我建议把四元数的四个分量和欧拉角的三个分量分开画。为什么？因为四元数分量之间是耦合的，放在一起看容易眼花。欧拉角则更直观，适合快速判断姿态是否收敛。

4.3 收敛性分析：解算到底稳不稳？

收敛性，说白了就是姿态解算能不能快速跟上真实姿态，并且稳定下来。我一般从两个维度看：静态收敛和动态收敛。

4.3.1 静态收敛

飞机放在地上不动，看姿态数据。理想情况下，Roll和Pitch应该在0°附近，Yaw保持不变。如果出现缓慢漂移，那多半是陀螺零偏没补偿好，或者加速度计权重太低。

• 漂移量：每分钟超过0.5°就算异常
• 噪声幅度：±0.2°以内算正常，超过±0.5°说明传感器噪声大或滤波参数太激进

4.3.2 动态收敛

飞机做机动动作后，看姿态能否快速回到稳态。我习惯用阶跃响应来分析：给一个俯仰阶跃指令，看实际俯仰角跟踪的延迟和超调。

4.4 震荡分析：揪出隐藏的抖动源

姿态震荡，是飞控调试中最头疼的问题之一。震荡可能来自传感器噪声、滤波参数、甚至机体结构共振。我一般按以下步骤排查：

1. 看频率：用FFT分析欧拉角或角速度的频谱。低频震荡（< 5Hz）多半是控制环问题；高频震荡（> 20Hz）可能是传感器噪声或结构共振。
2. 看相位：对比四元数和角速度的相位关系。如果两者相位差接近90°，说明是积分漂移导致的震荡。
3. 看幅值：震荡幅值是否随时间增大？如果是，那系统可能处于不稳定边界，需要立即调整。

下面是我常用的FFT分析代码：

from scipy.fft import fft, fftfreq
import numpy as np

# 假设采样频率为200Hz
fs = 200.0
roll = log['ATT.Roll'].values

N = len(roll)
yf = fft(roll - np.mean(roll))
xf = fftfreq(N, 1/fs)

# 只取正频率部分
idx = np.where(xf > 0)
freqs = xf[idx]
magnitude = 2.0/N * np.abs(yf[idx])

# 找到峰值频率
peak_idx = np.argmax(magnitude)
print(f"主震荡频率: {freqs[peak_idx]:.2f} Hz")

4.5 姿态估计核心流程

为了让你更直观地理解姿态估计的整个链路，我画了一张流程图。它展示了从传感器原始数据到最终姿态输出的关键步骤，以及我们分析日志时需要关注的节点。

4.6 实战案例：一次典型的震荡排查

我记得有一次，一个客户反馈飞机在悬停时Yaw轴有周期性摆动。我拿到日志后，先看了Yaw角的曲线——确实有±3°的震荡，频率大约2Hz。然后我看了角速度的频谱，发现2Hz处有个明显的尖峰。

接着我检查了四元数，发现Q[3]（对应Z轴旋转）的幅值变化与Yaw角完全同步。这说明震荡确实来自姿态解算，而不是外部扰动。进一步分析发现，是磁力计的Yaw修正增益设置得太高，导致解算在Yaw轴上产生了过冲。

把增益从0.1降到0.05后，震荡就消失了。嗯，有时候问题就是这么简单，但如果不从日志里一步步分析，你可能永远找不到根因。