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

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

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

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

如何表达刚体在空间中的位置与姿态

姿态参数如何表达？不同表达方式直接的转换关系？

旋转矩阵？转换矩阵？有什么意义和性质？转置代表什么？

如何表示连续变换？------与RPY有关

齐次坐标的意义------简化公式？
务必自己推导全部公式，并理解每个符号的含义

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

• [3.4 欧拉四元数变换](#3.4 欧拉四元数变换)
• • • [3.4.1 四元数的数学性质](#3.4.1 四元数的数学性质)
    • [3.4.2 四元数与轴角的转换](#3.4.2 四元数与轴角的转换)
    • [3.4.3 四元数旋转矢量](#3.4.3 四元数旋转矢量)
  • [3.5 欧拉角（ZYX变换）与RPY角 Euler Angles](#3.5 欧拉角（ZYX变换）与RPY角 Euler Angles)
  • • [3.5.1 欧拉角与四元数的转换](#3.5.1 欧拉角与四元数的转换)
    • [3.5.2 欧拉角与轴角的转换](#3.5.2 欧拉角与轴角的转换)
  • [3.6 连续转动](#3.6 连续转动)
  • [3.7 齐次矩阵的表达](#3.7 齐次矩阵的表达)
  • [3.8 点与向量在不同坐标系下的表达](#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θ→scalepart→vectorpart]= 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 =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−θ)v1sin(2−θ)v2sin(2−θ)v3sin(2−θ) = cos2θ−v1sin2θ−v2sin2θ−v3sin2θ =[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=[s1v 1]⋅[s2v 2]=[s1s2−v 1Tv 2s1v 2+s2v 1+v 1×v 2]= [s1v 1−v 1Ts1E+v ~1]L(q1)[s2v 2]= [s2v 2−v 2Ts2E−v ~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]= 1−2q32−2q422(q2q3+q1q4)2(q2q4−q1q3)2(q2q3−q1q4)1−2q22−2q422(q3q4+q1q2)2(q2q4+q1q3)2(q3q4−q1q2)1−2q22−2q32 = 2q12+2q22−12(q2q3+q1q4)2(q2q4−q1q3)2(q2q3−q1q4)2q12+2q32−12(q3q4+q1q2)2(q2q4+q1q3)2(q3q4−q1q2)2q12+2q42−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×4Bˉ3×4= −q2−q3−q4q1q4−q3−q4q1q2q3−q2q1 = −q2−q3−q4q1−q4q3q4q1−q2−q3q2q1

则有：
[ 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×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]= 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 4q1q32−q234q1q13−q314q1q21−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] θ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] αβγ = arctan1−2(q12+q22)2(q1q2+q3q4)arcsin(2(q1q3−q2q4))arctan1−2(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β(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(−q33q23)arcsin(q13)arctan(−q11q12) = arctan(v3A)2(1−cosθ)+cosθv1Asinθ−v2Av3A(1−cosθ)arcsin(v1Av3A(1−cosθ)+v2Asinθ)arctan(v1A)2(1−cosθ)+cosθv3Asinθ−v1Av2A(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−1Fn−2]⋯[QF1F]r F=[QMFn−1][QFn−1Fn−2]⋯[QF2F1]r 1F=[QMFn−1][QFn−1Fn−2]⋯[QF3F2]r 2F=eθnv nF⋯eθ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]⋯[QMFn−1]r F=eθ1v 1Feθ2v 2F1⋯eθnv nFn−1r 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 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 P2F−R P1F]=[R P2F1]−[R P1F1]=[R P2F−R 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