?nx?nRTH??y?nz??0oxoyoz0axayaz0px?py?? （2.59） pz??1??1为了求解角度，从An开始，依次用A1?1左乘上述两个矩阵，得到：
?nx?nA1?1??yoxoyaxaypx?py???A1?1[RHS]?A2A3A4A5A6 （2.60） ??nzozazpz??0001????C1S100??nxoxaxpx??0010?????nyo?yaypy??S1?C100?ozazp??A2A3A4A5A6 z??0001????nz?0001????nxC1?nyS1oxC1?oyS1axC1?ayS1PxC1?PyS1??n?zoZazpz??nn??xS1?yC1oxS1?oyC1axS1?ayC1PxS1?PyC1??0001????C?S 234C5C6?S234S6?C234C5C6234C6C234S5C234a4?C23a3?C2a2??S234C5C6?C234S6?S234C5C6?C234C6S234S5S234a4?S23a3?S2a?2???SC?5C6S5S650??0001??根据方程的3，4元素，有：
pSpyx1?pyC1?0 ? ?1?arctapn)(和?1???1?180 x根据1，4元素和2，4元素，可得：
pxC1?pyS1?C234a4?C23a3?C2a2p z?S234a4?S23a3?S2a2整理上面两个方程并对两边平方，然后将平方值相加，得：
(p2xC1?pyS1?C234a4)?(C23a3?C2a2)2(p2z?S234a4)?(S23a3?S2a2)2
(p?p)2?(p222xC1yS1?C234a4z?S234a4)?a2?a3?2a2a3?S2S23?C2C23?
根据式（2.56）的三角函数方程，可得：
S2S23?C2C23?cos???2??3???2??cos?3
于是：
CpxC1?p2yS1?C234a4)?(pz?S234a4)2?a222?a33?(2a 2a32.61）
2.62）
2.63）
2.64）
（ （ （ （
在这个方程中，除S234和C234外，每个变量都是已知的，S234和C234将在后面求出。已知：
S3??1?C3
2于是可得：
?3?arctan
S3 （2.65） C3因为关节2，3和4都是平行的，左乘A2和A3的逆不会产生有用的结果。下一步左乘A1~A4的逆，
结果为：
?nx?n?1?1?1?1A4A3A2A1??y?nz??0乘开后可得：
oxoyoz0axayaz0px?py??1?1?1?1??A4A3A2A1[RHS]?A5A6 （2.66） ?pz?1?C234(C1ox?S1oy)?C234(C1ax?C234(C1px?S1py)?S234pz??C234(C1nx?S1ny)???S1ay)?S234axS234oz?C34a2?C4a3?a4?S234nz?C1ny?S1nxC1oy?S1oxC1ay?S1ax0?? ??S234(C1nx?S1ny)??S234(C1ox?S1oy)??S234(C1ax?S1ay)??S234(C1Px?S1Py)?C234pz???C234ozC234az?S34a2?S4a3?C234nz???0001???C5C6?SC??56?S6??0
?C5S6?S5S6C60S5?C5000?0?? （2.67） 0??1?根据式（2.67）矩阵的3，3元素，
-S234（C1ax?S1ay)?C234az?0 ?
?234?arctan(az)和?234??234?180? （2.68）
C1ax?S1ay由此可计算S234和C234，如前面所讨论过的，它们可用来计算?3。
现在再参照式（2.63），并在这里重复使用它就可计算角?2的正弦和余弦值。具体步骤如下：
?pxC1?pyS1?C234a4?C23a3?C2a2 ?pz?S234a4?S23a3?S2a2?由于C12?C1C2?S1S2以及S12?S1C2?C1S2，可得：
?pxC1?pyS1?C234a4??C2C3?S2S3?a3?C2a2 （2.69） ?pz?S234a4?(S2C3?C2S3)a3?S2a2?上面两个方程中包含两个未知数，求解C2和S2，可得：
(C3a3?a2)(pz?S234a4)?S3a3(pxC1?pyS1?C234a4)??S2?2(C3a3?a2)2?S32a3? （2.70） ?(C3a3?a2)(pxC1?pyS1?C234a4)?S3a3(pz?S234a4)?C2?222?(Ca?a)?S3323a3?尽管这个方程较复杂，但它的所有元素都是已知的，因此可以计算得到：
?2?arctan既然?2和?3已知，进而可得：
(C3a3?a2)(pz?S234a4)?S3a3(pxC1?pyS1?C234a4)(C3a3?a2)(pxC1?pyS1?C234a4)?S3a3(pz?S234a4) （2.71）
?4??234??2??3 （2.72）
因为式（2.68）中的?234有两个解，所以?4也有两个解。 根据式（2.67）中的1，3元素和2，3元素，可以得到：
（C1ax?S1ay)?S234az?S5?C234 （2.73） ?C??Ca?Sa51y1x?C3(4C1ax?S1ay)?S23a4z和 ?5?arcta2 （2.74） nS1ax?C1ay也许已注意到，因为对于?6没有解耦方程，所以必须用A5矩阵的逆左乘式（2.67）来对它解耦。这样做后可得到：
?C5C234?C1nx?S1ny??S234nz???S5?S1nx?C1ny???S234?C1nx?S1ny??C234nz?0??0???C5C234?C1ox?S1oy??S234oz?S5?S1ox?C1oy??S234?C1ox?S1oy??C234oz00??0010?0??0?? ?0?1???C6?S?6?0??0
?S6C60000100?0?? (2.75) 0??1?根据式（2.75）中的2，1元素和2，2元素，得到：
?6?arctan?S234(C1nx?S1ny)?C234nz?S234(C1ox?S1oy)?C234oz （2.76）
至此找到了6个方程，它们合在一起给出了机器人置于任何期望位姿时所需的关节值。虽然这种方法仅适用于给定的机器人，但也可采取类似的方法来处理其他的机器人。 值得注意的是，仅仅因为机器人的最后三个关节交于一个公共点才使得这个方法有可能求解，否则就不能用这个方法来求解，而只能直接求解矩阵或通过计算矩阵的逆来求解未知的量。大多数工业机器人都有相交的腕关节。 2.10 机器人的逆运动学编程
求解机器人逆运动问题所建立的方程可以直接用于驱动机器人到达一个位置。事实上，没有机器人真正用正运动方程求解这个问题，所用到的仅为计算关节值的6个方程，并反过来用它们驱动机器人到达期望位置。这样做是必须的，其实际原因是：计算机计算正运动方程的逆或将值代入正运动方
程，并用高斯消去法来求解未知量（关节变量）将花费大量时间。 为使机器人按预定的轨迹运动，譬如说直线，那么在一秒内必须多次反复计算关节变量。现假设机器人沿直线从起点A运动到终点B，如果期间不采取其他措施，那么机器人从A运动到B的轨迹难以预测。机器人将运动它的所有关节直到他们都到达终值，这是机器人便到达了终点B，然而，机器人手在两点间运行的路径是未知的，它取决于机器人每个关节的变化率。为了使机器人按直线运动，必须把这一路径分成如图2.30所示的许多小段，让机器人按照分好的小段路径在两点间依次运动。这就意味着对每一小段路径都必须计算新的逆运动学解。典型情况下，每秒钟要对位置反复计算50~200次。也就是说，如果计算逆解耗时5~20ms以上，那么机器人将丢失精度或不能按照指定路径运动。用来计算新解的时间越短，机器人的运动据越精确。因此，必须尽量减少不必要的计算，从而使计算机控制器能做更多的逆解计算。这也就是为什么设计者必须事先做好所有的数学处理，并仅需为计算机控制器编程来计算最终的解的原因。第5章将详细讨论这个问题。
图2.30 直线的小段运动
对于早先讨论过的旋转机器人情况，给定最终的期望位姿为：
RTHDESIRED?nx?n??y?nz??0oxoyoz0axayaz0px?py?? pz??1?为了计算未知角度，控制器所需要的所有计算是如下的一组逆解：
py?1?arctan()和?1??1?180?px?234?arctan(C3?az)和?234??234?180?C1ax?S1ay22(pxC1?pyS1?C234a4)2?(pz?S234a4)2?a2?a32a2a32S3??1?C3?3?arctanS3C3(C3a3?a2)(pz?S234a4)?S3a3(pxC1?pyS1?C234a4)(C3a3?a2)(pxC1?pyS1?C234a4)?S3a3(pz?S234a4)?2?arctan?4??234??2??3C234(C1ax?S1ay)?S234az?5?arctanS1ax?C1ay?6?arctan?S234(C1nx?S1ny)?C234nz?S234(C1ox?S1oy)?C234oz （2.77）
虽然以上计算也并不简单，但用这些方程来计算角度要比对矩阵求逆或使用高斯消去法计算要快得多。这里所有的运算都是简单的算术运算和三角运算。
2.11 机器人的退化和灵巧特性
退化 当机器人失去一个自由度，并因此不按所期望的状态运动时即称机器人发生了退化。在两种条件下会发生退化：（1）机器人关节达到其物理极限而不能进一步运动，（2）如果两个相似关节的z轴共线时，机器人可能会在其工作空间中变为退化状态。这意味此时无论哪个关节运动都将产生同样的运动，结果是控制器将不知道是哪个关节在运动。无论哪一种情况，机器人的自由度总数都小
于6，因此机器人的方程无解。在关节共线时，位置矩阵的行列式也为零。图2.31显示了一个处于垂直构型的简单机器人，其中关节1和6共线。可以看到，无论关节1或关节6旋转，末端执行器的旋转结果都是一样的。实际上，这时指令控制器采取紧急行动是十分重要的，否则机器人将停止运行。应注意，这种情况只在两关节相似时才会发生，反之，如果一个关节是滑动型的，而另一个是旋转型的（例如斯坦福机械手臂的关节3和关节4），那么即使它们的z轴共线，机器人也不会出现退化的现象。
Paul指出：如果sin(?4),sin(?5),或sin(?5)为0，机器人就将退化。显然，可以适当设计?4和?5来防止机器人退化。此外，如果任何时候?5接近0?或者180?，机器人就将变成退化状态。
灵巧 一般认为只要确定了机器人手的位姿，就能为具有六个自由度的机器人在其工作范围内的任何位置定位和定姿。实际上，随着机器人越来越接近其工作空间的极限，虽然机器人仍可能定位在期望的点上，但却有可能不能定姿在期望的位姿上。能对机器人定位但不能对它定姿的点的区域成为不灵巧区域。
图2.31 处于退化状态的机器人
2.12 D-H表示法的基本问题
虽然D-H表示法已广泛用于机器人的运动建模和分析，并已成为解决该问题的标准方法，但它在技术上仍存在着根本的缺陷，很多研究者试图通过改进D-H表示法来解决这个问题。其根本问题是：由于所有的运动都是关于x和z轴的，而无法表示关于y轴的运动，因此只要有任何关于y轴的运动，此方法就不适用，而且这种情况十分普遍。例如，假设原本应该平行的两个关节轴在安装时有一点小的偏差，由于两轴之间存在小的夹角，因此需要沿y轴运动。由于实际的工业机器人在其制造过程中都存在一定的误差，所以该误差不能用D-H法来建模。
例2.20（续） 斯坦福机械手臂的参考坐标系。图2.32显示了例2.20中（如图2.29所示）斯坦福机械手臂的参考坐标系。为了改进可视性，进行了简化。表2.3所示为斯坦福机械手臂的参数表。