一、起点公式
连续时间单位四元数 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,z]Tq=[w,x,y,z]^Tq=[w,x,y,z]T 的乘积应当对应:
-
第一行(对应 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,z\]T=−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,z\]T=−xωx−yωy−zωz.
ωx, 0, ωz, −ωy\]⋅\[w,x,y,z\]T=ω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,z\]T=ωxw+0⋅x+ωzy−ωyz.
ωy, −ωz, 0, ωx\]⋅\[w,x,y,z\]T=ω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,z\]T=ωyw−ωzx+ωxz.
ωz, ωy, −ωx, 0\]⋅\[w,x,y,z\]T=ω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,z\]T=ω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,ωz]T\omega = [\omega_x,\omega_y,\omega_z]^Tω=[ωx,ωy,ωz]T,设叉乘(反对称)矩阵为
ω\]×=(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, −ωT\]\[0,\\; -\\omega\^T\]\[0,−ωT\],下部左列是 ω\\omegaω,右下块是 −\[ω\]×-\[\\omega\]_\\times−\[ω\]×。可以检验:把这个块矩阵乘以 q=\[w; u\]q=\[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,z\]Tq=\[w,x,y,z\]\^Tq=\[w,x,y,z\]T。若用 scalar-last q′=\[x,y,z,w\]Tq'=\[x,y,z,w\]\^Tq′=\[x,y,z,w\]T,矩阵形式需重新排列行列。
*** ** * ** ***
### 五、导数的雅可比(快速引用-详细参考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。
```cpp
#include