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

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

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θ​​→scalepart→vectorpart​]= ​cos2θ​v1​sin2θ​v2​sin2θ​v3​sin2θ​​ ​= ​q1​q2​q3​q4​​ ​

3.4.1 四元数的数学性质

1. 归一性 : $q^{\mathrm{T}}q={\sum }_{i=1}^n{q_{\mathrm{i}}}^2=1$
2. 四元数的正交性（逆） : $q^{\mathrm{T}}q=1\Rightarrow q^{\mathrm{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. 四元数的乘法 : $$\begin{array}{rl}{q}_1\cdot q_2& =\left[\begin{array}{c}{s}_1\\ v_1\end{array}\right]\cdot \left[\begin{array}{c}{s}_2\\ v_2\end{array}\right]=\left[\begin{array}{c}{s}_1{s}_2-{v_1}^{\mathrm{T}}{v}_2\\ s_1{v}_2+s_2{v}_1+v_1\times v_2\end{array}\right]\\ & =\begin{array}{c}\left[\begin{array}{cc}{s}_1& -{v_1}^{\mathrm{T}}\\ v_1& s_{1}E+{\tilde{v}}_1\end{array}\right]\\ L\left(q_1\right)\end{array}\left[\begin{array}{c}{s}_2\\ v_2\end{array}\right]=\begin{array}{c}\left[\begin{array}{cc}{s}_2& -{v_2}^{\mathrm{T}}\\ v_2& s_{2}E-{\tilde{v}}_2\end{array}\right]\\ R\left(q_2\right)\end{array}\left[\begin{array}{c}{s}_1\\ v_1\end{array}\right]\end{array}$$
   其中：$L\left({q_1}^{\mathrm{T}}\right)=L{\left(q_1\right)}^{\mathrm{T}}$，$R\left({q_1}^{\mathrm{T}}\right)=R{\left(q_1\right)}^{\mathrm{T}}$。
5. 四元数的同一性 :
   当$\theta =0$时：${q|}_{\theta =0}=\left[\begin{array}{c}1\\ 0\end{array}\right]$

根据上述定义，可将轴角变换的旋转矩阵$\left[Q_{\mathrm{B}}^A\right]$ 改写为：
[ 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​​ ​​
则有：
$$\left[Q_{\mathrm{B}}^A\right]=B_{3\times 4}{{\bar{B}}_{3\times 4}}^{\mathrm{T}}$$
上述矩阵具有如下性质：
$$\begin{gathered} 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}-q\cdot q^{\mathrm{T}} \\ {\bar{B}}_{3\times 4}\cdot q=0 \end{gathered}$$
因此，若已知旋转矩阵： [ 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$，可通过上述四元数矢量进行旋转变化：
$$\begin{gathered} \left[\begin{array}{c}0\\ {R^{\prime }}^F\end{array}\right]=q^F\cdot \left[\begin{array}{c}0\\ R^F\end{array}\right]\cdot {\left(q^F\right)}^{-1}=q^F\cdot \left[\begin{array}{c}0\\ R^F\end{array}\right]\cdot {\left(q^F\right)}^{\mathrm{T}} \\ =L\left(q\right)R{\left(q\right)}^{\mathrm{T}}\left[\begin{array}{c}0\\ R^F\end{array}\right]=R{\left(q\right)}^{\mathrm{T}}L\left(q\right)\left[\begin{array}{c}0\\ R^F\end{array}\right] \end{gathered}$$
同理也可以将上述视为矢量的坐标系变换，其转换矩阵拓展为$4\times 4$，进而有转换矩阵${\left[Q_{\mathrm{B}}^A\right]}_{4\times 4}^{\mathrm{T}}$：
$${\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)$$
此时，则有：
$$\left[\begin{array}{c}0\\ R^B\end{array}\right]={\left[Q_{\mathrm{B}}^A\right]}_{4\times 4}^{\mathrm{T}}\left[\begin{array}{c}0\\ R^A\end{array}\right]$$

3.5 欧拉角（ZYX变换）与RPY角 Euler Angles

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

欧拉角（ZYX变换）的旋转变换描述为：绕固定坐标系的基矢量$k^F$回转$\gamma$，得到新标架$\left\{F_1:\left(i_1^F,j_1^F,k_1^F\right)\right\}$；再绕基矢量$j_1^F$回转$\beta$，得到新标架$\left\{F_2:\left(i_2^F,j_2^F,k_2^F\right)\right\}$；最后绕基矢量$i_2^F$回转$\alpha$，得到新标架$\left\{F_3:\left(i_3^F,j_3^F,k_3^F\right)\right\}$为最终的变换姿态（运动坐标系下连续转动，右乘）。因此对于多次连续转动而言有：

$$\left[Q_{\mathrm{M}}^F\right]=\left[Q_{{\mathrm{F}}_1}^F\left(k^F,\gamma \right)\right]\left[Q_{{\mathrm{F}}_2}^{F_1}\left(j_1^F,\beta \right)\right]\left[Q_{{\mathrm{F}}_3\left(M\right)}^{F_2}\left(i_2^F,\alpha \right)\right]]$$

而对于RPY角（滚转角Roll，仰俯角Pitch，偏航角Yaw）而言，其旋转变换描述为：绕固定坐标系的基矢量$i^F$回转$\alpha$，得到新标架$\left\{F_1:\left(i_1^F,j_1^F,k_1^F\right)\right\}$；再绕固定坐标系的基矢量$j^F$回转$\beta$，得到新标架$\left\{F_2:\left(i_2^F,j_2^F,k_2^F\right)\right\}$；最后绕固定坐标系的基矢量$k^F$回转$\gamma$，得到新标架$\left\{F_3:\left(i_3^F,j_3^F,k_3^F\right)\right\}$为最终的变换姿态（固定坐标系下连续转动，左乘）。因此对于多次连续转动而言有：
$$\left[Q_{\mathrm{M}}^F\right]=\left[Q_{{\mathrm{F}}_3\left(M\right)}^{F_2}\left(k^F,\gamma \right)\right]\left[Q_{{\mathrm{F}}_2}^{F_1}\left(j^F,\beta \right)\right]\left[Q_{{\mathrm{F}}_1}^F\left(i^F,\alpha \right)\right]$$

进而求解出其转换矩阵为：
[ 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 连续转动

• 固定坐标系下运动基矢量连续转动：俗称为左乘）即每一次转动后，新的转动轴与刚体上的矢量在固定坐标系下重新定义，用数学公式表达为：
  $$\begin{array}{rl}{r^{\prime }}^F=\left[Q_{\mathrm{M}}^F\right]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]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]{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]{r_2}^F\\ & =e^{{\theta }_{\mathrm{n}}{v_{\mathrm{n}}}^F}\cdots e^{{\theta }_2{v_2}^F}{e}^{{\theta }_1{v_1}^F}{r}^F\end{array}$$
• 运动坐标系下运动基矢量连续转动：（俗称为右乘）即每一次转动后，新的转动轴与刚体上的矢量在上一次的运动坐标系下重新定义，用数学公式表达为：
  $${r^{\prime }}^F=\left[Q_{\mathrm{M}}^F\right]r^F=\left[Q_{F_1}^F\right]\left[Q_{F_2}^{F_1}\right]\cdots \left[Q_{\mathrm{M}}^{F_{n-1}}\right]r^F=e^{{\theta }_1{v_1}^F}{e}^{{\theta }_2{v_2}^{F_1}}\cdots e^{{\theta }_{\mathrm{n}}{v_{\mathrm{n}}}^{F_{n-1}}}{r}^F$$

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

3.7 齐次矩阵的表达

3.1~3.6 只考虑了坐标系姿态的表达，专注于如何求解/表达$\left[Q_{\mathrm{M}}^F\right]$, 而对于更一般的情况:(忽略原点重合)

$$R_{\mathrm{P}}^F=\left[Q_{\mathrm{M}}^F\right]R_{\mathrm{P}}^M+R_{\mathrm{M}}^F$$

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

$$R_{\mathrm{P}}^F=\left[Q_{\mathrm{M}}^F\right]R_{\mathrm{P}}^M+R_{\mathrm{M}}^F\Rightarrow \left[\begin{array}{c}{R}_{\mathrm{P}}^F\\ 1\end{array}\right]={\left[\begin{array}{cc}\left[Q_{\mathrm{F}}^M\right]& R_{\mathrm{M}}^F\\ 0_{1\times 3}& 1\end{array}\right]}_{4\times 4}\left[\begin{array}{c}{R}_{\mathrm{P}}^M\\ 1\end{array}\right]$$
$$\Rightarrow \left[T_{\mathrm{M}}^F\right]=\left[\begin{array}{cc}\left[Q_{\mathrm{M}}^F\right]& R_{\mathrm{M}}^F\\ 0& 1\end{array}\right]$$
令：$\left[R_{\mathrm{P}}^F\right]=\left[\begin{array}{c}{R}_{\mathrm{P}}^F\\ 1\end{array}\right]\in {\mathbb{R}}^4$，则有：
$$\left[R_{\mathrm{P}}^F\right]=\left[T_{\mathrm{M}}^F\right]\left[R_{\mathrm{P}}^M\right]$$

对于向量 $R_{{\mathrm{P}}_1{\mathrm{P}}_2}^F$ 而言，则有：
$$\left[R_{{\mathrm{P}}_1{\mathrm{P}}_2}^F\right]=\left[R_{{\mathrm{P}}_2}^F-R_{{\mathrm{P}}_1}^F\right]=\left[\begin{array}{c}{R}_{{\mathrm{P}}_2}^F\\ 1\end{array}\right]-\left[\begin{array}{c}{R}_{{\mathrm{P}}_1}^F\\ 1\end{array}\right]=\left[\begin{array}{c}{R}_{{\mathrm{P}}_2}^F-R_{{\mathrm{P}}_1}^F\\ 0\end{array}\right]=\left[\begin{array}{c}{R}_{{\mathrm{P}}_1{\mathrm{P}}_2}^F\\ 0\end{array}\right]$$

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

对于固定坐标系下同一点/向量，在不同坐标系$\left\{A\right\},\left\{B\right\}$下进行表达，存在如下转换关系：
$$R_{\mathrm{Vector}}^A=\left[Q_{\mathrm{B}}^A\right]R_{\mathrm{Vector}}^B$$
$$R_{\mathrm{P}}^A=\left[Q_{\mathrm{B}}^A\right]R_{\mathrm{P}}^B+R_{\mathrm{B}}^A$$

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