四元数 (Quaternion)微分-四元数导数的矩阵表示推导（8）

一、起点公式

连续时间单位四元数 q(t)q(t)q(t) 的导数为：

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

写出 q=(w,u)q=(w,\mathbf{u})q=(w,u)，其中 u=(x,y,z)T\mathbf{u}=(x,y,z)^Tu=(x,y,z)T。四元数乘法公式如下：

(a,b)⊗(c,d)=(ac−b⋅d,  ad+cb+b×d) (a,\mathbf{b})\otimes(c,\mathbf{d})=(ac-\mathbf{b}\cdot\mathbf{d},\; a\mathbf{d}+c\mathbf{b}+\mathbf{b}\times\mathbf{d}) (a,b)⊗(c,d)=(ac−b⋅d,ad+cb+b×d)

对 qqq 与 (0,ω)(0,\boldsymbol{\omega})(0,ω) 代入得：

• 标量部分：

w˙=12(−u⋅ω)=12(−xωx−yωy−zωz). \dot w = \tfrac{1}{2}(-\mathbf{u}\cdot\boldsymbol{\omega}) = \tfrac{1}{2}(-x\omega_x - y\omega_y - z\omega_z). w˙=21(−u⋅ω)=21(−xωx−yωy−zωz).

• 向量部分（分量逐项）：

v˙=12(wω+u×ω). \dot{\mathbf{v}}=\tfrac{1}{2}\big(w\boldsymbol{\omega} + \mathbf{u}\times\boldsymbol{\omega}\big). v˙=21(wω+u×ω).

写出分量：

x˙=12(wωx+(yωz−zωy)),y˙=12(wωy+(zωx−xωz)),z˙=12(wωz+(xωy−yωx)). \begin{aligned} \dot x &= \tfrac{1}{2}\big(w\omega_x + (y\omega_z - z\omega_y)\big),\\ \dot y &= \tfrac{1}{2}\big(w\omega_y + (z\omega_x - x\omega_z)\big),\\ \dot z &= \tfrac{1}{2}\big(w\omega_z + (x\omega_y - y\omega_x)\big). \end{aligned} x˙y˙z˙=21(wωx+(yωz−zωy)),=21(wωy+(zωx−xωz)),=21(wωz+(xωy−yωx)).

---

二、目标 ------ 以矩阵乘法写成 q˙=12 Ω(ω) q\dot q = \tfrac{1}{2}\,\Omega(\boldsymbol{\omega})\, qq˙=21Ω(ω)q

设想要 4×4 矩阵 Ω(ω)\Omega(\boldsymbol{\omega})Ω(ω) 使得 Ω(ω)q\Omega(\boldsymbol{\omega}) qΩ(ω)q 等于上面的向量（去掉 1/2）。由上面的分量表达，直接写出矩阵的每一行与 q=w,x,y,zTq=w,x,y,z^Tq=w,x,y,zT 的乘积应当对应：

• 第一行（对应   2w˙  =  −xωx−yωy−zωz\;2\dot w\; =\; -x\omega_x - y\omega_y - z\omega_z2w˙=−xωx−yωy−zωz）：

  0,  −ωx,  −ωy,  −ωz⋅w,x,y,zT=−xωx−yωy−zωz. 0,\\; -\\omega_x,\\; -\\omega_y,\\; -\\omega_z \cdot w,x,y,z^T = -x\omega_x - y\omega_y - z\omega_z. 0,−ωx,−ωy,−ωz⋅w,x,y,zT=−xωx−yωy−zωz.

• 第二行（对应   2x˙  =  wωx+yωz−zωy\;2\dot x\; =\; w\omega_x + y\omega_z - z\omega_y2x˙=wωx+yωz−zωy）：

  ωx,  0,  ωz,  −ωy⋅w,x,y,zT=ωxw+0⋅x+ωzy−ωyz. \\omega_x,\\; 0,\\; \\omega_z,\\; -\\omega_y \cdot w,x,y,z^T = \omega_x w + 0\cdot x + \omega_z y - \omega_y z. ωx,0,ωz,−ωy⋅w,x,y,zT=ωxw+0⋅x+ωzy−ωyz.

• 第三行（对应   2y˙  =  wωy+zωx−xωz\;2\dot y\; =\; w\omega_y + z\omega_x - x\omega_z2y˙=wωy+zωx−xωz）：

  ωy,  −ωz,  0,  ωx⋅w,x,y,zT=ωyw−ωzx+ωxz. \\omega_y,\\; -\\omega_z,\\; 0,\\; \\omega_x \cdot w,x,y,z^T = \omega_y w - \omega_z x + \omega_x z. ωy,−ωz,0,ωx⋅w,x,y,zT=ωyw−ωzx+ωxz.

• 第四行（对应   2z˙  =  wωz+xωy−yωx\;2\dot z\; =\; w\omega_z + x\omega_y - y\omega_x2z˙=wωz+xωy−yωx）：

  ωz,  ωy,  −ωx,  0⋅w,x,y,zT=ωzw+ωyx−ωxy. \\omega_z,\\; \\omega_y,\\; -\\omega_x,\\; 0 \cdot w,x,y,z^T = \omega_z w + \omega_y x - \omega_x y. ωz,ωy,−ωx,0⋅w,x,y,zT=ωzw+ωyx−ωxy.

把这四行组合成矩阵，就得到：

  Ω(ω)=(0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0).   \boxed{\; \Omega(\boldsymbol{\omega}) = \begin{pmatrix} 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{pmatrix}. \;} Ω(ω)= 0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0 .

因此

q˙  =  12 Ω(ω) q \dot q \;=\; \tfrac{1}{2}\,\Omega(\boldsymbol{\omega})\, q q˙=21Ω(ω)q

与上面逐分量展开完全一致（这是对逐元素推导的直接验证）。

---

三、块矩阵与叉乘矩阵的紧凑表示

用块矩阵形式可更直观地看到结构。设 ω=ωx,ωy,ωzT\omega = \\omega_x,\\omega_y,\\omega_z^Tω=ωx,ωy,ωzT，设叉乘（反对称）矩阵为

ω×=(0−ωzωyωz0−ωx−ωyωx0). \\omega_\times = \begin{pmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{pmatrix}. ω×= 0ωz−ωy−ωz0ωxωy−ωx0 .

那么 Ω(ω)\Omega(\omega)Ω(ω) 可写为块形式：

  Ω(ω)(0−ωTω−ω×)   \boxed{\; \Omega(\boldsymbol{\omega}) \begin{pmatrix} 0 & -\boldsymbol{\omega}^T \\ \boldsymbol{\omega} & -\\boldsymbol{\\omega}_\times \end{pmatrix} \;} Ω(ω)(0ω−ωT−ω×)

即上边第一行是 0,  −ωT0,\\; -\\omega\^T0,−ωT，下部左列是 ω\omegaω，右下块是 −ω×-\\omega_\times−ω×。可以检验：把这个块矩阵乘以 q=w; uq=w;\\, \\mathbf{u}q=w;u 恰好给出分量展开的结果。

---

四、关于不同约定（左乘 vs 右乘、角速度参考系）------常见误区说明

• 上面推导基于公式 q˙=12 q⊗(0,ω)\dot q = \tfrac12\, q \otimes (0,\omega)q˙=21q⊗(0,ω)，其中 ω\omegaω 是 body-frame 的角速度（机体坐标下）。如果你使用的是惯性系表达（inertial-frame）并采用 q˙=12(0,ω)⊗q\dot q = \tfrac12 (0,\omega)\otimes qq˙=21(0,ω)⊗q，则对应的矩阵会不同（会出现 −ΩT-\Omega^T−ΩT 或其它符号差异）。因此实际使用时必须确认角速度表示的参考系与四元数乘法约定（左/右乘）。
• 另外，不同库对四元数分量顺序的约定不同（scalar-first 或 scalar-last）。我们的 Ω(ω)\Omega(\omega)Ω(ω) 对应的是 scalar-first q=w,x,y,zTq=w,x,y,z^Tq=w,x,y,zT。若用 scalar-last q′=x,y,z,wTq'=x,y,z,w^Tq′=x,y,z,wT，矩阵形式需重新排列行列。

---

五、导数的雅可比（快速引用-详细参考7节内容）

由 q˙=12Ω(ω)q\dot q = \tfrac12 \Omega(\omega) qq˙=21Ω(ω)q，可直接得到雅可比：

• 关于 qqq：

  ∂q˙∂q=12Ω(ω). \frac{\partial \dot q}{\partial q} = \tfrac12 \Omega(\omega). ∂q∂q˙=21Ω(ω).

• 关于 ω\omegaω：

  从分量式直接写出一个 4×34\times34×3 矩阵（把乘以 ω\omegaω 的矩阵抽出）：

  ∂q˙∂ω=12(−x−y−zw0−z0wx00w) \frac{\partial \dot q}{\partial \boldsymbol{\omega}} = \tfrac12 \begin{pmatrix} -x & -y & -z \\ w & 0 & -z \\ 0 & w & x \\ 0 & 0 & w \end{pmatrix} ∂ω∂q˙=21 −xw00−y0w0−z−zxw

  （建议直接使用更清晰的块形式）

  ∂q˙∂ω=12(−uTwI3+u×). \frac{\partial \dot q}{\partial \boldsymbol{\omega}} = \tfrac12 \begin{pmatrix} -\mathbf{u}^T \\ w I_3 + \\mathbf{u}_\times \end{pmatrix}. ∂ω∂q˙=21(−uTwI3+u×).

  以上两种形式等价（后者更紧凑且便于实现）。

---

六、数值/实现------Eigen 风格代码（scalar-first）

下面给出可直接拷贝的 C++ / Eigen 代码片段，用于构造 Ω(ω)\Omega(\omega)Ω(ω) 以及计算 q˙=0.5∗Ω∗q\dot q = 0.5 * \Omega * qq˙=0.5∗Ω∗q。

#include <Eigen/Dense>

// q is Eigen::Vector4d [w,x,y,z]^T
// omega is Eigen::Vector3d [wx,wy,wz]^T

Eigen::Matrix4d OmegaFromOmega(const Eigen::Vector3d& omega) {
    double wx = omega(0), wy = omega(1), wz = omega(2);

    Eigen::Matrix4d Omega;
    Omega << 0.0,   -wx,   -wy,   -wz,
             wx,    0.0,   wz,    -wy,
             wy,   -wz,    0.0,   wx,
             wz,    wy,   -wx,    0.0;
    return Omega;
}

// usage:
// Eigen::Vector4d q_dot = 0.5 * OmegaFromOmega(omega) * q;

若个人更喜欢用叉乘矩阵的块表达（更易读）：

Eigen::Matrix3d skew(const Eigen::Vector3d& v) {
    Eigen::Matrix3d S;
    S <<  0.0,   -v.z(),  v.y(),
          v.z(),  0.0,   -v.x(),
         -v.y(),  v.x(),  0.0;
    return S;
}

Eigen::Matrix4d OmegaFromOmega_block(const Eigen::Vector3d& w) {
    Eigen::Matrix4d M = Eigen::Matrix4d::Zero();
    M(0,1) = -w(0); M(0,2) = -w(1); M(0,3) = -w(2);
    M.block<3,1>(1,0) = w;
    M.block<3,3>(1,1) = -skew(w);
    return M;
}

---

七、简单验证

举例：令 q=1,0,0,0Tq=1,0,0,0^Tq=1,0,0,0T（单位四元数），ω=ωx,0,0T\omega=\\omega_x,0,0^Tω=ωx,0,0T。则

• 手工分量推导：
  w˙=0\dot w = 0w˙=0（因为 u=0）；
  x˙=12(1⋅ωx+0)=12ωx\dot x = \tfrac12 (1 \cdot \omega_x + 0) = \tfrac12 \omega_xx˙=21(1⋅ωx+0)=21ωx;
  y˙=z˙=0\dot y=\dot z=0y˙=z˙=0.

• 用矩阵 Ω(ω)\Omega(\omega)Ω(ω)：
  Ωq=0,ωx,ωy,ωzT\Omega q = 0,\\omega_x, \\omega_y, \\omega_z^TΩq=0,ωx,ωy,ωzT with only ωx nonzero -> gives same result. 所以一致。

可以用上面 Eigen 代码快速测试更多随机输入，比较分量展开与矩阵乘法结果必然完全一致。

---

八、总结要点

1. Ω(ω)\Omega(\omega)Ω(ω) 的标准块写法： Ω=(0−ωTω−ω×)\displaystyle \Omega=\begin{pmatrix}0 & -\omega^T\\4pt\omega & -\\omega_\times\end{pmatrix}Ω=(0ω−ωT−ω×)。
2. 本推导假定 q=w,x,y,zTq=w,x,y,z^Tq=w,x,y,zT（scalar-first）并采用 q˙=12q⊗(0,ω)\dot q = \tfrac12 q\otimes(0,\omega)q˙=21q⊗(0,ω) ------ 即 ω\omegaω 为 body-frame 角速度。
3. 若大家代码库使用不同的四元数顺序或不同的乘法约定（左/右），请相应重排列或取转置/符号变换。
4. ∂q˙/∂q=12Ω(ω)\partial \dot q/\partial q = \tfrac12 \Omega(\omega)∂q˙/∂q=21Ω(ω) 可直接用于线性化（EKF）或状态转移雅可比。

---