本文仅供学习使用，总结很多本现有讲述运动学或动力学书籍后的总结，从矢量的角度进行分析，方法比较传统，但更易理解，并且现有的看似抽象方法，两者本质上并无不同。
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 \~1L(q1)s2v 2= s2v 2−v 2Ts2E−v \~2R(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} QBA4×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) QBA4×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=QMFr F=QMFn−1QFn−1Fn−2⋯QF1Fr F=QMFn−1QFn−1Fn−2⋯QF2F1r 1F=QMFn−1QFn−1Fn−2⋯QF3F2r 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=QMFr F=QF1FQF2F1⋯QMFn−1r 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=QMFR 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=QMFR PM+R MF⇒R PF1=\[QFM01×3R MF1]4×4R 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=\[QMF0R 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=QBAR 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=QBAR PB+R BA
1
2
3
4
5
6
7
8
9
1
2
3
4
5
6
7
8
9