[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-4(1) 刚体的速度与角速度

本文仅供学习使用,总结很多本现有讲述运动学或动力学书籍后的总结,从矢量的角度进行分析,方法比较传统,但更易理解,并且现有的看似抽象方法,两者本质上并无不同。

2024年底本人学位论文发表后方可摘抄

若有帮助请引用
本文参考:
.
食用方法

求解逻辑:速度与加速度都是在知道角速度与角加速度的前提下------旋转运动更重要

所求得的速度表达-需要考虑是否为刚体相对固定点!

旋转矩阵?转换矩阵?有什么意义和性质?------与角速度与角加速度的关系
务必自己推导全部公式,并理解每个符号的含义

机构运动学与动力学分析与建模 Ch00-4 刚体的速度与角速度 Part1

  • [4. 刚体的速度与角速度](#4. 刚体的速度与角速度)
    • [4.1 角速度的表达](#4.1 角速度的表达)
      • [4.1.1 欧拉参数表示角速度](#4.1.1 欧拉参数表示角速度)
      • [4.1.2 轴角参数表示角速度](#4.1.2 轴角参数表示角速度)
      • [4.1.3 轴角参数表示角速度](#4.1.3 轴角参数表示角速度)

4. 刚体的速度与角速度

对于运动坐标系下任意一点 P i P_{\mathrm{i}} Pi而言,有:
R ⃗ P F = R ⃗ M F + [ Q M F ] R ⃗ P i M ⇒ v ⃗ P F = v ⃗ M F + [ Q ˙ M F ] R ⃗ P i M + [ Q M F ] R ⃗ ˙ P i M = ω ⃗ F × R ⃗ P F = ω ⃗ ~ F R ⃗ P F = ω ⃗ ~ F ( R ⃗ M F + [ Q M F ] R ⃗ P i M ) ⇒ [ Q ˙ M F ] R ⃗ P i M + [ Q M F ] R ⃗ ˙ P i M = ω ⃗ ~ F [ Q M F ] R ⃗ P i M ⇒ v ⃗ P i M = ( [ Q M F ] T ω ⃗ ~ F [ Q M F ] − [ Q M F ] T [ Q ˙ M F ] ) R ⃗ P i M = ( ( [ Q M F ] T ω ⃗ F ) ~ − [ Q M F ] T [ Q ˙ M F ] ) R ⃗ P i M = ( ω ⃗ ~ M − [ Q M F ] T [ Q ˙ M F ] ) R ⃗ P i M \begin{split} &\vec{R}{\mathrm{P}}^{F}=\vec{R}{\mathrm{M}}^{F}+\left[ Q_{\mathrm{M}}^{F} \right] \vec{R}{\mathrm{P}{\mathrm{i}}}^{M} \\ &\Rightarrow \vec{v}{\mathrm{P}}^{F}=\vec{v}{\mathrm{M}}^{F}+\left[ \dot{Q}{\mathrm{M}}^{F} \right] \vec{R}{\mathrm{P}{\mathrm{i}}}^{M}+\left[ Q{\mathrm{M}}^{F} \right] \dot{\vec{R}}{\mathrm{P}{\mathrm{i}}}^{M}=\vec{\omega}^F\times \vec{R}{\mathrm{P}}^{F}=\tilde{\vec{\omega}}^F\vec{R}{\mathrm{P}}^{F}=\tilde{\vec{\omega}}^F\left( \vec{R}{\mathrm{M}}^{F}+\left[ Q{\mathrm{M}}^{F} \right] \vec{R}{\mathrm{P}{\mathrm{i}}}^{M} \right) \\ &\Rightarrow \left[ \dot{Q}{\mathrm{M}}^{F} \right] \vec{R}{\mathrm{P}{\mathrm{i}}}^{M}+\left[ Q{\mathrm{M}}^{F} \right] \dot{\vec{R}}{\mathrm{P}{\mathrm{i}}}^{M}=\tilde{\vec{\omega}}^F\left[ Q_{\mathrm{M}}^{F} \right] \vec{R}{\mathrm{P}{\mathrm{i}}}^{M} \\ &\Rightarrow \vec{v}{\mathrm{P}{\mathrm{i}}}^{M}=\left( \left[ Q_{\mathrm{M}}^{F} \right] ^{\mathrm{T}}\tilde{\vec{\omega}}^F\left[ Q_{\mathrm{M}}^{F} \right] -\left[ Q_{\mathrm{M}}^{F} \right] ^{\mathrm{T}}\left[ \dot{Q}{\mathrm{M}}^{F} \right] \right) \vec{R}{\mathrm{P}{\mathrm{i}}}^{M}=\left( \widetilde{\left( \left[ Q{\mathrm{M}}^{F} \right] ^{\mathrm{T}}\vec{\omega}^F \right) }-\left[ Q_{\mathrm{M}}^{F} \right] ^{\mathrm{T}}\left[ \dot{Q}{\mathrm{M}}^{F} \right] \right) \vec{R}{\mathrm{P}{\mathrm{i}}}^{M}=\left( \tilde{\vec{\omega}}^M-\left[ Q{\mathrm{M}}^{F} \right] ^{\mathrm{T}}\left[ \dot{Q}{\mathrm{M}}^{F} \right] \right) \vec{R}{\mathrm{P}_{\mathrm{i}}}^{M} \end{split} R PF=R MF+[QMF]R PiM⇒v PF=v MF+[Q˙MF]R PiM+[QMF]R ˙PiM=ω F×R PF=ω ~FR PF=ω ~F(R MF+[QMF]R PiM)⇒[Q˙MF]R PiM+[QMF]R ˙PiM=ω ~F[QMF]R PiM⇒v PiM=([QMF]Tω ~F[QMF]−[QMF]T[Q˙MF])R PiM=(([QMF]Tω F) −[QMF]T[Q˙MF])R PiM=(ω ~M−[QMF]T[Q˙MF])R PiM

因此,当 P i P_{\mathrm{i}} Pi为刚体上的固定点时,有: v ⃗ P i M = 0 \vec{v}{\mathrm{P}{\mathrm{i}}}^{M}=0 v PiM=0,进而可知:

Q M F \] T ω ⃗ \~ F \[ Q M F \] − \[ Q M F \] T \[ Q ˙ M F \] = 0 ⇒ ω ⃗ \~ F = \[ Q ˙ M F \] \[ Q M F \] T ω ⃗ \~ M − \[ Q M F \] T \[ Q ˙ M F \] = 0 ⇒ ω ⃗ \~ M = \[ Q M F \] T \[ Q ˙ M F \] \\begin{split} \\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\tilde{\\vec{\\omega}}\^F\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] -\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] =0\&\\Rightarrow \\tilde{\\vec{\\omega}}\^F=\\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}} \\\\ \\tilde{\\vec{\\omega}}\^M-\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] =0\&\\Rightarrow \\tilde{\\vec{\\omega}}\^M=\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \\end{split} \[QMF\]Tω \~F\[QMF\]−\[QMF\]T\[Q˙MF\]=0ω \~M−\[QMF\]T\[Q˙MF\]=0⇒ω \~F=\[Q˙MF\]\[QMF\]T⇒ω \~M=\[QMF\]T\[Q˙MF

对转换矩阵 [ Q M F ] T \left[ Q_{\mathrm{M}}^{F} \right] ^{\mathrm{T}} [QMF]T而言,有:

Q M F \] T \[ Q M F \] = 0 ⇒ \[ Q ˙ M F \] T \[ Q M F \] + \[ Q M F \] T \[ Q ˙ M F \] = 0 ⇒ \[ Q ˙ M F \] T \[ Q M F \] + \[ \[ Q ˙ M F \] T \[ Q M F \] \] T = 0 \\begin{split} \&\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] =0 \\\\ \\Rightarrow \&\\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] +\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] =0 \\\\ \\Rightarrow \&\\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] +\\left\[ \\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\right\] \^{\\mathrm{T}}=0 \\end{split} ⇒⇒\[QMF\]T\[QMF\]=0\[Q˙MF\]T\[QMF\]+\[QMF\]T\[Q˙MF\]=0\[Q˙MF\]T\[QMF\]+\[\[Q˙MF\]T\[QMF\]\]T=0 即, \[ Q ˙ M F \] T \[ Q M F \] \\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \[Q˙MF\]T\[QMF\]为反(斜)对称矩阵。 因此,对于矩阵 ω ⃗ \~ F \\tilde{\\vec{\\omega}}\^F ω \~F与 ω ⃗ \~ M \\tilde{\\vec{\\omega}}\^M ω \~M具有如下转换关系: ω ⃗ \~ M = \[ Q M F \] T ω ⃗ \~ F \[ Q M F \] ω ⃗ \~ F = \[ Q M F \] ω ⃗ \~ M \[ Q M F \] T \\begin{split} \\tilde{\\vec{\\omega}}\^M\&=\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\tilde{\\vec{\\omega}}\^F\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\\\ \\tilde{\\vec{\\omega}}\^F\&=\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\tilde{\\vec{\\omega}}\^M\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}} \\end{split} ω \~Mω \~F=\[QMF\]Tω \~F\[QMF\]=\[QMF\]ω \~M\[QMF\]T 进而可将上式中的`项term` \[ Q ˙ M F \] R ⃗ P i M \\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M} \[Q˙MF\]R PiM改写为(**下式仅当 P i P_{\\mathrm{i}} Pi为刚体上的固定点时成立** ): \[ Q ˙ M F \] R ⃗ P i M = { ω ⃗ \~ F \[ Q M F \] R ⃗ P i M = ω ⃗ \~ F R ⃗ P i F = ω ⃗ F × R ⃗ P i F \[ Q M F \] ω ⃗ \~ M R ⃗ P i M = \[ Q M F \] ( ω ⃗ M × R ⃗ P i M ) \\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M}=\\begin{cases} \\tilde{\\vec{\\omega}}\^F\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M}=\\tilde{\\vec{\\omega}}\^F\\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{F}=\\vec{\\omega}\^F\\times \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{F}\\\\ \\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\tilde{\\vec{\\omega}}\^M\\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M}=\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\left( \\vec{\\omega}\^M\\times \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M} \\right)\\\\ \\end{cases} \[Q˙MF\]R PiM=⎩ ⎨ ⎧ω \~F\[QMF\]R PiM=ω \~FR PiF=ω F×R PiF\[QMF\]ω \~MR PiM=\[QMF\](ω M×R PiM) ### 4.1 角速度的表达 #### 4.1.1 欧拉参数表示角速度 结合定义矩阵: B 3 × 4 = \[ − q 2 q 1 − q 4 q 3 − q 3 q 4 q 1 − q 2 − q 4 − q 3 q 2 q 1 \] B_{3\\times 4}=\\left\[ \\begin{array}{cccc} -q_2\& q_1\& -q_4\& q_3\\\\ -q_3\& q_4\& q_1\& -q_2\\\\ -q_4\& -q_3\& q_2\& q_1\\\\ \\end{array} \\right\] B3×4= −q2−q3−q4q1q4−q3−q4q1q2q3−q2q1 B ˉ 3 × 4 = \[ − q 2 q 1 q 4 − q 3 − q 3 − q 4 q 1 q 2 − q 4 q 3 − q 2 q 1 \] \\bar{B}_{3\\times 4}=\\left\[ \\begin{array}{cccc} -q_2\& q_1\& q_4\& -q_3\\\\ -q_3\& -q_4\& q_1\& q_2\\\\ -q_4\& q_3\& -q_2\& q_1\\\\ \\end{array} \\right\] Bˉ3×4= −q2−q3−q4q1−q4q3q4q1−q2−q3q2q1 , 带入同样的式子可得: ω ⃗ \~ F = 2 B ˉ B ˉ ˙ T ω ⃗ \~ M = 2 B B ˙ T \\begin{split} \\tilde{\\vec{\\omega}}\^F\&=2\\bar{B}\\dot{\\bar{B}}\^{\\mathrm{T}} \\\\ \\tilde{\\vec{\\omega}}\^M\&=2B\\dot{B}\^{\\mathrm{T}} \\end{split} ω \~Fω \~M=2BˉBˉ˙T=2BB˙T 将上式展开,由四元数的归一化可知: q ˙ 1 q 1 + q ˙ 2 q 2 + q ˙ 3 q 3 + q ˙ 4 q 4 = 0 \\dot{q}_1q_1+\\dot{q}_2q_2+\\dot{q}_3q_3+\\dot{q}_4q_4=0 q˙1q1+q˙2q2+q˙3q3+q˙4q4=0,可得: \[ w 1 F w 2 F w 3 F \] = 2 \[ q ˙ 4 q 3 − q ˙ 3 q 4 + q ˙ 2 q 1 − q ˙ 1 q 2 q ˙ 2 q 4 − q ˙ 1 q 3 + q ˙ 4 q 2 − q ˙ 3 q 1 q ˙ 3 q 2 − q ˙ 4 q 1 + q ˙ 1 q 4 − q ˙ 2 q 3 \] \[ w 1 M w 2 M w 3 M \] = 2 \[ q 4 q ˙ 3 − q 3 q ˙ 4 − q 2 q ˙ 1 + q 1 q ˙ 2 q 2 q ˙ 4 + q 1 q ˙ 3 − q 4 q ˙ 2 − q 3 q ˙ 1 q 3 q ˙ 2 − q 4 q ˙ 1 + q 1 q ˙ 4 − q 2 q ˙ 3 \] \\begin{split} \\left\[ \\begin{array}{c} {w_1}\^F\\\\ {w_2}\^F\\\\ {w_3}\^F\\\\ \\end{array} \\right\] \&=2\\left\[ \\begin{array}{c} \\dot{q}_4q_3-\\dot{q}_3q_4+\\dot{q}_2q_1-\\dot{q}_1q_2\\\\ \\dot{q}_2q_4-\\dot{q}_1q_3+\\dot{q}_4q_2-\\dot{q}_3q_1\\\\ \\dot{q}_3q_2-\\dot{q}_4q_1+\\dot{q}_1q_4-\\dot{q}_2q_3\\\\ \\end{array} \\right\] \\\\ \\left\[ \\begin{array}{c} {w_1}\^M\\\\ {w_2}\^M\\\\ {w_3}\^M\\\\ \\end{array} \\right\] \&=2\\left\[ \\begin{array}{c} q_4\\dot{q}_3-q_3\\dot{q}_4-q_2\\dot{q}_1+q_1\\dot{q}_2\\\\ q_2\\dot{q}_4+q_1\\dot{q}_3-q_4\\dot{q}_2-q_3\\dot{q}_1\\\\ q_3\\dot{q}_2-q_4\\dot{q}_1+q_1\\dot{q}_4-q_2\\dot{q}_3\\\\ \\end{array} \\right\] \\end{split} w1Fw2Fw3F w1Mw2Mw3M =2 q˙4q3−q˙3q4+q˙2q1−q˙1q2q˙2q4−q˙1q3+q˙4q2−q˙3q1q˙3q2−q˙4q1+q˙1q4−q˙2q3 =2 q4q˙3−q3q˙4−q2q˙1+q1q˙2q2q˙4+q1q˙3−q4q˙2−q3q˙1q3q˙2−q4q˙1+q1q˙4−q2q˙3 继续观察上式,将上式进行化简: ω ⃗ F = 2 B q ⃗ ˙ = − 2 B ˙ q ⃗ ω ⃗ M = 2 B ˉ q ⃗ ˙ = − 2 B ˉ ˙ q ⃗ \\vec{\\omega}\^F=2B\\dot{\\vec{q}}=-2\\dot{B}\\vec{q} \\\\ \\vec{\\omega}\^M=2\\bar{B}\\dot{\\vec{q}}=-2\\dot{\\bar{B}}\\vec{q} ω F=2Bq ˙=−2B˙q ω M=2Bˉq ˙=−2Bˉ˙q 进而可将 \[ Q ˙ M F \] R ⃗ P i M = \[ Q M F \] ( ω ⃗ M × R ⃗ P i M ) \\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M}=\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\left( \\vec{\\omega}\^M\\times \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M} \\right) \[Q˙MF\]R PiM=\[QMF\](ω M×R PiM)改写为(下式仅当 P i P_{\\mathrm{i}} Pi为刚体上的固定点时成立): \[ Q ˙ M F \] R ⃗ P i M = \[ Q M F \] ( ω ⃗ M × R ⃗ P i M ) = − \[ Q M F \] ( R ⃗ P i M × ω ⃗ M ) = − \[ Q M F \] R ⃗ \~ P i M ( 2 B ˉ q ⃗ ˙ ) \\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M}=\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\left( \\vec{\\omega}\^M\\times \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M} \\right) =-\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\left( \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M}\\times \\vec{\\omega}\^M \\right) =-\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\tilde{\\vec{R}}_{\\mathrm{P}_{\\mathrm{i}}}\^{M}\\left( 2\\bar{B}\\dot{\\vec{q}} \\right) \[Q˙MF\]R PiM=\[QMF\](ω M×R PiM)=−\[QMF\](R PiM×ω M)=−\[QMF\]R \~PiM(2Bˉq ˙) > **因为所有表达方式都能转换成欧拉参数-四元数的形式,因此上式在计算过程中具有普适性。** 进而可知: ∂ ( \[ Q M F \] R ⃗ P i M ) ∂ q ⃗ = − \[ Q M F \] R ⃗ \~ P i M ( 2 B ˉ ) \\frac{\\partial \\left( \\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\vec{R}_{\\mathrm{P}_{\\mathrm{i}}}\^{M} \\right)}{\\partial \\vec{q}}=-\\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \\tilde{\\vec{R}}_{\\mathrm{P}_{\\mathrm{i}}}\^{M}\\left( 2\\bar{B} \\right) ∂q ∂(\[QMF\]R PiM)=−\[QMF\]R \~PiM(2Bˉ) #### 4.1.2 轴角参数表示角速度 将 \[ θ v 1 v 2 v 3 \] = \[ 2 a r c cos ⁡ ( q 1 ) q 2 sin ⁡ θ 2 q 3 sin ⁡ θ 2 q 4 sin ⁡ θ 2 \] \\left\[ \\begin{array}{c} \\theta\\\\ v_1\\\\ v_2\\\\ v_3\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} 2\\mathrm{arc}\\cos \\left( q_1 \\right)\\\\ \\frac{q_2}{\\sin \\frac{\\theta}{2}}\\\\ \\frac{q_3}{\\sin \\frac{\\theta}{2}}\\\\ \\frac{q_4}{\\sin \\frac{\\theta}{2}}\\\\ \\end{array} \\right\] θv1v2v3 = 2arccos(q1)sin2θq2sin2θq3sin2θq4 带入 \[ w 1 F w 2 F w 3 F \] = 2 \[ q ˙ 4 q 3 − q ˙ 3 q 4 + q ˙ 2 q 1 − q ˙ 1 q 2 q ˙ 2 q 4 − q ˙ 1 q 3 + q ˙ 4 q 2 − q ˙ 3 q 1 q ˙ 3 q 2 − q ˙ 4 q 1 + q ˙ 1 q 4 − q ˙ 2 q 3 \] , \[ w 1 M w 2 M w 3 M \] = 2 \[ q 4 q ˙ 3 − q 3 q ˙ 4 − q 2 q ˙ 1 + q 1 q ˙ 2 q 2 q ˙ 4 + q 1 q ˙ 3 − q 4 q ˙ 2 − q 3 q ˙ 1 q 3 q ˙ 2 − q 4 q ˙ 1 + q 1 q ˙ 4 − q 2 q ˙ 3 \] \\left\[ \\begin{array}{c} {w_1}\^F\\\\ {w_2}\^F\\\\ {w_3}\^F\\\\ \\end{array} \\right\] =2\\left\[ \\begin{array}{c} \\dot{q}_4q_3-\\dot{q}_3q_4+\\dot{q}_2q_1-\\dot{q}_1q_2\\\\ \\dot{q}_2q_4-\\dot{q}_1q_3+\\dot{q}_4q_2-\\dot{q}_3q_1\\\\ \\dot{q}_3q_2-\\dot{q}_4q_1+\\dot{q}_1q_4-\\dot{q}_2q_3\\\\ \\end{array} \\right\] , \\left\[ \\begin{array}{c} {w_1}\^M\\\\ {w_2}\^M\\\\ {w_3}\^M\\\\ \\end{array} \\right\] =2\\left\[ \\begin{array}{c} q_4\\dot{q}_3-q_3\\dot{q}_4-q_2\\dot{q}_1+q_1\\dot{q}_2\\\\ q_2\\dot{q}_4+q_1\\dot{q}_3-q_4\\dot{q}_2-q_3\\dot{q}_1\\\\ q_3\\dot{q}_2-q_4\\dot{q}_1+q_1\\dot{q}_4-q_2\\dot{q}_3\\\\ \\end{array} \\right\] w1Fw2Fw3F =2 q˙4q3−q˙3q4+q˙2q1−q˙1q2q˙2q4−q˙1q3+q˙4q2−q˙3q1q˙3q2−q˙4q1+q˙1q4−q˙2q3 , w1Mw2Mw3M =2 q4q˙3−q3q˙4−q2q˙1+q1q˙2q2q˙4+q1q˙3−q4q˙2−q3q˙1q3q˙2−q4q˙1+q1q˙4−q2q˙3 可得: \[ w 1 F w 2 F w 3 F \] = \[ 2 ( v ˙ 3 v 2 − v ˙ 2 v 3 ) sin ⁡ 2 θ 2 + v ˙ 1 sin ⁡ θ + θ ˙ v 1 2 ( v ˙ 1 v 3 − v ˙ 3 v 1 ) sin ⁡ 2 θ 2 + v ˙ 2 sin ⁡ θ + θ ˙ v 2 2 ( v ˙ 2 v 1 − v ˙ 1 v 2 ) sin ⁡ 2 θ 2 + v ˙ 3 sin ⁡ θ + θ ˙ v 3 \] \[ w 1 M w 2 M w 3 M \] = \[ 2 ( v 3 v ˙ 2 − v 2 v ˙ 3 ) sin ⁡ 2 θ 2 + v ˙ 1 sin ⁡ θ + θ ˙ v 1 2 ( v 1 v ˙ 3 − v 3 v ˙ 1 ) sin ⁡ 2 θ 2 + v ˙ 2 sin ⁡ θ + θ ˙ v 2 2 ( v 2 v ˙ 1 − v 1 v ˙ 2 ) sin ⁡ 2 θ 2 + v ˙ 3 sin ⁡ θ + θ ˙ v 3 \] \\begin{split} \\left\[ \\begin{array}{c} {w_1}\^F\\\\ {w_2}\^F\\\\ {w_3}\^F\\\\ \\end{array} \\right\] \&=\\left\[ \\begin{array}{c} 2\\left( \\dot{v}_3v_2-\\dot{v}_2v_3 \\right) \\sin \^2\\frac{\\theta}{2}+\\dot{v}_1\\sin \\theta +\\dot{\\theta}v_1\\\\ 2\\left( \\dot{v}_1v_3-\\dot{v}_3v_1 \\right) \\sin \^2\\frac{\\theta}{2}+\\dot{v}_2\\sin \\theta +\\dot{\\theta}v_2\\\\ 2\\left( \\dot{v}_2v_1-\\dot{v}_1v_2 \\right) \\sin \^2\\frac{\\theta}{2}+\\dot{v}_3\\sin \\theta +\\dot{\\theta}v_3\\\\ \\end{array} \\right\] \\\\ \\left\[ \\begin{array}{c} {w_1}\^M\\\\ {w_2}\^M\\\\ {w_3}\^M\\\\ \\end{array} \\right\] \&=\\left\[ \\begin{array}{c} 2\\left( v_3\\dot{v}_2-v_2\\dot{v}_3 \\right) \\sin \^2\\frac{\\theta}{2}+\\dot{v}_1\\sin \\theta +\\dot{\\theta}v_1\\\\ 2\\left( v_1\\dot{v}_3-v_3\\dot{v}_1 \\right) \\sin \^2\\frac{\\theta}{2}+\\dot{v}_2\\sin \\theta +\\dot{\\theta}v_2\\\\ 2\\left( v_2\\dot{v}_1-v_1\\dot{v}_2 \\right) \\sin \^2\\frac{\\theta}{2}+\\dot{v}_3\\sin \\theta +\\dot{\\theta}v_3\\\\ \\end{array} \\right\] \\end{split} w1Fw2Fw3F w1Mw2Mw3M = 2(v˙3v2−v˙2v3)sin22θ+v˙1sinθ+θ˙v12(v˙1v3−v˙3v1)sin22θ+v˙2sinθ+θ˙v22(v˙2v1−v˙1v2)sin22θ+v˙3sinθ+θ˙v3 = 2(v3v˙2−v2v˙3)sin22θ+v˙1sinθ+θ˙v12(v1v˙3−v3v˙1)sin22θ+v˙2sinθ+θ˙v22(v2v˙1−v1v˙2)sin22θ+v˙3sinθ+θ˙v3 整理为: ω ⃗ F = 2 v ⃗ F × v ⃗ ˙ F sin ⁡ 2 θ 2 + v ⃗ ˙ F sin ⁡ θ + θ ˙ v ⃗ F ω ⃗ M = 2 v ⃗ ˙ F × v ⃗ F sin ⁡ 2 θ 2 + v ⃗ ˙ F sin ⁡ θ + θ ˙ v ⃗ F \\begin{split} \\vec{\\omega}\^F\&=2\\vec{v}\^F\\times \\dot{\\vec{v}}\^F\\sin \^2\\frac{\\theta}{2}+\\dot{\\vec{v}}\^F\\sin \\theta +\\dot{\\theta}\\vec{v}\^F \\\\ \\vec{\\omega}\^M\&=2\\dot{\\vec{v}}\^F\\times \\vec{v}\^F\\sin \^2\\frac{\\theta}{2}+\\dot{\\vec{v}}\^F\\sin \\theta +\\dot{\\theta}\\vec{v}\^F \\end{split} ω Fω M=2v F×v ˙Fsin22θ+v ˙Fsinθ+θ˙v F=2v ˙F×v Fsin22θ+v ˙Fsinθ+θ˙v F #### 4.1.3 轴角参数表示角速度 对于ZYX欧拉角而言,有: { \[ Q M F \] = \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q F 2 F 1 ( j ⃗ F , β ) \] \[ Q F 1 F ( i ⃗ F , α ) \] ω ⃗ \~ F = \[ Q ˙ M F \] \[ Q M F \] T ω ⃗ \~ F = { \[ Q ˙ F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q F 2 F 1 ( j ⃗ F , β ) \] \[ Q F 1 F ( i ⃗ F , α ) \] ⋅ \[ Q F 1 F ( i ⃗ F , α ) \] T \[ Q F 2 F 1 ( j ⃗ F , β ) \] T \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] T + \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q ˙ F 2 F 1 ( j ⃗ F , β ) \] \[ Q F 1 F ( i ⃗ F , α ) \] ⋅ \[ Q F 1 F ( i ⃗ F , α ) \] T \[ Q F 2 F 1 ( j ⃗ F , β ) \] T \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] T + \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q F 2 F 1 ( j ⃗ F , β ) \] \[ Q ˙ F 1 F ( i ⃗ F , α ) \] ⋅ \[ Q F 1 F ( i ⃗ F , α ) \] T \[ Q F 2 F 1 ( j ⃗ F , β ) \] T \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] T ω ⃗ \~ F = { \[ Q ˙ F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] T + \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q ˙ F 2 F 1 ( j ⃗ F , β ) \] \[ Q F 2 F 1 ( j ⃗ F , β ) \] T \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] T + \[ \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q F 2 F 1 ( j ⃗ F , β ) \] \] \[ Q ˙ F 1 F ( i ⃗ F , α ) \] ⋅ \[ Q F 1 F ( i ⃗ F , α ) \] T \[ \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q F 2 F 1 ( j ⃗ F , β ) \] \] T ω ⃗ \~ F = ω ⃗ \~ F 3 ( M ) F 2 + \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] ω ⃗ \~ F 2 F 1 \~ + \[ \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q F 2 F 1 ( j ⃗ F , β ) \] \] ω ⃗ \~ F 1 F \~ ⇒ ω ⃗ F = ω ⃗ F 3 ( M ) F 2 + \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] ω ⃗ F 2 F 1 + \[ \[ Q F 3 ( M ) F 2 ( k ⃗ F , γ ) \] \[ Q F 2 F 1 ( j ⃗ F , β ) \] \] ω ⃗ F 1 F ⇒ ω ⃗ F = \[ 0 0 γ ˙ \] + \[ cos ⁡ γ − sin ⁡ γ 0 sin ⁡ γ cos ⁡ γ 0 0 0 1 \] \[ 0 β ˙ 0 \] + \[ cos ⁡ γ − sin ⁡ γ 0 sin ⁡ γ cos ⁡ γ 0 0 0 1 \] \[ cos ⁡ β 0 sin ⁡ β 0 1 0 − sin ⁡ β 0 cos ⁡ β \] \[ α ˙ 0 0 \] ⇒ ω ⃗ F = \[ cos ⁡ β cos ⁡ γ − sin ⁡ γ 0 cos ⁡ β sin ⁡ γ cos ⁡ γ 0 − sin ⁡ β 0 1 \] \[ α ˙ β ˙ γ ˙ \] \\begin{split} \&\\begin{cases} \\left\[ Q_{\\mathrm{M}}\^{F} \\right\] =\\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\left\[ Q_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\]\\\\ \\tilde{\\vec{\\omega}}\^F=\\left\[ \\dot{Q}_{\\mathrm{M}}\^{F} \\right\] \\left\[ Q_{\\mathrm{M}}\^{F} \\right\] \^{\\mathrm{T}}\\\\ \\end{cases} \\\\ \\tilde{\\vec{\\omega}}\^F\&=\\begin{cases} \\left\[ \\dot{Q}_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\left\[ Q_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\] \\cdot \\left\[ Q_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \^{\\mathrm{T}}+\\\\ \\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ \\dot{Q}_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\left\[ Q_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\] \\cdot \\left\[ Q_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \^{\\mathrm{T}}+\\\\ \\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\left\[ \\dot{Q}_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\] \\cdot \\left\[ Q_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \^{\\mathrm{T}}\\\\ \\end{cases} \\\\ \\tilde{\\vec{\\omega}}\^F\&=\\begin{cases} \\left\[ \\dot{Q}_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \^{\\mathrm{T}}+\\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ \\dot{Q}_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \^{\\mathrm{T}}\\\\ +\\left\[ \\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\right\] \\left\[ \\dot{Q}_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\] \\cdot \\left\[ Q_{\\mathrm{F}_1}\^{F}\\left( \\vec{i}\^F,\\alpha \\right) \\right\] \^{\\mathrm{T}}\\left\[ \\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\right\] \^{\\mathrm{T}}\\\\ \\end{cases} \\\\ \\tilde{\\vec{\\omega}}\^F\&=\\tilde{\\vec{\\omega}}_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}+\\widetilde{\\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\tilde{\\vec{\\omega}}_{\\mathrm{F}_2}\^{F_1}}+\\widetilde{\\left\[ \\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\right\] \\tilde{\\vec{\\omega}}_{\\mathrm{F}_1}\^{F}} \\\\ \\Rightarrow \\vec{\\omega}\^F\&=\\vec{\\omega}_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}+\\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\vec{\\omega}_{\\mathrm{F}_2}\^{F_1}+\\left\[ \\left\[ Q_{\\mathrm{F}_3\\left( M \\right)}\^{F_2}\\left( \\vec{k}\^F,\\gamma \\right) \\right\] \\left\[ Q_{\\mathrm{F}_2}\^{F_1}\\left( \\vec{j}\^F,\\beta \\right) \\right\] \\right\] \\vec{\\omega}_{\\mathrm{F}_1}\^{F} \\\\ \\Rightarrow \\vec{\\omega}\^F\&=\\left\[ \\begin{array}{c} 0\\\\ 0\\\\ \\dot{\\gamma}\\\\ \\end{array} \\right\] +\\left\[ \\begin{matrix} \\cos \\gamma\& -\\sin \\gamma\& 0\\\\ \\sin \\gamma\& \\cos \\gamma\& 0\\\\ 0\& 0\& 1\\\\ \\end{matrix} \\right\] \\left\[ \\begin{array}{c} 0\\\\ \\dot{\\beta}\\\\ 0\\\\ \\end{array} \\right\] +\\left\[ \\begin{matrix} \\cos \\gamma\& -\\sin \\gamma\& 0\\\\ \\sin \\gamma\& \\cos \\gamma\& 0\\\\ 0\& 0\& 1\\\\ \\end{matrix} \\right\] \\left\[ \\begin{matrix} \\cos \\beta\& 0\& \\sin \\beta\\\\ 0\& 1\& 0\\\\ -\\sin \\beta\& 0\& \\cos \\beta\\\\ \\end{matrix} \\right\] \\left\[ \\begin{array}{c} \\dot{\\alpha}\\\\ 0\\\\ 0\\\\ \\end{array} \\right\] \\\\ \\Rightarrow \\vec{\\omega}\^F\&=\\left\[ \\begin{matrix} \\cos \\beta \\cos \\gamma\& -\\sin \\gamma\& 0\\\\ \\cos \\beta \\sin \\gamma\& \\cos \\gamma\& 0\\\\ -\\sin \\beta\& 0\& 1\\\\ \\end{matrix} \\right\] \\left\[ \\begin{array}{c} \\dot{\\alpha}\\\\ \\dot{\\beta}\\\\ \\dot{\\gamma}\\\\ \\end{array} \\right\] \\end{split} ω \~Fω \~Fω \~F⇒ω F⇒ω F⇒ω F⎩ ⎨ ⎧\[QMF\]=\[QF3(M)F2(k F,γ)\]\[QF2F1(j F,β)\]\[QF1F(i F,α)\]ω \~F=\[Q˙MF\]\[QMF\]T=⎩ ⎨ ⎧\[Q˙F3(M)F2(k F,γ)\]\[QF2F1(j F,β)\]\[QF1F(i F,α)\]⋅\[QF1F(i F,α)\]T\[QF2F1(j F,β)\]T\[QF3(M)F2(k F,γ)\]T+\[QF3(M)F2(k F,γ)\]\[Q˙F2F1(j F,β)\]\[QF1F(i F,α)\]⋅\[QF1F(i F,α)\]T\[QF2F1(j F,β)\]T\[QF3(M)F2(k F,γ)\]T+\[QF3(M)F2(k F,γ)\]\[QF2F1(j F,β)\]\[Q˙F1F(i F,α)\]⋅\[QF1F(i F,α)\]T\[QF2F1(j F,β)\]T\[QF3(M)F2(k F,γ)\]T=⎩ ⎨ ⎧\[Q˙F3(M)F2(k F,γ)\]\[QF3(M)F2(k F,γ)\]T+\[QF3(M)F2(k F,γ)\]\[Q˙F2F1(j F,β)\]\[QF2F1(j F,β)\]T\[QF3(M)F2(k F,γ)\]T+\[\[QF3(M)F2(k F,γ)\]\[QF2F1(j F,β)\]\]\[Q˙F1F(i F,α)\]⋅\[QF1F(i F,α)\]T\[\[QF3(M)F2(k F,γ)\]\[QF2F1(j F,β)\]\]T=ω \~F3(M)F2+\[QF3(M)F2(k F,γ)\]ω \~F2F1 +\[\[QF3(M)F2(k F,γ)\]\[QF2F1(j F,β)\]\]ω \~F1F =ω F3(M)F2+\[QF3(M)F2(k F,γ)\]ω F2F1+\[\[QF3(M)F2(k F,γ)\]\[QF2F1(j F,β)\]\]ω F1F= 00γ˙ + cosγsinγ0−sinγcosγ0001 0β˙0 + cosγsinγ0−sinγcosγ0001 cosβ0−sinβ010sinβ0cosβ α˙00 = cosβcosγcosβsinγ−sinβ−sinγcosγ0001 α˙β˙γ˙

相关推荐
kyle~1 小时前
机器人传感器系统---时间戳对齐
机器人
淮北49415 小时前
ros调试工具foxglove使用指南三:在3d空间写写画画(Panel->3D ->Scene entity)
python·学习·3d·机器人
AntResearch18 小时前
ICLR 2025 Spotlight:让机器人实现「自主进化」,蚂蚁数科、清华提出具身协同框架 BodyGen
机器人
东雁西飞20 小时前
MATLAB 控制系统设计与仿真 - 33
开发语言·算法·matlab·机器人·自动控制
郭涤生1 天前
第二章:ROS架构_《ROS机器人开发实践》_notes
架构·机器人
MobiCetus1 天前
【MachineLearning】生成对抗网络 (GAN)
linux·人工智能·python·深度学习·神经网络·生成对抗网络·机器人
古希腊掌握嵌入式的神1 天前
[ROS]ROS系统是如何协调工作机器人
机器人·ros
硅谷秋水1 天前
DataPlatter:利用最少成本数据提升机器人操控的泛化能力
人工智能·深度学习·计算机视觉·语言模型·机器人
Mr.Winter`1 天前
深度强化学习 | 基于优先级经验池的DQN算法(附Pytorch实现)
人工智能·pytorch·神经网络·机器学习·机器人·强化学习
GIS数据转换器2 天前
在机器人和无人机时代,测绘人的出路在哪里?
大数据·人工智能·信息可视化·机器人·自动驾驶·汽车·无人机