本文仅供学习使用，总结很多本现有讲述运动学或动力学书籍后的总结，从矢量的角度进行分析，方法比较传统，但更易理解，并且现有的看似抽象方法，两者本质上并无不同。
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 四元数的数学性质
- 归一性 : 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
- 四元数的正交性（逆） : 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
- 四元数的转置（共轭） ------旋转轴不变，旋转角相反 : 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 ]
- 四元数的乘法 : 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。 - 四元数的同一性 :
当 θ = 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