深入理解PX4无人机中的四元数导数与角速度关系

前言

在PX4无人机的姿态估计和控制系统中，我们经常会看到这样一个公式：

$$\dot{q}=\frac{1}{2}q\otimes \omega$$

很多初学者会困惑：为什么四元数的导数与角速度相关？四元数导数到底表示什么含义？ 本文将从数学推导、物理直觉和工程应用三个角度，深入解析这个问题。

---

一、四元数导数的数学形式

1.1 基本公式

四元数 q 描述刚体姿态时，其微分方程为：

$$\dot{q}=\frac{1}{2}q\otimes \omega$$

其中：

• $\dot{q}$ 是四元数的时间导数
• $q$ 是当前姿态四元数
• $\omega =[0,{\omega }_x,{\omega }_y,{\omega }_z{]}^T$ 是角速度的四元数形式
• $\otimes$ 表示四元数乘法

1.2 展开形式

经过一系列变换，可以变成矩阵形式（这段推导很复杂，我们下期讲，敬请期待）：

$$\dot{q}=\frac{1}{2}\Omega (\omega )\cdot q$$

其中 $\Omega (\omega )$ 是由角速度构成的反对称矩阵：

$$\Omega (\omega )=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]$$

---

二、四元数导数的物理含义

2.1 几何含义：姿态的瞬时变化率

四元数的导数 $\dot{q}$ 描述的是姿态的瞬时变化率

类比理解：

• 位置的导数 → 速度（描述"位置如何变化"）
• 四元数的导数 → 姿态变化率（描述"姿态如何变化"）

它告诉我们：在当前时刻，姿态正在以什么样的方式旋转。

2.2 物理含义：刚体的旋转状态

$\dot{q}$ 反映了刚体的旋转运动状态

想象一架无人机：

• 如果 $\dot{q}=0$ → 姿态不变，无人机不旋转
• 如果 $\dot{q}\ne 0$ → 姿态正在改变，无人机正在转动
• $|\dot{q}|$ 的大小反映旋转的"快慢"

2.3 坐标系视角

$$\dot{q}=\frac{1}{2}q\otimes \omega$$

这个公式揭示了重要的坐标系关系：

物理量	坐标系	含义
$\omega$ (角速度)	体坐标系	在机体坐标系中测量的瞬时旋转矢量
$\dot{q}$ (四元数导数)	惯性系	从惯性坐标系观察到的姿态变化率
$q$ (当前姿态)	变换关系	连接两个坐标系的"桥梁"

---

三、为什么是这个关系？

3.1 直观理解

1. 姿态变化的本质

   • 角速度 $\omega$ 描述的是刚体在体坐标系中的旋转速率
   • 四元数描述的是从参考坐标系到体坐标系的旋转
   • 四元数的变化率自然就由当前姿态和角速度共同决定

2. 为什么是 1/2 倍关系？

   • 这源于四元数的数学性质
   • 四元数用单位长度表示旋转，实际旋转角度是四元数"角度"的2倍
   • 因此导数关系中出现 1/2 的系数

3. 为什么需要四元数乘法？

   • 简单的线性关系 $\dot{q}=k\omega$ 无法正确描述旋转的叠加
   • 四元数乘法正确地处理了坐标系之间的变换

3.2 具体例子

假设无人机当前姿态为 $q$，绕Z轴以角速度 ${\omega }_z$ 旋转：

# 角速度（体坐标系）
ω = [0, 0, ωz]

# 四元数形式
ω_quat = [0, 0, 0, ωz]

# 四元数导数
q̇ = 0.5 * quaternion_multiply(q, ω_quat)

物理意义：

• 经过 $\Delta t$ 时间后，姿态变为：$q_{new}\approx q+\dot{q}\cdot \Delta t$
• 这描述了从姿态 $q$ 出发，沿着旋转方向"运动"的轨迹

---

四、与欧拉角的对比

4.1 表示方法对比

表示方法	姿态	导数	与角速度关系
欧拉角	$[\varphi ,\theta ,\psi ]$	$[\dot{\varphi },\dot{\theta },\dot{\psi }]$	需要雅可比矩阵转换
四元数	$q=[q_w,q_x,q_y,q_z]$	$\dot{q}=[{\dot{q}}_w,{\dot{q}}_x,{\dot{q}}_y,{\dot{q}}_z]$	简洁的乘法关系

4.2 欧拉角的问题

对于欧拉角，其导数与角速度的关系为：

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi /\cos \theta & \cos \varphi /\cos \theta \end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$$

问题：

• 当 $\theta =\pm 90°$ 时出现奇异性（万向节死锁）
• 转换矩阵复杂，计算量大

四元数的优势：

• 公式简洁：$\dot{q}=\frac{1}{2}q\otimes \omega$
• 无奇异性
• 数值稳定性好

---

五、在PX4中的应用

5.1 姿态预测

通过陀螺仪测得的角速度，积分更新四元数姿态：

// PX4源码中的姿态积分（简化版）
void attitude_update(float dt) {
    // 读取陀螺仪角速度
    Vector3f gyro = imu.get_gyro();
    
    // 计算四元数导数
    Quatf q_dot = 0.5f * _q * Quatf(0, gyro(0), gyro(1), gyro(2));
    
    // 欧拉积分
    _q = _q + q_dot * dt;
    
    // 归一化
    _q.normalize();
}

5.2 EKF状态估计

在扩展卡尔曼滤波器中传播姿态状态：

// 状态预测
q(t+Δt) = q(t) + q̇(t) · Δt

// 其中
q̇(t) = (1/2) · q(t) ⊗ ω(t)

这就是姿态的运动学方程，类似于：

• 位置运动学：$\dot{x}=v$
• 姿态运动学：$\dot{q}=f(q,\omega )$

5.3 姿态控制

计算期望的角速度来达到目标姿态：

// 姿态误差四元数
Quatf q_error = q_target * q_current.inversed();

// 提取误差轴角
Vector3f error_axis;
float error_angle;
q_error.to_axis_angle(error_axis, error_angle);

// 计算期望角速度（P控制）
Vector3f omega_desired = Kp * error_axis * error_angle;

---

六、代码实现示例

6.1 Python实现

import numpy as np

def quaternion_multiply(q1, q2):
    """四元数乘法"""
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

def quaternion_derivative(q, omega):
    """
    计算四元数导数
    q: 当前四元数 [w, x, y, z]
    omega: 角速度 [wx, wy, wz] (rad/s)
    """
    # 角速度转四元数形式
    omega_quat = np.array([0, omega[0], omega[1], omega[2]])
    
    # 计算导数
    q_dot = 0.5 * quaternion_multiply(q, omega_quat)
    
    return q_dot

def integrate_attitude(q, omega, dt):
    """
    姿态积分
    q: 当前四元数
    omega: 角速度
    dt: 时间步长
    """
    q_dot = quaternion_derivative(q, omega)
    q_new = q + q_dot * dt
    
    # 归一化
    q_new = q_new / np.linalg.norm(q_new)
    
    return q_new

# 示例：绕Z轴旋转
q = np.array([1, 0, 0, 0])  # 初始姿态
omega = np.array([0, 0, 1])  # 1 rad/s 绕Z轴
dt = 0.01  # 10ms

# 积分100步
for i in range(100):
    q = integrate_attitude(q, omega, dt)
    print(f"Step {i}: q = {q}")

6.2 C++实现（类PX4风格）

class Quaternion {
public:
    float w, x, y, z;
    
    Quaternion operator*(const Quaternion& q) const {
        // 四元数乘法
        return Quaternion(
            w*q.w - x*q.x - y*q.y - z*q.z,
            w*q.x + x*q.w + y*q.z - z*q.y,
            w*q.y - x*q.z + y*q.w + z*q.x,
            w*q.z + x*q.y - y*q.x + z*q.w
        );
    }
    
    void normalize() {
        float norm = sqrt(w*w + x*x + y*y + z*z);
        w /= norm; x /= norm; y /= norm; z /= norm;
    }
};

Quaternion quaternion_derivative(const Quaternion& q, const Vector3f& omega) {
    // 角速度转四元数
    Quaternion omega_quat(0, omega.x, omega.y, omega.z);
    
    // 计算导数
    Quaternion q_dot = q * omega_quat;
    q_dot.w *= 0.5f;
    q_dot.x *= 0.5f;
    q_dot.y *= 0.5f;
    q_dot.z *= 0.5f;
    
    return q_dot;
}

---

七、深入理解：四维空间的运动

7.1 四元数空间的几何

四元数可以看作四维单位球面 $S^3$ 上的点：

$$q_w^2+q_x^2+q_y^2+q_z^2=1$$

四元数导数的几何意义：

• $\dot{q}$ 是四元数在 $S^3$ 球面上的切向量
• 角速度 $\omega$ 决定了切向量的方向和大小
• 姿态演化就是四元数沿着切线方向在球面上"运动"

7.2 为什么需要归一化

由于数值积分误差，四元数可能偏离单位球面：

# 积分后检查
norm = np.linalg.norm(q)
if abs(norm - 1.0) > 1e-6:
    q = q / norm  # 重新归一化

---

八、常见问题FAQ

Q1: 为什么不直接用角速度积分？

A: 角速度是在体坐标系中定义的，而姿态是相对于惯性系的。直接积分角速度无法正确描述姿态的演化，必须通过四元数微分方程来连接两个坐标系。

Q2: 四元数导数的单位是什么？

A: 四元数本身是无量纲的，其导数的单位是 $s^{-1}$（每秒）。如果角速度是 rad/s，则 $\dot{q}$ 的单位也是 $s^{-1}$。

Q3: 可以用其他积分方法吗？

A: 可以！上面的例子用的是简单的欧拉法。实际PX4中使用更高阶的方法：

• 龙格-库塔法（RK4）：精度更高
• 指数映射法：保持单位四元数性质

Q4: 为什么PX4选择四元数而不是欧拉角？

A: 主要原因：

1. 无万向节死锁
2. 数值稳定性好
3. 插值方便（Slerp）
4. 计算效率高（相比旋转矩阵）

---

九、总结

1. 四元数导数的本质：描述姿态在四维单位球面上的运动速率

2. 与角速度的关系：通过公式 $\dot{q}=\frac{1}{2}q\otimes \omega$ 将体坐标系的角速度转换为惯性系的姿态变化率

3. 物理意义：反映刚体的旋转状态，是姿态运动学方程的基础

4. 工程应用：在PX4中用于姿态预测、状态估计和控制

5. 核心优势：公式简洁、无奇异性、数值稳定

理解四元数导数与角速度的关系，是掌握无人机姿态估计和控制的关键一步。希望本文能帮助你建立清晰的物理和数学直觉！

---

参考资料

1. PX4 Autopilot Documentation: https://docs.px4.io/
2. “Quaternion kinematics for the error-state Kalman filter” - Joan Solà
3. “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors” - James Diebel
4. PX4 源码：src/lib/mathlib/math/Quaternion.hpp

---

关注我，获取更多无人机技术干货！

如果本文对你有帮助，欢迎点赞、收藏、转发！有问题欢迎在评论区讨论~