【机器人学导论笔记】二、空间描述与变换

最新推荐文章于 2024-05-26 22:55:27 发布

城主_全栈开发

最新推荐文章于 2024-05-26 22:55:27 发布

阅读量546

点赞数 1

分类专栏：机器人文章标签：机器人线性代数算法

本文链接：https://blog.csdn.net/hustle28214/article/details/131708930

版权

机器人专栏收录该内容

5 篇文章 1 订阅

订阅专栏

文章详细介绍了机器人学中描述机械臂姿态和坐标系变换的方法，包括平移坐标系、旋转坐标系和一般坐标系的映射，以及旋转矩阵和齐次变换矩阵的概念。此外，还讲解了欧拉角的不同表示方式，如X-Y-Z固定角、Z-Y-X欧拉角和Z-Y-Z欧拉角，以及等效轴角坐标系表示法。文章强调了齐次变换矩阵在坐标系变换中的重要性，并介绍了平移算子和旋转算子的计算。

摘要由CSDN通过智能技术生成

阅读提示：
本文是基于《机器人学导论》第三版的学习笔记，除去书中内容，有一些自己的心得和理解。所以会对一些方面进行概括，长话短说。若需要专业解释强烈建议找到原书翻看定义，收获会很大！

位置、姿态和坐标系

要描述一个机械臂的姿态，我们应当明白，假设我们要描述手的姿态，容易想知对于指端来说，操作目标物体的轨迹应当在一个坐标系内被描述。然而，对于多元件的机械臂系统来说，仅仅靠这一个坐标系是不够的，甚至很混乱。所以我们应当设立一个参考系，然后建立操作手的坐标系，这样就能完全确定操作手的位置。

因此，点的位置可用矢量描述，物体的姿态可用坐标系描述。描述连体坐标系可用固定在物体上的坐标系来描述。
我们用参考系A的三个主轴单位矢量来描述连体坐标系B。设其为 $X_A$ , $Y_A$ , $Z_A$ ，将这三个单位矢量排列成一个 $3\times3$ 的矩阵，称其为旋转矩阵。它是一个相对表达，是坐标系B相对于坐标系A的表达，所以用符号 $^AP$ 来表示。
在这里插入图片描述

由两个单位矢量的点积可得到二者之间夹角的余弦，所以，旋转矩阵的各分量也常被称为方向余弦。
在这里插入图片描述

进一步观察式子，
在这里插入图片描述

因此，可得： $^B_AR=^A_BR^T$ 。(重要结论)

进一步可得，旋转矩阵的逆矩阵等于它的转置：

$^B_AR^A_BR^T=\left[\begin{matrix}^A\hat{X}^T_B\\^A\hat{Y}^T_B\\^A\hat{Z}^T_B\end{matrix}\right]\begin{pmatrix}^A\hat{X}^T_B&^A\hat{Y}^T_B&^A\hat{Z}^T_B\end{pmatrix}=I_3$

实际上，一个正交阵的逆等于它的转置，这个结论也能很快证明上述结论。

$^A_BR=^B_AR^{-1}=^B_AR^T$

得到了位置和姿态，描述坐标系，需要一个原点。我们为方便起见，将物体上任选的一点作为连体坐标系的原点。于是我们将一个位置矢量和一个旋转矩阵组合在一起，描述一个坐标系：

$\{B\}=\{^A_BR,^AP_{BORG}\}$

其中 $^AP_{BORG}$ 是确定坐标系原点的位置矢量。

特别地，如果参考系中的位置矢量是零矢量，那么它表示的就是姿态。

映射

映射描述从坐标系到坐标系的变换。
我们将讨论三种映射，并引入齐次变换矩阵这一重要工具。

平移坐标系的映射

在这里插入图片描述

这是最简单的一种映射，用矢量相加的方法表示：

$^AP=^BP+^AP_{BORG}$

旋转坐标系的映射

我们有之前所得的旋转矩阵的转置和逆的结论：

$^A_BR=^B_AR^{-1}=^B_AR^T$

一个旋转矩阵即为三个一组的列向量或者三个一组的行向量：

$^A_BR=\begin{pmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{pmatrix}=\left[\begin{matrix}^B\hat{X}^T_A\\^B\hat{Y}^T_A\\^B\hat{Z}^T_A\end{matrix}\right]$
因此，可知 $^AP$ 的分量计算如下：

$^Ap_x=^B\hat{X}_A\cdot^BP$
$^Ap_y=^B\hat{Y}_A\cdot^BP$
$^Ap_z=^B\hat{Z}_A\cdot^BP$

写成简化形式：

$^AP=^A_BR^BP$

问题：如何表示坐标系{B}相对于坐标系{A}绕Z轴旋转30度？Z轴指向纸面向外。

先作图：
在这里插入图片描述

1.在A中写出B的单位矢量，排成旋转矩阵。

得：
在这里插入图片描述

稍微解释一下，我们知道第一列表示的是 $^A\hat{X}_B$ ，取单位矢量 $e_b$ 表示B的X轴在A中的位置，可由30°角得(0.866,0.5,0)，其中0表示相对于Z轴无移动。以此类推。

2.已知：
在这里插入图片描述

3.求出：
在这里插入图片描述

一般坐标系的映射

一般坐标系的映射可以概括为两步。

首先将 $^BP$ 变换到一个中间坐标系，这个坐标系与{A}的姿态相同、原点与{B}的原点重合。这可以用左乘矩阵 $^A_BR$ 得到。
然后用简单的矢量加法平移原点：

$^AP=^A_BR^BP+^AP_{BORG}$

更进一步地，我们可以将这组操作变换为使用一个矩阵算子表示从一个坐标系到另一个坐标系的映射。这个矩阵算子是 $4\times4$ 的矩阵，将最后一行作为[0 0 0 1]，在4x1矩阵中添加最后一个分量为1。亦即：
在这里插入图片描述

将这个4x4的矩阵称为齐次变换矩阵。由于它可以表示坐标系间的变换，所以这是一个非常重要的概念！同时可以说，将来的运算将紧紧围绕齐次变换展开，请务必巩固相关知识。

算子：平移，旋转和变换

算子：用于坐标系间点的映射的通用数学表达式。
本章主要介绍平移算子，旋转算子和平移加旋转算子。

平移算子

实际上我们之前已经介绍了平移的矢量计算式。本节介绍将这种运算浓缩为一个算子，对算子的表示进行进一步探讨。

我们知道平移计算方法：

$^AP_2=^AP_1+^AQ$

用算子写出平移变换：

$^AP_2=D_Q(q)^AP_1$

算子 $D_Q(q)$ 可被看做一个特殊的齐次变换阵：
在这里插入图片描述

由齐次变换矩阵分块可知，旋转矩阵是一个单位阵，原点阵的分量是平移矢量Q的分量。其中， $q=\sqrt{q_x^2+q^2_y+q^2_z}$ 。

现在我们初步认识了符号 $D_Q(q)$ ，这个符号今后将用来表示坐标系和映射。

旋转算子

可以用旋转变换算子来定义旋转矩阵。

旋转矩阵不涉及两个具体的坐标系。所以，写的时候不需要写下标或上标。引入旋转矩阵 $R$ ，这个已经比较熟悉的公式：

$^AP_2=R^AP_1$

用另一个符号定义旋转算子，以明确是绕哪个轴旋转：

$^AP_2=R_K(\theta)^AP_1$

符号 $R_K(\theta)$ 是一个旋转算子，表示绕K轴旋转θ角度。这个算子可写成齐次变换矩阵。

代入到式子：

$^A_BR=\begin{pmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{pmatrix}=\left[\begin{matrix}^B\hat{X}^T_A\\^B\hat{Y}^T_A\\^B\hat{Z}^T_A\end{matrix}\right]$

可得：

$R_K(\theta)=\left[\begin{matrix}cos\theta&-sin\theta&0&0\\sin\theta&cos\theta&0&0\\0&0&1&0\\0&0&0&1\end{matrix}\right]$

再回到刚刚那个例子，也就是绕Z轴旋转30度。此时，旋转算子可写成：

在这里插入图片描述

变换算子

即平移加旋转的算子。一个变换通常被认为是由一个广义旋转矩阵和位置矢量分量组成的齐次变换的形式。

如下例：

$^AP_2=T^AP_1$

小结

这几节的核心就在于这个齐次变换矩阵。它有3个定义：

1.它是坐标系的描述。

2.它是变换映射。

3.它是变换算子。

变换算法

本节介绍变换的乘法和变换的逆运算。

混合变换

我们有这样一个目标：即已知 $^CP$ ，求 $^AP$ 。

已知坐标系{C}相对于坐标系{B}，并且已知坐标系{B}相对于坐标系{A}。所以，我们可将 $^CP$ 变换成 $^BP$ ：

$^BP=^B_CT^CP$

然后将 $^BP$ 变换成 $^AP$ ：

$^AP=^A_BT^BP$

联立上两式可以得到确定解。换言之，我们实际运算中，需要寻找ab和bc的这两个变换式，实现变换的传递。

由此定义：

$^A_CT=^A_BT^B_CT$

逆变换

由公式

$^A_BT=^B_AT^{-1}$

得若需要得到A相对于B的描述，需要求这个矩阵的逆。

利用变换的性质求逆：

为了求 $^B_AT$ ，必须由 $^A_BR$ 和 $^AP_{BORG}$ 求出 $^A_BT$ 和 $^BP_{AORG}$ 。首先，回顾关于旋转矩阵的讨论：

$^B_AR=^A_BR^T$

其次利用

$^AP=^A_BR^BP+^AP_{BORG}$

将 $^AP_{BORG}$ 转变成在{B}中的描述：

$^B(^AP_{BORG})=^B_AR^AP_{BORG}+^BP_{AORG}$

左边应为0，因为在A原点处离B原点的距离变换为在B原点处离B原点的距离就是0。所以变换后可得：

$^BP_{AORG}=-^B_AR^AP_{BORG}=-^A_BR{^T}^AP_{BORG}$

从而写出：

在这里插入图片描述

变换方程

回顾之前所学我们发现简单变换方程的构造依赖于三个坐标系所确定的两个变换式，那么多个坐标系也以此类推，使用的总是变换相乘的方法：

在这里插入图片描述
从而：

由这个变换方程我们可以解出：

在这里插入图片描述

从这个变换方程中我们可以得出C的两个可能的描述为：

在这里插入图片描述

和
在这里插入图片描述

例：假定已知上图中变换 $^B_TT$ 描述了操作臂指端的坐标系{T}，它是相对于操作臂基座的坐标系B的，又已知工作台相对于操作臂基座的空间位置（因为已知与工作台相连的坐标系{S}是 $^B_ST$ ），并且已知工作台上螺栓的坐标系相对于工作台坐标系的位置，即 $^S_GT$ 。计算螺栓相对操作手的位姿， $^T_GT$ 。
由公式推导得到相对于操作手坐标系的螺栓坐标系为：

在这里插入图片描述

姿态的其他描述方法

旋转矩阵又被称为标准正交矩阵。由正交矩阵的凯莱公式可得，对于任何正交阵 $R$ ，存在一个反对称矩阵 $S$ ，满足：

$R=(I_3-S)^{-1}(I_3+S)$

式中是一个 $3\times3$ 单位阵。一个3维反对称阵（即 $S=-S^T$ ）可由三个参数 $s_x,s_y,s_z)$ 表示为

$S=\left[\begin{matrix}0&-s_z&s_y\\s_z&0&-s_x\\-s_y&s_x&0\end{matrix}\right]$

因此，任何3x3旋转矩阵都可以用三个参量确定。

显然，旋转矩阵的九个分量线性相关。实际上，对于一个旋转矩阵R很容易写出六个线性无关的分量。如上所述，假定R为三列：

在这里插入图片描述

由单位轴知识可知，三个矢量是参考坐标系中某坐标系的单位轴。每个矢量都是单位矢量，且相互垂直，所以9个矩阵元素有6个约束：

在这里插入图片描述

这些约束看起来非常臃肿，是否能够找到一种表达方法，用三个参量即可简便地表达？几个姿态表达法能够帮助我们做到这一点。

平移运动直观，但旋转变换往往并不直观，主要困难是旋转一般不互逆。即, $^A_BR^B_CR$ 与 $^B_CR^A_BR$ 不同。频繁地输入九个元素的正交阵不是一件容易的工作，而我们引入这些姿态表达法则能简化运算。

X-Y-Z固定角

这种方法是，首先将坐标系{B}与一个已知参考坐标系{A}重合。先将{B}绕 $\hat{X}^A$ 旋转 $\gamma$ 角，再绕绕 $\hat{Y}^A$ 旋转 $\beta$ 角，最后绕 $\hat{Z}^A$ 旋转 $\alpha$ 角。

每个旋转都是绕着参考坐标系{A}的某个轴完成的。我们称这种方法为X-Y-Z固定角。有时，我们会称这些角为回转角、俯仰角和偏转角。但是使用中约定可能不同。

在这里插入图片描述

此时可直接推导等价旋转矩阵 $^A_BR_{XYZ}(\gamma,\beta,\alpha)$ ：

在这里插入图片描述
其中 $c\alpha$ 是 $cos\alpha$ 的简写， $s\alpha$ 是 $sin\alpha$ 的简写。
最重要的是搞清楚上式中的旋转顺序，将旋转看作算子，先X再Y后Z。

上式乘积后可得：

在这里插入图片描述

仅当旋转是按照上述顺序进行时，（2.64）才是正确的。这一点要万万记牢！

接着，不妨来看一组逆问题：

从一个旋转矩阵等价推出X-Y-Z固定角坐标系，这样的逆解取决于求解一组超越方程：如果方程相当于一个旋转矩阵，那么就有九个方程和三个未知量。在这九个方程中有六个方程是相关的，因此实际上只有三个方程和三个未知量。（超越方程：方程中有无法用自变数的多项式或开方表示的函数，常用近似解法去解，是目前计算数学中一个重要的问题）

令
在这里插入图片描述
由(2.64)式通过计算 $r_{11}$ 和 $r_{21}$ 的平方和的平方根，可求得 $cos\beta$ 。然后用 $r_{31}$ 除以 $cos\beta$ 再求其反正切可求得 $\beta$ 。那么，只要 $c\beta≠0$ ，就可以用 $r_{21}/c\beta$ 除以 $r_{11}/c\beta$ ，得到 $\alpha$ 角，用 $r_{32}/c\beta$ 除以 $r_{33}/c\beta$ ，得到 $\gamma$ 角，
即
在这里插入图片描述
式中 $A t a n 2 (y, x)$ 是一个双参变量的反正切函数。

虽然存在第二个解，但在上式中取 $\beta$ 的正根以得到单解，满足 $-90°≤\beta≤90°$ 。这样就可以在各种姿态表示法之间定义一一对应的映射函数。但是在某些情况下有必要求出所有的解，这在操作臂逆运动学那一章会讨论。如果 $\beta=±90°$ (即 $c\beta=0$ )，下式的解会退化。如此，只能求出 $\alpha$ 和 $\beta$ 的和或者差。这种情况下，一般取 $\alpha=0.0$ ，结果如下：

如 $\beta=90.0°$ ，解得：
在这里插入图片描述
如 $\beta=-90.0°$ ，解得：

Z-Y-X欧拉角

有另外一种坐标系的表示法：首先将坐标系{B}与一个已知参考坐标系{A}重合。先将{B}绕 $\hat{Z}^B$ 旋转 $\alpha$ 角，再绕 $\hat{Y}^B$ 旋转 $\beta$ 角，最后绕 $\hat{X}^B$ 旋转 $\gamma$ 角。
在这里插入图片描述

同样都是重合后绕三个轴旋转，但是顺序有所不同。具体可以看图，所以运算上也出现比较大的不同，必须加以区分。这种方法叫做Z-Y-X欧拉角。

绕 $\hat{Z}$ 轴旋转 $\alpha$ 角使 $\hat{X}$ 旋转到 $\hat{X}^{'}$ ， $\hat{Y}$ 旋转到 $\hat{Y}^{'}$ ，加撇号表明这是由欧拉角描述的旋转。

用中间坐标系 ${B^{'}\}$ 和 ${B^{''}\}$ 来表示 $^A_BR_{Z^{'}Y^{'}X^{'}}(\alpha,\beta,\gamma)$ 。如果，把这些旋转看成是坐标系的描述，就可以立即写出：

$^A_BR=^A_{B^{'}}R^{B^{'}}_{B^{''}}R^{B^{''}}_{B^{}}R$

上式右边的每个旋转描述都是按照Z-Y-X欧拉角的定义给出的，即B相对于A的最终姿态为：

在这里插入图片描述
结果不太直观，但是与X-Y-Z固定角所得的结果相同。

Z-Y-Z欧拉角

先将{B}绕 $\hat{Z}^B$ 旋转 $\alpha$ 角，再绕 $\hat{Y}^B$ 旋转 $\beta$ 角，最后绕 $\hat{Z}^B$ 旋转 $\gamma$ 角。

按照上一节的推导，可得到等效矩阵

在这里插入图片描述
从旋转矩阵得出Z-Y-Z欧拉角介绍如下：

已知

$^A_BR_{Z^{'}Y^{'}Z^{'}}(\alpha,\beta,\gamma)=\left[\begin{matrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{matrix}\right]$

如 $sin\beta≠0$ ，可得到

$\beta=Atan2(\sqrt{r^2_{31}+r^2_{32}},r_{33})$
$\alpha=Atan2(r_{23}/s\beta,r_{13}/s\beta)$
$\gamma=Atan2(r_{32}/s\beta,-r_{31}/s\beta)$

虽然存在第二个解（在式中取 $\beta$ 的正平方根），但我们总是求满足 $0.0≤\beta≤180.0°$ 的单解。如果 $\beta=0.0$ 或 $180.0 °$ ，上式的结果就会退化。在这种情况下，仅能求出 $\alpha$ 和 $\gamma$ 的和或差。这种情况下一般取 $\alpha=0.0$ ，结果如下：

在这里插入图片描述

其他角坐标系的表示法

总共有24种表示法，这三种是典型方法，都被称作角坐标系表示法。24种表示法中，12种为固定角表示法，另外12种为欧拉角坐标系法。二者有对偶性。

等效轴角坐标系表示法

这种方法是，首先将坐标系{B}和一个已知参考坐标系{A}重合，然后将{B}绕矢量 $^A\hat{K}$ 按右手定则旋转 $\theta$ 角。

在这里插入图片描述
矢量 $\hat{K}$ 有时被称为有限旋转的等效轴。{B}相对于{A}的一般姿态可用 $^A_BR(\hat{K},\theta)$ 或 $R_K(\theta)$ 来表示，称作等效轴角坐标系表示法。确定矢量 $^A\hat{K}$ 只需要两个参数，因为它的长度恒等于1。角度确定了第三个参数。经常用旋转量 $\theta$ 乘以单位方向矢量 $\hat{K}$ 形成一个简单的 $3\times1$ 的矢量来描述姿态，用 $K$ 表示（没有“帽号”），如上图所示。
当选择{A}的主轴中的一个轴作为旋转轴时，则等效旋转矩阵成为我们熟悉的平面旋转矩阵：

在这里插入图片描述

若旋转轴为一般轴，则等效旋转阵为：

在这里插入图片描述

对于任何旋转轴和任何角度，都能很容易地构造出等效旋转阵。

如此也产生了一个逆问题，也就是从一个给定的等效旋转阵求出 $\hat{K}$ 和 $\theta$ 。此处先给出一部分结果。

如果
在这里插入图片描述
然后

在这里插入图片描述
并且

由上式总可以计算出一个在0度到180度之间的 $\theta$ 值。对于任意一对轴-角 $(^A\hat{K},\theta)$ ，存在另一对轴-角，即 $(-^A\hat{K},-\theta)$ ，它们在空间中的姿态相同，可用同样的旋转矩阵描述。因此，在将旋转矩阵转化为等效轴角坐标系表示法时，我们需要对解进行选择。一个更加重要的问题是，对于小角度的旋转，此时的轴将变得不确定。如果转动量 $\theta$ 为0，那么旋转轴将完全无法确定，如果 $\theta=0$ 或 $180 ° 时$ ，上式将无解。

例：坐标系{B}最初与坐标系{A}重合。我们使坐标系{B}绕矢量 $\hat{K}=\left[\begin{matrix}0.707&0.70&70.0\end{matrix}\right]$ （ $^A\hat{K}$ 经过原点）旋转，转角 $\theta=30°$ ，求坐标系{B}的描述。

代入式(2.80)可得坐标系描述的旋转矩阵分量。因为原点没有改变，所以位置矢量是 $\left[\begin{matrix}0&0&0\end{matrix}\right]^T$ 。因此有
在这里插入图片描述
我们需要尽量讨论绕经过参考系原点的轴的旋转。若遇到的问题不属于这种情况时，我们可以定义另一个坐标系，该坐标系的原点在轴上，为此将这类问题简化为“经过原点的轴”的情况来解决，然后求解这个变换方程。

例：坐标系{B}最初与坐标系{A}重合。我们使坐标系{B}绕矢量 $\hat{K}=\left[\begin{matrix}0.707&0.70&70.0\end{matrix}\right]$ （此矢量经过点 $^AP=\left[\begin{matrix}1.0&2.0&3.0\end{matrix}\right]$ ）旋转，转角 $\theta=30°$ ，求坐标系{B}的描述。
在这里插入图片描述

在旋转之前，坐标系{A}和坐标系{B}是重合的。如上图所示，定义了两个新的坐标系{ $A^{'}$ }和{ $B^{'}$ }具有相同的方向，但它们的原点在旋转轴上，并且相对于坐标系{A}的原点有一定的偏移量。我们选择

在这里插入图片描述

同样，坐标系{B}相对于坐标系{ $B^{'}$ }的描述是

在这里插入图片描述
现在，保持其他关系不变，使坐标系{ $B^{'}$ }绕坐标系{ $A^{'}$ }旋转。因为旋转轴经过原点，所以用式(2.80)来计算坐标系{ $B^{'}$ }相对于{ $A^{'}$ }的旋转量。代入式（2.80）得到描述的旋转矩阵分量。又因为原点没有平移，所以位置矢量是 $0,0,0)^T$ 。因此，我们得到
在这里插入图片描述
我们可以用一个变换方程来计算要求的坐标系

$^A_BT=^A_{A^{'}}T^{A^{'}}_{B^{'}}T^{B^{'}}_{B}T$

从而得到

在这里插入图片描述
绕一个不经过原点的轴旋转会引起位置变化，另外加上姿态变化。该姿态变化跟旋转线轴通过原点引起的姿态变化一样。注意，我们可以使用任意定义的坐标系{ $A^{'}$ }和{ $B^{'}$ }，只要它们的原点选在旋转轴线上，旋转轴线方向的选择是任意的，而坐标系原点可以选择旋转轴线的任意位置。

欧拉参数

一种通过四个数值来表示的姿态表示法叫做欧拉参数。

定义：根据等效旋转轴 $\hat{K}=\left[\begin{matrix}k_x& k_y&k_z\end{matrix}\right]^T$ 和等效旋转角 $\theta$ 得到欧拉参数如下：

$\epsilon_1=k_xsin\theta/2$
$\epsilon_2=k_ysin\theta/2$
$\epsilon_3=k_zsin\theta/2$
$\epsilon_4=cos\theta/2$

显然这四个参数不独立，并有关系式：

$\epsilon_1^2+\epsilon_2^2+\epsilon_3^2+\epsilon_4^2=1$

这个关系总是不变。

因此，一个姿态可描述为四维空间中单位超球面上的一点。

有时，可以将欧拉参数看做是一个 $3\times1$ 的矢量加上一个标量。但是，虽然欧拉参数是一个 $4\times1$ 的矢量，我们还是将欧拉参数看作是一个单位四元数。

用欧拉参数组表示的旋转矩阵 $R_{\epsilon}$ 是

在这里插入图片描述
已知一个旋转矩阵得到对应的欧拉参数是

在这里插入图片描述
注意，从计算的意义上来讲，如果旋转矩阵表示的是绕某一轴 $180 °$ 的旋转，上式将失去意义，因为 $\epsilon_4=0$ 。但可以看出，当取极限时，式（2.92）中的所有表达式都趋向于无穷大，对于上述情况也是如此。实际上从上式的定义中可以看出，对于所有的 $\epsilon_i$ ，其值在[-1,1]间。