PX4姿态解算技术详解（四）：姿态更新/递推与共锥补偿

在完成姿态初始化之后，姿态估计器需要根据陀螺仪提供的角运动信息，持续地推算姿态随时间的变化。这一过程称为姿态更新（Attitude Update）或姿态递推（Attitude Propagation），是姿态估计算法中最核心、执行频率最高的环节。

姿态更新的本质是一个积分问题 ：已知初始姿态和角速度，求解随时间演化的姿态轨迹。由于三维旋转的不可交换性（Non-commutativity） ，简单的标量积分方法会失效，必须采用符合旋转群 SO(3)\mathrm{SO}(3)SO(3) 结构的积分策略。本章将从数学原理出发，结合 attitude_estimator_q 和 EKF2 的源码实现，介绍 PX4 中的姿态递推方法。

4.1 姿态更新的数学基础

4.1.1 四元数运动学方程

设 qnb(t)\mathbf{q}_{nb}(t)qnb(t) 为从机体系 bbb 到导航系 nnn 旋转的单位四元数，机体坐标系下的角速度为 ωb(t)\boldsymbol{\omega}^b(t)ωb(t)，则四元数满足运动学方程：

q˙nb(t)=12qnb(t)⊗[0ωb(t)] \dot{\mathbf{q}}{nb}(t) = \frac{1}{2} \mathbf{q}{nb}(t) \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}^b(t) \end{bmatrix} q˙nb(t)=21qnb(t)⊗[0ωb(t)]

其中 ⊗\otimes⊗ 为 Hamilton 四元数乘积。在 PX4 的 matrix::Quaternion 类中，这一运算由 derivative1() 实现：

Matrix41 derivative1(const Matrix31 &w) const
{
    const Quaternion &q = *this;
    Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
    return q * v * Type(0.5);
}

4.1.2 一阶欧拉积分

在数字系统中，连续方程必须离散化。最简单的方法是一阶前向欧拉积分：

qk+1=qk+12qk⊗[0ωk]Δt \mathbf{q}_{k+1} = \mathbf{q}_k + \frac{1}{2} \mathbf{q}_k \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_k \end{bmatrix} \Delta t qk+1=qk+21qk⊗[0ωk]Δt

这正是 attitude_estimator_q 采用的基本形式：

_q += _q.derivative1(corr) * dt;
_q.normalize();

每次积分后调用 normalize() 强制四元数回到单位球面，以抵消数值误差导致的范数漂移。

4.1.3 一阶积分的局限：不可交换误差

一阶欧拉积分假设在一个控制周期 Δt\Delta tΔt 内角速度恒定。然而三维旋转具有不可交换性：绕不同轴旋转的顺序会影响最终姿态。

当飞行器经历圆锥运动（Coning Motion） ------即两个正交轴上存在同频率、有相位差的角速度振荡时，简单积分会产生沿第三轴的虚假漂移，称为圆锥误差（Coning Error）。对于高动态飞行器，必须通过更精细的积分算法或高采样率来抑制这一误差。

4.2 attitude_estimator_q 模块的姿态递推

attitude_estimator_q 的姿态递推逻辑集中在 update(float dt) 函数中（src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp）。它是一个基于互补滤波思想的简化估计器：利用陀螺仪递推姿态，同时利用加速度计、磁力计等构造误差修正量反馈到角速度上。

4.2.1 修正角速度的构造

每个周期，update() 首先计算总的角速度修正量 corr：

Vector3f corr;

// 1. 外部航向修正（视觉 / Mocap / 磁力计）
// 将航向误差投影到机体系 Z 轴方向
corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get();

// 2. 加速度计修正（重力方向对齐）
Vector3f k(...);  // 当前机体 Z 轴在导航系中的投影（由四元数优化计算）
corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();

// 3. 陀螺零偏估计
if (spinRate < 0.175f) {
    _gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);
}
_rates = _gyro + _gyro_bias;
corr += _rates;

修正后的总角速度包含了陀螺仪原始测量、零偏估计以及外部测量（磁/视觉/加速度计）的融合修正。

4.2.2 四元数积分与归一化

_q += _q.derivative1(corr) * dt;
_q.normalize();

if (!_q.isAllFinite()) {
    _q = q_last;
    _rates.zero();
    _gyro_bias.zero();
    return false;
}

_q.derivative1(corr) * dt 计算了在一个步长内的四元数变化量 Δq\Delta \mathbf{q}Δq，并直接加到当前四元数上。若结果异常（非有限值），则回退到上一周期有效状态。

4.2.3 方法特点

• 基于角速率：直接使用角速度（rad/s）进行一阶欧拉积分；
• 互补滤波结构：通过外部测量修正角速度误差，再反馈到积分中；
• 无地球自转补偿：低成本方案，忽略地球自转影响；
• 圆锥误差由底层 IMU 模块处理：本身不处理，但依赖的传感器数据已经过预处理。

4.3 不可交换误差与共锥补偿

4.3.1 不可交换性的数学根源

三维旋转群的不可交换性意味着：对于两个旋转 R1\mathbf{R}_1R1 和 R2\mathbf{R}_2R2，通常有 R1R2≠R2R1\mathbf{R}_1 \mathbf{R}_2 \neq \mathbf{R}_2 \mathbf{R}_1R1R2=R2R1。当用角速度 ω(t)\boldsymbol{\omega}(t)ω(t) 描述连续旋转时，总旋转不能简单地对各轴角速度分别积分再相加，而必须考虑积分路径上的顺序关系。

4.3.2 圆锥运动与圆锥误差

圆锥运动 是指刚体绕一个固定轴做圆锥摆式的周期性摆动。例如，X 轴和 Y 轴上同时存在同频、等幅但有 90°90°90° 相位差的角振动：

ωx=Acos⁡(Ωt),ωy=Asin⁡(Ωt) \omega_x = A \cos(\Omega t), \quad \omega_y = A \sin(\Omega t) ωx=Acos(Ωt),ωy=Asin(Ωt)

理论上这种运动的纯旋转轴始终位于 XY 平面内，不应产生 Z 轴方向的净旋转。但如果对 ωx\omega_xωx 和 ωy\omega_yωy 分别独立积分后再合成四元数，数值误差会导致沿 Z 轴的虚假常值漂移，即圆锥误差。

4.3.3 PX4 中的共锥补偿实现

在 PX4 中，圆锥补偿不在 attitude_estimator_q 或 EKF2 中直接实现，而是下沉到了传感器驱动层 ------src/modules/sensors/Integrator.hpp 中的 IntegratorConing 类：

class IntegratorConing : public Integrator
{
public:
    inline void put(const matrix::Vector3f &val, const float dt)
    {
        if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) {
            // 梯形积分得到 delta_alpha
            const matrix::Vector3f delta_alpha{integrate(val, dt)};

            // 计算圆锥修正项（基于二子样旋转矢量法）
            _beta += ((_last_alpha + _last_delta_alpha * (1.f / 6.f)) % delta_alpha) * 0.5f;
            _last_delta_alpha = delta_alpha;
            _last_alpha = _alpha;

            // 累加积分
            _alpha += delta_alpha;
        }
    }

    bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
    {
        if (Integrator::reset(integral, integral_dt)) {
            integral += _beta;  // 将圆锥修正项应用到角增量输出
            _beta.zero();
            return true;
        }
        return false;
    }

private:
    matrix::Vector3f _beta{0.f, 0.f, 0.f};  /**< 累积的圆锥修正量 */
};

这段代码实现的是一种基于旋转矢量的二阶递推算法 ，而非传统意义上只在单一输出周期内计算的"二子样"或"三子样"圆锥算法。它的特殊之处在于：在每个陀螺采样点都实时计算并累积修正量，从而在整个积分周期内持续抑制不可交换误差。

将代码中的叉乘公式展开：

β+=12(αk−1+16Δθk−1)×Δθk=12αk−1×Δθk⏟不可交换性修正+112Δθk−1×Δθk⏟圆锥误差修正 \boldsymbol{\beta} += \frac{1}{2} \left( \boldsymbol{\alpha}{k-1} + \frac{1}{6} \Delta \boldsymbol{\theta}{k-1} \right) \times \Delta \boldsymbol{\theta}k = \underbrace{\frac{1}{2} \boldsymbol{\alpha}{k-1} \times \Delta \boldsymbol{\theta}k}{\text{不可交换性修正}} + \underbrace{\frac{1}{12} \Delta \boldsymbol{\theta}_{k-1} \times \Delta \boldsymbol{\theta}k}{\text{圆锥误差修正}} β+=21(αk−1+61Δθk−1)×Δθk=不可交换性修正 21αk−1×Δθk+圆锥误差修正 121Δθk−1×Δθk

这里包含了两类物理意义不同的修正项：

• 不可交换性修正项 12αk−1×Δθk\frac{1}{2} \boldsymbol{\alpha}_{k-1} \times \Delta \boldsymbol{\theta}k21αk−1×Δθk：反映了已累积的总转角 αk−1\boldsymbol{\alpha}{k-1}αk−1 与当前采样步的角增量 Δθk\Delta \boldsymbol{\theta}_kΔθk 之间的非交换耦合。这直接来源于旋转矢量微分方程中的高阶项 12σ×ω\frac{1}{2} \boldsymbol{\sigma} \times \boldsymbol{\omega}21σ×ω。
• 圆锥误差修正项 112Δθk−1×Δθk\frac{1}{12} \Delta \boldsymbol{\theta}_{k-1} \times \Delta \boldsymbol{\theta}_k121Δθk−1×Δθk：反映了相邻两个采样步之间因顺序不可交换而产生的虚假漂移，与经典文献中双样本圆锥补偿的系数一致。

因此，最终的修正后角增量输出为：

Δθcorrected=Δθtrapezoidal+β \Delta \boldsymbol{\theta}{corrected} = \Delta \boldsymbol{\theta}{trapezoidal} + \boldsymbol{\beta} Δθcorrected=Δθtrapezoidal+β

该算法的设计参考了 Riseborough 和 Challinger 对 Lee 等人文献的实现（Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms ），其核心思想是：将旋转矢量法的二阶修正以递推形式嵌入到高采样率的陀螺积分器中，而不是等到姿态估计器的主周期再统一做多子样补偿。

4.3.4 对上层估计器的影响

vehicle_imu 模块在发布 IMU 数据时，调用 IntegratorConing::reset() 获取已补偿过的角增量 delta_angle，再计算角速度 gyro = delta_angle / dt。

这意味着 EKF2 和 attitude_estimator_q 接收到的 delta_angle 和 gyro 数据，其分子都已经过旋转矢量递推修正。上层估计器间接享受了精度提升，而无需重复实现复杂的补偿算法。

4.4 EKF2 中的姿态递推

EKF2 的姿态递推比 attitude_estimator_q 更为严谨。其核心差异在于：EKF2 不直接对角速度做欧拉积分，而是利用 IMU 模块预处理后的角增量 delta_ang，通过四元数的精确复合来更新姿态。

4.4.1 主滤波器的姿态预测

在 Ekf::predictState()（src/modules/ekf2/EKF/ekf.cpp）中：

void Ekf::predictState(const imuSample &imu_delayed)
{
    // 扣除陀螺零偏
    const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt;
    Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled;

    // 地球自转补偿
    corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * imu_delayed.delta_ang_dt;

    // 将角增量直接转为四元数微分旋转
    const Quatf dq(AxisAnglef{corrected_delta_ang});

    // 精确复合
    _state.quat_nominal = (_state.quat_nominal * dq).normalized();
    _R_to_earth = Dcmf(_state.quat_nominal);
}

这段代码的数学本质是：将一小段时间内的角增量 delta_ang 直接视为轴角，通过 AxisAnglef -> Quatf 的映射构造出一个精确的微分旋转四元数 dq，再与当前姿态复合。

相比 attitude_estimator_q 的 _q += q_dot * dt 一阶近似，这种基于角增量的复合方法截断误差更小，因为 delta_ang 在单个 IMU 采样周期内通常只有几毫弧度，此时 AxisAnglef -> Quatf 的映射非常接近真实旋转。

4.4.2 输出预测器中的递推

EKF2 还维护了一个输出预测器 （src/modules/ekf2/EKF/output_predictor/output_predictor.cpp），用于提供平滑的高频姿态输出：

const Quatf dq(AxisAnglef{delta_angle_corrected});
_output_new.quat_nominal = _output_new.quat_nominal * dq;
_output_new.quat_nominal.normalize();

其递推逻辑与主滤波器一致，同样采用 AxisAnglef -> Quatf -> 复合 的精确更新链。

4.4.3 两种递推方法的对比

特性	attitude_estimator_q	EKF2
积分对象	角速度 gyro (rad/s)	角增量 delta_ang (rad)
积分方法	一阶前向欧拉积分	四元数精确复合（轴角映射）
圆锥补偿	依赖 IMU 模块预处理	依赖 IMU 模块预处理
地球自转补偿	无	有（扣除 _earth_rate_NED）
陀螺零偏	简单低通积分估计	EKF 状态估计并在线扣除
误差修正	互补滤波（修正角速度）	EKF（状态+协方差联合更新）

从数学上看，attitude_estimator_q 可以视为 EKF2 递推的一个零阶近似 ：当 dt 足够小且角速度变化缓慢时，两种方法结果接近；但当 dt 增大或角速度剧烈变化时，EKF2 的精确复合方法能显著减小截断误差。

4.5 本章小结

姿态递推是姿态估计从"静态初始化"走向"动态跟踪"的核心环节。

• attitude_estimator_q 采用一阶欧拉积分 结合互补滤波修正，以极简的计算量实现了可用的姿态递推；
• 圆锥补偿 作为抑制不可交换误差的关键技术，在 PX4 中被下沉到 IMU 驱动层 （IntegratorConing），使上层估计器无需重复实现；
• EKF2 采用基于角增量的精确四元数复合 ，并加入了地球自转补偿 和在线零偏扣除，代表了更高精度的递推方案。

两种方法体现了工程中的典型权衡：attitude_estimator_q 以简洁换效率，EKF2 以复杂度换精度。理解它们之间的联系与差异，是进一步深入研究 EKF2 多源融合框架的必要准备。

关于我们：

灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。

实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。