图2.32 斯坦福机械手臂的坐标系
关于斯坦福机械手臂逆运动解的推导，请见参考文献[5,13]。以下是斯坦福手臂逆运动学解的结果汇总：
?1?arctan()?arctanpxpyd22?r2?d2 其中 r?22 （2.78） px?pyC1px?S1py （2.79） ?2?arctanpzd3?S2?C1px?S1py??C2pz （2.80）
?4?arctan(?5?arctan?S1ax?C1ayC2(C1ax?S1ay)?S2az)和?4??4?180? 如果 ?5?0 （2.81）
C4[C2(C1ax?S1ay)?S2az]?S4[?S1ax?C1ay]S2(C1ax?S1ay)?C2az （2.82）
?6?arctanS6 其中 C6S6??C5?C4C2?C1ox?S1oy??S2oz?S4?S1ox?C1oy}
?S5?S2?C1ox?S1oy??C2oz? （2.83）
????C6??S4[C2?C1ox?S1oy??S2oz]?C4?S1ox?C1oy
表2.3
图2.32所示斯坦福机械手臂的参数表
??# ? d a ? 1 2 3 4 5 6 ?1 0 ?2 0 d2 0 0 0 0 0 0 -90 90 0 -90 90 0 d3 0 0 0 ?4 ?5 ?6 2.13 设计项目：四自由度机器人
利用本书中所介绍的四自由度机器人，结合本章所学的知识进行四自由度机器人的正逆运动学分析。
SCARA型机器人的运动学模型的建立，包括机器人运动学方程的表示，以及运动学正解、逆解等，这些是研究机器人控制的重要基础，也是开放式机器人系统轨迹规划的重要基础。为了描述SCARA型机器人各连杆之间的数学关系，在此采用Denavit和Hertenberg提出的齐次变换矩阵的方法，即D-H法。SCARA型机器人操作臂可以看作是一个开式运动链。它是由一系列连杆通过转动或移动关节串联而成的。为了研究操作臂各连杆之间的位移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系。 2.13.1 SCARA机器人坐标系的建立
1.SCARA机器人坐标系建立原则根据D-H坐标系建立方法，SCARA机器人的每个关节坐标系的建立可参照以下的三原则
(1)zn轴沿着第n个关节的运动轴;基坐标系的选择为:当第一关节变量为零时，零坐标系与一坐标系重合。
(2)xn轴垂直于zn轴并指向离开zn轴的方向。 (3)yn轴的方向按右手定则确定。
2.构件参数的确定根据D-H构件坐标系表示法，构件本身的结构参数an?1、
?n?1和相对位置参数dn、?n可由以下的方法确定:
(1)?n为绕zn轴(按右手定则)由xn?1轴到xn轴的关节角。 (2)dn为沿zn轴，将xn?1轴平移至xn轴的距离。 (3)an?1为沿xn?1轴从zn?1量至zn轴的距离。
(4)?n?1为绕xn?1轴(按右手定则)由zn?1轴到zn轴的偏转角。
3.变换矩阵的建立全部的连杆规定坐标系之后，就可以按照下列的顺序来建立相邻两连杆n-1和n之间的相对关系:
(1)绕xn?1轴转?n?1角。 (2)沿xn?1轴移动an?1。 (3)绕zn轴转?n角。 (4)沿zn轴移动dn。
这种关系可由表示连杆n对连杆n-1相对位置齐次变换n?1Tn来表征。即
n?1Tn?Tr(xn?1,?n?1)Tt(xn?1,an?1)Tr(zn,?n)Tt(zn,dn)
cos?n?sin?n0an?1???sin?cos??cos?cos??sin??dsin?nn?1nn?1n?1nn?1n?1? Tn???sin?nsin?n?1cos?nsin?ncos?n?1dncos?n?1???0001??（2.84）
展开上式得
由于n?1Tn描述第n个连杆相对于第n-1连杆的位姿，对于SCARA教学机器人(四个自由度)，机器人的末端装置即为连杆4的坐标系，它与基座的关系为:
0T4?0T11T22T33T4
图2.33SCARA机器人的D-H连杆坐标系的建立 Figure2.33 Construct of SCARA D-H link coordinate systems
如图2.33坐标系，可写出连杆n相对于n-1变换矩阵n?1Tn:
?c1?s1?sc10T1??1?00??0000100??c2??s0? 1T2??20??0??1??0?s2c2000010?1l?1?00? 2?T3???00???1??00100?c40l2??s00?3? T4??4?01?d3???01??0?s4c40000100?0?? 0??1?（2.85）
其中：cn?cos?n,sn?sin?n以下相同。
相应的连杆初始位置及参数列于表2.4，表中?n、dn为关节变量。
表2.4 SCARA机器人的杆件参数 Table2.4 Component Link Parameters of SCARE
构件 1 2 3 4 an?1 ?n?1 dn ?n ?1 cos?n?1 sin?n?1 0 l1 l2 0 0 0 0 0 0 d3 0 1 1 1 1 0 0 0 0 ?2 0 0 ?4 2.13.2 SCARA机器人的正运动学分析 各连杆变换矩阵相乘，可得到SCARA机器人末端执行器的位姿方程(正运动学方程)为
?nxoxaxpx??noap?yyy?00123T4?T1??1?T2??2?T3?d3?T4??4???y?nzozazpz???0001???c1c2c4?s1s2s4?c1s2s4?s1c2s4?c1c2s4?s1s2s4?c1s2c4?s1c2c4?scc?csc?sss?ccs?scs?css?ssc?ccc124124124124??124124124124?00?00?0c1c2l2?s1s2l2?c1l1?0s1c2l2?c1s2l2?s1l1???1?d3?00? (2.86)
式(2.86)表示了SCARA手臂变换矩阵0T4，它描述了末端连杆坐标系{4}相对基坐标系{0}的位姿
2.13.3 SCARA机器人的逆运动学分析
1.求关节变量?1为了分离变量，对方程的两边同时左乘0T1?1??1?, 得:
0T1?1??1?0T4?1T2??2?2T3?d3?3T4??4?
即：
?c1s1??sc?11?00??00
00100??nx?n0???y0??nz??1??0oxoyoz0axayaz0px??c2c4?s2s4?sc?cspy????2424pz??0??1??0?c2s4?s2c4?s2s4?c2c4000c2l2?l1?0s2l2??1?d3??01?左右矩阵中的第一行第四个元素(1.4)，第二行第四个元素(2.4)分别相等。即:
cos?1?px?sin?1?py?cos?2?l2?l1?sin?1?px?cos?1?py?sin?2?l2(2.87)
由以上两式联立可得:
?? ???1?A2?1?arctan??A?（2.88）
式中：A???????pypx
l12?l22?px2?py22l1?px?py22;??arctan
2求关节变量?2由式(2.87)可得:
?2?arctan??（2.89）
式中：r?rsin??1?????rcos????l??11???
px?py;??arctan22pypx
3求关节变量d3再令左右矩阵中的第三行第四个元素(3.4)相等，可得:
d3??pz (2.90)
4求关节变量?4再令左右矩阵中的第一行第一个元素、第二行第一个元素(1.1，2.1)分别相等，即：
cos?1?nx?sin?1ny?cos?2?cos?4?sin?2?sin?4?sin?1?nx?cos?1ny?sin?2?cos?4?cos?2?sin?4
由上两式可求得:
?4?arctan???sin?1?nx?cos?1ny?cos??n?sin?n1x1y???? ??2?（2.91）
至此，机器人的所有运动学逆解都已求出。在逆解的求解过程中只进行了一次矩阵逆乘，从而使计算过程大为简化，从?1的表达式中可以看出它有两个解，所以SCARA机器人应该存在两组解。运动学分析提供了机器人运动规划和轨迹控制的理论基础。
2.14 小结
本章讨论了用矩阵表示点、向量、坐标系、及变换的方法，并利用矩阵讨论了几种特定类型机器人的正逆运动方程以及欧拉角和RPY姿态角，这些特定类型机器人包括直角坐标、圆柱坐标和球坐标机器人。然而，本章的主旨是学习如何表示多自由度机器人在空间的运动，以及如何用Denavit-Hartenberg表示法导出机器人的正逆运动学方程。这种方法可用于表示任何一种机器人的构型，而不管关节的数量和类型，以及关节和连杆的偏移和扭转。
下一章将接着讨论机器人的微分运动，实际等效于机器人的速度分析。