[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-3(2) 刚体的位形 Configuration of Rigid Body

发布时间:2024年01月10日

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

2024年底本人学位论文发表后方可摘抄
若有帮助请引用
本文参考:
.

食用方法
如何表达刚体在空间中的位置与姿态
姿态参数如何表达?不同表达方式直接的转换关系?
旋转矩阵?转换矩阵?有什么意义和性质?转置代表什么?
如何表示连续变换?——与RPY有关
齐次坐标的意义——简化公式?
务必自己推导全部公式,并理解每个符号的含义


3.4 欧拉四元数变换

同样基于罗德里格旋转公式,定义四个欧拉参数为:
q ? = [ s v ? ] = [ cos ? θ 2 → s c a l e ?? p a r t v ? sin ? θ 2 → v e c t o r ?? p a r t ] = [ cos ? θ 2 v 1 sin ? θ 2 v 2 sin ? θ 2 v 3 sin ? θ 2 ] = [ q 1 q 2 q 3 q 4 ] \vec{q}=\left[ \begin{array}{c} s\\ \vec{v}\\ \end{array} \right] =\left[ \begin{matrix} \cos \frac{\theta}{2}& \rightarrow scale\,\,part\\ \vec{v}\sin \frac{\theta}{2}& \rightarrow vector\,\,part\\ \end{matrix} \right] =\left[ \begin{array}{c} \cos \frac{\theta}{2}\\ v_1\sin \frac{\theta}{2}\\ v_2\sin \frac{\theta}{2}\\ v_3\sin \frac{\theta}{2}\\ \end{array} \right] =\left[ \begin{array}{c} q_1\\ q_2\\ q_3\\ q_4\\ \end{array} \right] q ?=[sv ?]=[cos2θ?v sin2θ??scalepartvectorpart?]= ?cos2θ?v1?sin2θ?v2?sin2θ?v3?sin2θ?? ?= ?q1?q2?q3?q4?? ?

3.4.1 四元数的数学性质

  1. 归一性 : q ? T q ? = ∑ i = 1 n q i 2 = 1 \vec{q}^{\mathrm{T}}\vec{q}=\sum_{i=1}^n{{q_{\mathrm{i}}}^2}=1 q ?Tq ?=i=1n?qi?2=1
  2. 四元数的正交性(逆) : q ? T q ? = 1 ? q ? T = q ? ? 1 \vec{q}^{\mathrm{T}}\vec{q}=1\Rightarrow \vec{q}^{\mathrm{T}}=\vec{q}^{-1} q ?Tq ?=1?q ?T=q ??1
  3. 四元数的转置(共轭)——旋转轴不变,旋转角相反 : q ? T = q ? ? 1 = [ cos ? ( ? θ 2 ) v 1 sin ? ( ? θ 2 ) v 2 sin ? ( ? θ 2 ) v 3 sin ? ( ? θ 2 ) ] = [ cos ? θ 2 ? v 1 sin ? θ 2 ? v 2 sin ? θ 2 ? v 3 sin ? θ 2 ] = [ s ? v ? ] \vec{q}^{\mathrm{T}}=\vec{q}^{-1}=\left[ \begin{array}{c} \cos \left( \frac{-\theta}{2} \right)\\ v_1\sin \left( \frac{-\theta}{2} \right)\\ v_2\sin \left( \frac{-\theta}{2} \right)\\ v_3\sin \left( \frac{-\theta}{2} \right)\\ \end{array} \right] =\left[ \begin{array}{c} \cos \frac{\theta}{2}\\ -v_1\sin \frac{\theta}{2}\\ -v_2\sin \frac{\theta}{2}\\ -v_3\sin \frac{\theta}{2}\\ \end{array} \right] =\left[ \begin{array}{c} s\\ -\vec{v}\\ \end{array} \right] q ?T=q ??1= ?cos(2?θ?)v1?sin(2?θ?)v2?sin(2?θ?)v3?sin(2?θ?)? ?= ?cos2θ??v1?sin2θ??v2?sin2θ??v3?sin2θ?? ?=[s?v ?]
  4. 四元数的乘法 : q ? 1 ? q ? 2 = [ s 1 v ? 1 ] ? [ s 2 v ? 2 ] = [ s 1 s 2 ? v ? 1 T v ? 2 s 1 v ? 2 + s 2 v ? 1 + v ? 1 × v ? 2 ] = [ s 1 ? v ? 1 T v ? 1 s 1 E + v ? ~ 1 ] ? L ( q 1 ) [ s 2 v ? 2 ] = [ s 2 ? v ? 2 T v ? 2 s 2 E ? v ? ~ 2 ] ? R ( q 2 ) [ s 1 v ? 1 ] \begin{split} \vec{q}_1\cdot \vec{q}_2&=\left[ \begin{array}{c} s_1\\ \vec{v}_1\\ \end{array} \right] \cdot \left[ \begin{array}{c} s_2\\ \vec{v}_2\\ \end{array} \right] =\left[ \begin{array}{c} s_1s_2-{\vec{v}_1}^{\mathrm{T}}\vec{v}_2\\ s_1\vec{v}_2+s_2\vec{v}_1+\vec{v}_1\times \vec{v}_2\\ \end{array} \right] \\ &=\begin{array}{c} \underbrace{\left[ \begin{matrix} s_1& -{\vec{v}_1}^{\mathrm{T}}\\ \vec{v}_1& s_1E+\tilde{\vec{v}}_1\\ \end{matrix} \right] }\\ L\left( q_1 \right)\\ \end{array}\left[ \begin{array}{c} s_2\\ \vec{v}_2\\ \end{array} \right] =\begin{array}{c} \underbrace{\left[ \begin{matrix} s_2& -{\vec{v}_2}^{\mathrm{T}}\\ \vec{v}_2& s_2E-\tilde{\vec{v}}_2\\ \end{matrix} \right] }\\ R\left( q_2 \right)\\ \end{array}\left[ \begin{array}{c} s_1\\ \vec{v}_1\\ \end{array} \right] \end{split} q ?1??q ?2??=[s1?v 1??]?[s2?v 2??]=[s1?s2??v 1?Tv 2?s1?v 2?+s2?v 1?+v 1?×v 2??]= [s1?v 1???v 1?Ts1?E+v ~1??]?L(q1?)?[s2?v 2??]= [s2?v 2???v 2?Ts2?E?v ~2??]?R(q2?)?[s1?v 1??]?
    其中: L ( q 1 T ) = L ( q 1 ) T L\left( {q_1}^{\mathrm{T}} \right) =L\left( q_1 \right) ^{\mathrm{T}} L(q1?T)=L(q1?)T R ( q 1 T ) = R ( q 1 ) T R\left( {q_1}^{\mathrm{T}} \right) =R\left( q_1 \right) ^{\mathrm{T}} R(q1?T)=R(q1?)T
  5. 四元数的同一性 :
    θ = 0 \theta =0 θ=0时: q ? ∣ θ = 0 = [ 1 0 ? ] \left. \vec{q} \right|_{\theta =0}=\left[ \begin{array}{c} 1\\ \vec{0}\\ \end{array} \right] q ?θ=0?=[10 ?]

根据上述定义,可将轴角变换的旋转矩阵 [ Q B A ] \left[ Q_{\mathrm{B}}^{A} \right] [QBA?] 改写为:
[ Q B A ] = [ 1 ? 2 q 3 2 ? 2 q 4 2 2 ( q 2 q 3 ? q 1 q 4 ) 2 ( q 2 q 4 + q 1 q 3 ) 2 ( q 2 q 3 + q 1 q 4 ) 1 ? 2 q 2 2 ? 2 q 4 2 2 ( q 3 q 4 ? q 1 q 2 ) 2 ( q 2 q 4 ? q 1 q 3 ) 2 ( q 3 q 4 + q 1 q 2 ) 1 ? 2 q 2 2 ? 2 q 3 2 ] = [ 2 q 1 2 + 2 q 2 2 ? 1 2 ( q 2 q 3 ? q 1 q 4 ) 2 ( q 2 q 4 + q 1 q 3 ) 2 ( q 2 q 3 + q 1 q 4 ) 2 q 1 2 + 2 q 3 2 ? 1 2 ( q 3 q 4 ? q 1 q 2 ) 2 ( q 2 q 4 ? q 1 q 3 ) 2 ( q 3 q 4 + q 1 q 2 ) 2 q 1 2 + 2 q 4 2 ? 1 ] \left[ Q_{\mathrm{B}}^{A} \right] =\left[ \begin{matrix} 1-2{q_3}^2-2{q_4}^2& 2\left( q_2q_3-q_1q_4 \right)& 2\left( q_2q_4+q_1q_3 \right)\\ 2\left( q_2q_3+q_1q_4 \right)& 1-2{q_2}^2-2{q_4}^2& 2\left( q_3q_4-q_1q_2 \right)\\ 2\left( q_2q_4-q_1q_3 \right)& 2\left( q_3q_4+q_1q_2 \right)& 1-2{q_2}^2-2{q_3}^2\\ \end{matrix} \right] =\left[ \begin{matrix} 2{q_1}^2+2{q_2}^2-1& 2\left( q_2q_3-q_1q_4 \right)& 2\left( q_2q_4+q_1q_3 \right)\\ 2\left( q_2q_3+q_1q_4 \right)& 2{q_1}^2+2{q_3}^2-1& 2\left( q_3q_4-q_1q_2 \right)\\ 2\left( q_2q_4-q_1q_3 \right)& 2\left( q_3q_4+q_1q_2 \right)& 2{q_1}^2+2{q_4}^2-1\\ \end{matrix} \right] [QBA?]= ?1?2q3?2?2q4?22(q2?q3?+q1?q4?)2(q2?q4??q1?q3?)?2(q2?q3??q1?q4?)1?2q2?2?2q4?22(q3?q4?+q1?q2?)?2(q2?q4?+q1?q3?)2(q3?q4??q1?q2?)1?2q2?2?2q3?2? ?= ?2q1?2+2q2?2?12(q2?q3?+q1?q4?)2(q2?q4??q1?q3?)?2(q2?q3??q1?q4?)2q1?2+2q3?2?12(q3?q4?+q1?q2?)?2(q2?q4?+q1?q3?)2(q3?q4??q1?q2?)2q1?2+2q4?2?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 × 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 ] \begin{split} 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] \\ \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] \end{split} B3×4?Bˉ3×4??= ??q2??q3??q4??q1?q4??q3???q4?q1?q2??q3??q2?q1?? ?= ??q2??q3??q4??q1??q4?q3??q4?q1??q2???q3?q2?q1?? ??
则有:
[ Q B A ] = B 3 × 4 B ˉ 3 × 4 T \left[ Q_{\mathrm{B}}^{A} \right] =B_{3\times 4}{\bar{B}_{3\times 4}}^{\mathrm{T}} [QBA?]=B3×4?Bˉ3×4?T
上述矩阵具有如下性质:
B 3 × 4 B 3 × 4 T = B ˉ 3 × 4 B ˉ 3 × 4 T = E B 3 × 4 T B 3 × 4 = B ˉ 3 × 4 T B ˉ 3 × 4 = E 4 × 4 ? q ? ? q ? T B ˉ 3 × 4 ? q ? = 0 ? B_{3\times 4}{B_{3\times 4}}^{\mathrm{T}}=\bar{B}_{3\times 4}{\bar{B}_{3\times 4}}^{\mathrm{T}}=E \\ {B_{3\times 4}}^{\mathrm{T}}B_{3\times 4}={\bar{B}_{3\times 4}}^{\mathrm{T}}\bar{B}_{3\times 4}=E_{4\times 4}-\vec{q}\cdot \vec{q}^{\mathrm{T}} \\ \bar{B}_{3\times 4}\cdot \vec{q}=\vec{0} B3×4?B3×4?T=Bˉ3×4?Bˉ3×4?T=EB3×4?TB3×4?=Bˉ3×4?TBˉ3×4?=E4×4??q ??q ?TBˉ3×4??q ?=0
因此,若已知旋转矩阵: [ Q B A ] = [ q 11 q 12 q 13 q 21 q 22 q 23 q 31 q 23 q 33 ] \left[ Q_{\mathrm{B}}^{A} \right] =\left[ \begin{matrix} q_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{23}& q_{33}\\ \end{matrix} \right] [QBA?]= ?q11?q21?q31??q12?q22?q23??q13?q23?q33?? ?,则可求解四元数参数为:

[ q 1 q 2 q 3 q 4 ] = [ 1 2 q 11 + q 22 + q 33 + 1 q 32 ? q 23 4 q 1 q 13 ? q 31 4 q 1 q 21 ? q 12 4 q 1 ] \left[ \begin{array}{c} q_1\\ q_2\\ q_3\\ q_4\\ \end{array} \right] =\left[ \begin{array}{c} \frac{1}{2}\sqrt{q_{11}+q_{22}+q_{33}+1}\\ \frac{q_{32}-q_{23}}{4q_1}\\ \frac{q_{13}-q_{31}}{4q_1}\\ \frac{q_{21}-q_{12}}{4q_1}\\ \end{array} \right] ?q1?q2?q3?q4?? ?= ?21?q11?+q22?+q33?+1 ?4q1?q32??q23??4q1?q13??q31??4q1?q21??q12??? ?

3.4.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] ?θv1?v2?v3?? ?= ?2arccos(q1?)sin2θ?q2??sin2θ?q3??sin2θ?q4??? ?
  • 轴角转换为四元数
    [ q 1 q 2 q 3 q 4 ] = [ cos ? θ 2 v 1 sin ? θ 2 v 2 sin ? θ 2 v 3 sin ? θ 2 ] \left[ \begin{array}{c} q_1\\ q_2\\ q_3\\ q_4\\ \end{array} \right] =\left[ \begin{array}{c} \cos \frac{\theta}{2}\\ v_1\sin \frac{\theta}{2}\\ v_2\sin \frac{\theta}{2}\\ v_3\sin \frac{\theta}{2}\\ \end{array} \right] ?q1?q2?q3?q4?? ?= ?cos2θ?v1?sin2θ?v2?sin2θ?v3?sin2θ?? ?

3.4.3 四元数旋转矢量

对于任意矢量 R ? F \vec{R}^F R F,可通过上述四元数矢量进行旋转变化:
[ 0 R ? ′ F ] = q ? F ? [ 0 R ? F ] ? ( q ? F ) ? 1 = q ? F ? [ 0 R ? F ] ? ( q ? F ) T = L ( q ) R ( q ) T [ 0 R ? F ] = R ( q ) T L ( q ) [ 0 R ? F ] \left[ \begin{array}{c} 0\\ {\vec{R}^{\prime}}^F\\ \end{array} \right] =\vec{q}^F\cdot \left[ \begin{array}{c} 0\\ \vec{R}^F\\ \end{array} \right] \cdot \left( \vec{q}^F \right) ^{-1}=\vec{q}^F\cdot \left[ \begin{array}{c} 0\\ \vec{R}^F\\ \end{array} \right] \cdot \left( \vec{q}^F \right) ^{\mathrm{T}} \\ =L\left( q \right) R\left( q \right) ^{\mathrm{T}}\left[ \begin{array}{c} 0\\ \vec{R}^F\\ \end{array} \right] =R\left( q \right) ^{\mathrm{T}}L\left( q \right) \left[ \begin{array}{c} 0\\ \vec{R}^F\\ \end{array} \right] [0R F?]=q ?F?[0R F?]?(q ?F)?1=q ?F?[0R F?]?(q ?F)T=L(q)R(q)T[0R F?]=R(q)TL(q)[0R F?]
同理也可以将上述视为矢量的坐标系变换,其转换矩阵拓展为 4 × 4 4\times 4 4×4,进而有转换矩阵 [ Q B A ] 4 × 4 T \left[ Q_{\mathrm{B}}^{A} \right]^{\mathrm{T}} _{4\times 4} [QBA?]4×4T?
[ Q B A ] 4 × 4 = L ( q ) R ( q ) T = R ( q ) T L ( q ) \left[ Q_{\mathrm{B}}^{A} \right] _{4\times 4}=L\left( q \right) R\left( q \right) ^{\mathrm{T}}=R\left( q \right) ^{\mathrm{T}}L\left( q \right) [QBA?]4×4?=L(q)R(q)T=R(q)TL(q)
此时,则有:
[ 0 R ? B ] = [ Q B A ] 4 × 4 T [ 0 R ? A ] \left[ \begin{array}{c} 0\\ \vec{R}^B\\ \end{array} \right] =\left[ Q_{\mathrm{B}}^{A} \right]^{\mathrm{T}} _{4\times 4}\left[ \begin{array}{c} 0\\ \vec{R}^A\\ \end{array} \right] [0R B?]=[QBA?]4×4T?[0R A?]

3.5 欧拉角(ZYX变换)与RPY角 Euler Angles

欧拉角是一种较为原始的旋转表示方式,在实际的算法运用过程中,除了描述已知姿态的刚体角度外,在实际计算中,效果很差(具有奇异性、高度非线性、计算复杂)。因此,并不推荐用欧拉角来描述转换矩阵。本节仅对部分重点内容进行介绍。

欧拉角(ZYX变换)的旋转变换描述为:绕固定坐标系的基矢量 k ? F \vec{k}^{F} k F回转 γ \gamma γ,得到新标架 { F 1 : ( i ? 1 F , j ? 1 F , k ? 1 F ) } \left\{ F_1:\left( \vec{i}_{1}^{F},\vec{j}_{1}^{F},\vec{k}_{1}^{F} \right) \right\} {F1?:(i 1F?,j ?1F?,k 1F?)};再绕基矢量 j ? 1 F \vec{j}_{1}^{F} j ?1F?回转 β \beta β,得到新标架 { F 2 : ( i ? 2 F , j ? 2 F , k ? 2 F ) } \left\{ F_2:\left( \vec{i}_{2}^{F},\vec{j}_{2}^{F},\vec{k}_{2}^{F} \right) \right\} {F2?:(i 2F?,j ?2F?,k 2F?)};最后绕基矢量 i ? 2 F \vec{i}_{2}^{F} i 2F?回转 α \alpha α,得到新标架 { F 3 : ( i ? 3 F , j ? 3 F , k ? 3 F ) } \left\{ F_3:\left( \vec{i}_{3}^{F},\vec{j}_{3}^{F},\vec{k}_{3}^{F} \right) \right\} {F3?:(i 3F?,j ?3F?,k 3F?)}为最终的变换姿态(运动坐标系下连续转动,右乘)。因此对于多次连续转动而言有:

[ Q M F ] = [ Q F 1 F ( k ? F , γ ) ] [ Q F 2 F 1 ( j ? 1 F , β ) ] [ Q F 3 ( M ) F 2 ( i ? 2 F , α ) ] ] \left[ Q_{\mathrm{M}}^{F} \right] =\left[ Q_{\mathrm{F}_1}^{F}\left( \vec{k}^F,\gamma \right) \right] \left[ Q_{\mathrm{F}_2}^{F_1}\left( \vec{j}_{1}^{F},\beta \right) \right] \left[ Q_{\mathrm{F}_3\left( M \right)}^{F_2}\left( \vec{i}_{2}^{F},\alpha \right) \right] ] [QMF?]=[QF1?F?(k F,γ)][QF2?F1??(j ?1F?,β)][QF3?(M)F2??(i 2F?,α)]]

而对于RPY角(滚转角Roll,仰俯角Pitch,偏航角Yaw)而言,其旋转变换描述为:绕固定坐标系的基矢量 i ? F \vec{i}^{F} i F回转 α \alpha α,得到新标架 { F 1 : ( i ? 1 F , j ? 1 F , k ? 1 F ) } \left\{ F_1:\left( \vec{i}_{1}^{F},\vec{j}_{1}^{F},\vec{k}_{1}^{F} \right) \right\} {F1?:(i 1F?,j ?1F?,k 1F?)};再绕固定坐标系的基矢量 j ? F \vec{j}^{F} j ?F回转 β \beta β,得到新标架 { F 2 : ( i ? 2 F , j ? 2 F , k ? 2 F ) } \left\{ F_2:\left( \vec{i}_{2}^{F},\vec{j}_{2}^{F},\vec{k}_{2}^{F} \right) \right\} {F2?:(i 2F?,j ?2F?,k 2F?)};最后绕固定坐标系的基矢量 k ? F \vec{k}^{F} k F回转 γ \gamma γ,得到新标架 { F 3 : ( i ? 3 F , j ? 3 F , k ? 3 F ) } \left\{ F_3:\left( \vec{i}_{3}^{F},\vec{j}_{3}^{F},\vec{k}_{3}^{F} \right) \right\} {F3?:(i 3F?,j ?3F?,k 3F?)}为最终的变换姿态(固定坐标系下连续转动,左乘)。因此对于多次连续转动而言有:
[ Q M F ] = [ Q F 3 ( M ) F 2 ( k ? F , γ ) ] [ Q F 2 F 1 ( j ? F , β ) ] [ Q F 1 F ( i ? F , α ) ] \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] [QMF?]=[QF3?(M)F2??(k F,γ)][QF2?F1??(j ?F,β)][QF1?F?(i F,α)]

进而求解出其转换矩阵为:
[ Q M F ] = [ cos ? β cos ? γ ? cos ? β sin ? γ sin ? β sin ? α sin ? β cos ? γ + cos ? α sin ? γ ? sin ? α sin ? β sin ? γ + cos ? α cos ? γ ? sin ? α cos ? β ? cos ? α sin ? β cos ? γ + sin ? α sin ? γ cos ? α sin ? β cos ? γ + sin ? α cos ? γ cos ? α cos ? β ] \left[ Q_{\mathrm{M}}^{F} \right] =\left[ \begin{matrix} \cos \beta \cos \gamma& -\cos \beta \sin \gamma& \sin \beta\\ \sin \alpha \sin \beta \cos \gamma +\cos \alpha \sin \gamma& -\sin \alpha \sin \beta \sin \gamma +\cos \alpha \cos \gamma& -\sin \alpha \cos \beta\\ -\cos \alpha \sin \beta \cos \gamma +\sin \alpha \sin \gamma& \cos \alpha \sin \beta \cos \gamma +\sin \alpha \cos \gamma& \cos \alpha \cos \beta\\ \end{matrix} \right] [QMF?]= ?cosβcosγsinαsinβcosγ+cosαsinγ?cosαsinβcosγ+sinαsinγ??cosβsinγ?sinαsinβsinγ+cosαcosγcosαsinβcosγ+sinαcosγ?sinβ?sinαcosβcosαcosβ? ?

同理,若已知转换矩阵: [ Q B A ] = [ q 11 q 12 q 13 q 21 q 22 q 23 q 31 q 32 q 33 ] \left[ Q_{\mathrm{B}}^{A} \right] =\left[ \begin{matrix} q_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\\ \end{matrix} \right] [QBA?]= ?q11?q21?q31??q12?q22?q32??q13?q23?q33?? ?, 则有:
[ α β γ ] = [ a r c tan ? ( ? q 23 q 33 ) a r c sin ? ( q 13 ) a r c tan ? ( ? q 12 q 11 ) ] \left[ \begin{array}{c} \alpha\\ \beta\\ \gamma\\ \end{array} \right] =\left[ \begin{array}{c} \mathrm{arc}\tan \left( -\frac{q_{23}}{q_{33}} \right)\\ \mathrm{arc}\sin \left( q_{13} \right)\\ \mathrm{arc}\tan \left( -\frac{q_{12}}{q_{11}} \right)\\ \end{array} \right] ?αβγ? ?= ?arctan(?q33?q23??)arcsin(q13?)arctan(?q11?q12??)? ?

3.5.1 欧拉角与四元数的转换

  • 欧拉角转换为四元数:
    q ? = [ cos ? α 2 cos ? β 2 cos ? γ 2 + sin ? α 2 sin ? β 2 sin ? γ 2 sin ? α 2 cos ? β 2 cos ? γ 2 ? cos ? α 2 sin ? β 2 sin ? γ 2 cos ? α 2 sin ? β 2 cos ? γ 2 + sin ? α 2 cos ? β 2 sin ? γ 2 cos ? α 2 cos ? β 2 sin ? γ 2 ? sin ? α 2 sin ? β 2 cos ? γ 2 ] \vec{q}=\left[ \begin{array}{c} \cos \frac{\alpha}{2}\cos \frac{\beta}{2}\cos \frac{\gamma}{2}+\sin \frac{\alpha}{2}\sin \frac{\beta}{2}\sin \frac{\gamma}{2}\\ \sin \frac{\alpha}{2}\cos \frac{\beta}{2}\cos \frac{\gamma}{2}-\cos \frac{\alpha}{2}\sin \frac{\beta}{2}\sin \frac{\gamma}{2}\\ \cos \frac{\alpha}{2}\sin \frac{\beta}{2}\cos \frac{\gamma}{2}+\sin \frac{\alpha}{2}\cos \frac{\beta}{2}\sin \frac{\gamma}{2}\\ \cos \frac{\alpha}{2}\cos \frac{\beta}{2}\sin \frac{\gamma}{2}-\sin \frac{\alpha}{2}\sin \frac{\beta}{2}\cos \frac{\gamma}{2}\\ \end{array} \right] q ?= ?cos2α?cos2β?cos2γ?+sin2α?sin2β?sin2γ?sin2α?cos2β?cos2γ??cos2α?sin2β?sin2γ?cos2α?sin2β?cos2γ?+sin2α?cos2β?sin2γ?cos2α?cos2β?sin2γ??sin2α?sin2β?cos2γ?? ?
  • 四元数转换为欧拉角:
    [ α β γ ] = [ a r c tan ? 2 ( q 1 q 2 + q 3 q 4 ) 1 ? 2 ( q 1 2 + q 2 2 ) a r c sin ? ( 2 ( q 1 q 3 ? q 2 q 4 ) ) a r c tan ? 2 ( q 1 q 2 + q 3 q 4 ) 1 ? 2 ( q 1 2 + q 2 2 ) ] \left[ \begin{array}{c} \alpha\\ \beta\\ \gamma\\ \end{array} \right] =\left[ \begin{array}{c} \mathrm{arc}\tan \frac{2\left( q_1q_2+q_3q_4 \right)}{1-2\left( {q_1}^2+{q_2}^2 \right)}\\ \mathrm{arc}\sin \left( 2\left( q_1q_3-q_2q_4 \right) \right)\\ \mathrm{arc}\tan \frac{2\left( q_1q_2+q_3q_4 \right)}{1-2\left( {q_1}^2+{q_2}^2 \right)}\\ \end{array} \right] ?αβγ? ?= ?arctan1?2(q1?2+q2?2)2(q1?q2?+q3?q4?)?arcsin(2(q1?q3??q2?q4?))arctan1?2(q1?2+q2?2)2(q1?q2?+q3?q4?)?? ?

3.5.2 欧拉角与轴角的转换

  • 欧拉角转换为轴角:
    θ = a r c cos ? ( cos ? β cos ? γ ? sin ? α sin ? β sin ? γ + cos ? α ( cos ? γ + cos ? β ) ? 1 2 ) v ? F = 1 2 sin ? θ [ cos ? α sin ? β cos ? γ + sin ? α ( cos ? γ + cos ? β ) sin ? β ( 1 ? cos ? α cos ? γ ) ? sin ? α sin ? γ sin ? α sin ? β cos ? γ + sin ? γ ( cos ? α + cos ? β ) ] \begin{split} \theta &=\mathrm{arc}\cos \left( \frac{\cos \beta \cos \gamma -\sin \alpha \sin \beta \sin \gamma +\cos \alpha \left( \cos \gamma +\cos \beta \right) -1}{2} \right) \\ \vec{v}^F&=\frac{1}{2\sin \theta}\left[ \begin{array}{c} \cos \alpha \sin \beta \cos \gamma +\sin \alpha \left( \cos \gamma +\cos \beta \right)\\ \sin \beta \left( 1-\cos \alpha \cos \gamma \right) -\sin \alpha \sin \gamma\\ \sin \alpha \sin \beta \cos \gamma +\sin \gamma \left( \cos \alpha +\cos \beta \right)\\ \end{array} \right] \end{split} θv F?=arccos(2cosβcosγ?sinαsinβsinγ+cosα(cosγ+cosβ)?1?)=2sinθ1? ?cosαsinβcosγ+sinα(cosγ+cosβ)sinβ(1?cosαcosγ)?sinαsinγsinαsinβcosγ+sinγ(cosα+cosβ)? ??
  • 轴角转换为欧拉角:
    [ α β γ ] = [ a r c tan ? ( ? q 23 q 33 ) a r c sin ? ( q 13 ) a r c tan ? ( ? q 12 q 11 ) ] = [ a r c tan ? v 1 A sin ? θ ? v 2 A v 3 A ( 1 ? cos ? θ ) ( v 3 A ) 2 ( 1 ? cos ? θ ) + cos ? θ a r c sin ? ( v 1 A v 3 A ( 1 ? cos ? θ ) + v 2 A sin ? θ ) a r c tan ? v 3 A sin ? θ ? v 1 A v 2 A ( 1 ? cos ? θ ) ( v 1 A ) 2 ( 1 ? cos ? θ ) + cos ? θ ] \left[ \begin{array}{c} \alpha\\ \beta\\ \gamma\\ \end{array} \right] =\left[ \begin{array}{c} \mathrm{arc}\tan \left( -\frac{q_{23}}{q_{33}} \right)\\ \mathrm{arc}\sin \left( q_{13} \right)\\ \mathrm{arc}\tan \left( -\frac{q_{12}}{q_{11}} \right)\\ \end{array} \right] =\left[ \begin{array}{c} \mathrm{arc}\tan \frac{v_{1}^{A}\sin \theta -v_{2}^{A}v_{3}^{A}\left( 1-\cos \theta \right)}{\left( v_{3}^{A} \right) ^2\left( 1-\cos \theta \right) +\cos \theta}\\ \mathrm{arc}\sin \left( v_{1}^{A}v_{3}^{A}\left( 1-\cos \theta \right) +v_{2}^{A}\sin \theta \right)\\ \mathrm{arc}\tan \frac{v_{3}^{A}\sin \theta -v_{1}^{A}v_{2}^{A}\left( 1-\cos \theta \right)}{\left( v_{1}^{A} \right) ^2\left( 1-\cos \theta \right) +\cos \theta}\\ \end{array} \right] ?αβγ? ?= ?arctan(?q33?q23??)arcsin(q13?)arctan(?q11?q12??)? ?= ?arctan(v3A?)2(1?cosθ)+cosθv1A?sinθ?v2A?v3A?(1?cosθ)?arcsin(v1A?v3A?(1?cosθ)+v2A?sinθ)arctan(v1A?)2(1?cosθ)+cosθv3A?sinθ?v1A?v2A?(1?cosθ)?? ?

3.6 连续转动

  • 固定坐标系下运动基矢量连续转动:俗称为左乘)即每一次转动后,新的转动轴与刚体上的矢量在固定坐标系下重新定义,用数学公式表达为:
    r ? ′ F = [ Q M F ] r ? F = [ Q M F n ? 1 ] [ Q F n ? 1 F n ? 2 ] ? [ Q F 1 F ] r ? F = [ Q M F n ? 1 ] [ Q F n ? 1 F n ? 2 ] ? [ Q F 2 F 1 ] r ? 1 F = [ Q M F n ? 1 ] [ Q F n ? 1 F n ? 2 ] ? [ Q F 3 F 2 ] r ? 2 F = e θ n v ? n F ? e θ 2 v ? 2 F e θ 1 v ? 1 F r ? F \begin{split} {\vec{r}^{\prime}}^F=\left[ Q_{\mathrm{M}}^{F} \right] \vec{r}^F=\left[ Q_{\mathrm{M}}^{F_{n-1}} \right] \left[ Q_{F_{n-1}}^{F_{n-2}} \right] \cdots \left[ Q_{F_1}^{F} \right] \vec{r}^F&=\left[ Q_{\mathrm{M}}^{F_{n-1}} \right] \left[ Q_{F_{n-1}}^{F_{n-2}} \right] \cdots \left[ Q_{F_2}^{F_1} \right] {\vec{r}_1}^F=\left[ Q_{\mathrm{M}}^{F_{n-1}} \right] \left[ Q_{F_{n-1}}^{F_{n-2}} \right] \cdots \left[ Q_{F_3}^{F_2} \right]{\vec{r}_2}^F \\ &=e^{\theta _{\mathrm{n}}{\vec{v}_{\mathrm{n}}}^F}\cdots e^{\theta _2{\vec{v}_2}^F}e^{\theta _1{\vec{v}_1}^F}\vec{r}^F \end{split} r F=[QMF?]r F=[QMFn?1??][QFn?1?Fn?2??]?[QF1?F?]r F?=[QMFn?1??][QFn?1?Fn?2??]?[QF2?F1??]r 1?F=[QMFn?1??][QFn?1?Fn?2??]?[QF3?F2??]r 2?F=eθn?v n?F?eθ2?v 2?Feθ1?v 1?Fr F?
  • 运动坐标系下运动基矢量连续转动:(俗称为右乘)即每一次转动后,新的转动轴与刚体上的矢量在上一次的运动坐标系下重新定义,用数学公式表达为:
    r ? ′ F = [ Q M F ] r ? F = [ Q F 1 F ] [ Q F 2 F 1 ] ? [ Q M F n ? 1 ] r ? F = e θ 1 v ? 1 F e θ 2 v ? 2 F 1 ? e θ n v ? n F n ? 1 r ? F {\vec{r}^{\prime}}^F=\left[ Q_{\mathrm{M}}^{F} \right] \vec{r}^F=\left[ Q_{F_1}^{F} \right] \left[ Q_{F_2}^{F_1} \right] \cdots \left[ Q_{\mathrm{M}}^{F_{n-1}} \right] \vec{r}^F=e^{\theta _1{\vec{v}_1}^F}e^{\theta _2{\vec{v}_2}^{F_1}}\cdots e^{\theta _{\mathrm{n}}{\vec{v}_{\mathrm{n}}}^{F_{n-1}}}\vec{r}^F r F=[QMF?]r F=[QF1?F?][QF2?F1??]?[QMFn?1??]r F=eθ1?v 1?Feθ2?v 2?F1??eθn?v n?Fn?1?r F

绕两条不同轴进行转动的转换矩阵乘积不可交换。

3.7 齐次矩阵的表达

3.1~3.6 只考虑了坐标系姿态的表达,专注于如何求解/表达 [ Q M F ] \left[ Q_{\mathrm{M}}^{F} \right] [QMF?], 而对于更一般的情况:(忽略原点重合)
在这里插入图片描述
R ? P F = [ Q M F ] R ? P M + R ? M F \vec{R}_{\mathrm{P}}^{F}=\left[ Q_{\mathrm{M}}^{F} \right] \vec{R}_{\mathrm{P}}^{M}+\vec{R}_{\mathrm{M}}^{F} R PF?=[QMF?]R PM?+R MF?

引入齐次矩阵Homogeneous Transformation Matrix: [ T M F ] \left[ T_{\mathrm{M}}^{F} \right] [TMF?]

R ? P F = [ Q M F ] R ? P M + R ? M F ? [ R ? P F 1 ] = [ [ Q F M ] R ? M F 0 1 × 3 1 ] 4 × 4 [ R ? P M 1 ] \vec{R}_{\mathrm{P}}^{F}=\left[ Q_{\mathrm{M}}^{F} \right] \vec{R}_{\mathrm{P}}^{M}+\vec{R}_{\mathrm{M}}^{F}\Rightarrow \left[ \begin{array}{c} \vec{R}_{\mathrm{P}}^{F}\\ 1\\ \end{array} \right] =\left[ \begin{matrix} \left[ Q_{\mathrm{F}}^{M} \right]& \vec{R}_{\mathrm{M}}^{F}\\ 0_{1\times 3}& 1\\ \end{matrix} \right] _{4\times 4}\left[ \begin{array}{c} \vec{R}_{\mathrm{P}}^{M}\\ 1\\ \end{array} \right] R PF?=[QMF?]R PM?+R MF??[R PF?1?]=[[QFM?]01×3??R MF?1?]4×4?[R PM?1?]
? [ T M F ] = [ [ Q M F ] R ? M F 0 1 ] \Rightarrow \left[ T_{\mathrm{M}}^{F} \right] =\left[ \begin{matrix} \left[ Q_{\mathrm{M}}^{F} \right]& \vec{R}_{\mathrm{M}}^{F}\\ 0& 1\\ \end{matrix} \right] ?[TMF?]=[[QMF?]0?R MF?1?]
令: [ R ? P F ] = [ R ? P F 1 ] ∈ R 4 \left[ \vec{R}_{\mathrm{P}}^{F} \right] =\left[ \begin{array}{c} \vec{R}_{\mathrm{P}}^{F}\\ 1\\ \end{array} \right] \in \mathbb{R} ^4 [R PF?]=[R PF?1?]R4,则有:
[ R ? P F ] = [ T M F ] [ R ? P M ] \left[ \vec{R}_{\mathrm{P}}^{F} \right] =\left[ T_{\mathrm{M}}^{F} \right] \left[ \vec{R}_{\mathrm{P}}^{M} \right] [R PF?]=[TMF?][R PM?]

对于向量 R ? P 1 P 2 F \vec{R}_{\mathrm{P}_1\mathrm{P}_2}^{F} R P1?P2?F? 而言,则有:
[ R ? P 1 P 2 F ] = [ R ? P 2 F ? R ? P 1 F ] = [ R ? P 2 F 1 ] ? [ R ? P 1 F 1 ] = [ R ? P 2 F ? R ? P 1 F 0 ] = [ R ? P 1 P 2 F 0 ] \left[ \vec{R}_{\mathrm{P}_1\mathrm{P}_2}^{F} \right] =\left[ \vec{R}_{\mathrm{P}_2}^{F}-\vec{R}_{\mathrm{P}_1}^{F} \right] =\left[ \begin{array}{c} \vec{R}_{\mathrm{P}_2}^{F}\\ 1\\ \end{array} \right] -\left[ \begin{array}{c} \vec{R}_{\mathrm{P}_1}^{F}\\ 1\\ \end{array} \right] =\left[ \begin{array}{c} \vec{R}_{\mathrm{P}_2}^{F}-\vec{R}_{\mathrm{P}_1}^{F}\\ 0\\ \end{array} \right] =\left[ \begin{array}{c} \vec{R}_{\mathrm{P}_1\mathrm{P}_2}^{F}\\ 0\\ \end{array} \right] [R P1?P2?F?]=[R P2?F??R P1?F?]=[R P2?F?1?]?[R P1?F?1?]=[R P2?F??R P1?F?0?]=[R P1?P2?F?0?]

3.8 点与向量在不同坐标系下的表达

对于固定坐标系下同一点/向量,在不同坐标系 { A } , { B } \left\{ A \right\} ,\left\{ B \right\} {A},{B}下进行表达,存在如下转换关系:
R ? V e c t o r A = [ Q B A ] R ? V e c t o r B \vec{R}_{\mathrm{Vector}}^{A}=\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{Vector}}^{B} R VectorA?=[QBA?]R VectorB?
R ? P A = [ Q B A ] R ? P B + R ? B A \vec{R}_{\mathrm{P}}^{A}=\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{P}}^{B}+\vec{R}_{\mathrm{B}}^{A} R PA?=[QBA?]R PB?+R BA?

1
2
3
4
5
6
7
8
9
1
2
3
4
5
6
7
8
9

文章来源:https://blog.csdn.net/LiongLoure/article/details/135500448
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。