机器人正运动学

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 连杆连接的描述

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

相邻的两个连杆之间有一个公共的关节轴。沿两个相邻连杆公共轴线方向的距离称为连杆偏距，在关节轴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$是一个变量。

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

连杆的长度$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，可以使以后的计算尽量简单。

3.3.3 连杆参数

因此，机器人的每个连杆都可以用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}$.

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

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

坐标系${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$的交点位置。

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

连杆参数有如下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}$的角度。

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

3.4.3 建立连杆坐标系的步骤

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的位置和姿态。

3.5.1 连杆变换的推导

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

对于任意给定的机器人，这个变换是只有一个变量的函数，另外3个参数由机械系统确定。通过对每个连杆逐一建立坐标系，我们把运动学问题分解为$n$个子问题。为了解决每个子问题，即$^{i-1}_{i}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=^{i-1}_RT^R_QT^Q_PT^P_iT^iP$（1）

简化：

$^{i-1}P=^{i-1}_iT^iP$（2）

易知：

$^{i-1}_iT=^{i-1}_RT^R_QT^Q_PT^P_iT$（3）

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

$^{i-1}_iT=R_X(\alpha_{i-1})D_X(a_i)R_Z(\theta_i)D_Z(d_i)$（4）

或者

$^{i-1}_iT=Screw_X(a_{i-1},\alpha_{i-1})Screw_Z(d_i,\theta_i)$（5）

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

矩阵连乘后得到：

3.5.2 连杆变换的连乘

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

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

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

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

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

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

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

3.7 坐标系的标准命名

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

3.7.1 基坐标系B

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

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

3.7.2 固定坐标系S

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

它确定了目标坐标系。

3.7.3 腕部坐标系W

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

它确定了工具坐标系。

3.7.4 工具坐标系T

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

3.7.5 目标坐标系G

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

3.8 工具的位置

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

$^S_TT=^B_ST^{-1}$ $^B_WT^W_TT$

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

3.9 计算问题

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

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

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

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