【机器人学导论笔记】三、操作臂正运动学

3.1 概述

操作臂正运动学研究操作臂的运动特性，主要涉及与运动有关的几何参数和时间参数。本章中，只研究静止状态下操作臂连杆的位置和姿态。

处理这些复杂的几何参数需要一些步骤：首先需要在操作臂的每个连杆上分别固接一个连杆坐标系，然后再描述这些连杆坐标系之间的关系。并且，还需要研究当各个连杆通过关节连接起来后，连杆坐标系之间的相对关系。

本章重点是把操作臂关节变量作为自变量，描述操作臂末端执行器的位置和姿态与操作臂基座之间的函数关系。

3.2 连杆的描述

操作臂可以看成由一系列通过关节连接成运动链的刚体。我们将这些刚体称为连杆。通过关节将两个相邻的连杆连接起来。当两个刚体之间的相对运动是两个平面相互之间的相对滑动时，连接相邻两个刚体的运动副称为低副。

下图是6种常用的低副关节。

通常优先选择仅具有一个自由度的关节作为连杆的连接方式。（极少数选择n个自由度的，一般可看成由n个单自由度的关节把n-1个长度为0的连杆连接而成的）大部分操作臂中包括转动关节和移动关节。

从操作臂的固定基座开始为连杆进行编号，可以称固定基座为连杆0.第一个可动连杆为连杆1，以此类推。

为了确定末端执行器在三维空间中的位置和姿态，操作臂至少需要6个关节。典型操作臂具有5个或6个关节。

建立机构运动学方程时，为了确定操作臂两个相邻关节轴的位置关系，可把连杆看作一个刚体。其中，用空间中的直线来表示关节轴，用空间中的一条直线或一个向量表示关节i，连杆i绕关节轴i相对于连杆i-1转动。

由此可知，描述一个连杆的运动时，可用两个参数表示。它们定义了空间中两个关节轴的相对位置。

三维空间中，任意两轴间距离均确定，当两轴不平行时公垂线仅有1条。平行则有无数条。

第一个参数：连杆长度

关节轴之间公垂线长度称为连杆长度，如下图所示，$a_{i-1}$即为关节轴i和关节轴i-1的连杆长度。所以很容易知道，连杆长度和径向无关。

同时也有这样一种描述连杆长度的方法，即是以关节轴i-1为轴线作一个圆柱，并且把圆柱半径向外扩大，直到圆柱面和另一关节轴相交，此时圆柱半径即等于$a_{i-1}$。

这条公垂线有且只有一条（当然了，两个关节轴是异面直线），这样就有了唯一性，可以描述两连杆间的相对位置关系。

第二个参数：连杆扭转角

作一个平面，使该平面与两关节轴间的公垂线垂直，然后把关节轴i-1和关节轴i投影到该平面上，按右手法则从轴i-1绕$a_{i-1}$（由轴i-1指向轴i）转向轴i测量两轴线的夹角。当两个关节轴线相交时，两轴线的夹角可在两者所在的平面下测量，但$a_{i-1}$没有意义。此时，$a_{i-1}$的符号可以随意选取。

怎么想象呢？

可以想象这两根关节轴是两根双节棍。以其中一根双节棍为基座，另一根双节棍相对旋转。

有了这两个参数，我们就可以确定两连杆间的相对位置关系了。

3.3 连杆连接的描述

处于运动链中间位置的连杆

相邻的两个连杆之间有一个公共的关节轴。沿两个相邻连杆公共轴线方向的距离称为连杆偏距，在关节轴i上的连杆偏距记为$d_i$，用另一个参数描述两相邻连杆绕公共轴线旋转的夹角，称为关节角，记为${\theta }_i$。

理解这个$d_i$实际上就是理解提到的公共关节轴。

这个公共关节轴，以图3-4为例，实际上就是$a_{i-1}$和$a_i$夹着的这个轴i。

$a_i$是连杆i的长度，也就是轴i和轴i+1之间的公垂线的长度。它和$a_{i-1}$之间的距离就是它们在轴i上的交点之间的距离，这就是$d_i$。这也很好推出，因为两个都是垂足，直接取得距离就行。

然后，把$a_i$向下平移，垂足重合，作$a_{i-1}$的延长线，其和$a_i$的夹角即为${\theta }_i$。

图3-4表示相互连接的连杆i-1和连杆i。$a_i$表示连接连杆i两端关节轴的公垂线长度。从公垂线$a_{i-1}$与关节轴i的交点到公垂线$a_i$关节轴i交点的有向距离即为描述相邻两连杆连接关系的第一个参数，即连杆偏距$d_i$。连杆偏距$d_i$的表示方法如图3-4所示。当关节i为移动关节时，连杆偏距$d_i$是一个变量。平移公垂线$a_{i-1}$和$a_i$绕关节轴i旋转所形成的夹角即为描述相邻两连杆连接关系的第二个参数，即关节角${\theta }_i$，如图3-4。双斜线标为平行线。当关节i为转动关节时，关节角${\theta }_i$是一个变量。

运动链中首端连杆和末端连杆

连杆的长度$a_i$和连杆扭转角${\alpha }_i$取决于关节轴线i和i+1，因此在本节中按从$a_i$到$a_{n-1}$以及从${\alpha }_1$到${\alpha }_{n-1}$的规定讨论。

对于运动链中的两端的连杆，其参数习惯设定为0，即$a_0=a_n=0.0$，${\alpha }_0={\alpha }_n=0.0$。在本节中，按照上面的规定对关节2到关节n-1的连杆偏距$d_i$和关节角${\theta }_i$进行了定义。

如果关节1为转动关节，则${\theta }_1$的零位可以任意选取，并且设定$d_1=0.0$。同样，如果关节1为移动关节，则$d_1$的零位可以任意选取，并且设定${\theta }_1=0.0$。这种设定方法完全适用于关节n。

当一个参数可以任意选取时，把另一个参数设定为0，可以使以后的计算尽量简单。

连杆参数

因此，机器人的每个连杆都可以用4个运动学参数来描述，其中两个参数用于描述连杆本身，另两个参数用于描述连杆之间的连接关系。

通常，对于转动关节，${\theta }_i$变三不变，对于移动关节，$d_i$变三不变。

这种用连杆参数描述运动关系的规则称为$Denavit-Hartenberg$方法。

根据上述方法，可以确定任意机构的$Denavit-Hartenberg$参数，并用这些参数来描述该机构。

例如对于一个6关节机器人，需要用18个参数完全描述这些固定的运动学参数。如果是6个转动关节的机器人，这是18个固定参数可以分6组($a_i,{\alpha }_i,d_i$)表示。

需要注意的是，这个就叫做DH，找这方面的资料找DH就可以了。

3.4 连杆坐标系的定义

固连在连杆i上的固连坐标系称为坐标系{i}.

运动链中间位置连杆坐标系的定义

连杆固连坐标系的确定方法：

坐标系{i}的$\hat{Z}$轴称为${\hat{Z}}_i$，并与关节轴i重合，坐标系{i}的原点位于公垂线$a_i$与关节轴i的交点处。${\hat{X}}_i$沿$a_i$方向由关节i指向关节i+1。

当$a_i=0$时，${\hat{X}}_i$垂直于${\hat{Z}}_i$和${\hat{Z}}_{i+1}$所在的平面。按右手定则绕${\hat{Z}}_i$轴的转角定义为${\alpha }_i$，由于方向（时针）不确定，所以需要正负号。${\hat{Z}}_i$轴的位置也由右手定则确定。

这样，坐标系$i$的定义就完成了。

定义完单个坐标系后，我们需要将其应用到首端和末端的坐标系定义中，假设动系中仅存在这两个连杆，那么我们可以把一个连杆坐标系看作是参考坐标系。它通常是连杆1的坐标系。当关节变量1为0时，设定参考坐标系$0$与$1$重合。

在这个条件下总有：
$a_0=0.0$；
${\alpha }_0=0.0$

当关节1为转动关节时，

$d_1=0.0$；

当关节1为移动关节时，

${\theta }_0=0.0$。

为了简化坐标系的选取步骤，我们先设定${\hat{X}}_N$轴与${\hat{X}}_{N+1}$轴方向相同，由此有：

theta=0.0$

同时为了简化计算，取$d_n=0.0$的点为原点。

而对于移动关节，为了简化计算，选${\theta }_n=0.0$，当$d_n=0.0$时，原点选在${\hat{X}}_{n-1}$轴与关节$n$的交点位置。

连杆参数在连杆坐标系中的表示方法

连杆参数有如下4个定义：

$a_i=$沿${\hat{Z}}_i$轴，从${\hat{Z}}_i$移动到${\hat{Z}}_{i+1}$的距离。（通常大于0）

${\alpha }_i=$沿${\hat{Z}}_i$轴，从${\hat{Z}}_i$旋转到${\hat{Z}}_{i+1}$的角度。

$d_i=$沿${\hat{Z}}_i$轴，从${\hat{X}}_i$移动到${\hat{X}}_{i+1}$的距离。

${\theta }_i=$沿${\hat{Z}}_i$轴，从${\hat{X}}_i$旋转到${\hat{X}}_{i+1}$的角度。

所以，只需要确定两个坐标轴的变换距离和角度，就可以确定一个新的坐标系。

建立连杆坐标系的步骤

1. 找出各关节轴，标出延长线。
2. 找出相邻关节轴的公垂线/交点，指定该公垂线垂足/交点为原点。
3. 规定${\hat{Z}}_i$正方向。
4. 规定${\hat{X}}_i$正方向，若两个关节轴相交，则规定${\hat{X}}_i$轴垂直于关节轴$i$和$i+1$所在的平面。
5. 按照右手定则确定${\hat{Y}}_i$轴。
6. 当第一个关节变量为0时，规定坐标系{0}和{1}重合。对于坐标系{N}，其原点和${\hat{X}}_N$的方向可以任意选取，但是选取时，通常尽量使连杆参数为0。

一个三连杆机械臂能够说明我们的参数：

三个关节均为转动关节，因此我们称其为RRR（或3R）机构。

定义参考坐标系，即坐标系{0}，固定在基座上。当第一个关节变量值$({\theta }_1)$为0时，坐标系{1}与{0}重合。

这个机械臂所有的关节轴线都与机械臂所在的平面垂直。所有$\hat{Z}$轴相互平行，没有连杆偏距。所有的$d_i$都为0。所有关节都是旋转关节，因此当转角都为0时，所有$\hat{X}$轴一定在一条直线上。

连杆参数如图3.8所示。

所有${\alpha }_i$都为0，因为没有需要相对转动的${\hat{Z}}_i$轴。

没有L3的原因：原点在最后一个关节轴上，L3自然为0。

3.5 操作臂正运动学

这一节的任务是导出相邻连杆间坐标系变换的一般形式，并将这些独立的变换联系起来求出连杆$n$相对于连杆0的位置和姿态。

连杆变换的推导

通过任务我们可以得知，我们的切入点是求出坐标系之间的变换。我们仍然继续讨论坐标系{i}相对于坐标系{i-1}的变换。一般这个变换是由4个连杆参数得出的函数。这4个参数我们已经在上一节得出。

对于任意给定的机器人，这个变换是只有一个变量的函数，另外3个参数由机械系统确定。通过对每个连杆逐一建立坐标系，我们把运动学问题分解为$n$个子问题。为了解决每个子问题，即${}_i^{i-1}T$，再将每个子问题再分解成4个次子问题。

通过这样的拆解，我们可以发现，假如我们需要求解坐标系{n}相对于坐标系{1}的变换，我们就需要先去求2对1的变换，然后去求$a_2$、${\alpha }_2$、$d_2$、${\theta }_2$作为变量的函数，假如此时$a_1$是变量，那么其他就由机械系统决定好；接着，讨论完这4个次子问题，就去求3对2，4对3，…n-1对n，以此类推。

我们为每个连杆定义3个中间坐标系——{P}、{Q}、{R}。

上图是原书的图片，我将其简化后为如图所示的逻辑：

运算推导：

上面的PQR变换可以写成：

${}^{i-1}P{=}_R^{i-1}{T}_Q^R{T}_P^Q{T}_i^P{T}^{i}P$（1）

简化：

${}^{i-1}P{=}_i^{i-1}{T}^{i}P$（2）

易知：

${}_i^{i-1}T{=}_R^{i-1}{T}_Q^R{T}_P^Q{T}_i^{P}T$（3）

考虑每一个变换矩阵，改写（3）式：

${}_i^{i-1}T=R_X({\alpha }_{i-1})D_X(a_i)R_Z({\theta }_i)D_Z(d_i)$（4）

或者

${}_i^{i-1}T=Scre{w}_X(a_{i-1},{\alpha }_{i-1})Scre{w}_Z(d_i,{\theta }_i)$（5）

其中，$Scre{w}_Q(r,\varphi )$代表沿$\hat{Q}$轴平移$r$，再绕$\hat{Q}$轴旋转角度$\varphi$的组合变换。

矩阵连乘后得到：

连杆变换的连乘

在上面的研究我们发现，只要有了坐标系和4个参数，就能建立运动学方程去求解变换了，并且我们还可以做更多事，比如研究机械臂的运动学问题。

我们很容易能够得到坐标系N相对于坐标系0的变换矩阵，它是一个矩阵连乘后的矩阵结果。

得到了机器人关节位置传感器的值，我们就能通过上述结果求解末端连杆的位置和姿态（比如夹取的钳头）。

3.6 驱动器空间、关节空间和笛卡尔空间

对于一个具有n个自由度的操作臂来说，它的所有连杆位置可由一组n个关节变量确定。我们称之为$n\times 1$的关节向量。关节向量组成的空间称为关节空间。驱动器空间则是基于笛卡尔坐标系的机器人运动空间。同时，假设每个运动关节都由某种驱动器驱动，考虑到驱动器为止。我们将关节向量表示为一组驱动器变量方程，即驱动向量，其组成的空间也就是驱动器空间。

这样，我们可以选择用三种不同的描述去描述操作臂的位置和姿态：驱动器空间描述、笛卡尔空间描述、关节空间描述。

采用驱动器空间描述需要注意的是，必须确定驱动器位置和关节位置的对应关系。这样逻辑才算完整。

3.7 坐标系的标准命名

坐标系需要规范、标准的命名加以区别，才能够应对实际的繁杂运算工作。

基坐标系{B}

基坐标系{B}位于操作臂的基座上，它仅是赋予坐标系{0}的另一个名称，因为它固连在机器人的静止部位，所以有时称为连杆0。

它确定了固定坐标系和腕部坐标系。

固定坐标系{S}

固定坐标系{S}位置与任务相关，它位于工作台的一个角上，其是一个通用坐标系，所有运动都相对于它执行。所以，它还有这样的别称：任务坐标系、世界坐标系或通用坐标系。它通常根据基坐标系确定，即${}_S^{B}T$。

它确定了目标坐标系。

腕部坐标系{W}

腕部坐标系{W}附于操作臂的末端连杆。也可以称为坐标系{N}。大多数情况，它的原点位于操作臂的手腕上，随着操作臂的末端连杆移动。它通常根据基坐标系确定，即$W{=}_W^{B}T{=}_N^{0}T$。

它确定了工具坐标系。

工具坐标系{T}

工具坐标系{T}附于机器人所夹持工具的末端。当手部没有夹持工具时，工具坐标系的原点位于机器人的指尖之间（图中机器人抓持轴销的末端）。它根据腕部坐标系确定。

目标坐标系{G}

目标坐标系{G}是对机器人移动工具到达的位置描述。机器人运动结束时，工具坐标系应当与目标坐标系重合（图中位于将要插入轴销的轴孔）。它通常根据固定坐标系确定。

3.8 工具的位置

工具坐标系非常重要，因为工具的使用是任务成败的根本。我们需要计算工具坐标系相对于固定坐标系{S}的变换矩阵。

${}_T^{S}T{=}_S^B{T}^{-1}$${}_W^B{T}_T^{W}T$

上述方程有时被称为定位函数，是广义的运动学方程，这样就可以计算手臂的位置了。按上图所示，定位的结果是轴销相对于工作台顶角处的位置和姿态。

3.9 计算问题

1. 定点数表示法较常采用，变量变化范围较小，且容易确定。它所需的位数一般不超过24位。

2. 增加局部变量来减少乘和加的次数，避免计算机重复运行相同的语句。

3. 计算问题主要在于超越函数的计算（正弦、余弦）。现在常用查表的方式去计算，能够节省时间。

4. 矩阵运算需要选择简单的列计算。