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

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

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

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

---

刚体的位形可以用六个独立（坐标）参数完全描述：三个位置参数用于描述运动刚体上运动坐标系 { M } \left\{ M \right\} {M}原点$M$在固定坐标系 { F } \left\{ F \right\} {F}的投影参数，三个转动参数用于描述运动坐标系 { M } \left\{ M \right\} {M}的基矢量相对于固定坐标系 { F } \left\{ F \right\} {F}的基矢量的姿态，而描述这种姿态的变换，则是需要确定矩阵$\left[Q_{\mathrm{M}}^F\right]$。

因此为描述空间坐标系中任意一刚体的运动状态，首先需要描述刚体的位置矢量${\vec{R}}_{\mathrm{M}}^F$与姿态矩阵$\left[Q_{\mathrm{M}}^F\right]$

• 广义参考系坐标 Reference Coordinates：为方便后续动力学方程的建立与推导，常用广义坐标矢量参数${\vec{q}}_{\mathrm{M}}^F$来描述运动刚体的形位，其中：
  $${\vec{q}}_{\mathrm{M}}^F=\left[{\vec{R}}_{\mathrm{M}}^F,{\vec{\theta }}_{\mathrm{M}}^F\right]$$
• ${\vec{\theta }}_{\mathrm{M}}^F$可以用多种方法来描述（通常包含3或4个角度参数 ），这些角度参数用于描述矩阵$\left[Q_{\mathrm{M}}^F\right]$

对于刚体的运动状态而言，其运动坐标系的原点$M$的位置矢量${\vec{R}}_{\mathrm{M}}^F$表示与点的运动状态表示相同，因此需要探究如何用角度参数来描述转换矩阵。

3. 转换矩阵与旋转矩阵——刚体的位置与姿态描述

转换矩阵用于表述两个坐标系$\left\{A:\left({\vec{i}}^A,{\vec{j}}^A,{\vec{k}}^A\right)\right\}$ 与$\left\{B:\left({\vec{i}}^B,{\vec{j}}^B,{\vec{k}}^B\right)\right\}$的基矢量之间的转换关系：
$$\left[\begin{array}{c}{\vec{i}}^B\\ {\vec{j}}^B\\ {\vec{k}}^B\end{array}\right]={\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]$$
其中，转换矩阵${\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}$表示坐标系 { B } \left\{ B \right\} {B}的基矢量在坐标系 { A } \left\{ A \right\} {A}中的表达，可将向量在不同的基矢量坐标系下进行表示。特殊的：若将基矢量替换成对应基矢量的向量投影，则可以表示为：两个原点重合的坐标系中，对同一向量的不同表达的转换关系；

上式也可以理解为：对坐标系$\left\{A:\left({\vec{i}}^A,{\vec{j}}^A,{\vec{k}}^A\right)\right\}$进行了$\left[Q_{\mathrm{B}}^A\right]$的旋转，此时将转换矩阵与向量的运算理解为张量与向量的运算，即得到了旋转后的向量在坐标系 { A } \left\{ A \right\} {A}中的表达，此时实际上，对原始坐标系 { A } \left\{ A \right\} {A}的基矢量同样进行了旋转，形成了新坐标系 { B } \left\{ B \right\} {B}的基矢量，其仍在坐标系 { A } \left\{ A \right\} {A}下表达。

$$\left[\begin{array}{c}{r_1^A}^{\prime }\\ {r_2^A}^{\prime }\\ {r_3^A}^{\prime }\end{array}\right]=\left[Q_{\mathrm{B}}^A\right]\left[\begin{array}{c}{r}_1^A\\ r_2^A\\ r_3^A\end{array}\right]$$

目前，人们采用不同的角度参数$\vec{\theta }$来对旋转矩阵进行描述

• Representing an orientation —— from definition
  将原矢量进行旋转变换，得到该坐标系下新矢量的坐标投影参数：
  $${\vec{R}}_{{\mathrm{p}}^{\prime }}^F=\left[Q_{\mathrm{B}}^A\right]{\vec{R}}_{\mathrm{p}}^F$$
• Changing the reference frame
  对坐标系进行转换，基于坐标系 { B } \left\{ B \right\} {B}中的该矢量的坐标投影参数${\vec{R}}_{\mathrm{p}}^B$，得到该矢量在坐标系 { A } \left\{ A \right\} {A}中的坐标投影参数${\vec{R}}_{\mathrm{p}}^A$：
  $${\vec{R}}_{\mathrm{p}}^A=\left[Q_{\mathrm{B}}^A\right]{\vec{R}}_{\mathrm{p}}^B$$

3.1 轴角变换

假设两个坐标系 { A } \left\{ A \right\} {A}与 { B } \left\{ B \right\} {B}的原点重合，其中坐标系 { B } \left\{ B \right\} {B}为坐标系 { A } \left\{ A \right\} {A}绕轴${\vec{v}}^F$（单位向量）旋转$\theta$所得到的。因此对于坐标系 { A } \left\{ A \right\} {A}中的点$P$，经过转换后，得到点$P^{\prime }$，此时点$P^{\prime }$在坐标系 { B } \left\{ B \right\} {B}中的矢量投影与点$P$在坐标系 { A } \left\{ A \right\} {A}中的投影分量相同。而在转换过程中，点$P^{\prime }$在坐标系 { A } \left\{ A \right\} {A}中的表达发生变化，即有：$\left[{P^{\prime }}_1^{\mathrm{B}},{P^{\prime }}_2^{\mathrm{B}},{P^{\prime }}_2^{\mathrm{B}}\right]=\left[P_1^A,P_2^A,P_2^A\right]$，因此对式$\left[\begin{array}{c}{\vec{i}}^B\\ {\vec{j}}^B\\ {\vec{k}}^B\end{array}\right]={\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]$有：
$$\begin{array}{rl}& {\left[\begin{array}{c}{\vec{i}}^B\\ {\vec{j}}^B\\ {\vec{k}}^B\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{P}_1^{\mathrm{B}}\\ P_2^{\mathrm{B}}\\ P_3^{\mathrm{B}}\end{array}\right]={\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{P}_1^A\\ P_2^A\\ P_3^A\end{array}\right]\\ & ?{\left({\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]\right)}^{\mathrm{T}}\left[\begin{array}{c}{P}_1^{\mathrm{B}}\\ P_2^{\mathrm{B}}\\ P_3^{\mathrm{B}}\end{array}\right]={\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{P}_1^A\\ P_2^A\\ P_3^A\end{array}\right]\\ & ?{\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]}^{\mathrm{T}}\left[Q_{\mathrm{B}}^A\right]\left[\begin{array}{c}{P}_1^{\mathrm{B}}\\ P_2^{\mathrm{B}}\\ P_3^{\mathrm{B}}\end{array}\right]={\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{P}_1^A\\ P_2^A\\ P_3^A\end{array}\right]\\ & ?\left[Q_{\mathrm{B}}^A\right]\left[\begin{array}{c}{P}_1^{\mathrm{B}}\\ P_2^{\mathrm{B}}\\ P_3^{\mathrm{B}}\end{array}\right]=\left[\begin{array}{c}{P}_1^A\\ P_2^A\\ P_3^A\end{array}\right]=\left[\begin{array}{c}{P^{\prime }}_1^{\mathrm{B}}\\ {P^{\prime }}_2^{\mathrm{B}}\\ {P^{\prime }}_3^{\mathrm{B}}\end{array}\right]\end{array}$$
上式写明：坐标系 { B } \left\{ B \right\} {B}中，点$P$与点$P^{\prime }$之间的旋转关系。此时$P^{\prime }$为运动刚体上的固定点，对点$P$在坐标系 { B } \left\{ B \right\} {B}下的投影参数进行${\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}$旋转变化所得到的点$P^{\prime }$在坐标系 { B } \left\{ B \right\} {B}下的投影参数。同时，对于$P$与$P^{\prime }$而言，其在某坐标系下表达的旋转关系是一致的，因此对于：$\left[Q_{\mathrm{B}}^A\right]\left[\begin{array}{c}{P}_1^A\\ P_2^A\\ P_3^A\end{array}\right]=\left[\begin{array}{c}{P^{\prime }}_1^A\\ {P^{\prime }}_2^A\\ {P^{\prime }}_3^A\end{array}\right]$同样成立。

同理，利用几何关系对图进行分析，进而求得罗德里格旋转公式Rodrigues’ Rotation Formula：
$$\begin{array}{rl}& {\vec{R}}_{{\mathrm{p}}^{\prime }}^F={\vec{R}}_{\mathrm{p}}^F+\left({\vec{v}}^F\times {\vec{R}}_{{\mathrm{p}}^{\prime }}^F\right)\sin \theta +2\left[{\vec{v}}^F\times \left({\vec{v}}^F\times {\vec{R}}_{{\mathrm{p}}^{\prime }}^F\right)\right]{\sin }^2\frac{\theta }{2}={\vec{R}}_{\mathrm{p}}^F+{\widetilde{\stackrel{⃗}{v}}}^F{\vec{R}}_{{\mathrm{p}}^{\prime }}^F\sin \theta +2{\left({\widetilde{\stackrel{⃗}{v}}}^F\right)}^2{\vec{R}}_{{\mathrm{p}}^{\prime }}^F{\sin }^2\frac{\theta }{2}\\ & ?{\vec{R}}_{{\mathrm{p}}^{\prime }}^F=\left[E+{\widetilde{\stackrel{⃗}{v}}}^F\sin \theta +2{\left({\widetilde{\stackrel{⃗}{v}}}^F\right)}^2\sin \frac{\theta }{2}\right]{\vec{R}}_{\mathrm{p}}^F=\left[Q_{\mathrm{B}}^A\right]{\vec{R}}_{\mathrm{p}}^F\end{array}$$

而上式给出了：坐标系 { A } \left\{ A \right\} {A}中，点$P$与点$P^{\prime }$之间的转换关系。此时$P$为运动刚体上的固定点，对点$P$在坐标系 { A } \left\{ A \right\} {A}下的投影参数进行$\left[Q_{\mathrm{B}}^A\right]$旋转变化，所得到的点$P^{\prime }$在坐标系 { A } \left\{ A \right\} {A}下的投影参数。

可见，对于旋转矩阵$\left[Q_{\mathrm{B}}^A\right]$有三种含义：

• 将原矢量进行旋转变换，得到该坐标系下新矢量的坐标投影参数：${\vec{R}}_{{\mathrm{p}}^{\prime }}^F=\left[Q_{\mathrm{B}}^A\right]{\vec{R}}_{\mathrm{p}}^F$；
• 对坐标系进行转换，基于坐标系 { B } \left\{ B \right\} {B}中的该矢量的坐标投影参数${\vec{R}}_{\mathrm{p}}^B$，得到该矢量在坐标系 { A } \left\{ A \right\} {A}中的坐标投影参数${\vec{R}}_{\mathrm{p}}^A$：${\vec{R}}_{\mathrm{p}}^A=\left[Q_{\mathrm{B}}^A\right]{\vec{R}}_{\mathrm{p}}^B$；
• 坐标系 { A } \left\{ A \right\} {A}中的基矢量在坐标系 { B } \left\{ B \right\} {B}中的表达： $\left[\begin{array}{c}{\vec{i}}^B\\ {\vec{j}}^B\\ {\vec{k}}^B\end{array}\right]={\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]$。

对罗德里格旋转公式进一步进行变换，将其改写为$\left[Q_{\mathrm{B}}^A\right]=E+{\widetilde{\stackrel{⃗}{v}}}^F\sin \theta +2{\left({\widetilde{\stackrel{⃗}{v}}}^F\right)}^2\left(1?\cos \theta \right)$，进而利用泰勒展开式，将旋转矩阵$\left[Q_{\mathrm{B}}^A\right]$进一步改写：
$$\left[Q_{\mathrm{B}}^A\right]=E+\theta {\widetilde{\stackrel{⃗}{v}}}^F+\frac{{\left(\theta \right)}^2}{2!}{\left(\widetilde{\stackrel{⃗}{v}}\right)}^2+\frac{{\left(\theta \right)}^3}{3!}{\left(\widetilde{\stackrel{⃗}{v}}\right)}^3+?+\frac{{\left(\theta \right)}^n}{n!}{\left(\widetilde{\stackrel{⃗}{v}}\right)}^n=e^{\theta \widetilde{\stackrel{⃗}{v}}}$$
可将轴角变换的转换矩阵写成指数形式。

• 综合上述推导，可得到轴角变换的旋转矩阵$\left[Q_{\mathrm{B}}^A\right]$为：
  $$\left[Q_{\mathrm{B}}^A\right]=\left[\begin{array}{ccc}{\left(v_1^A\right)}^2\left(1?\cos \theta \right)+\cos \theta & v_1^A{v}_2^A\left(1?\cos \theta \right)?v_3^A\sin \theta & v_1^A{v}_3^A\left(1?\cos \theta \right)+v_2^A\sin \theta \\ v_1^A{v}_2^A\left(1?\cos \theta \right)+v_3^A\sin \theta & {\left(v_2^A\right)}^2\left(1?\cos \theta \right)+\cos \theta & v_2^A{v}_3^A\left(1?\cos \theta \right)?v_1^A\sin \theta \\ v_1^A{v}_3^A\left(1?\cos \theta \right)?v_2^A\sin \theta & v_2^A{v}_3^A\left(1?\cos \theta \right)+v_1^A\sin \theta & {\left(v_3^A\right)}^2\left(1?\cos \theta \right)+\cos \theta \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]$，可计算出其轴角参数：
  $$\begin{array}{rl}\theta & =\mathrm{arc}\cos \left(\frac{q_{11}+q_{22}+q_{33}?1}{2}\right)\\ {\vec{v}}^F& =\frac{1}{2\sin \theta }\left[\begin{array}{c}{q}_{32}?q_{23}\\ q_{13}?q_{31}\\ q_{21}?q_{12}\end{array}\right]\end{array}$$

3.2 罗德里格变换Rodrigues’ Transform

结合上节所述内容，定义罗德里格参数Rodriguez Paremeters为：
$${\vec{\gamma }}^F={\vec{v}}^F\tan \frac{\theta }{2}=\left[\begin{array}{c}{v}_1^F\\ v_2^F\\ v_3^F\end{array}\right]\tan \frac{\theta }{2}=\left[\begin{array}{c}{\gamma }_1^F\\ {\gamma }_2^F\\ {\gamma }_3^F\end{array}\right]$$

进而将罗德里格旋转公式改写为：
$$\left[Q_{\mathrm{M}}^F\right]=E+\frac{2}{1+{\left(\gamma \right)}^2}\left({\widetilde{\stackrel{⃗}{\gamma }}}^F+{\left({\widetilde{\stackrel{⃗}{\gamma }}}^F\right)}^2\right),\gamma ={\left({\widetilde{\stackrel{⃗}{\gamma }}}^F\right)}^{\mathrm{T}}{\widetilde{\stackrel{⃗}{\gamma }}}^F={\tan }^2\frac{\theta }{2}$$

而罗德里格变换的旋转矩阵$\left[Q_{\mathrm{B}}^A\right]$为：
$$\left[Q_{\mathrm{B}}^A\right]=\left[\begin{array}{ccc}1+{\left({\gamma }_1^F\right)}^2?{\left({\gamma }_2^F\right)}^2?{\left({\gamma }_3^F\right)}^2& 2\left({\gamma }_1^F{\gamma }_2^F?{\gamma }_3^F\right)& 2\left({\gamma }_1^F{\gamma }_3^F+{\gamma }_2^F\right)\\ 2\left({\gamma }_1^F{\gamma }_2^F+{\gamma }_3^F\right)& 1?{\left({\gamma }_1^F\right)}^2+{\left({\gamma }_2^F\right)}^2?{\left({\gamma }_3^F\right)}^2& 2\left({\gamma }_2^F{\gamma }_3^F?{\gamma }_1^F\right)\\ 2\left({\gamma }_1^F{\gamma }_3^F?{\gamma }_2^F\right)& 2\left({\gamma }_2^F{\gamma }_3^F+{\gamma }_1^F\right)& 1?{\left({\gamma }_1^F\right)}^2?{\left({\gamma }_2^F\right)}^2+{\left({\gamma }_3^F\right)}^2\end{array}\right]$$

• 罗德里格参数与欧拉参数的转换
  $$\left[\begin{array}{c}{{\gamma }_1}^F\\ {{\gamma }_2}^F\\ {{\gamma }_3}^F\end{array}\right]=\left[\begin{array}{c}\frac{q_2}{q_1}\\ \frac{q_3}{q_1}\\ \frac{q_4}{q_1}\end{array}\right]$$

$$\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\left[\begin{array}{c}\frac{1}{\sqrt{1+{\gamma }^2}}\\ \frac{{{\gamma }_1}^F}{\sqrt{1+{\gamma }^2}}\\ \frac{{{\gamma }_2}^F}{\sqrt{1+{\gamma }^2}}\\ \frac{{{\gamma }_3}^F}{\sqrt{1+{\gamma }^2}}\end{array}\right]$$

3.3 方向余弦变换

由上节可知，转换矩阵$\left[Q_{\mathrm{B}}^A\right]$表示坐标系 { B } \left\{ B \right\} {B}中的基矢量在坐标系 { A } \left\{ A \right\} {A}中的表达，即：
$$\left[\begin{array}{c}{\vec{i}}^B\\ {\vec{j}}^B\\ {\vec{k}}^B\end{array}\right]={\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\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]}^{\mathrm{T}}\left[\begin{array}{c}{\vec{i}}^A\\ {\vec{j}}^A\\ {\vec{k}}^A\end{array}\right]$$

进而将转换矩阵内的元素展开：
$${\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}=\left[\begin{array}{ccc}{\vec{i}}^A?{\vec{i}}^B& {\vec{j}}^A?{\vec{i}}^B& {\vec{k}}^A?{\vec{i}}^B\\ {\vec{i}}^A?{\vec{j}}^B& {\vec{j}}^A?{\vec{j}}^B& {\vec{k}}^A?{\vec{j}}^B\\ {\vec{i}}^A?{\vec{k}}^B& {\vec{j}}^A?{\vec{k}}^B& {\vec{k}}^A?{\vec{k}}^B\end{array}\right]$$
进一步观察，可以将该矩阵转化为：
$${\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}=\left[\begin{array}{ccc}{\vec{i}}^A?{\vec{i}}^B& {\vec{j}}^A?{\vec{i}}^B& {\vec{k}}^A?{\vec{i}}^B\\ {\vec{i}}^A?{\vec{j}}^B& {\vec{j}}^A?{\vec{j}}^B& {\vec{k}}^A?{\vec{j}}^B\\ {\vec{i}}^A?{\vec{k}}^B& {\vec{j}}^A?{\vec{k}}^B& {\vec{k}}^A?{\vec{k}}^B\end{array}\right]=\left[\begin{array}{ccc}{Q}_{\mathrm{Bi}}^A& Q_{\mathrm{Bj}}^A& Q_{\mathrm{Bk}}^A\end{array}\right]=\left[\begin{array}{c}{Q}_{\mathrm{Ai}}^B\\ Q_{\mathrm{Aj}}^B\\ Q_{\mathrm{Ak}}^B\end{array}\right]$$
其中，$\left[\begin{array}{ccc}{Q}_{\mathrm{Bi}}^A& Q_{\mathrm{Bj}}^A& Q_{\mathrm{Bk}}^A\end{array}\right]$中，每一项表示坐标系 { B } \left\{ B \right\} {B}中的基矢量在坐标系 { A } \left\{ A \right\} {A}下的表达，而$\left[\begin{array}{c}{Q}_{\mathrm{Ai}}^B\\ Q_{\mathrm{Aj}}^B\\ Q_{\mathrm{Ak}}^B\end{array}\right].$中，每一项表示坐标系 { A } \left\{ A \right\} {A}中的基矢量在坐标系 { B } \left\{ B \right\} {B}下的表达。因此该形式的矩阵被称为方向余弦矩阵Direction Cosine Matrix。

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