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

一、起点公式

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

$$\dot{q}=\frac{1}{2}q\otimes (0,\mathbf{\omega }).$$

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

$$(a,\mathbf{b})\otimes (c,\mathbf{d})=(ac-\mathbf{b}\cdot \mathbf{d},a\mathbf{d}+c\mathbf{b}+\mathbf{b}\times \mathbf{d})$$

对 $q$ 与 $(0,\mathbf{\omega })$ 代入得：

• 标量部分：

$$\dot{w}=\frac{1}{2}(-\mathbf{u}\cdot \mathbf{\omega })=\frac{1}{2}(-x{\omega }_x-y{\omega }_y-z{\omega }_z).$$

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

$$\dot{\mathbf{v}}=\frac{1}{2}(w\mathbf{\omega }+\mathbf{u}\times \mathbf{\omega }).$$

写出分量：

$$\begin{array}{rl}\dot{x}& =\frac{1}{2}(w{\omega }_x+(y{\omega }_z-z{\omega }_y)),\\ \dot{y}& =\frac{1}{2}(w{\omega }_y+(z{\omega }_x-x{\omega }_z)),\\ \dot{z}& =\frac{1}{2}(w{\omega }_z+(x{\omega }_y-y{\omega }_x)).\end{array}$$

---

二、目标 —— 以矩阵乘法写成 $\dot{q}=\frac{1}{2}\Omega (\mathbf{\omega })q$

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

• 第一行（对应 $2\dot{w}=-x{\omega }_x-y{\omega }_y-z{\omega }_z$）：

  $$[0,-{\omega }_x,-{\omega }_y,-{\omega }_z]\cdot [w,x,y,z{]}^T=-x{\omega }_x-y{\omega }_y-z{\omega }_z.$$

• 第二行（对应 $2\dot{x}=w{\omega }_x+y{\omega }_z-z{\omega }_y$）：

  $$[{\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.$$

• 第三行（对应 $2\dot{y}=w{\omega }_y+z{\omega }_x-x{\omega }_z$）：

  $$[{\omega }_y,-{\omega }_z,0,{\omega }_x]\cdot [w,x,y,z{]}^T={\omega }_{y}w-{\omega }_{z}x+{\omega }_{x}z.$$

• 第四行（对应 $2\dot{z}=w{\omega }_z+x{\omega }_y-y{\omega }_x$）：

  $$[{\omega }_z,{\omega }_y,-{\omega }_x,0]\cdot [w,x,y,z{]}^T={\omega }_{z}w+{\omega }_{y}x-{\omega }_{x}y.$$

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

$$\boxed{\Omega (\mathbf{\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).}$$

因此

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

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

---

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

用块矩阵形式可更直观地看到结构。设 $\omega =[{\omega }_x,{\omega }_y,{\omega }_z{]}^T$，设叉乘（反对称）矩阵为

$$[\omega {]}_{\times }=\left(\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right).$$

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

$$\boxed{\Omega (\mathbf{\omega })\left(\begin{array}{cc}0& -{\mathbf{\omega }}^T\\ \mathbf{\omega }& -[\mathbf{\omega }{]}_{\times }\end{array}\right)}$$

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

---

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

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

---

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

由 $\dot{q}=\frac{1}{2}\Omega (\omega )q$，可直接得到雅可比：

• 关于 $q$：

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

• 关于 $\omega$：
  从分量式直接写出一个 $4\times 3$ 矩阵（把乘以 $\omega$ 的矩阵抽出）：

  $$\frac{\partial \dot{q}}{\partial \mathbf{\omega }}=\frac{1}{2}\left(\begin{array}{ccc}-x& -y& -z\\ w& 0& -z\\ 0& w& x\\ 0& 0& w\end{array}\right)$$

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

  $$\frac{\partial \dot{q}}{\partial \mathbf{\omega }}=\frac{1}{2}\left(\begin{array}{c}-{\mathbf{u}}^T\\ w{I}_3+[\mathbf{u}{]}_{\times }\end{array}\right).$$

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

---

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

下面给出可直接拷贝的 C++ / Eigen 代码片段，用于构造 $\Omega (\omega )$ 以及计算 $\dot{q}=0.5\ast \Omega \ast 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,0{]}^T$（单位四元数），$\omega =[{\omega }_x,0,0{]}^T$。则

• 手工分量推导：
  $\dot{w}=0$（因为 u=0）；
  $\dot{x}=\frac{1}{2}(1\cdot {\omega }_x+0)=\frac{1}{2}{\omega }_x$;
  $\dot{y}=\dot{z}=0$.

• 用矩阵 $\Omega (\omega )$：
  $\Omega q=[0,{\omega }_x,{\omega }_y,{\omega }_z{]}^T$ with only ωx nonzero -> gives same result. 所以一致。

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

---

八、总结要点

1. $\Omega (\omega )$ 的标准块写法： $\Omega =\left(\begin{array}{cc}0& -{\omega }^T\\ \omega & -[\omega {]}_{\times }\end{array}\right)$。
2. 本推导假定 $q=[w,x,y,z{]}^T$（scalar-first）并采用 $\dot{q}=\frac{1}{2}q\otimes (0,\omega )$ —— 即 $\omega$ 为 body-frame 角速度。
3. 若大家代码库使用不同的四元数顺序或不同的乘法约定（左/右），请相应重排列或取转置/符号变换。
4. $\partial \dot{q}/\partial q=\frac{1}{2}\Omega (\omega )$ 可直接用于线性化（EKF）或状态转移雅可比。

---