在前面的章节中，我们已经建立了姿态的数学描述体系，完成了初始对准，并掌握了基于陀螺仪的姿态递推方法。然而，单纯的陀螺积分存在一个根本性的问题：漂移。无论陀螺仪的精度多高，其测量误差（包括零偏、噪声和温漂）经过积分后都会随时间累积，导致横滚角和俯仰角逐渐偏离真实值。
为了抑制这种漂移，必须引入一个具有长期稳定性 的参考量对陀螺积分结果进行周期性修正。在无人机飞行的大多数时间里，加速度计感知的重力方向就是这样一个理想的"锚点"。本章将聚焦于此，系统介绍 PX4 attitude_estimator_q 模块如何利用重力矢量观测构造误差、修正姿态，并深入剖析其中蕴含的工程思想。
5.1 互补滤波的基本思想
5.1.1 为什么需要互补滤波
各类传感器的频率特性呈现互补关系：陀螺仪高频响应好但存在低频漂移；加速度计高频噪声大，但在静止或匀速时其均值稳定指向重力反方向。互补滤波的核心思想可以概括为：用陀螺仪的短期精度主导姿态的动态响应，用加速度计的长期稳定性修正陀螺仪的低频漂移。
从频域角度看，互补滤波相当于设计了一个高通滤波器处理陀螺数据、一个低通滤波器处理加速度计数据，然后将两者叠加。在时域中，这一思想简化为一个直观的反馈结构：以陀螺积分为预测，以加速度计为观测，计算预测与观测之间的误差，并反馈修正预测。
5.1.2 Mahony 互补滤波的数学框架
Mahony 互补滤波的经典结构包含四个环节：
- 预测：利用陀螺仪角速度对当前姿态进行积分；
- 观测：通过加速度计获取重力方向观测值；
- 误差构造：将预测姿态下的理论重力方向与实际观测方向做对比，得到误差向量；
- 反馈修正：将误差向量转换为角速度修正量，叠加到陀螺仪读数上，再重新积分。
attitude_estimator_q 虽然没有显式地写出比例-积分（PI）控制器，但其 corr 的构造和 _gyro_bias 的更新本质上就是 Mahony 框架的工程化变体。
5.2 重力矢量观测的理论基础
5.2.1 已知参考量：导航系中的重力
在导航系（NED）中，重力方向是严格已知的单位向量：
gn=[001]T \mathbf{g}^n = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix}^T gn=[001]T
如果我们已知从机体系到导航系的旋转矩阵 Cbn\mathbf{C}_b^nCbn，那么导航系中的重力向量 gn\mathbf{g}^ngn 变换到机体系中应为：
gb=(Cbn)Tgn=Cnb[001] \mathbf{g}^b = \left( \mathbf{C}_b^n \right)^T \mathbf{g}^n = \mathbf{C}_n^b \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} gb=(Cbn)Tgn=Cnb 001
由于 Cbn\mathbf{C}_b^nCbn 是正交矩阵，其逆等于转置，所以 Cnb=(Cbn)T\mathbf{C}_n^b = \left( \mathbf{C}_b^n \right)^TCnb=(Cbn)T。上式取出的正是 Cnb\mathbf{C}_n^bCnb 的第三列 ------也就是 PX4 中 C_b^n 的第三行 。它表示：导航系 Z 轴（Down）在机体系中的单位投影向量。
设这一向量为 k\mathbf{k}k，则：
k=第 3 行(Cbn)=[C31C32C33]T \mathbf{k} = \text{第 3 行}(\mathbf{C}b^n) = \begin{bmatrix} C{31} & C_{32} & C_{33} \end{bmatrix}^T k=第 3 行(Cbn)=[C31C32C33]T
在 PX4 代码中，这个提取被手工展开为直接从四元数元素计算，避免构造完整的 3×33 \times 33×3 矩阵：
cpp
// 等价于取 C_b^n 的第三行（即导航系 Z 轴在机体系中的投影）
Vector3f k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
);这三行代码就是第 2 章中 DCM 元素与四元数关系的显式展开。展开的依据是：
Cbn=[⋯⋯⋯⋯⋯⋯2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32] \mathbf{C}_b^n = \begin{bmatrix} \cdots & \cdots & \cdots \\ \cdots & \cdots & \cdots \\ 2(q_1q_3-q_0q_2) & 2(q_0q_1+q_2q_3) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} Cbn= ⋯⋯2(q1q3−q0q2)⋯⋯2(q0q1+q2q3)⋯⋯q02−q12−q22+q32
第三行正是上面代码中的三个分量。
5.2.2 加速度计的观测向量
加速度计测量的是比力（Specific Force） ，定义为载体加速度减去重力加速度：
f=amotion−g \mathbf{f} = \mathbf{a}_{motion} - \mathbf{g} f=amotion−g
在静止或匀速飞行时，载体加速度为零，加速度计输出即为重力加速度的反方向：
f=−gb \mathbf{f} = -\mathbf{g}^b f=−gb
因此，归一化后的加速度计读数 (_accel - _pos_acc).normalized() 就是机体系中重力反方向的单位观测向量。其中 _pos_acc 是可选的 GPS 速度差分运动加速度补偿，用于扣除飞行器的非引力加速度。
5.3 误差向量的构造：为什么是 k×a\mathbf{k} \times \mathbf{a}k×a？
5.3.1 叉乘的方向与大小
当姿态估计存在误差时，理论预测的 k\mathbf{k}k（导航系 Down 在机体系中的投影）与加速度计观测到的重力反方向 a\mathbf{a}a 之间会有一个夹角 θ\thetaθ。Mahony 算法选择用两者的叉乘来构造误差：
e=k×a \mathbf{e} = \mathbf{k} \times \mathbf{a} e=k×a
这个选择有两层确定的几何意义：
方向 ：根据右手定则，e\mathbf{e}e 指向使 k\mathbf{k}k 向 a\mathbf{a}a 旋转的最短路径的转轴方向。
大小 ：由于两者都是单位向量，∣e∣=sinθ≈θ|\mathbf{e}| = \sin\theta \approx \theta∣e∣=sinθ≈θ（小角度下），即误差角的大小。
5.3.2 为什么是 k×a\mathbf{k} \times \mathbf{a}k×a，而不是 a×k\mathbf{a} \times \mathbf{k}a×k？
叉乘的反交换律告诉我们 k×a=−a×k\mathbf{k} \times \mathbf{a} = -\mathbf{a} \times \mathbf{k}k×a=−a×k。那么为什么 Mahony 算法选择 k×a\mathbf{k} \times \mathbf{a}k×a？
关键在于误差的定义方式 ：我们定义的姿态误差是**"理论值偏离观测值的角度"。理论值 k\mathbf{k}k 需要向观测值 a\mathbf{a}a 旋转才能消除误差。根据右手定则，k×a\mathbf{k} \times \mathbf{a}k×a 给出的正是 将 k\mathbf{k}k 转向 a\mathbf{a}a 所需的旋转轴**。如果写成 a×k\mathbf{a} \times \mathbf{k}a×k，则代表的是将 a\mathbf{a}a 转向 k\mathbf{k}k 的旋转轴------这与我们想要的姿态修正方向相反。
这个方向选择与姿态的右乘更新天然对应。下面用一个具体例子说明。
5.3.3 与右乘修正的联系：一个具体例子
假设当前姿态估计为 q\mathbf{q}q，机体系中有一个向量 v\mathbf{v}v。将其变换到导航系的正确操作是：
vn=q⊗[0v]⊗q−1 \mathbf{v}^n = \mathbf{q} \otimes \begin{bmatrix} 0 \\ \mathbf{v} \end{bmatrix} \otimes \mathbf{q}^{-1} vn=q⊗[0v]⊗q−1
如果 q\mathbf{q}q 存在误差，那么 vn\mathbf{v}^nvn 就会偏离真实方向。为了修正这个误差，我们需要在机体系内 对 v\mathbf{v}v 做一个微小旋转，然后再用 q\mathbf{q}q 转到导航系。
设这个微小修正旋转为 δq\delta\mathbf{q}δq（对应机体系下的微小角增量），则修正后的姿态为：
qnew=q⊗δq \mathbf{q}_{new} = \mathbf{q} \otimes \delta\mathbf{q} qnew=q⊗δq
这就是右乘 。它的物理意义是：先对机体系向量做微小修正旋转，再用原姿态将其转到导航系。
验证一下：对于同一个机体系向量 v\mathbf{v}v：
vnewn=qnew⊗[0v]⊗qnew−1=q⊗δq⊗[0v]⊗δq−1⊗q−1 \mathbf{v}{new}^n = \mathbf{q}{new} \otimes \begin{bmatrix} 0 \\ \mathbf{v} \end{bmatrix} \otimes \mathbf{q}_{new}^{-1} = \mathbf{q} \otimes \delta\mathbf{q} \otimes \begin{bmatrix} 0 \\ \mathbf{v} \end{bmatrix} \otimes \delta\mathbf{q}^{-1} \otimes \mathbf{q}^{-1} vnewn=qnew⊗[0v]⊗qnew−1=q⊗δq⊗[0v]⊗δq−1⊗q−1
可以看到，v\mathbf{v}v 先被 δq\delta\mathbf{q}δq 在机体系内旋转了一下，然后再被 q\mathbf{q}q 转到导航系。这正是我们需要的：在传感器原生坐标系（机体系）内完成误差修正。
而 k×a\mathbf{k} \times \mathbf{a}k×a 给出的误差向量 e\mathbf{e}e，正是构造这个 δq\delta\mathbf{q}δq 所需要的旋转轴角。将其乘以增益和步长后，对应到四元数更新就是：
q(t+Δt)≈q(t)⊗[1ωcorrΔt/2] \mathbf{q}(t + \Delta t) \approx \mathbf{q}(t) \otimes \begin{bmatrix} 1 \\ \boldsymbol{\omega}_{corr} \Delta t / 2 \end{bmatrix} q(t+Δt)≈q(t)⊗[1ωcorrΔt/2]
即 _q += _q.derivative1(corr) * dt 的右乘本质。
5.3.4 从误差向量到角速度修正
得到误差向量 e\mathbf{e}e 后，将其乘以比例增益，叠加到陀螺仪读数上：
ωcorr=ωgyro+Kp⋅e \boldsymbol{\omega}{corr} = \boldsymbol{\omega}{gyro} + K_p \cdot \mathbf{e} ωcorr=ωgyro+Kp⋅e
在 PX4 代码中：
cpp
corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();其中 _param_att_w_acc.get() 就是比例增益 KpK_pKp（参数 ATT_W_ACC），% 是叉乘运算符。这一行将姿态误差直接转换为了机体系下的角速度修正量，随后通过右乘更新到姿态中。
5.4 attitude_estimator_q 的工程实现与改进
5.4.1 对经典 Mahony 的简化
经典的 Mahony 互补滤波显式地包含一个 PI 控制器：
ωcorr=ωgyro+Kpe+Ki∫e dt \boldsymbol{\omega}{corr} = \boldsymbol{\omega}{gyro} + K_p \mathbf{e} + K_i \int \mathbf{e} \, dt ωcorr=ωgyro+Kpe+Ki∫edt
PX4 的 attitude_estimator_q 做了一个重要的工程简化：没有显式的 PI 分离结构 。它把比例修正和积分修正统一到同一个 corr 向量中：
- 比例项 ：
corr += (k % a) * w_acc------ 直接根据当前姿态误差产生修正角速度； - 积分项（零偏估计） ：
_gyro_bias += corr * w_gyro_bias * dt------ 对corr做低通积分来估计陀螺零偏。
这种简化的好处是代码结构极其紧凑，比例和积分耦合在同一个反馈回路中。
5.4.2 陀螺零偏估计：对偏置微分模型的积分
陀螺零偏本质上是陀螺仪输出中的一个缓慢变化的系统性误差。PX4 将其建模为一个微分方程：
b˙=Ki⋅e \dot{\mathbf{b}} = K_i \cdot \mathbf{e} b˙=Ki⋅e
其中 e\mathbf{e}e 是姿态误差（由 corr 代理），KiK_iKi 是积分增益。在离散时间中，这等价于对零偏做数值积分：
cpp
if (spinRate < 0.175f) { // 约 10°/s
_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);
}这里的 _gyro_bias += ... * dt 正是对陀螺仪偏置微分模型进行前向欧拉积分 。当姿态收敛后，corr 趋于零，积分器自然停止更新，零偏估计达到稳态。
经典 Mahony 的积分项会持续运行，但 PX4 增加了关键保护：
- 低速门限：只在近似静止或低速运动时更新零偏，避免动态加速度污染估计；
- 硬限幅 ：
_bias_max将零偏限制在物理合理范围内。
5.4.3 加速度计融合的鲁棒性设计
经典 Mahony 通常假设飞行器始终处于静止状态。PX4 针对实际飞行场景增加了模长门限：
cpp
if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
(accel_norm_sq < upper_accel_limit * upper_accel_limit))) {
corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();
}当不使用 GPS 补偿时，只有加速度计模长在 [0.9g,1.1g][0.9g, 1.1g][0.9g,1.1g] 范围内，才认为测量值可信。剧烈机动时自动关闭重力融合，防止姿态被拉偏。
5.4.4 GPS 运动加速度补偿
PX4 进一步引入了 _pos_acc------由 GPS 速度差分得到的机体运动加速度。将其从加速度计中扣除后，可以更纯净地提取重力方向：
cpp
corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();这在水平加速或转弯时显著降低了姿态漂移。
5.4.5 闭环反馈链的完整图景
将以上环节串联，一个完整周期的姿态校正流程如下：
cpp
// Step 1: 构造总修正角速度
_rates = _gyro + _gyro_bias; // 原始陀螺 + 估计零偏
corr += _rates; // 将修正角速度加入 corr
// Step 2: 右乘更新姿态（在机体系内叠加微小修正后转到导航系）
_q += _q.derivative1(corr) * dt;
_q.normalize();corr 已包含：加速度计重力观测误差 →\rightarrow→ 横滚/俯仰修正；陀螺仪原始读数 + 零偏补偿 →\rightarrow→ 基础角速度。四元数对 corr 积分后，新的姿态预测更贴近真实值，形成持续收敛的负反馈闭环。
5.5 与 EKF2 修正机制的区别
attitude_estimator_q 的互补滤波结构简洁直观，适合理解姿态校正的基本原理。EKF2 则采用了更严密的统计框架。
| 特性 | attitude_estimator_q（互补滤波） | EKF2（扩展卡尔曼滤波） |
|---|---|---|
| 误差计算 | 直接叉乘 k×a\mathbf{k} \times \mathbf{a}k×a | 通过观测方程计算新息（Innovation） |
| 修正方式 | 误差直接加权叠加到角速度 | 通过卡尔曼增益 和协方差矩阵加权更新 |
| 零偏估计 | 对 corr 低通积分（偏置微分模型的欧拉积分） |
完整的 EKF 状态估计，含协方差传播 |
| 动态适应 | 模长门限 + 低速零偏保护 | 自动协方差自适应 |
| 多源融合 | 各传感器独立计算误差后叠加 | 统一的状态空间框架，统计最优融合 |
在 EKF2 中，重力方向不再通过简单叉乘直接修正姿态，而是作为伪观测 进入扩展卡尔曼滤波框架，计算预测与测量之间的新息向量，再通过卡尔曼增益确定最优修正量。这种方法在线性最小方差意义下最优，但代价是更高的计算复杂度。
5.6 本章小结
姿态校正是姿态估计从"开环积分"走向"闭环融合"的关键环节。本章系统介绍了 PX4 attitude_estimator_q 模块中互补滤波的核心实现：
- 重力观测的本质 ：导航系 Down 轴通过 Cbn\mathbf{C}_b^nCbn 的第三行投影到机体系得到理论方向 k\mathbf{k}k，再与加速度计观测方向做叉乘获得误差旋转轴；
- 叉乘方向的物理意义 ：k×a\mathbf{k} \times \mathbf{a}k×a 表示"理论值向观测值旋转"的轴和角，与右乘更新天然匹配------误差在机体系内修正后，通过右乘复合到当前姿态；
- 工程改进：PX4 在经典 Mahony 基础上简化了 PI 结构，增加了模长门限、GPS 运动补偿、低速零偏保护和对偏置微分模型的积分限幅，使其从理论原型蜕变为可靠的飞行级实现。
掌握了重力观测与互补滤波的闭环逻辑，就为理解下一章------如何利用磁力计等第二参考矢量完成偏航校正------奠定了直接的工程基础。
关于我们：
灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。
实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。