《机器人学导论》学习-01空间描述与变换

一、描述：位置、姿态与坐标系

1、位置描述

用一个位置矢量来描述空间中一个点的位置。

位置描述为：${}_{}^A\text{P}=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$

2、姿态描述

物体的姿态可以用固定在物体上的坐标系{B}来描述。

2.1 姿态描述为：
$${}_B^A\text{R}=\left[\begin{array}{ccc}& {}_{}^A{\hat{\text{X}}}_B{}_{}^A{\hat{\text{Y}}}_B{}_{}^A{\hat{\text{Z}}}_B& \end{array}\right]=\left[\begin{array}{ccc}& {\hat{\text{X}}}_B\cdot {\hat{\text{X}}}_A{\hat{\text{Y}}}_B\cdot {\hat{\text{X}}}_A{\hat{\text{Z}}}_B\cdot {\hat{\text{X}}}_A& \\ & {\hat{\text{X}}}_B\cdot {\hat{\text{Y}}}_A{\hat{\text{Y}}}_B\cdot {\hat{\text{Y}}}_A{\hat{\text{Z}}}_B\cdot {\hat{\text{Y}}}_A& \\ & {\hat{\text{X}}}_B\cdot {\hat{\text{Z}}}_A{\hat{\text{Y}}}_B\cdot {\hat{\text{Z}}}_A{\hat{\text{Z}}}_B\cdot {\hat{\text{Z}}}_A& \end{array}\right]$$
2.2 注意：
（1） ${}_{}^A\hat{\text{X}}$表示{B}上X轴的单位向量在用{A}表达；
（2）${\hat{\text{X}}}_B\cdot {\hat{\text{X}}}_A$表示{B}上X轴的单位向量在{A}上X轴单位向量上的投影。
2.3 特性：
${}_B^A\text{R}$被称为旋转矩阵，它表示坐标系{B}相对于坐标系{A}的表达。（注意：左上标A表示参考的坐标系；左下标B表示被描述的坐标系）
（1）${}_B^A\text{R}{=}_A^B{\text{R}}_{}^T{=}_A^B{\text{R}}_{}^{-1‘}$(旋转矩阵是正交矩阵）
（2）$\left|{}_B^A\text{R}\right|=1$

3、坐标系的描述

（1）一个坐标系可以等价于用一个位置矢量和一个旋转矩阵来描述；
（2）用${}_B^A\text{R}$和${}_{}^A{\text{P}}_{BORG}$来描述坐标系{B}，其中的${}_{}^A{\text{P}}_{BORG}$是确定坐标系{B}原点的位置，则：{B}={${}_B^A\text{R}$，${}_{}^A{\text{P}}_{BORG}$}。

二、映射：坐标系到坐标系的变换

1、关于平移坐标系的映射

{A}和{B}坐标系具有相同的姿态（没有旋转。只有平移），可以用矢量相加的方法求点P在坐标系{A}中的表达：
$${}_{}^A\text{P}{=}_{}^B\text{P}{+}_{}^A{\text{P}}_{BORG}$$
注意：不同坐标系的矢量，只有在坐标系姿态相同的情况下才能进行相加

2、关于旋转坐标系的映射

将空间中某点P相对于坐标系{B}的描述${}_{}^B\text{P}$，转换成该点相对于坐标系[A}的描述${}_{}^A\text{P}$:
$${}_{}^A\text{P}{=}_B^A\text{R}{\cdot }_{}^B\text{P}$$
注意：从映射的角度来看，原矢量P在空间中的位置没有发生改变，只是换了一种描述方式对其进行表达。

3、关于一般坐标系的映射

先将${}_{}^B\text{P}$变换到一个中间的坐标系{C}（此时变为${}_{}^C\text{P}$），这个中间坐标系和{A}坐标系的姿态相同、原点和{B}的原点重合，然后用简单的矢量加法，将原点进行平移，得到${}_{}^A\text{P}$：$${}_B^A\text{R}{=}_B^C\text{R}$$
$${}_{}^A{\text{P}}_{BORG}{=}_{}^A{\text{P}}_{CORG}$$
$${}_{}^C\text{P}{=}_B^C\text{R}{\cdot }_{}^B\text{P}$$
$${}_{}^A\text{P}{=}_{}^C\text{P}{+}_{}^A{\text{P}}_{CORG}$$
综上：$${}_{}^A\text{P}{=}_B^A\text{R}{\cdot }_{}^B\text{P}{+}_{}^A{\text{P}}_{BORG}$$
将上式改写成新的形式：
$${}_{}^A\text{P}{=}_B^A\text{T}{\cdot }_{}^B\text{P}$$
对于一般坐标系的映射：坐标系{B}相对于坐标系{A}的变换描述为${}_B^A\text{T}$。
$$\left[\begin{array}{c}{}_{}^A\text{P}\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_B^A\text{R}& {}_{}^A{\text{P}}_{BORG}\\ 000& 1\end{array}\right]\cdot \left[\begin{array}{c}{}_{}^B\text{P}\\ 1\end{array}\right]\Rightarrow \{\begin{array}{c}{}_{}^A\text{P}{=}_B^A\text{R}{\cdot }_{}^B\text{P}{+}_{}^A{\text{P}}_{BORG}\\ 1=1\end{array}$$
式中：${}_B^A\text{T}=\left[\begin{array}{cc}{}_B^A\text{R}& {}_{}^A{\text{P}}_{BORG}\\ 000& 1\end{array}\right]$，被称为齐次变换矩阵。
齐次变换矩阵左上角表示旋转，右上角表示平移。

三、算子：平移、旋转和变换

用于坐标系间点的映射的通用数学表达式称为算子，包括点的平移算子、矢量旋转算子和平移加旋转的算子。

1、平移算子

矢量${}_{}^A{\text{P}}_1$通过矢量${}_{}^A\text{Q}$进行了平移，得到新的矢量${}_{}^A{\text{P}}_2$，表示为：
$${}_{}^A{\text{P}}_1{=}_{}^A\text{Q}{+}_{}^A{\text{P}}_2$$
用平移算子表示：
$${}_{}^A{\text{P}}_2={\text{D}}_{Q(q)}{\cdot }_{}^A{\text{P}}_1$$
式中：q是沿矢量$\hat{\text{Q}}$方向平移的距离，其是有符号的；平移算子${\text{D}}_{Q(q)}$可以被看成是一种特殊的齐次变换矩阵，其中的旋转矩阵为单位矩阵。
$${\text{D}}_{Q(q)}=\left[\begin{array}{cccc}1& 0& 0& {\text{q}}_x\\ 0& 1& 0& {\text{q}}_y\\ 0& 0& 1& {\text{q}}_z\\ 0& 0& 0& 1\end{array}\right]$$
式中，${\text{q}}_x、{\text{q}}_y和{\text{q}}_z$是平移矢量$\hat{\text{Q}}$的分量，并且有：$q=\sqrt{{\text{q}}_x^2+{\text{q}}_y^2+{\text{q}}_z^2}$

2、旋转算子

矢量${}_{}^A{\text{P}}_1$通过旋转矩阵R变成了一个新的矢量${}_{}^A{\text{P}}_2$，表示为：$${}_{}^A{\text{P}}_2=R{\cdot }_{}^A{\text{P}}_1$$
针对这种情况，一般用下面的形式表达：
$${}_{}^A{\text{P}}_2={\text{R}}_K\left(\theta \right){\cdot }_{}^A{\text{P}}_1$$
式中的${\text{R}}_K\left(\theta \right)$是一个旋转算子，表示绕$\hat{K}$轴旋转了$\theta$角。旋转算子也可以写成齐次变换矩阵其中位置矢量为零。
例如，沿着$\hat{Z}$轴旋转了$\theta$角度的旋转算子：
$${\text{R}}_Z\left(\theta \right)=\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]$$
注意：矢量经过某一旋转R得到的旋转矩阵与一个坐标系相对于参考坐标系经过某一旋转R得到的选转矩阵是相同的。

3、变换算子

算子$T$将矢量${}_{}^A{\text{ P}}_1$平移并旋转得到一个新的矢量${}_{}^A{\text{ P}}_2$：
$${}_{}^A{\text{P}}_2=T{\cdot }_{}^A{\text{P}}_1$$

四、总结和说明

齐次坐标变换矩阵的三种用法（定义）：

1、描述一个坐标系（相对于另一个坐标系）的位姿。

${}_B^A\text{T}$表示相对于坐标系{A}的坐标系{B}。

2、将点由一个坐标系的表达，转换到另一个坐标系下的表达（变换映射）。

$${}_{}^A\text{P}{=}_B^A\text{T}{\cdot }_{}^B\text{P}$$

3、将点（向量）在同一坐标系中进行转动（变换算子）。

$${}_{}^A{\text{P}}_2=T{\cdot }_{}^A{\text{P}}_1$$