PX4姿态解算技术详解（三）：姿态初始化——双矢量粗对准

在姿态估计算法启动之初，系统必须首先获得一个合理的初始姿态。没有有效的初始值，任何基于积分的递推滤波器都将迅速发散。与精确的卡尔曼滤波更新不同，初始姿态估计 通常采用一种更直接、更稳健的方法------粗对准（Coarse Alignment）。

粗对准的核心思想是：利用导航系中已知方向的物理矢量（如重力、地磁场），与机体系中的传感器测量值进行匹配，直接求解两个坐标系之间的旋转关系。当使用两组不共面的矢量 时，这一方法被称为双矢量粗对准（Two-Vector Alignment），它是惯性导航与飞行控制领域最经典、应用最广泛的姿态初始化技术。

PX4 的 attitude_estimator_q 模块的姿态初始化，正是双矢量粗对准的一个标准工程实现。而在更为复杂的 EKF2 中，虽然采用了分步策略（先 tilt 对准、后 yaw 对准），但其底层数学原理依然根植于矢量匹配与方向余弦矩阵构造。

本章将系统介绍双矢量粗对准的数学原理，并深入对照 PX4 源码中的具体实现。

3.1 双矢量粗对准的数学原理

3.1.1 问题描述

设导航坐标系为 nnn（NED），机体坐标系为 bbb。我们的目标是求出从机体系到导航系的旋转矩阵 Cbn\mathbf{C}_b^nCbn（或其等价表示）。

如果在导航系中存在两个已知方向且不共线的单位向量 V1n\mathbf{V}_1^nV1n 和 V2n\mathbf{V}_2^nV2n，并且在机体系中通过传感器测得了对应的观测向量 V1b\mathbf{V}_1^bV1b 和 V2b\mathbf{V}_2^bV2b（同样不共线），那么我们可以利用这两组矢量在两坐标系中的对应关系，唯一确定旋转矩阵。

3.1.2 构造正交基（Gram-Schmidt 过程）

单一矢量只能约束两个旋转自由度，无法唯一确定三维旋转。因此，我们需要利用两个不共面矢量构造出完整的右手正交标架：

第一步，取第一个参考矢量及其观测值，在机体系中归一化：

kb=V1b∥V1b∥ \mathbf{k}^b = \frac{\mathbf{V}_1^b}{\|\mathbf{V}_1^b\|} kb=∥V1b∥V1b

第二步 ，取第二个参考矢量，将其在机体系中的投影去除与 kb\mathbf{k}^bkb 平行的分量（即正交化），再归一化：

ib=V2b−kb(V2b⋅kb)∥V2b−kb(V2b⋅kb)∥ \mathbf{i}^b = \frac{\mathbf{V}_2^b - \mathbf{k}^b (\mathbf{V}_2^b \cdot \mathbf{k}^b)}{\|\mathbf{V}_2^b - \mathbf{k}^b (\mathbf{V}_2^b \cdot \mathbf{k}^b)\|} ib=∥V2b−kb(V2b⋅kb)∥V2b−kb(V2b⋅kb)

第三步，通过叉乘得到第三个正交基向量：

jb=kb×ib \mathbf{j}^b = \mathbf{k}^b \times \mathbf{i}^b jb=kb×ib

此时，{ib,jb,kb}\{\mathbf{i}^b, \mathbf{j}^b, \mathbf{k}^b\}{ib,jb,kb} 构成了机体系中的一组标准正交基。由于 ib\mathbf{i}^bib 和 kb\mathbf{k}^bkb 分别来源于导航系中已知的 V2\mathbf{V}_2V2 和 V1\mathbf{V}_1V1，这组基实际上就是导航系基轴在机体系中的投影。

3.1.3 构造方向余弦矩阵

根据 DCM 的定义，若矩阵的行向量是导航系基轴在机体系中的投影，则该矩阵就是从导航系到机体系 的坐标变换矩阵 Cnb\mathbf{C}_n^bCnb：

Cnb=[(ib)T(jb)T(kb)T] \mathbf{C}_n^b = \begin{bmatrix} (\mathbf{i}^b)^T \\ (\mathbf{j}^b)^T \\ (\mathbf{k}^b)^T \end{bmatrix} Cnb= (ib)T(jb)T(kb)T

而我们通常需要的是从机体系到导航系的变换 Cbn\mathbf{C}_b^nCbn。由于旋转矩阵是正交矩阵，其逆等于转置，因此：

Cbn=(Cnb)T=[ibjbkb] \mathbf{C}_b^n = (\mathbf{C}_n^b)^T = \begin{bmatrix} \mathbf{i}^b & \mathbf{j}^b & \mathbf{k}^b \end{bmatrix} Cbn=(Cnb)T=[ibjbkb]

或者，也可以直接将 ib\mathbf{i}^bib、jb\mathbf{j}^bjb、kb\mathbf{k}^bkb 作为行向量 填充进 DCM，此时该矩阵的语义就是 Cbn\mathbf{C}_b^nCbn。这正是 PX4 attitude_estimator_q 中的实现方式。

3.1.4 参考矢量的选择：重力与地磁场

在实际的无人机应用中，最常用的两组参考矢量是：

• 重力加速度矢量 ：在静止或匀速运动时，加速度计测得的是重力加速度的负方向，即 gb=−aaccel\mathbf{g}^b = -\mathbf{a}_{accel}gb=−aaccel。在导航系中，重力方向为 gn=[0,0,1]T\mathbf{g}^n = [0, 0, 1]^Tgn=[0,0,1]T（NED 坐标系，Z 轴向下）。
• 地磁场矢量 ：磁力计测得的是机体系中的地磁场方向 mb\mathbf{m}^bmb。在导航系中，地磁场方向可以通过世界地磁模型（WMM）获得，或者在小范围内近似为水平面内的北向分量与垂直分量的组合。

这两组矢量通常不共面（地磁场并非严格垂直），因此足以唯一确定三维姿态。然而需要注意：

• 若飞行器处于大加速度运动状态，加速度计测得的将不是纯粹的重力，导致对准误差；
• 若地磁场受到环境干扰（如建筑、电力线），磁力计的参考矢量将失真；
• 若飞行器位于地球磁极附近，地磁场接近垂直，可能与重力矢量共线，导致双矢量法退化（丧失一个自由度）。

因此，双矢量粗对准通常要求飞行器在静止或低速平稳状态下执行，且远离强磁干扰环境。

3.2 attitude_estimator_q 中的双矢量粗对准实现

PX4 的 attitude_estimator_q 模块（源码位于 src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp）实现了一个轻量级的姿态估计器，其初始化函数 init_attitude_q() 是教科书式的双矢量粗对准代码。下面逐行进行解读。

3.2.1 测量输入与参考矢量定义

初始化函数所需的输入数据已经在模块运行过程中通过传感器订阅获得：

• _accel：机体坐标系下的加速度计测量值（比力，单位 m/s²）；
• _mag：机体坐标系下的磁力计测量值（单位高斯 Gauss）；
• _mag_decl：本地磁偏角（由 GPS 位置或用户参数计算得到，单位弧度）。

在导航系（NED）中，理想参考矢量为：

• 重力方向：gn=[0,0,1]T\mathbf{g}^n = [0, 0, 1]^Tgn=[0,0,1]T（向下）
• 水平面内地磁场方向：mhorizontaln=[cos⁡δ,sin⁡δ,0]T\mathbf{m}^n_{horizontal} = [\cos\delta, \sin\delta, 0]^Tmhorizontaln=[cosδ,sinδ,0]T（磁北偏东，δ\deltaδ 为磁偏角）

3.2.2 代码实现解析

bool AttitudeEstimatorQ::init_attitude_q()
{
    // 'k' is Earth Z axis (Down) unit vector in body frame
    Vector3f k = -_accel;
    k.normalize();

Step 1：构造 kkk 轴（导航系 Z 轴在机体系中的投影）

加速度计测得的比力在静止时等于重力加速度的负方向（a=−g\mathbf{a} = -\mathbf{g}a=−g），因此 −aaccel-\mathbf{a}_{accel}−aaccel 即为重力方向在机体系中的投影。通过 normalize() 归一化后，k 就是导航系 Z 轴（Down）在机体系中的单位方向向量：

kb=−aaccel∥−aaccel∥ \mathbf{k}^b = \frac{-\mathbf{a}{accel}}{\|-\mathbf{a}{accel}\|} kb=∥−aaccel∥−aaccel

    // 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
    Vector3f i = (_mag - k * (_mag * k));
    i.normalize();

Step 2：构造 iii 轴（导航系 X 轴在机体系中的投影）

磁力计测量值 m\mathbf{m}m 通常包含垂直分量（受俯仰/横滚影响）。为了提取水平面内的北向分量，需要将 m\mathbf{m}m 投影到与 k 正交的平面上。代码中的表达式 _mag - k * (_mag * k) 正是向量投影公式：

m⊥=m−(m⋅k)k \mathbf{m}_{\perp} = \mathbf{m} - (\mathbf{m} \cdot \mathbf{k}) \mathbf{k} m⊥=m−(m⋅k)k

归一化后，i 即为导航系 X 轴（North，严格来说是磁北）在机体系中的单位投影：

ib=m⊥∥m⊥∥ \mathbf{i}^b = \frac{\mathbf{m}{\perp}}{\|\mathbf{m}{\perp}\|} ib=∥m⊥∥m⊥

    // 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
    Vector3f j = k % i;

Step 3：构造 jjj 轴（导航系 Y 轴在机体系中的投影）

通过叉乘 k % i（即 k×i\mathbf{k} \times \mathbf{i}k×i），利用右手定则直接得到第三个正交基向量 j。由于 k 和 i 已经是单位正交向量，j 自动满足单位长度和正交性：

jb=kb×ib \mathbf{j}^b = \mathbf{k}^b \times \mathbf{i}^b jb=kb×ib

    // Fill rotation matrix
    Dcmf R;
    R.row(0) = i;
    R.row(1) = j;
    R.row(2) = k;

Step 4：填充旋转矩阵

按照 PX4 Dcm.hpp 的语义约定，R 的行向量分别是导航系 NED 三轴在机体系中的投影。因此，R 的语义正是从机体到导航 的坐标变换矩阵 Cbn\mathbf{C}_b^nCbn：

Cbn=[(ib)T(jb)T(kb)T] \mathbf{C}_b^n = \begin{bmatrix} (\mathbf{i}^b)^T \\ (\mathbf{j}^b)^T \\ (\mathbf{k}^b)^T \end{bmatrix} Cbn= (ib)T(jb)T(kb)T

    // Convert to quaternion
    _q = R;

Step 5：隐式转换为四元数

由于 attitude_estimator_q 使用四元数 _q 作为递推状态，这里通过 _q = R 调用 Quaternion(const Dcm<Type>&) 构造器，将 DCM 无缝转换为四元数。这是 PX4 matrix 库无缝隐式转换特性的典型应用。

    // Compensate for magnetic declination
    Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
    _q = _q * decl_rotation;

Step 6：磁偏角补偿

上述过程构造出的是磁北坐标系 到机体系的旋转。为了得到以真北 为参考的导航系（NED），需要叠加一个绕 Z 轴的磁偏角旋转。代码中先构造了一个仅含偏航角的欧拉角四元数 decl_rotation，再与当前姿态复合：

qNED←body=qNED←body(raw)⊗qdecl \mathbf{q}{NED \leftarrow body} = \mathbf{q}{NED \leftarrow body}^{(raw)} \otimes \mathbf{q}_{decl} qNED←body=qNED←body(raw)⊗qdecl

这里右乘 decl_rotation 的物理意义是：先在机体坐标系中执行一个偏航旋转（将磁北对齐到真北），再通过 _q 将结果变换到导航系。

    _q.normalize();

    if (_q.isAllFinite() && _q.length() > 0.95f && _q.length() < 1.05f) {
        _initialized = true;
    } else {
        _initialized = false;
    }

    return _initialized;
}

Step 7：归一化与有效性检验

由于数值计算或传感器异常可能导致构造出的四元数非单位化甚至非有限，代码通过 normalize() 强制单位化，并检查长度是否在 [0.95,1.05][0.95, 1.05][0.95,1.05] 范围内，以及所有元素是否有限（isAllFinite()）。只有通过这些检验，才将 _initialized 置为 true，允许后续的姿态递推开始执行。

3.2.3 初始化触发时机

init_attitude_q() 并非只在系统启动时调用一次，而是在 update() 函数中被持续检查：

bool AttitudeEstimatorQ::update(float dt)
{
    if (!_initialized) {
        if (!_data_good) {
            return false;
        }
        return init_attitude_q();
    }
    // ... 正常姿态更新逻辑
}

这意味着，如果某次初始化失败（例如传感器数据异常），模块会在下一个周期再次尝试。这种设计确保了系统在各种启动条件下都能尽可能稳健地完成粗对准。

3.3 EKF2 中的分步粗对准策略

与 attitude_estimator_q 的一次性双矢量对准不同，PX4 的统一估计框架 EKF2 （源码位于 src/modules/ekf2/EKF/）采用了更为复杂的分步粗对准 策略：先利用加速度计完成 tilt（横滚+俯仰）对准 ，再利用磁力计或 GPS 等其他信息逐步完成 yaw（偏航）对准。这一策略的目的是在强噪声、传感器时延不一致的条件下，提供更鲁棒的初始状态估计。

3.3.1 Tilt 对准：基于单矢量的初始俯仰/横滚估计

EKF2 的 tilt 初始化由 Ekf::initialiseTilt() 函数实现（源码 src/modules/ekf2/EKF/ekf.cpp）：

bool Ekf::initialiseTilt()
{
    const float accel_norm = _accel_lpf.getState().norm();
    const float gyro_norm = _gyro_lpf.getState().norm();

    if (accel_norm < 0.8f * CONSTANTS_ONE_G ||
        accel_norm > 1.2f * CONSTANTS_ONE_G ||
        gyro_norm > math::radians(15.0f)) {
        return false;
    }

    // get initial tilt estimate from delta velocity vector, assuming vehicle is static
    _state.quat_nominal = Quatf(_accel_lpf.getState(), Vector3f(0.f, 0.f, -1.f));
    _R_to_earth = Dcmf(_state.quat_nominal);

    return true;
}

这段代码首先对加速度计和陀螺仪的测量值进行了静态检验：

• 加速度计模长必须在 0.8g∼1.2g0.8g \sim 1.2g0.8g∼1.2g 之间（确保飞行器基本静止，测量值主要是重力）；
• 陀螺仪模长必须小于 15°/s15°/s15°/s（确保没有剧烈转动）。

只有满足静态条件，才会执行 tilt 对准。对准的核心是这行代码：

_state.quat_nominal = Quatf(_accel_lpf.getState(), Vector3f(0.f, 0.f, -1.f));

这里调用了 Quaternion 的一个特殊构造器：Quatf(src, dst)，它构造的是将向量 src 旋转到与 dst 对齐的最短弧四元数。其数学本质是：

• src = _accel_lpf.getState()，即机体系中的加速度计测量值（重力反方向）；
• dst = [0, 0, -1]，即导航系中的重力方向（NED 中向下）。

这个四元数直接描述了从机体系到导航系的 tilt（俯仰+横滚）旋转。值得注意的是，单矢量对准只能确定俯仰和横滚两个自由度，偏航角在此步骤中是完全任意的（默认为 0）。

3.3.2 Yaw 对准：基于磁力计的双矢量航向初始化

在完成 tilt 对准后，EKF2 的偏航角仍然是未知的。EKF2 并不会像 attitude_estimator_q 那样一次性用双矢量法解出完整姿态，而是通过后续的磁力计融合逐步收敛航向。如果磁力计数据可用且满足条件，EKF2 会调用 resetMagHeading() 进行航向硬重置（Heading Reset）。

源码位于 src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp：

void Ekf::resetMagHeading(const Vector3f &mag)
{
    // use mag bias if variance good
    Vector3f mag_bias{0.f, 0.f, 0.f};
    // ... (bias handling omitted)

    // rotate the magnetometer measurements into earth frame using a zero yaw angle
    const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);

    // the angle of the projection onto the horizontal gives the yaw angle
    const Vector3f mag_earth_pred = R_to_earth * (mag - mag_bias);

    const float declination = getMagDeclination();
    float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;

    // update quaternion states and corresponding covariances
    resetQuatStateYaw(yaw_new, yaw_new_variance);
}

这段代码的数学逻辑非常精妙：

1. 假设 yaw = 0，利用当前的 tilt 矩阵（已知的俯仰+横滚）将磁力计测量值旋转到导航系；
2. 此时，磁力计在导航系水平面内的投影与真北之间的夹角，就是当前实际偏航角的误差；
3. 通过 atan2 提取这一夹角，并叠加磁偏角修正，得到真实的偏航角 yaw_new；
4. 最后通过 resetQuatStateYaw() 将新的偏航角注入到四元数状态中。

本质上，这也是双矢量思想的体现：重力矢量完成了 tilt 对准（第一矢量），磁力计矢量在此基础上完成了 yaw 对准（第二矢量）。只是 EKF2 将这一过程分成了两个时间步，并在每个步骤中加入了更严格的条件检验和协方差重置。

3.3.3 世界地磁模型（WMM）的辅助作用

当 GPS 位置可用时，EKF2 还会查询世界地磁模型（WMM），获取当地的磁偏角、磁倾角和磁场强度：

bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longitude_deg)
{
    const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg));
    const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg));
    const float strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg);
    // ...
    _wmm_earth_field_gauss = Dcmf(Eulerf(0, -inclination_rad, declination_rad))
                             * Vector3f(strength_gauss, 0, 0);
}

这段代码用欧拉角构造了一个 DCM，将"指向北、水平、大小为 strength" 的理想磁场向量，旋转到与当地实际地磁场（考虑磁偏角和磁倾角）对齐的方向。这个 _wmm_earth_field_gauss 随后被用于：

• 评估磁力计测量值是否与理论模型一致（一致性检查）；
• 在磁力计仅提供两轴数据时，合成缺失的 Z 轴磁场测量；
• 在 resetMagHeading 中提供准确的磁偏角参考。

3.4 双矢量粗对准的工程要点总结

通过对比 attitude_estimator_q 和 EKF2 的实现，我们可以总结出双矢量粗对准在工程实践中的几个关键要点：

静态条件检验

无论是哪种实现，都要求在初始化时飞行器处于近似静止状态 。这是因为加速度计只有在静止时才能可靠地测量重力方向。EKF2 通过 0.8g∼1.2g0.8g \sim 1.2g0.8g∼1.2g 的加速度范围和 15°/s15°/s15°/s 的陀螺仪阈值来强制执行这一条件；attitude_estimator_q 虽然没有显式的运动检验，但如果加速度偏离重力，初始化出的 k 轴就会错误，导致后续收敛变慢。

矢量不共面条件

双矢量法的前提是两条参考矢量不共线。在地球表面，重力矢量垂直向下，而地磁场矢量通常与水平面有一个倾角（磁倾角）。这意味着在大多数地区，重力与地磁场不共面，满足双矢量法的几何条件。但在磁极附近，地磁场接近垂直，可能与重力几乎共线，此时双矢量法会退化，需要借助 GPS 航迹推算或外部视觉等其他信息来完成初始化。

磁偏角补偿的时机

attitude_estimator_q 在初始化时直接进行磁偏角补偿，将磁北对齐到真北；而 EKF2 在 tilt 初始化时并不涉及磁偏角，而是在 yaw 重置时通过 getMagDeclination() 动态获取并补偿。EKF2 的磁偏角来源也更加丰富：支持 WMM 实时查询、保存参数、以及飞行中根据 _state.mag_I 自适应学习。

从 DCM 到四元数的转换

两个模块最终都将初始姿态存储为四元数，因为四元数是 PX4 工程实现中的标准状态表示。attitude_estimator_q 通过 _q = R 直接隐式转换；EKF2 则通过 Quatf(src, dst) 或 resetQuatStateYaw() 直接构造/修改四元数。

3.5 本章小结

姿态初始化是姿态估计的第一步，也是最关键的一步。双矢量粗对准利用重力和地磁场这两组在导航系中方向已知的物理矢量，通过 Gram-Schmidt 正交化构造出机体系中的标准正交基，进而直接求解出初始的旋转矩阵（DCM）。

在 PX4 中，这一理论得到了清晰的工程映射：

• attitude_estimator_q 采用了一次性的经典双矢量法，代码简洁、物理意义明确，是理解粗对准原理的最佳入口；
• EKF2 采用了分步策略：先用加速度计完成 tilt 对准（单矢量），再用磁力计分步完成 yaw 对准（双矢量的第二阶段），并加入了静态检验、WMM 辅助和协方差重置等高级工程手段，以适应更复杂的飞行环境。

掌握了双矢量粗对准的数学原理与源码实现，就为后续理解姿态递推、误差修正以及多传感器融合算法奠定了坚实的基础。

关于我们：

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

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