任务2 工业机器人正向运动学及实例
一、任务导入
在研究机器人时我们要对机器人的各个关节进行分析计算，不仅仅要研究关节自己的位置，也要研究关节之间的位姿关系。
机器人运动学包括正向运动学和逆向运动学，正向运动学即给定机器人各关节变量，计算机器人末端的位置姿态；逆向运动学即已知机器人末端的位置姿态，计算机器人对应位置的全部关节变量。
在本任务中，我们将先学习工业机器人的正向运动学。
正向运动
二、工业机器人正向运动学方程及实例
工业机器人正向运动学主要解决正向运动学方程的建立及手部位姿的求解问题。
当为机器人的每个关节建立了连杆坐标系之后，就可以获得相邻两个连杆坐标系之间的变换矩阵Ai（i=1，2，…，n。n为机器人的自由度数），则有下列矩阵
Tn=A1·A2··An（3-1）
矩阵Tn就是工业机器人手部坐标系相对于固定坐标系的位姿，我们称式（3-1）为机器人正向运动学方程，Tn是如下的（4×4）矩阵：
（3-2）
式中，前三列表示手部的姿态，第四列表示手部的位置。
事实上，很多工业机器人在设计时，常常使某些连杆参数取特殊值，使得连杆坐标系比较容易建立，同时，相邻两个连杆坐标系之间的变换矩阵Ai也比较容易获得，可以不必非得按照D—H法来做。下面通过两个实例来介绍建立工业机器人运动学方程的方法。
1.平面关节型工业机器人的运动学方程
图3-12（a）所示为具有一个肩关节、一个肘关节和一个腕关节的3自由度SCARA型工
图3-12 SCARA型工业机器人的坐标系
业机器人。考虑到关节轴线相互平行，并且连杆都在一个平面内的特点，将固定坐标系{0}和连杆1、连杆2、连杆3的坐标系{1}、{2}、{3}分别建立在关节1、关节2、关节3和手部的中心，如图3-12（a）所示。坐标系{3}就是手部坐标系。连杆参数中
为变量，d、l、均为常量。建立了连杆坐标系之后，即可列出该工业机器人的连杆参数，如表3-2所示。
表3-2 3自由度SCARA型工业机器人的连杆参数
该SCARA型工业机器人的运动学方程为：
T3＝A1A2A3
式中，Ai（i=1，2，3）表示坐标系{i}相对于坐标系{i-1}的齐次变换矩阵。
由表3-2中每一行的参数可得出齐次变换矩阵A1、A2和A3。因为该SCARA型工业机器人的各连杆之间的关系比较简单，可以参考图3-12（b）直接写出矩阵A1、A2和A3。我们以A1为例说明其计算方法。{1}系的运动过程是：先沿X0移动l1，再绕Z0转动1，因为转动是相对固定坐标系进行的，所以，Rot（z，1）应该左乘Trans（l1，0，0）。因此，A1、A2和A3分别为：
A1＝Rot（z，1）·Trans（l1，0，0）
A2＝Rot（z，2）·Trans（l2，0，0）
A3＝Rot（z，3）·Trans（l3，0，0）
即：
因此，可以写出
（3-3）
式中：c123=cos（1+2+3）；s123=sin（1+2+3）；c12=cos（1+2）；s12=sin（1+2）；c1=cos1；s1=sin1。（在以后的叙述中，cos可用c表示，sin可用s表示）
T3表示手部坐标系{3}（即手部）在固定坐标系中的位置和姿态。
式（3-3）则为图3-12（a）所示平面关节型工业机器人的正向运动学方程。
当l1、l2、l3和转角变量1、2、3给定时，可以算出T3的具体数值。如图3-12（b）所示，设l1=l2=100，l3=20；1=30°，2=-60°，3=-30°，则可以根据式（3-3）求出手部的位姿矩阵表达式为：
2.斯坦福工业机器人的运动学方程
图3-13和图3-14为斯坦福工业机器人简图及研究人员赋给各连杆的坐标系。表3-3是研究人员根据设定的坐标系得出的斯坦福工业机器人各连杆的参数。按照表3-3中每一行的参数可得出齐次变换矩阵A1～A6。
图3-13 斯坦福工业机器人及连杆坐标系图
3-14 斯坦福工业机器人机构简图
表3-3 斯坦福工业机器人连杆参数
{1}系与{0}系是旋转关节连接，如图3-15（a）所示。{1}系相对于{0}系的变换过程是：{1}系绕{0}系的X0轴作1=-90°的旋转，然后{1}系绕{0}系的Z0作变量1的旋转，所以：
（3-4）
图3-15 斯坦福机器人手臂坐标系之间的关系
{2}系与{1}系是旋转关节连接，连杆距离为d2，如图3-15（b）所示。{2}系相对于{1}系的变换过程是：{2}系绕{1}系的X1轴作2＝90°的旋转，然后{2}系沿着{1}系的Z1轴正向作d2距离的平移，再绕{1}系的Z1轴作变量2的旋转，所以：
（3-5）
{3}系与{2}系是移动关节连接，如图3-15（c）所示。坐标系{3}沿着坐标系{2}的Z2轴正向作变量d3的平移。所以：
（3-6）
图3-16 斯坦福工业机器人手腕关节
图3-16是斯坦福工业机器人手腕三个关节的示意图，它们都是转动关节，关节变量为4、5及6，并且三个关节的中心重合。下面根据图3-17所示手腕坐标系之间的关系写出齐次变换矩阵A4～A6。
如图3-17（a）所示，{4}系相对于{3}系的变换过程是：{4}系绕{3}系的X3轴作4=-90°的旋转，然后绕{3}系的Z3轴作变量4的旋转，所以：
（3-7）
如图3-17（b）所示，{5}系相对于{4}系的变换过程是：{5}系绕{4}系的X4轴作5＝90°的旋转，然后绕{4}系的Z4轴作变量5的旋转，所以：
（3-8）
图3-17 斯坦福工业机器人手腕坐标系之间的关系
如图3-17（c）所示，{6}系沿着{5}系的Z5轴作距离H的平移，并绕{5}系的Z5轴作变量6的旋转，所以：
（3-9）
这样，所有杆的A矩阵已建立。如果要知道非相邻连杆间的关系，只要用相应的A矩阵连乘即可。如：
3T6＝A4A5A6
2T6＝A3A4A5A6
1T6＝A2A3A4A5A6
斯坦福工业机器人运动学方程为：
0T6＝A1A2A3A4A5A6（3-10）
方程（3-10）右边的结果就是最后一个坐标系—手部坐标系{6}相对于固定坐标系{0}的位置和姿态矩阵，各元素均为i和di（i=1，2，…，6）的函数。当i和di给出后，可以计算出斯坦福工业机器人手部坐标系{6}的位置p和姿态n、o、a。这就是斯坦福工业机器人手部位姿的解，这个求解过程叫做斯坦福工业机器人运动学正解。
例3-4 斯坦福工业机器人连杆参数如表3-3所示。现已知关节变量为：1＝90°，2＝90°，d3＝300mm，4＝90°，5＝90°，6＝90°，并且，已知工业机器人结构参数d2＝100mm，H＝50mm。根据斯坦福工业机器人运动学方程式进行正向运动学求解，写出手部位置及姿态（即{6}系相对{0}系的齐次变换矩阵）。
解：设图3-18是斯坦福工业机器人的零位（起始位置），按本例给出的关节变量进行图解，工业机器人手部及各连杆状态如图3-18所示。
图3-18 斯坦福机器人手部及各杆件状态
利用式（3-4）～式（3-9）可求得矩阵A1、A2、A3、A4、A5及A6。所以，坐标系{6}的位姿矩阵可根据运动学方程式求出：
（3-11）
式中：
（3-12）
式中：ci=cosi，si=sini（i=1，2，…，6）
将本例给出的已知数据（1＝90°，2＝90°，d3＝300mm，4＝90°，5＝90°，6＝90°以及d2＝100mm，H＝50mm）代入式（3-12）前面九个公式，可得姿态矢量n、o、a的分量分别为（注意cos90°＝0）：
将本例给出的已知数据代入式（3-12）后面三个公式，可得位置矢量的分量px、py、pz分别为（注意cos90°＝0）：
根据以上计算结果，可以写出手部位姿矩阵的数值解（即{6}系相对{0}系的齐次变换矩阵）为：
该（4×4）矩阵即为斯坦福工业机器人在题目给定情况下手部的位姿矩阵，即运动学正解。
假如H＝0，姿态矢量n、o、a不变，只有位置矢量p改变，则有：
图3-19 斯坦福机器人手部坐标系的位姿
根据图3-18可以画出{6}系和{0}系的关系如图3-19所示，根据图3-19很容易写出：
这说明前面的计算正确。
根据上面的计算结果可知，连杆参数中的线性尺寸只引起位置矢量p的改变，对姿态矢量n、o、a没有影响。这是因为平移不改变坐标系的姿态的缘故。