四元数 (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,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 // 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; ``` 若个人更喜欢用叉乘矩阵的块表达(更易读): ```cpp 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\]Tq=\[1,0,0,0\]\^Tq=\[1,0,0,0\]T(单位四元数),ω=\[ωx,0,0\]T\\omega=\[\\omega_x,0,0\]\^Tω=\[ωx,0,0\]T。则 * 手工分量推导: 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,ωz\]T\\Omega q = \[0,\\omega_x, \\omega_y, \\omega_z\]\^TΩq=\[0,ωx,ωy,ωz\]T 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,z\]Tq=\[w,x,y,z\]\^Tq=\[w,x,y,z\]T(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)或状态转移雅可比。 *** ** * ** ***

相关推荐
西西弗Sisyphus2 小时前
四元数(Quaternion)、叉积(Cross Product)与点积(Dot Product)之间的关系
线性代数·机器学习·行列式·叉积·点积·四元数
却道天凉_好个秋2 小时前
OpenCV(二十八):双边滤波
人工智能·opencv·计算机视觉
kyle~2 小时前
算法---贪心算法(Greedy Algorithm)
算法·贪心算法
fashion 道格2 小时前
C 语言数组拼接:从基础实现到细节优化
算法
头发还没掉光光3 小时前
Linux多线程之自旋锁与读写锁
linux·运维·算法
fashion 道格3 小时前
C 语言希尔排序:原理、实现与性能深度解析
数据结构·算法·排序算法
初夏睡觉3 小时前
P1048 [NOIP 2005 普及组] 采药
数据结构·c++·算法
小欣加油3 小时前
leetcode 1513 仅含1的子串数
c++·算法·leetcode·职场和发展
树在风中摇曳3 小时前
【C语言预处理器全解析】宏、条件编译、字符串化、拼接
c语言·算法