机器人运动学：空间描述和变换

在研究空间和刚体之间的关系的时候，工程师们更倾向于数学描述。通过数学工具来描述空间中的物体所处的位置、姿态，这个数学工具最好能用数来对应，这样我们就能借助代数来描述、计算他们之间的相互关系。

笛卡尔坐标系就是一种非常有效的工具，根据具体情况选择合适的坐标系能够减少计算量和复杂度。建立一个世界坐标系，所有在上面的物体和事情都可以很好的描述，是我们学习的重点。

机器人学中涉及坐标变换的，要么就是同一坐标系下的点的运动引起的变换；要么就是同一个点，表达的坐标系基准不同。

下面的内容都是三维笛卡尔坐标系范围内的讨论。

一、坐标系的描述

一个坐标系是由三个两两正交的单位向量来表示的，这个单位向量又叫做坐标系的主轴。需要了解常见记号的含义：

• 坐标系。 { A } \{A\} {A}表示坐标系$A$，它是由大括号包围的大写字母表示的就是坐标系名称；
• 坐标系主轴。${\hat{X}}_B$ 表示坐标系 { B } \{B\} {B}的$X$主轴，上面的帽子表示这是一个主轴，字母$X$表示主轴的具体类型，右下标表示坐标系名称 { B } \{B\} {B}；
• 坐标系下的点、向量。${}^{A}P$表示坐标轴 { A } \{A\} {A}下的点$P$，同时也表示坐标 { A } \{A\} {A}原点作为起点终点为$P$的向量；

1.1 坐标系原点描述

一旦坐标系建立，空间中任意一点既可用向量表达、也可以用坐标表达。

点的坐标是对应向量向参考坐标的三个主轴投影得到的，投影按顺序写成一列代表这个点：
$${}^{A}P=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]\text{\hspace{1em}(1)}$$

• 第一个元素代表向量向${\hat{X}}_A$的投影；
• 第二个元素代表向量向${\hat{Y}}_A$的投影；
• 第三个元素代表向量向${\hat{Z}}_A$的投影；

1.2 坐标系姿态描述

光有位置描述可不够，还需要加上姿态描述。

为了研究机械手末端，我们会在上面固定一个直角坐标系 { B } \{B\} {B}，其上的三个主轴记为${\hat{X}}_B$ ${\hat{Y}}_B$ ${\hat{Z}}_B$ 。坐标系之间的关系就是主轴之间的关系，主轴是一个向量，将这个向量分别往目标坐标系投影即可得到一个坐标值，如前面向量一样，向量结果将会被写成一个列向量。

将我们要描述的三个坐标轴 { B } \{B\} {B}上的主轴往 { A } \{A\} {A}投影就可以确定其唯一姿态，具体做法是：

• 首先，取 { B } \{B\} {B}坐标系中的第一个主轴${\hat{X}}_B$，往 { A } \{A\} {A}的${\hat{X}}_A$轴 ${\hat{Y}}_A$轴 ${\hat{Z}}_A$轴投影三次；
• 投影的结果按顺序写成第一列，记为${}^A{\hat{X}}_B$；
• 然后，再取 { B } \{B\} {B}坐标系中的第二个主轴${\hat{Y}}_B$，往 { A } \{A\} {A}的${\hat{X}}_A$轴 ${\hat{Y}}_A$轴 ${\hat{Z}}_A$轴投影三次；
• 投影的结果按顺序写成第二列，记为${}^A{\hat{Y}}_B$；
• 最后，再取 { B } \{B\} {B}坐标系中的最后一个主轴${\hat{Z}}_B$，往 { A } \{A\} {A}的${\hat{X}}_A$轴 ${\hat{Y}}_A$轴 ${\hat{Z}}_A$投影三次；
• 投影的结果按顺序写成第三列，记为${}^A{\hat{Z}}_B$。

最后我们将会得到一个$3\times 3$的矩阵，在机器人学上被称为旋转矩阵。特别要注意这些符号的表达：

• ${}^A{\hat{X}}_B$： { B } \{B\} {B}系$X$主轴向 { A } \{A\} {A}系的三个投影列向量，因为主轴是一个向量；
• ${}_B^{A}R$：将坐标系为 { A } \{A\} {A}作为参考坐标系表达的 { B } \{B\} {B}坐标系姿态，是一个$3\times 3$的矩阵；

坐标系向哪个坐标系投影就是在哪个坐标系下的旋转矩阵，就是以哪个坐标系为基准，

那么将 { B } \{B\} {B}的所有轴都投影至 { A } \{A\} {A}的三个主轴${\hat{X}}_A$ ${\hat{Y}}_A$ ${\hat{Z}}_A$，那么总计有九个投影，相同轴向 { A } \{A\} {A}的三个轴投影放在一列：
$$\begin{array}{rl}{}_B^{A}R& =\left[\begin{array}{ccc}{}^A{\hat{X}}_B& {}^A{\hat{Y}}_B& {}^A{\hat{Z}}_B\end{array}\right]\\ & =\left[\begin{array}{ccc}{\hat{X}}_B\cdot {\hat{X}}_A& {\hat{Y}}_B\cdot {\hat{X}}_A& {\hat{Z}}_B\cdot {\hat{X}}_A\\ {\hat{X}}_B\cdot {\hat{Y}}_A& {\hat{Y}}_B\cdot {\hat{Y}}_A& {\hat{Z}}_B\cdot {\hat{Y}}_A\\ {\hat{X}}_B\cdot {\hat{Z}}_A& {\hat{Y}}_B\cdot {\hat{Z}}_A& {\hat{Z}}_B\cdot {\hat{Z}}_A\end{array}\right]\end{array}\text{\hspace{1em}(2)}$$
旋转矩阵的逆等于其转置这是一个非常有用的性质。

根据点积的定义与主轴是单位向量的事实，可以得知：旋转矩阵每一个元素值的大小都代表着主轴之间的夹角余弦值。根据坐标的定义，坐标值其实就是这个点对应向量在坐标轴的投影数值大小，那么要得到${}^{A}P$，只需要将在 { B } \{B\} {B}上的投影继续投影至 { A } \{A\} {A}主轴上即可。

两个主轴夹角的余弦值，这个性质意味着，所有与主轴共线的向量长度都可以通过乘以旋转矩阵上的元素得到在另一个坐标系主轴上的长度大小。

向量$a$在$b$的投影可以转换成向量$a$与向量$b$的点积，记做$a\cdot b$，如果$a$ $b$都是单位向量，投影的数值等于两个方向的方向余弦，这就是为什么长度可以直接乘以其点积而不需要是余弦值的原因。${\hat{X}}_B\cdot {\hat{X}}_A$=${\hat{X}}_B\cdot {\hat{X}}_A$这是向量点积 的性质。

1.3 统一的坐标系的描述

坐标系描述=位置描述+姿态描述

从向量角度来说，总共需要四个向量来表示一个坐标系，其中姿态三个，位置一个；从矩阵角度来说，则需要一个位置列向量和$3\times 3$的姿态矩阵来表示：
{ B } = { B A R , A P B O R G } (3) \{B\}=\{_B^A{R},^AP_{BORG}\}{\tag{3}} {B}={BA​R,APBORG​}(3)
${}^A{P}_{BORG}$是一个表示相对于坐标 { A } \{A\} {A}的 { B } \{B\} {B}原点的位置列向量。

二、映射：不同坐标系点坐标变换

映射是理解换算的关键，最早接触映射还是在中学学习函数这一知识点的时候，函数就是一种映射，给我一个自变量，经过映射计算后将给出一个应变量。

坐标系之间的描述可以用一个原点向量和三个主轴投影组成的。同一个物理点在不同坐标系下的点的坐标值一定不同。这一节，我们将会给出一个坐标系下的点与另一个坐标系下点的映射关系：

2.1 平行坐标系的映射

如下图所示，两个坐标系的描述是纯平移，记从 { A } \{A\} {A}下 { B } \{B\} {B}描述为${}^A{P}_{BORG}$。已知空间中的同一点$P$在 { B } \{B\} {B}系下的表示：${}^{B}P$，试着求取$P$在 { A } \{A\} {A}系下的表达${}^{A}P$。

两个坐标系之间的关系为 ${}^A{P}_{BORG}$已知，在这种情况下，只需要对应坐标相加即，这是因为$P$在投影的主轴平行，表现的仅仅是对应分量的增加和减少。

$${}^{A}P{=}^{B}P{+}^A{P}_{BORG}\text{\hspace{1em}(4)}$$
$P$仍然是同一个点，只是描述它的坐标系改变了。

小结：坐标系间平行的映射，输入一个坐标系下的点，输出是另一个坐标系下的点。公式如下：
$${}^{A}P{=}^{B}P{+}^A{P}_{BORG}\text{\hspace{1em}(5)}$$

2.2 旋转坐标系的映射

如下图所示，坐标系之间的描述为已知为纯旋转关系，记${}_B^{A}R$为 { B } \{B\} {B}坐标系在 { A } \{A\} {A}坐标系姿态描述。已知空间中的同一点$P$在 { B } \{B\} {B}系下的表示：${}^{B}P$，试着求取$P$在 { A } \{A\} {A}系下的表达${}^{A}P$。

转换关系如下：
$${}^{A}P{=}_B^A{R}^{B}P\text{\hspace{1em}(6)}$$
已知一个坐标系下的点坐标，只需要左乘一个将原坐标系向目标坐标系的旋转矩阵即可。如上，${}^{B}P$已知，又有${}_B^{A}R。$

{ B } \{B\} {B}系下点${}^{B}P$坐标已知，也就是该点对应向量被分解成 { B } \{B\} {B}主轴的三个分向量：
KaTeX parse error: Got group of unknown type: 'internal'
$x_b$为$P$点在${\hat{X}}_B$上的投影，将这个投影乘以旋转矩阵第一个元素，即$x_b$所在主轴与${\hat{X}}_A$主轴的夹角为$r_{11}$得到$x_b$在新的主轴上的投影大小，$y_b$是$P$点在${\hat{Y}}_B$上的投影，乘以$r_{12}$得到$y_b$在${\hat{X}}_A$上的投影大小，$y_b$是$P$点在${\hat{Z}}_B$上的投影，乘以$r_{13}$得到$z_b$在${\hat{Z}}_A$上的投影大小，将三个和加起来即为${}^{B}P$在 { A } \{A\} {A}下的${\hat{X}}_A$表示，即$x_a$。

$$\begin{array}{r}{}^A{P}_x{=}^B{\hat{X}}_A{\cdot }^{B}P\\ {}^A{P}_y{=}^B{\hat{Y}}_A{\cdot }^{B}P\\ {}^A{P}_z{=}^B{\hat{Z}}_A{\cdot }^{B}P\end{array}$$
写成矩阵形式：
$$\left[\begin{array}{c}{x}_a\\ y_a\\ z_a\end{array}\right]={\left[\begin{array}{ccc}{}^B{\hat{X}}_A& {}^B{\hat{Y}}_A& {}^B{\hat{Z}}_A\end{array}\right]}^T\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]={(}_A^{B}R{)}^T{}^{B}P{=}_B^A{R}^{B}P{=}^{A}P$$

小结：坐标系间仅有旋转的映射，输入一个坐标系下的点，输出是另一个坐标系下的点。公式如下：
$${}^{A}P{=}_B^A{R}^{B}P\text{\hspace{1em}(7)}$$

2.3 一般坐标系的映射

两个坐标系可能是上述两种关系的混合，如下：

已知空间中的同一点$P$在 { B } \{B\} {B}系下的表示：${}^{B}P$，试着求取$P$在 { A } \{A\} {A}系下的表达${}^{A}P$。做法如下，首先将${}^{B}P$经过左乘${}_B^{A}R$得到旋转至中间坐标系（与 { A } \{A\} {A}同姿态），然后再经过加法，求得${}^{A}P$，整合后：
$${}^{A}P{=}_B^{A}R{\cdot }^{B}P{+}^A{P}_{BORG}\text{\hspace{1em}(8)}$$
经过上述计算就完成了坐标切换。写成矩阵形式：
$$\left[\begin{array}{c}{}^{A}P\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_B^{A}R& {}^A{P}_{BORG}\\ 000& 1\end{array}\right]\left[\begin{array}{c}{}^{B}P\\ 1\end{array}\right]$$
那么$T=\left[\begin{array}{cc}{}_B^{A}R& {}^A{P}_{BORG}\\ 000& 1\end{array}\right]$则是实现转换的一个左乘矩阵。

小结：更加通用的映射，坐标系间既有旋转又有平移的映射，输入一个坐标系下的点，输出是另一个坐标系下的点。公式如下：
$${}^{A}P{=}_B^{A}R{\cdot }^{B}P{+}^A{P}_{BORG}\text{\hspace{1em}(9)}$$

通用映射的矩阵形式：
$${}^{A}P{=}_B^A{T}^{B}P\text{\hspace{1em}(10)}$$

三、算子：同个坐标系下点的变换

前面的映射是为了解决不同坐标系下的点的变换，这一节的映射则是为了解决同一坐标系下点的变换。

3.1 平移算子

平移将空间中的一个点沿着一个已知的矢量方向移动一定距离。空间中点的平移与此点向另一个坐标系的映射有相同数学表达式，因为你可以将平移看作是坐标系的移动。

$${}^A{P}_2{=}^A{P}_1{+}^{A}Q\text{\hspace{1em}(11)}$$
矩阵形式：
$${}^A{P}_2=D_Q(q{)}^A{P}_1$$
其中$q$是沿着矢量$\hat{Q}$方向的数量。$D_Q$的具体形式如下：
$$D_Q(q)=\left[\begin{array}{cccc}1& 0& 0& q_x\\ 0& 1& 0& q_y\\ 0& 0& 1& q_z\\ 0& 0& 0& 1\end{array}\right]$$

3.2 旋转算子

可以推导：
$${}^A{P}_2=R^A{P}_1$$
数学表达式完全相同，不过$R$含义变成了绕广义轴旋转的旋转矩阵，而且因为涉及到不同坐标系，$R$坐标没有任何符号，表示在同一个坐标系下。

。用矩阵表示：
$${}^A{P}_2=R_K(\theta {)}^A{P}_1$$
$K$为$Z$轴旋转的$R_K(\theta )$具体形式如下:
$$R_K(\theta )=\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& 0\\ \sin \theta & \cos \theta & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

3.3 变换算子

同理，变换算子$T$也是其次矩阵形式。

四、总结

一个齐次矩阵完成了三个方面的描述：

• 坐标系描述
• 变换映射
• 变换算子

---

20220331 Fix了一些公式错误，删除了公式上的错误。
20220402 增加了引言部分，删除了开头不太通顺的句子。
20220718 增加了一些方便理解的描述，修改了符号错误。