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

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

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

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

机构运动学与动力学分析与建模 Ch00-2质量刚体的在坐标系下运动Part2

  • 3.4 欧拉四元数变换
      • 3.4.1 四元数的数学性质
      • 3.4.2 四元数与轴角的转换
      • 3.4.3 四元数旋转矢量
    • 3.5 欧拉角(ZYX变换)与RPY角 Euler Angles
      • 3.5.1 欧拉角与四元数的转换
      • 3.5.2 欧拉角与轴角的转换
    • 3.6 连续转动
    • 3.7 齐次矩阵的表达
    • 3.8 点与向量在不同坐标系下的表达


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θv1sin2θv2sin2θv3sin2θ = q1q2q3q4

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=1nqi2=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 =1q 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θ)v1sin(2θ)v2sin(2θ)v3sin(2θ) = cos2θv1sin2θv2sin2θv3sin2θ =[sv ]
  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 1q 2=[s1v 1][s2v 2]=[s1s2v 1Tv 2s1v 2+s2v 1+v 1×v 2]= [s1v 1v 1Ts1E+v ~1]L(q1)[s2v 2]= [s2v 2v 2Ts2Ev ~2]R(q2)[s1v 1]
    其中: L ( q 1 T ) = L ( q 1 ) T L\left( {q_1}^{\mathrm{T}} \right) =L\left( q_1 \right) ^{\mathrm{T}} L(q1T)=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(q1T)=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]= 12q322q422(q2q3+q1q4)2(q2q4q1q3)2(q2q3q1q4)12q222q422(q3q4+q1q2)2(q2q4+q1q3)2(q3q4q1q2)12q222q32 = 2q12+2q2212(q2q3+q1q4)2(q2q4q1q3)2(q2q3q1q4)2q12+2q3212(q3q4+q1q2)2(q2q4+q1q3)2(q3q4q1q2)2q12+2q421

进而定义矩阵:
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×4Bˉ3×4= q2q3q4q1q4q3q4q1q2q3q2q1 = q2q3q4q1q4q3q4q1q2q3q2q1
则有:
[ 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×4Bˉ3×4T
上述矩阵具有如下性质:
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×4B3×4T=Bˉ3×4Bˉ3×4T=EB3×4TB3×4=Bˉ3×4TBˉ3×4=E4×4q q TBˉ3×4q =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]= q11q21q31q12q22q23q13q23q33 ,则可求解四元数参数为:

[ 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] q1q2q3q4 = 21q11+q22+q33+1 4q1q32q234q1q13q314q1q21q12

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] θv1v2v3 = 2arccos(q1)sin2θq2sin2θq3sin2θ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] q1q2q3q4 = cos2θv1sin2θv2sin2θv3sin2θ

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]=[QF1F(k F,γ)][QF2F1(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,γ)][QF2F1(j F,β)][QF1F(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]= q11q21q31q12q22q32q13q23q33 , 则有:
[ α β γ ] = [ 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(q33q23)arcsin(q13)arctan(q11q12)

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] αβγ = arctan12(q12+q22)2(q1q2+q3q4)arcsin(2(q1q3q2q4))arctan12(q12+q22)2(q1q2+q3q4)

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β(1cosα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(q33q23)arcsin(q13)arctan(q11q12) = arctan(v3A)2(1cosθ)+cosθv1Asinθv2Av3A(1cosθ)arcsin(v1Av3A(1cosθ)+v2Asinθ)arctan(v1A)2(1cosθ)+cosθv3Asinθv1Av2A(1cosθ)

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=[QMFn1][QFn1Fn2][QF1F]r F=[QMFn1][QFn1Fn2][QF2F1]r 1F=[QMFn1][QFn1Fn2][QF3F2]r 2F=eθnv nFeθ2v 2Feθ1v 1Fr 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=[QF1F][QF2F1][QMFn1]r F=eθ1v 1Feθ2v 2F1eθnv nFn1r F

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

3.7 齐次矩阵的表达

3.1~3.6 只考虑了坐标系姿态的表达,专注于如何求解/表达 [ Q M F ] \left[ Q_{\mathrm{M}}^{F} \right] [QMF], 而对于更一般的情况:(忽略原点重合)
[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-3(2) 刚体的位形 Configuration of Rigid Body_第1张图片
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 PF1]=[[QFM]01×3R MF1]4×4[R PM1]
⇒ [ 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]0R MF1]
令: [ 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 PF1]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 P1P2F 而言,则有:
[ 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 P1P2F]=[R P2FR P1F]=[R P2F1][R P1F1]=[R P2FR P1F0]=[R P1P2F0]

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

你可能感兴趣的:(足式机器人,运动学与动力学,机器人)