深入理解PX4无人机中的四元数导数与角速度关系
深入理解PX4无人机中的四元数导数与角速度关系
前言
在PX4无人机的姿态估计和控制系统中，我们经常会看到这样一个公式：
q˙=12q⊗ω\dot{q} = \frac{1}{2} q \otimes \omegaq˙=21q⊗ω
很多初学者会困惑：为什么四元数的导数与角速度相关？四元数导数到底表示什么含义？ 本文将从数学推导、物理直觉和工程应用三个角度，深入解析这个问题。
一、四元数导数的数学形式
1.1 基本公式
四元数 q 描述刚体姿态时，其微分方程为：
q˙=12q⊗ω\dot{q} = \frac{1}{2} q \otimes \omegaq˙=21q⊗ω
其中：
- 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−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0
二、四元数导数的物理含义
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˙=21q⊗ω
这个公式揭示了重要的坐标系关系：
| 物理量 | 坐标系 | 含义 |
|---|---|---|
| ω\omegaω (角速度) | 体坐标系 | 在机体坐标系中测量的瞬时旋转矢量 |
| q˙\dot{q}q˙ (四元数导数) | 惯性系 | 从惯性坐标系观察到的姿态变化率 |
| qqq (当前姿态) | 变换关系 | 连接两个坐标系的"桥梁" |
三、为什么是这个关系？
3.1 直观理解
- 姿态变化的本质
- 角速度 ω\omegaω 描述的是刚体在体坐标系中的旋转速率
- 四元数描述的是从参考坐标系到体坐标系的旋转
- 四元数的变化率自然就由当前姿态和角速度共同决定
- 为什么是 1/2 倍关系？
- 这源于四元数的数学性质
- 四元数用单位长度表示旋转，实际旋转角度是四元数"角度"的2倍
- 因此导数关系中出现 1/2 的系数
- 为什么需要四元数乘法？
- 简单的线性关系 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 表示方法对比
| 表示方法 | 姿态 | 导数 | 与角速度关系 |
|---|---|---|---|
| 欧拉角 | [ϕ,θ,ψ][\phi, \theta, \psi][ϕ,θ,ψ] | [ϕ˙,θ˙,ψ˙][\dot{\phi}, \dot{\theta}, \dot{\psi}][ϕ˙,θ˙,ψ˙] | 需要雅可比矩阵转换 |
| 四元数 | q=[qw,qx,qy,qz]q = [q_w, q_x, q_y, q_z]q=[qw,qx,qy,qz] | q˙=[q˙w,q˙x,q˙y,q˙z]\dot{q} = [\dot{q}_w, \dot{q}_x, \dot{q}_y, \dot{q}_z]q˙=[q˙w,q˙x,q˙y,q˙z] | 简洁的乘法关系 |
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}ϕ˙θ˙ψ˙=100sinϕ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˙=21q⊗ω
- 无奇异性
- 数值稳定性好
五、在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: 主要原因：
- 无万向节死锁
- 数值稳定性好
- 插值方便（Slerp）
- 计算效率高（相比旋转矩阵）
九、总结
- 四元数导数的本质：描述姿态在四维单位球面上的运动速率
- 与角速度的关系：通过公式 q˙=12q⊗ω\dot{q} = \frac{1}{2} q \otimes \omegaq˙=21q⊗ω 将体坐标系的角速度转换为惯性系的姿态变化率
- 物理意义：反映刚体的旋转状态，是姿态运动学方程的基础
- 工程应用：在PX4中用于姿态预测、状态估计和控制
- 核心优势：公式简洁、无奇异性、数值稳定
理解四元数导数与角速度的关系，是掌握无人机姿态估计和控制的关键一步。希望本文能帮助你建立清晰的物理和数学直觉！
参考资料
- PX4 Autopilot Documentation: https://docs.px4.io/
- “Quaternion kinematics for the error-state Kalman filter” - Joan Solà
- “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors” - James Diebel
- PX4 源码：
src/lib/mathlib/math/Quaternion.hpp
关注我，获取更多无人机技术干货！
如果本文对你有帮助，欢迎点赞、收藏、转发！有问题欢迎在评论区讨论~
