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

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

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

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

---

3.4 欧拉四元数变换

同样基于罗德里格旋转公式，定义四个欧拉参数为：
$$\vec{q}=\left[\begin{array}{c}s\\ \vec{v}\end{array}\right]=\left[\begin{array}{cc}\cos \frac{\theta }{2}& \to scalepart\\ \vec{v}\sin \frac{\theta }{2}& \to vectorpart\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}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]$$

3.4.1 四元数的数学性质

1. 归一性 : ${\vec{q}}^{\mathrm{T}}\vec{q}={\sum }_{i=1}^n{q_{\mathrm{i}}}^2=1$
2. 四元数的正交性（逆） : ${\vec{q}}^{\mathrm{T}}\vec{q}=1?{\vec{q}}^{\mathrm{T}}={\vec{q}}^{?1}$
3. 四元数的转置（共轭）——旋转轴不变，旋转角相反 : ${\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]$
4. 四元数的乘法 : $$\begin{array}{rl}{\vec{q}}_1?{\vec{q}}_2& =\left[\begin{array}{c}{s}_1\\ {\vec{v}}_1\end{array}\right]?\left[\begin{array}{c}{s}_2\\ {\vec{v}}_2\end{array}\right]=\left[\begin{array}{c}{s}_1{s}_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{array}{cc}{s}_1& ?{{\vec{v}}_1}^{\mathrm{T}}\\ {\vec{v}}_1& s_{1}E+{\widetilde{\stackrel{⃗}{v}}}_1\end{array}\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{array}{cc}{s}_2& ?{{\vec{v}}_2}^{\mathrm{T}}\\ {\vec{v}}_2& s_{2}E?{\widetilde{\stackrel{⃗}{v}}}_2\end{array}\right]}\\ R\left(q_2\right)\end{array}\left[\begin{array}{c}{s}_1\\ {\vec{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$时：${\vec{q}|}_{\theta =0}=\left[\begin{array}{c}1\\ \vec{0}\end{array}\right]$

根据上述定义，可将轴角变换的旋转矩阵$\left[Q_{\mathrm{B}}^A\right]$ 改写为：
$$\left[Q_{\mathrm{B}}^A\right]=\left[\begin{array}{ccc}1?2{q_3}^2?2{q_4}^2& 2\left(q_2{q}_3?q_1{q}_4\right)& 2\left(q_2{q}_4+q_1{q}_3\right)\\ 2\left(q_2{q}_3+q_1{q}_4\right)& 1?2{q_2}^2?2{q_4}^2& 2\left(q_3{q}_4?q_1{q}_2\right)\\ 2\left(q_2{q}_4?q_1{q}_3\right)& 2\left(q_3{q}_4+q_1{q}_2\right)& 1?2{q_2}^2?2{q_3}^2\end{array}\right]=\left[\begin{array}{ccc}2{q_1}^2+2{q_2}^2?1& 2\left(q_2{q}_3?q_1{q}_4\right)& 2\left(q_2{q}_4+q_1{q}_3\right)\\ 2\left(q_2{q}_3+q_1{q}_4\right)& 2{q_1}^2+2{q_3}^2?1& 2\left(q_3{q}_4?q_1{q}_2\right)\\ 2\left(q_2{q}_4?q_1{q}_3\right)& 2\left(q_3{q}_4+q_1{q}_2\right)& 2{q_1}^2+2{q_4}^2?1\end{array}\right]$$

进而定义矩阵：
$$\begin{array}{rl}{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{array}$$
则有：
$$\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}?\vec{q}?{\vec{q}}^{\mathrm{T}} \\ {\bar{B}}_{3\times 4}?\vec{q}=\vec{0} \end{gathered}$$
因此，若已知旋转矩阵：$\left[Q_{\mathrm{B}}^A\right]=\left[\begin{array}{ccc}{q}_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{23}& q_{33}\end{array}\right]$，则可求解四元数参数为：

$$\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]$$

3.4.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]$$
• 轴角转换为四元数
  $$\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]$$

3.4.3 四元数旋转矢量

对于任意矢量${\vec{R}}^F$，可通过上述四元数矢量进行旋转变化：
$$\begin{gathered} \left[\begin{array}{c}0\\ {{\vec{R}}^{\prime }}^F\end{array}\right]={\vec{q}}^F?\left[\begin{array}{c}0\\ {\vec{R}}^F\end{array}\right]?{\left({\vec{q}}^F\right)}^{?1}={\vec{q}}^F?\left[\begin{array}{c}0\\ {\vec{R}}^F\end{array}\right]?{\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] \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\\ {\vec{R}}^B\end{array}\right]={\left[Q_{\mathrm{B}}^A\right]}_{4\times 4}^{\mathrm{T}}\left[\begin{array}{c}0\\ {\vec{R}}^A\end{array}\right]$$

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

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

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

$$\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]]$$

而对于RPY角（滚转角Roll，仰俯角Pitch，偏航角Yaw）而言，其旋转变换描述为：绕固定坐标系的基矢量${\vec{i}}^F$回转$\alpha$，得到新标架$\left\{F_1:\left({\vec{i}}_1^F,{\vec{j}}_1^F,{\vec{k}}_1^F\right)\right\}$；再绕固定坐标系的基矢量${\vec{j}}^F$回转$\beta$，得到新标架$\left\{F_2:\left({\vec{i}}_2^F,{\vec{j}}_2^F,{\vec{k}}_2^F\right)\right\}$；最后绕固定坐标系的基矢量${\vec{k}}^F$回转$\gamma$，得到新标架$\left\{F_3:\left({\vec{i}}_3^F,{\vec{j}}_3^F,{\vec{k}}_3^F\right)\right\}$为最终的变换姿态（固定坐标系下连续转动，左乘）。因此对于多次连续转动而言有：
$$\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]$$

进而求解出其转换矩阵为：
$$\left[Q_{\mathrm{M}}^F\right]=\left[\begin{array}{ccc}\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{array}\right]$$

同理，若已知转换矩阵：$\left[Q_{\mathrm{B}}^A\right]=\left[\begin{array}{ccc}{q}_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\end{array}\right]$, 则有：
$$\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]$$

3.5.1 欧拉角与四元数的转换

• 欧拉角转换为四元数：
  $$\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]$$
• 四元数转换为欧拉角：
  $$\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c}\mathrm{arc}\tan \frac{2\left(q_1{q}_2+q_3{q}_4\right)}{1?2\left({q_1}^2+{q_2}^2\right)}\\ \mathrm{arc}\sin \left(2\left(q_1{q}_3?q_2{q}_4\right)\right)\\ \mathrm{arc}\tan \frac{2\left(q_1{q}_2+q_3{q}_4\right)}{1?2\left({q_1}^2+{q_2}^2\right)}\end{array}\right]$$

3.5.2 欧拉角与轴角的转换

• 欧拉角转换为轴角：
  $$\begin{array}{rl}\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{array}$$
• 轴角转换为欧拉角：
  $$\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]$$

3.6 连续转动

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

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

3.7 齐次矩阵的表达

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

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

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

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

对于向量 ${\vec{R}}_{{\mathrm{P}}_1{\mathrm{P}}_2}^F$ 而言，则有：
$$\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]$$

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

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

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