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

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

前言

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

q˙=12q⊗ω\dot{q} = \frac{1}{2} q \otimes \omegaq˙​=21​q⊗ω

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

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

1.1 基本公式

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

q˙=12q⊗ω\dot{q} = \frac{1}{2} q \otimes \omegaq˙​=21​q⊗ω

其中：

• q˙\dot{q}q˙​ 是四元数的时间导数
• qqq 是当前姿态四元数
• ω=[0,ωx,ωy,ωz]T\omega = [0, \omega_x, \omega_y, \omega_z]^Tω=[0,ωx​,ωy​,ωz​]T 是角速度的四元数形式
• ⊗\otimes⊗ 表示四元数乘法

1.2 展开形式

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

q˙=12Ω(ω)⋅q\dot{q} = \frac{1}{2} \Omega(\omega) \cdot qq˙​=21​Ω(ω)⋅q

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

Ω(ω)=[0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0]\Omega(\omega) = \begin{bmatrix} 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{bmatrix}Ω(ω)=​0ωx​ωy​ωz​​−ωx​0−ωz​ωy​​−ωy​ωz​0−ωx​​−ωz​−ωy​ωx​0​​

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

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

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

类比理解：

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

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

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

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

想象一架无人机：

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

2.3 坐标系视角

q˙=12q⊗ω\dot{q} = \frac{1}{2} q \otimes \omegaq˙​=21​q⊗ω

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

三、为什么是这个关系？

3.1 直观理解

1. 姿态变化的本质
   • 角速度 ω\omegaω 描述的是刚体在体坐标系中的旋转速率
   • 四元数描述的是从参考坐标系到体坐标系的旋转
   • 四元数的变化率自然就由当前姿态和角速度共同决定
2. 为什么是 1/2 倍关系？
   • 这源于四元数的数学性质
   • 四元数用单位长度表示旋转，实际旋转角度是四元数"角度"的2倍
   • 因此导数关系中出现 1/2 的系数
3. 为什么需要四元数乘法？
   • 简单的线性关系 q˙=kω\dot{q} = k\omegaq˙​=kω 无法正确描述旋转的叠加
   • 四元数乘法正确地处理了坐标系之间的变换

3.2 具体例子

假设无人机当前姿态为 qqq，绕Z轴以角速度 ωz\omega_zωz​ 旋转：

# 角速度（体坐标系） ω =[0,0, ωz]# 四元数形式 ω_quat =[0,0,0, ωz]# 四元数导数 q̇ =0.5* quaternion_multiply(q, ω_quat)

物理意义：

• 经过 Δt\Delta tΔt 时间后，姿态变为：qnew≈q+q˙⋅Δtq_{new} \approx q + \dot{q} \cdot \Delta tqnew​≈q+q˙​⋅Δt
• 这描述了从姿态 qqq 出发，沿着旋转方向"运动"的轨迹

四、与欧拉角的对比

4.1 表示方法对比

4.2 欧拉角的问题

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

[ϕ˙θ˙ψ˙]=[1sin⁡ϕtan⁡θcos⁡ϕtan⁡θ0cos⁡ϕ−sin⁡ϕ0sin⁡ϕ/cos⁡θcos⁡ϕ/cos⁡θ][ωxωyωz]\begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} = \begin{bmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi/\cos\theta & \cos\phi/\cos\theta \end{bmatrix} \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix}​ϕ˙​θ˙ψ˙​​​=​100​sinϕtanθcosϕsinϕ/cosθ​cosϕtanθ−sinϕcosϕ/cosθ​​​ωx​ωy​ωz​​​

问题：

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

四元数的优势：

• 公式简洁：q˙=12q⊗ω\dot{q} = \frac{1}{2} q \otimes \omegaq˙​=21​q⊗ω
• 无奇异性
• 数值稳定性好

五、在PX4中的应用

5.1 姿态预测

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

// PX4源码中的姿态积分（简化版）voidattitude_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)

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

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

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 defquaternion_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 ])defquaternion_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 defintegrate_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 inrange(100): q = integrate_attitude(q, omega, dt)print(f"Step {i}: q = {q}")

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

classQuaternion{public:float w, x, y, z; Quaternion operator*(const Quaternion& q)const{// 四元数乘法returnQuaternion( 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 );}voidnormalize(){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 四元数空间的几何

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

qw2+qx2+qy2+qz2=1q_w^2 + q_x^2 + q_y^2 + q_z^2 = 1qw2​+qx2​+qy2​+qz2​=1

四元数导数的几何意义：

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

7.2 为什么需要归一化

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

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

八、常见问题FAQ

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

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

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

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

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

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

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

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

A: 主要原因：

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

九、总结

1. 四元数导数的本质：描述姿态在四维单位球面上的运动速率
2. 与角速度的关系：通过公式 q˙=12q⊗ω\dot{q} = \frac{1}{2} q \otimes \omegaq˙​=21​q⊗ω 将体坐标系的角速度转换为惯性系的姿态变化率
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

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

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