机器人学导论——操作臂运动学

一 说在前头

最近在学习《机器人学导论》（‘Introduction to ROBOTICS’-JOHN CRAIG）,文章内容为学习过程中的摘抄或者感悟，方便今后查阅。
假如我们想控制一组机械臂抓取物体，对于六轴机械臂来说，实际上我们只需要让末端执行器能够运动到指定位置并完成抓取控制即可。但有一个问题，与末端执行器连接的是一系列关节和连杆的组合，要如何控制每个关节的角度才能使末端执行器运动到指定位置呢？
这就需要用到操作臂运动学了，通过正逆运动学求解，我们可以简单地利用计算机控制机械臂。
如下图所示，为UR5机械臂的结构图，将来运动学建模分析时会用到。

二 连杆

2.1连杆和连杆连接的描述

Link：连杆——操作臂由刚性连杆组成

操作臂可以看成由一系列通过关节连接成运动链的刚体。
从操作臂的固定基座开始为连杆进行编号，称固定基座为连杆0，第一个可动连杆为连杆1，以此类推，操作臂最末端的连杆为连杆n。

为了确定末端执行器在三维空间的位置和姿态，操作臂至少需要6个关节（因为描述一个物体在空间的位姿需要6个参数——3个位置和3个姿态）。
要控制末端执行器运动到指定位置，就需要控制每个关节的角度，关节与关节之间由连杆连接，要构建运动学模型就需要知道连杆之间的关系。那么，要如何才能描述两个连杆之间的关系？
用空间中的直线来表示关节轴，只要确定两个关节轴（即两条直线）之间的距离和扭转角，就可以描述两个连杆之间的相对位置。

2.1.1连杆长度

关节轴i-1和关节轴i之间的公垂线长度a_i-1即为连杆长度。
两条不平行的直线之间有且只有一条公垂线，这样我们就可以描述两轴之间的距离关系了。

2.1.2连杆扭转角

如上图所示的α_i-1即为连杆扭转角。

假设作一个平面，并使该平面与两关节轴之间的公垂线垂直，然后把关节轴$i-1$和关节轴$i$投影到该平面上，在平面内轴$i-1$按照右手法则绕a_i-1，转向轴$i$，测量两轴线之间的夹角，用转角a_i-1,定义连杆$i-1$的扭转角。(上图中带有三条短划线的两条线为平行线)当两个关节轴线相交时，两轴线之间的夹角可以在两者所在的平面中测量，但是a_i-1没有意义。在这种特殊情况下，a_i-1的大小和符号可以任意选取。

笔者认为，连杆长度和连杆扭转角是用来确定两个关节（即两个轴）之间位置的两个参数，我们想象两个轴为两条直线，只要确定了公垂线长度（连杆长度）和偏移角度（连杆扭转角），就大致能够确定两条直线（两个轴）之间的相对位置关系了。
那么确定了两个关节之间的相对位置了，两个连杆间的连接关系如何确定呢？

2.1.3 连杆偏距

从公垂线a_i-1与关节轴$i$的交点到公垂线a_i与关节轴$i$的交点的有向距离即连杆偏距d_i。当关节$i$为移动关节时，连杆偏距d_i是一个变量。

2.1.4 关节角

平移公垂线a_i-1和a_i绕关节轴$i$旋转所形成的夹角，即关节角θ_i。图中，标有双斜线的直线为平行线。当关节$i$为转动关节时，关节角θ是一个变量。
连杆偏距是和关节角用来确定两个连杆之间连接关系的两个参数，我们想象两个连杆为两条线段（当然实际情况中不能直接把连杆当作是规则的线段，在这里我假设两相邻连杆可以等效为线段，并且与其对应的公垂线是重合的，想象一下这个特殊的例子，便于大家理解空间中连杆的关系），只要确定了两线段（即公垂线）的距离（连杆偏距）和两线段的偏移角度（即关节角），就大致能够确定两连杆之间的连接关系了。

2.1.5 连杆参数

综上，机械臂的每个连杆都可以用4个运动学参数来描述，其中两个参数用于描述连杆本身，另外两个用于描述连杆之间的连接关系。这种用连杆参数描述的机构运动关系的规则称为Denavit-Hartenberg方法，即D-H参数方法。我们常用的是带有转动关节的机械臂，那么这时θ_i就是关节变量，是我们可以控制的变量，其余三个参数不变，对于一个6关节的机械臂，我们只需要用3X6=18个参数，就可以描述运动学参数。

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

为了描述每个连杆与相邻连杆之间的相对位置关系，需要在每个连杆上定义一个固连坐标系。根据固连坐标系所在连杆的编号对固连坐标系命名，如固连在连杆$i$上的固连坐标系称为坐标系{$i$}。

下图相对于上面的图多了坐标系的描述：

这里引入的坐标系对后续分析运动学有很大的帮助。

坐标系{$i$}的$\hat{Z}$轴称为$\hat{Z}$_i，并与关节轴{$i$}重合，坐标系{$i$}的原点位于公垂线a_i与关节轴$i$的交点处。$\hat{X}$_i沿a_i方向由关节指向关节$i+1$。 当a_i=0时，$\hat{X}$_i垂直于$\hat{Z}$_i和$\hat{Z}$_i+1所在的平面。按右手定则绕$\hat{X}$_i轴的转角定义为α_i，$\hat{Y}$_i轴由右手定则确定,从而完成了对坐标系{$i$}的定义。

依照上述建立连杆坐标系的方法，我们可以定义连杆参数为：

连杆参数	基准轴	描述
ai	沿$\hat{X}$i轴	从$\hat{Z}$i移动到$\hat{Z}$i+1的距离
αi	绕$\hat{X}$i轴	从$\hat{Z}$i旋转到$\hat{Z}$i+1的角度
di	沿$\hat{Z}$i轴	从$\hat{X}$i-1移动到$\hat{X}$i的距离
θi	绕$\hat{Z}$i轴	从$\hat{X}$i-1旋转到$\hat{X}$i的角度

上表十分有用，需要时刻牢记，相比于上节抽象的文字描述，这里的描述则相对简洁易懂。

2.2.1 来分析一道例题

（例题为原书第3章的3.7节对两种工业机器人的运动学问题中的PUMA 560部分）

Unimation PUMA560是一个六自由度机器人，所有关节均为转动关节(即这是一个6R机构)。图3-18所示是所有关节角为零位时连杆坐标系的分布情况。图3- 19所示是机器人前臂的一些详细情况。


注意，当θ为0时，坐标系{0}(未在图中表示)与坐标系{1}重合。还要注意，这台机器人与许多工业机器人一样，关节4、5和6的轴线相交于同一点，并且交点与坐标系{4}、{5}、{6}的原点重合，而且关节轴4、5、6相互垂直。图3-20所示为机器人腕部机构的运动简图。

那么该如何求出这组机械臂的D-H参数呢？
对照下表，对每个连杆进行分析

连杆参数	基准轴	描述
ai	沿$\hat{X}$i轴	从$\hat{Z}$i移动到$\hat{Z}$i+1的距离
αi	绕$\hat{X}$i轴	从$\hat{Z}$i旋转到$\hat{Z}$i+1的角度
di	沿$\hat{Z}$i轴	从$\hat{X}$i-1移动到$\hat{X}$i的距离
θi	绕$\hat{Z}$i轴	从$\hat{X}$i-1旋转到$\hat{X}$i的角度

1. 当$i=1$时
   由于坐标系{0}和坐标系{1}重合，故
   沿$\hat{X}$₀轴，从$\hat{Z}$₀移动到$\hat{Z}$₁的距离为0；
   绕$\hat{X}$₀轴，从$\hat{Z}$₀旋转到$\hat{Z}$₁的角度为0；
   沿$\hat{Z}$₁轴，从$\hat{X}$₀移动到$\hat{X}$₁的距离为0；
   由于关节轴1为转动关节，所以θ₁为可变参数。

连杆参数	基准轴	描述	结果
a0	沿$\hat{X}$0轴	从$\hat{Z}$0移动到$\hat{Z}$1的距离	0
α0	绕$\hat{X}$0轴	从$\hat{Z}$0旋转到$\hat{Z}$1的角度	0
d1	沿$\hat{Z}$1轴	从$\hat{X}$0移动到$\hat{X}$1的距离	0
θ1	绕$\hat{Z}$1轴	从$\hat{X}$0旋转到$\hat{X}$1的角度	θ1

2. 当$i=2$时
   由于坐标系{1}原点和坐标系{2}原点重合，但坐标轴之间作了旋转变换，故
   沿$\hat{X}$₁轴，从$\hat{Z}$₁移动到$\hat{Z}$₂的距离为0；
   绕$\hat{X}$₁轴，从$\hat{Z}$₁旋转到$\hat{Z}$₂的角度为-90°；
   （这里的-90°怎么来的？在上一篇文章最后的4.3节提到了坐标轴旋转角度正负的判断方法：
   θ的符号由右手定则确定，即大拇指指向$\hat{X}$₁正方向，那么食指就是$\hat{Y}$₁，中指就是$\hat{Z}$₁，然后大拇指朝向屏幕或纸的里面，坐标系也跟着右手旋转，观察$\hat{Z}$₂相对于$\hat{Z}$₁是顺时针还是逆时针转动，如果是顺时针转动，那θ为正；否则为负。所以这边的旋转角度是-90°，即逆时针旋转90°。）
   沿$\hat{Z}$₂轴，从$\hat{X}$₁移动到$\hat{X}$₂的距离为0；
   由于关节轴2为转动关节，所以θ₂为可变参数。

连杆参数	基准轴	描述	结果
a1	沿$\hat{X}$1轴	从$\hat{Z}$1移动到$\hat{Z}$2的距离	0
α1	绕$\hat{X}$1轴	从$\hat{Z}$1旋转到$\hat{Z}$2的角度	-90°
d2	沿$\hat{Z}$2轴	从$\hat{X}$1移动到$\hat{X}$2的距离	0
θ2	绕$\hat{Z}$2轴	从$\hat{X}$1旋转到$\hat{X}$2的角度	θ2

3. 当$i=3$时
   如图3-18，
   沿$\hat{X}$₂轴，从$\hat{Z}$₂移动到$\hat{Z}$₃的距离为a₂；
   绕$\hat{X}$₂轴，从$\hat{Z}$₂旋转到$\hat{Z}$₃的角度为0；
   沿$\hat{Z}$₃轴，从$\hat{X}$₂移动到$\hat{X}$₃的距离为d₃；
   由于关节轴3为转动关节，所以θ₃为可变参数。

连杆参数	基准轴	描述	结果
a2	沿$\hat{X}$2轴	从$\hat{Z}$2移动到$\hat{Z}$3的距离	0
α2	绕$\hat{X}$2轴	从$\hat{Z}$2旋转到$\hat{Z}$3的角度	-90°
d3	沿$\hat{Z}$3轴	从$\hat{X}$2移动到$\hat{X}$3的距离	0
θ3	绕$\hat{Z}$3轴	从$\hat{X}$2旋转到$\hat{X}$3的角度	θ3

后面几个关节的D-H参数计算方式同上，就不一一列举了。最终的连杆参数表如下：

2.2.2 UR5机械臂的连杆参数

根据文章开头的UR5机械臂结构图和官方的文档数据我们也可以计算出其D-H参数如下：

三 操作臂运动学

我们已经能够分析相邻连杆之间的关系，下一步就是利用求得的连杆参数将独立的相邻连杆之间的变换联系起来，求出连杆n相对于连杆0的位置和姿态。

3.1 连杆变换的推导

在这一节，我们将导出相邻连杆间坐标系变换的一般形式，然后将这些独立的变换联系起来求出连杆n相对于连杆0的位置和姿态。
我们希望建立坐标系{i}相对于坐标系{i-1}的变换。一般这个变换是由四个连杆参数构成的函数。对任意给定的机器人，这个变换是只有一个变量的函数，另外三个参数是由机械系统确定的。通过对每个连杆逐一建立坐标系，我们把运动学问题分解成n个子问题。为了求解每个子问题，即^i-i_iT，我们将每个子问题再分解成四个次子问题。四个变换中的每一个变换都是仅有一个连杆参数的函数，通过观察能够很容易写出它的形式。
首先我们为每个连杆定义.三个中间坐标系一{P}，{Q}和{R}。下图所示是与前述一样的一对关节，图中定义了坐标系{P}、{Q}和{R}。注意，为了表示简洁起见，在每一个坐标系中仅给出了$\hat{X}$轴和$\hat{Z}$轴。

把在坐标系{i}中定义的矢量变换成在坐标系{i- 1}中的描述，这个变换矩阵可以写成：

即

其中

上式还可以写作

即

这里Screw_Q(r，φ)代表沿Q轴平移r、再绕Q轴旋转角度φ的组合变换。
则有

3.2 连杆变换的连乘

如果已经定义了连杆坐标系和相应的连杆参数，就能直接建立运动学方程。分别计算出各个连杆变换矩阵就能得出各个连杆参数的值。把这些连杆变换矩阵连乘就能得到一个坐标系{N}相对于坐标系{0}的变换矩阵:

变换矩阵⁰_NT是关于n个关节变量的函数。如果能得到机器人关节位置传感器的值，机器人末端连杆在笛卡儿坐标系里的位置和姿态就能通过⁰_NT计算出来。.

3.3 继续2.2.1节例题的分析

根据

我们可以求出每个连杆的变换矩阵：

再由连杆变换的连乘，我们可以得到⁰₆T。
有：

式中c₅是cosθ₅的缩写，s₆ 是sinθ₆的缩写等等。

另外有：

这里用了二角和公式：

则：

式中

最后得到：

式中