图2.28 简单六个自由度链式机器人的参考坐标系线图
从关节1开始，z0表示第一个关节，它是一个旋转关节。选择x0与参考坐标系的x轴平行，这样做仅仅是为了方便，x0是一个固定的坐标轴，表示机器人的基座，它是不动的。第一个关节的运动是围绕着z0-x0轴进行的，但这两个轴并不运动。接下来，在关节2处设定z1，因为坐标轴z0和z1是相交的，所以x1垂直于z0和z1。x2在z1和z2之间的公垂线方向上，x3在z2和z3之间的公垂线方向上，类似地，x4在z3和z4之间的公垂线方向上。最后，z5和z6是平行且共线的。
z5表示关节6的运动，而z6表示末端执行的运动。通常在运动方程中不包含末端执行器，但应包含末端执行器的坐标系，这是因为它可以容许进行从坐标系
z5?x5出发的变换。同时也要注意第一个和最后一个坐标系的原点的位置，它们将决定机器人的总编换方程。可以在第一个和最后的坐标系之间建立其他的（或不同的）中间坐标系，但只要第一个和最后的坐标系没有改变，机器人的总变换便是不变的。应注意的是，第一个关节的原点并不在关节的实际位置，但证明这样做是没有问题的，因为无论实际关节是高一点还是低一点，机器人的运动并不会有任何差异。因此，考虑原点位置时可不用考虑基座上关节的实际位置。 接下来，我们将根据已建立的坐标系来填写表2.2中的参数。参考前一节中任意两个坐标系之间的四个运动的顺序。从z0?x0开始，有一个旋转运动将x0转到了x1，为使得x0与x1轴重合，需要沿z1和沿x1的平移均为零，还需要一个旋转将z0转到z1，注意旋转是根据右手规则进行的，即将右手手指按旋转的方向弯曲，大拇指的方向则为旋转坐标轴的方向。到了这时，z0?x0就变换到了z1?x1。 接下来，绕z1旋转?2，将x1转到了x2，然后沿x2轴移动距离a2，使坐标系原点重合。由于前后两个z轴是平行的，所以没有必要绕x轴旋转。按照这样的步骤继续做下去，就能得到所需要的结果。
必须要认识到，与其他机械类似，机器人也不会保持原理图中所示的一种构型不变。尽管机器人的原理图是二维的，但必须要想象出机器人的运动，也就是说，机器人的不同连杆和关节在运动时，与之相连的坐标系也随之运动。如果这时原理图所示机器人构型的坐标轴处于特殊的位姿状态，当机器人移动时它们又会处于其他的点和姿态上。比如，x3总是沿着关节3与关节4之间连线a3的方向。当机器人的下臂绕关节2旋转而运动。在确定参数时，必须记住这一点。
表2.2
例2.19机器人的参数
# 1 2 3 4 5 6 ? ?1 d 0 0 0 0 0 0 a 0 ? 90 0 0 -90 90 0 ?2 a2 ?3 ?4 a3 a4 0 0 ?5 ?6 ?表示旋转关节的关节变量，d表示滑动关节的关节变量。因为这个机器人的关节全是旋转的，因此所有关节变量都是角度。
通过简单地从参数表中选取参数代入A矩阵，便可写出每两个相邻关节之间的变换。例如，在坐标系0和1之间的变换矩阵A1可通过将?（sin90?=1,cos90?=0, ?=90?）以及指定C1为?1等代入A矩阵得到，对其他关节的A2~A6矩阵也是这样，最后得：
?C1?SA1??1?0??0?C3?SA3??3?0??00S10?C110000??C2?S0?? A2??2?00???1??000?10?S2C200C4000C2a2?0S2a2?? 10??01??S30C3a3??C4?SC30S3a3?? A4??4?0010???001??00S5?S4C4a4?S4a4?? （2.55） 0??1?0??C6?S600??S?0?C50?C0066? A6??? ?0100?010????001?0001??特别注意：为简化最后的解，将用到下列三角函数关系式：
?C5?SA5??5?0??0S?1C?2?C?1S?2?S(?1??2)?S12C?1C?2?S?1S?2?C(?1??2)?C12在机器人的基座和手之间的总变换为：
（2.56）
RTH?A1A2A3A4A5A6
（2.57）
C1(C234a4??C1(C234C5C6?S234S6)C1(?C234C5C6?S234C6)C1(C234S5)???SSC??SSS?SCCa?Ca)1561561523322??S(CCC?SS)S(?CCC?SC)S(CS)S(Ca??123456234612345623461234512344????CSS??C1S5S6?C1C5C23a3?C2a2)156??SCC?SCC?CCSSSa?Sa?Sa234562345623462345234423322????0001??
例2.19 斯坦福机械手臂。在斯坦福机械手臂上指定坐标系（如图2.29所示），
并填写参数表。斯坦福机械手臂是一个球坐标手臂，即开始的两个关节是旋转的，第三个关节是滑动的，最后三个腕关节全是旋转关节。
图2.29 斯坦福机械手臂示意图
解：
在看本题解答之前，现根据自己的理解来做，问题的答案在本章的最后。建议在看解答中建立的坐标系和机械手臂的解之前，先试着自己做。 机器手臂最后的正运动学解是相邻关节之间的6个变换矩阵的乘积：
RTHSTANGORD?nx?n?0T6??y?nz??0oxoyoz0axayaz0px?py?? pz??1?其中
nx?C1?C2?C4C5C6?S4S6??S2S5C6??S1?S4C5C6?C4S6?nz??S2?C4C5C6?S4S6??C2S5C6ny?S1?C2?C4C5C6?S4S6??S2S5C6??C1?S4C5C6?C4S6? ox?C1??C2?C4C5C6?S4S6??S2S5C6??S1??S4C5C6?C4S6?oz?S2?C4C5C6?S4S6??C2S5C6ax?C1?C2C4S5?S2C6??S1S4S5ay?S1?C2C4S5?S2C6??C1S4S5 （2.58） az??S2C4S5?C2C5 oy?S1??C2?C4C5C6?S4S6??S2S5C6??C1??S4C5C6?C4S6?
px?C1S2d3?S1d2py?S1S2d3?C1d2 pz?C2d32.8 机器人的你运动学解
如前所述，这里真正重要的是你运动学解。为了使机器人手臂处于期望的位姿，如果有了逆运动学解就能确定每个关节的值。前面已对特定坐标系统的逆运动学解作了介绍。在这一部分，将研究求解逆运动方程的一般步骤。
现在你可能已经注意到，前面的运动方程中有许多角度的耦合，比如C234，这就使得无法从矩阵中提取足够的元素来求解单个的正弦和余弦项以计算角度。
?1
为使角度解耦，可例行地用单个RTH矩阵左乘An矩阵，使得方程右边不再包括
这个角度，于是可以找到产生角度的正弦值和余弦值的元素，并进而求得相应的角度。
这里概要地给出了这个方法，并将其用于例2.19中的简单机械手臂。虽然所给出的解决方法只针对这一给定构型的机器人，但也可以类似地用于其它机器人。正如在例2.19中看到的，表示机器人的最后方程为：
RTH?A1A2A3A4A5A6C1(C234a4??C1(C234C5C6?S234S6)C1(?C234C5C6?S234C6)C1(C234S5)???SSC??SSS?SCCa?Ca)1561523322?156? S(CCC?SS)S(?CCC?SC)S(CS)S(Ca??123456234612345623461234512344????CSS?C1S5S6?C1C5C23a3?C2a2)?156??SCC?CC?SCC?CCSSSa?Sa?Sa2345623462345623462345234423322????0001??为了书写方便，将上面的矩阵表示为[RHS](Right-Hand Side)。这里再次将机器人的期望位姿表示为：
?nx?nRTH??y?nz??0oxoyoz0axayaz0px?py?? （2.59） pz??1??1为了求解角度，从An开始，依次用A1?1左乘上述两个矩阵，得到：
?nx?n?1A1??y?nz??0oxoyoz0axayaz0px?py???A1?1[RHS]?A2A3A4A5A6 （2.60） pz??1??C1?0??S1??0S10?C1001000??nx?n0????y0??nz??1??0oxoyoz0axayaz0px?py???A2A3A4A5A6 pz??1??nxC1?nyS1oxC1?oyS1axC1?ayS1PxC1?PyS1???noapzZzz????nxS1?nyC1oxS1?oyC1axS1?ayC1PxS1?PyC1???0001?? （2.61）
?C234C5C6?S234S6?C234C5C6?S234C6C234S5C234a4?C23a3?C2a2??SCC?CS?SCC?CCSS?Sa?Sa?Sa2345623462345623462345234423322?????S5C6S5S6C50??0001??根据方程的3，4元素，有：
pypxS1?pyC1?0 ? ?1?arctan()和?1??1?180? （2.62）
px根据1，4元素和2，4元素，可得：
pxC1?pyS1?C234a4?C23a3?C2a2pz?S234a4?S23a3?S2a2 （2.63）
整理上面两个方程并对两边平方，然后将平方值相加，得：
(pxC1?pyS1?C234a4)2?(C23a3?C2a2)2(pz?S234a4)?(S23a3?S2a2)222
(pxC1?pyS1?C234a4)2?(pz?S234a4)2?a2?a3?2a2a3?S2S23?C2C23?
2根据式（2.56）的三角函数方程，可得：
S2S23?C2C23?cos???2??3???2??cos?3
于是：
C3?(pxC1?pyS1?C234a4)2?(pz?S234a4)2?a2?a32a2a322 （2.64）
在这个方程中，除S234和C234外，每个变量都是已知的，S234和C234将在后
面求出。已知：
S3??1?C3
2于是可得：
?3?arctanS3 （2.65） C3