四元数 (Quaternion)微分-单位四元数 q(t) 的导数详细推导（10）

---

上一节相关内容：四元数 (Quaternion)微分-从惯性系角速度推导四元数导数的矩阵表示（9）

一、符号与约定

1. 四元数写作 q = ( q 0 , q ) q = (q_0, \mathbf{q}) q=(q0,q)，其中 q 0 q_0 q0 为实部（scalar）， q = [ q 1 , q 2 , q 3 ] ⊤ \mathbf{q}=[q_1,q_2,q_3]^\top q=[q1,q2,q3]⊤ 为虚部（vector）。我们把四元数视为列向量
   q = [ q 0 q ] . q = \begin{bmatrix} q_0 \ \mathbf{q} \end{bmatrix}. q=[q0 q].

2. "纯四元数"（pure quaternion）把三维向量 ω ∈ R 3 \boldsymbol{\omega}\in\mathbb{R}^3 ω∈R3 表为 ( 0 , ω ) (0,\boldsymbol{\omega}) (0,ω)。

3. 四元数乘法（Hamilton 乘法）对于 p = ( p 0 , p ) p=(p_0,\mathbf{p}) p=(p0,p) 与 q = ( q 0 , q ) q=(q_0,\mathbf{q}) q=(q0,q) 定义为
   p ⊗ q = ( p 0 q 0 − p ⋅ q , p 0 q + q 0 p + p × q ) . p\otimes q = \big(p_0 q_0 - \mathbf{p}\cdot\mathbf{q}, p_0\mathbf{q} + q_0\mathbf{p} + \mathbf{p}\times\mathbf{q}\big). p⊗q=(p0q0−p⋅q,p0q+q0p+p×q).

   （这是标准的表达式，后面会用到。）

4. 关于角速度 ω \boldsymbol{\omega} ω 的参考系：本推导假定 ω \boldsymbol{\omega} ω 是表示机体角速度（body angular velocity），即在机体系中测量的角速度向量。若 ω \boldsymbol{\omega} ω 用惯性系表示，则公式要对应地改为右乘形式（后文说明）。

5. 四元数用于表示从机体到惯性框的旋转（常见约定之一），对应向量变换：
   v ∗ inertial = q ⊗ ( 0 , v ∗ body ) ⊗ q ∗ , \mathbf{v}*\text{inertial} = q \otimes (0,\mathbf{v}*\text{body}) \otimes q^*, v∗inertial=q⊗(0,v∗body)⊗q∗,

   其中 q ∗ q^* q∗ 是四元数共轭。

---

二、推导思路

从"旋转的时间导数导致矢量在惯性系与机体系的关系与角速度有关"出发。我们用四元数表示旋转，并对四元数表示的向量变换两边求导，利用向量随时间的导数满足 v ˙ = ω × v \dot{\mathbf{v}} = \boldsymbol{\omega}\times\mathbf{v} v˙=ω×v（相对于移动参考系的关系），最后通过代数变形得到四元数的时间导数与角速度之间的关系，即
q ˙ = 1 2 q ⊗ ( 0 , ω ) . \dot q = \tfrac12 q \otimes (0,\boldsymbol{\omega}). q˙=21q⊗(0,ω).

下面分步展开。

---

三、详细推导

1) 向量的旋转表示与对它求导

设 q ( t ) q(t) q(t) 为随时间变化的单位四元数，把机体系的向量 v b \mathbf{v}_b vb 映到惯性系为
( 0 , v i ( t ) ) = q ( t ) ⊗ ( 0 , v b ) ⊗ q ( t ) ∗ . (0,\mathbf{v}_i(t)) = q(t) \otimes (0,\mathbf{v}_b) \otimes q(t)^*. (0,vi(t))=q(t)⊗(0,vb)⊗q(t)∗.

对时间求导（记 q ∗ ≡ q ˉ q^*\equiv \bar q q∗≡qˉ）：
d d t ( q ⊗ ( 0 , v b ) ⊗ q ˉ ) = q ˙ ⊗ ( 0 , v b ) ⊗ q ˉ + q ⊗ ( 0 , v b ) ⊗ q ˉ ˙ . \frac{d}{dt}\big(q\otimes (0,\mathbf{v}_b)\otimes \bar q\big) = \dot q\otimes (0,\mathbf{v}_b)\otimes \bar q + q\otimes (0,\mathbf{v}_b)\otimes \dot{\bar q}. dtd(q⊗(0,vb)⊗qˉ)=q˙⊗(0,vb)⊗qˉ+q⊗(0,vb)⊗qˉ˙.

注意 v b \mathbf{v}_b vb 是机体固定向量（不随时间变化）时上式成立；若 v b \mathbf{v}_b vb 本身时间变化，需要加上它的导数项（此处假设 v b \mathbf{v}_b vb 固定以说明方向学）。

另一方面，从惯性/机体之间的运动学，若 ω \boldsymbol{\omega} ω 为机体系角速度，则惯性系下该向量随时间的导数满足：
d d t v i = ω i × v i . \frac{d}{dt}\mathbf{v}_i = \boldsymbol{\omega}_i \times \mathbf{v}_i. dtdvi=ωi×vi.

对于任意三维向量 x \mathbf{x} x，纯四元数形式的叉乘：
( 0 , ω ) ⊗ ( 0 , x ) = ( − ω ⋅ x , ; ω × x ) . (0,\boldsymbol{\omega})\otimes(0,\mathbf{x}) = (-\boldsymbol{\omega}\cdot\mathbf{x},; \boldsymbol{\omega}\times\mathbf{x}). (0,ω)⊗(0,x)=(−ω⋅x,;ω×x).

2) 使用四元数导数的代数关系

建立匹配关系：
q ˙ ⊗ ( 0 , v b ) ⊗ q ˉ q ⊗ ( 0 , v b ) ⊗ q ˉ ˙ = q ⊗ ( ( 0 , ω ) ⊗ ( 0 , v b ) ) ⊗ q ˉ . \dot q\otimes (0,\mathbf{v}_b)\otimes \bar q q\otimes (0,\mathbf{v}_b)\otimes \dot{\bar q} = q\otimes\big( (0,\boldsymbol{\omega})\otimes(0,\mathbf{v}_b) \big)\otimes \bar q. q˙⊗(0,vb)⊗qˉq⊗(0,vb)⊗qˉ˙=q⊗((0,ω)⊗(0,vb))⊗qˉ.

对上式左乘 q ˉ \bar q qˉ 并右乘 q q q 得
q ˉ ⊗ q ˙ ⊗ ( 0 , v b ) ( 0 , v b ) ⊗ q ˉ ˙ ⊗ q = ( 0 , ω ) ⊗ ( 0 , v b ) . \bar q\otimes\dot q\otimes (0,\mathbf{v}_b) (0,\mathbf{v}_b)\otimes\dot{\bar q}\otimes q = (0,\boldsymbol{\omega})\otimes(0,\mathbf{v}_b). qˉ⊗q˙⊗(0,vb)(0,vb)⊗qˉ˙⊗q=(0,ω)⊗(0,vb).

经过代数整理，并利用该式对任意 v b \mathbf{v}_b vb 成立，可得
q ˉ ⊗ q ˙ = 1 2 ( 0 , ω ) . \bar q\otimes\dot q = \tfrac12 (0,\boldsymbol{\omega}). qˉ⊗q˙=21(0,ω).

两边左乘 q q q 得
q ˙ = 1 2 , q ⊗ ( 0 , ω ) . \dot q = \tfrac12, q \otimes (0,\boldsymbol{\omega}). q˙=21,q⊗(0,ω).

---

四、分量形式验证

令 q = ( q 0 , q ) q=(q_0,\mathbf{q}) q=(q0,q)，纯四元数 ω q = ( 0 , ω ) \omega_q=(0,\boldsymbol{\omega}) ωq=(0,ω)。

根据乘法公式：
q ⊗ ω q = ( − q ⋅ ω , q 0 ω + q × ω ) . q\otimes \omega_q = \big( -\mathbf{q}\cdot\boldsymbol{\omega},q_0\boldsymbol{\omega} + \mathbf{q}\times\boldsymbol{\omega} \big). q⊗ωq=(−q⋅ω,q0ω+q×ω).

因此
q ˙ = 1 2 q ⊗ ( 0 , ω ) = 1 2 [ − q ⊤ ω q 0 ω + q × ω ] . \dot q = \tfrac12q\otimes(0,\boldsymbol{\omega}) = \tfrac12 \begin{bmatrix} -\mathbf{q}^\top \boldsymbol{\omega} \ q_0\boldsymbol{\omega} + \mathbf{q}\times\boldsymbol{\omega} \end{bmatrix}. q˙=21q⊗(0,ω)=21[−q⊤ω q0ω+q×ω].

即
q ˙ 0 = − 1 2 q ⊤ ω , q ˙ = 1 2 ( q 0 ω + q × ω ) . \dot q_0 = -\tfrac12\mathbf{q}^\top\boldsymbol{\omega}, \qquad \dot{\mathbf{q}} = \tfrac12\big(q_0\boldsymbol{\omega} + \mathbf{q}\times\boldsymbol{\omega}\big). q˙0=−21q⊤ω,q˙=21(q0ω+q×ω).

---

五、矩阵形式（便于数值实现）

将 q = [ q 0 , q 1 , q 2 , q 3 ] ⊤ q=[q_0,q_1,q_2,q_3]^\top q=[q0,q1,q2,q3]⊤，可构造 4 × 4 4\times4 4×4 矩阵 B ( ω ) B(\boldsymbol{\omega}) B(ω)：

q ˙ = 1 2 B ( ω ) q \dot q = \tfrac12 B(\boldsymbol{\omega}) q q˙=21B(ω)q

B ( ω ) = [ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y − ω x 0 ] = [ 0 − ω ⊤ ω − [ ω ] × ] . B(\boldsymbol{\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}= \begin{bmatrix} 0 & -\boldsymbol{\omega}^\top\\ \boldsymbol{\omega} & -[\boldsymbol{\omega}]_\times \end{bmatrix}. B(ω)= 0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0 =[0ω−ω⊤−[ω]×].

---

六、关于"左乘 vs 右乘"的约定

上面推导得到的是
q ˙ = 1 2 , q ⊗ ( 0 , ω ) . \dot q = \tfrac12, q\otimes (0,\boldsymbol{\omega}). q˙=21,q⊗(0,ω).

若采用另一类文献约定（例如四元数表示惯性到机体的旋转，或角速度在惯性系），公式可能是右乘：

q ˙ = 1 2 ( 0 , ω ) ⊗ q . \dot q = \tfrac12 (0,\boldsymbol{\omega})\otimes q. q˙=21(0,ω)⊗q.

务必检查使用的约定。

---

七、数值实现要点与注意事项

1. 角速度单位 ：( ω \boldsymbol{\omega} ω) 必须以**弧度每秒（ r a d / s rad/s rad/s）**为单位。
2. 四元数归一化 ：数值积分（如 Euler、Runge--Kutta）后要对 (q) 做单位化（normalize），以防数值误差累积导致 ( ∣ q ∣ ≠ 1 ) ( |q|\neq 1) (∣q∣=1)。
3. 积分方法：对刚体快速旋转使用更高阶的积分器（例如 RK4），或采用指数映射结合小步长积分以提高精度。
4. 小角近似 ：当 ( ∣ ω Δ t ∣ ) (|\boldsymbol{\omega}\Delta t|) (∣ωΔt∣) 很小时，可用小角近似做第一阶更新；但对大角速度或大采样间隔应避免此近似。
5. 边界条件：保持四元数连续（避免负号不一致导致跳变，通常把四元数约束为实部非负或按最接近上一时刻的表示选择符号）。

---

八、总结

四元数时间导数与机体角速度关系：

q ˙ = 1 2 q ⊗ ( 0 , ω ) \boxed{\dot q = \tfrac12 q \otimes (0,\boldsymbol{\omega})} q˙=21q⊗(0,ω)

分量形式：

q ˙ 0 = − 1 2 q ⊤ ω , q ˙ = 1 2 ( q 0 ω + q × ω ) . \dot q_0 = -\tfrac12\mathbf{q}^\top\boldsymbol{\omega}, \qquad \dot{\mathbf{q}} = \tfrac12(q_0\boldsymbol{\omega} + \mathbf{q}\times\boldsymbol{\omega}). q˙0=−21q⊤ω,q˙=21(q0ω+q×ω).

矩阵形式：

q ˙ = 1 2 [ 0 − ω ⊤ ω − [ ω ] × ] q . \dot q = \tfrac12 \begin{bmatrix} 0 & -\boldsymbol{\omega}^\top \\ \boldsymbol{\omega} & -[\boldsymbol{\omega}]_\times \end{bmatrix} q. q˙=21[0ω−ω⊤−[ω]×]q.

---

九、完整的数值积分示例代码（C++ / Python）

1、C++ 实现

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

using namespace Eigen;

// 四元数导数: dq/dt = 0.5 * q ⊗ (0, ω)
Quaterniond quat_derivative(const Quaterniond &q, const Vector3d &omega) {
    Quaterniond omega_q(0, omega.x(), omega.y(), omega.z());
    Quaterniond dq = q * omega_q;  // Hamilton product
    dq.coeffs() *= 0.5;            // 注意：Eigen 中 coeffs() 顺序为 (x, y, z, w)
    return Quaterniond(dq.w(), dq.x(), dq.y(), dq.z());
}

// Euler 积分
Quaterniond quat_integrate_euler(const Quaterniond &q, const Vector3d &omega, double dt) {
    Quaterniond dq = quat_derivative(q, omega);
    Quaterniond q_next(
        q.w() + dq.w() * dt,
        q.x() + dq.x() * dt,
        q.y() + dq.y() * dt,
        q.z() + dq.z() * dt
    );
    q_next.normalize();
    return q_next;
}

// RK4 积分
Quaterniond quat_integrate_rk4(const Quaterniond &q, const Vector3d &omega, double dt) {
    Quaterniond k1 = quat_derivative(q, omega);
    Quaterniond k2 = quat_derivative(q + 0.5 * dt * k1.coeffs(), omega);
    Quaterniond k3 = quat_derivative(q + 0.5 * dt * k2.coeffs(), omega);
    Quaterniond k4 = quat_derivative(q + dt * k3.coeffs(), omega);

    Quaterniond q_next;
    q_next.w() = q.w() + dt / 6.0 * (k1.w() + 2 * k2.w() + 2 * k3.w() + k4.w());
    q_next.x() = q.x() + dt / 6.0 * (k1.x() + 2 * k2.x() + 2 * k3.x() + k4.x());
    q_next.y() = q.y() + dt / 6.0 * (k1.y() + 2 * k2.y() + 2 * k3.y() + k4.y());
    q_next.z() = q.z() + dt / 6.0 * (k1.z() + 2 * k2.z() + 2 * k3.z() + k4.z());

    q_next.normalize();
    return q_next;
}

int main() {
    Quaterniond q(1, 0, 0, 0);     // 初始朝向：单位四元数
    Vector3d omega(0, 0, M_PI/2);  // 角速度：绕Z轴 90°/s
    double dt = 0.01;              // 时间步长
    double T = 2.0;                // 模拟 2 秒

    for (double t = 0; t < T; t += dt) {
        q = quat_integrate_rk4(q, omega, dt);
        std::cout << "t=" << t << " q=[" << q.w() << ", "
                  << q.vec().transpose() << "]" << std::endl;
    }
    return 0;
}

说明：

• Eigen::Quaterniond 内部存储顺序为 (x, y, z, w)，使用时注意。
• 若只需小步更新，可以改用 quat_integrate_euler()。
• 每步最后一定要 normalize() 保持单位长度。

---

2、Python 实现

import numpy as np

def quat_mul(q1, q2):
    """四元数乘法 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 quat_derivative(q, omega):
    """dq/dt = 0.5 * q ⊗ (0, ω)"""
    omega_q = np.array([0.0, *omega])
    dq = 0.5 * quat_mul(q, omega_q)
    return dq

def quat_normalize(q):
    return q / np.linalg.norm(q)

def quat_integrate_rk4(q, omega, dt):
    k1 = quat_derivative(q, omega)
    k2 = quat_derivative(q + 0.5*dt*k1, omega)
    k3 = quat_derivative(q + 0.5*dt*k2, omega)
    k4 = quat_derivative(q + dt*k3, omega)
    q_next = q + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
    return quat_normalize(q_next)

# 测试：绕Z轴以90°/s旋转2秒
q = np.array([1, 0, 0, 0])          # 初始单位四元数
omega = np.array([0, 0, np.pi/2])   # rad/s
dt = 0.01
T = 2.0

for t in np.arange(0, T, dt):
    q = quat_integrate_rk4(q, omega, dt)
    print(f"t={t:.2f} q={q}")