任务3 工业机器人逆向运动学及实例
一、任务导入
在任务2中，我们已经学习了工业机器人的正向运动学。与正向运动学相反，逆向运动学研究的是已知机器人末端的位置姿态，再计算机器人对应位置的全部关节变量。
在实际应用中，我们更常用的是机器人的逆向运动学，在确定了机器人末端的位置后，再计算每一个关节的运动情况。
逆向运动
二、工业机器人逆向运动学方程及实例
上面我们说明了正向求解问题，即给出关节变量
和d，求出手部位姿各矢量n、o、a和p，这种求解方法只需将关节变量代入运动学方程中即可得出。但在工业机器人控制中，问题往往相反，即在已知手部要到达的目标位姿的情况下如何求出关节变量，以驱动各关节的马达，使手部的位姿得到满足，这就是逆向运动学问题，也称求运动学逆解。
现以斯坦福工业机器人为例来介绍逆向求解的一种方法。为了书写简便，假设H=0，即坐标系{6}与坐标系{5}原点相重合。已知斯坦福工业机器人的运动学方程为：
T6＝A1A2A3A4A5A6
现在给出T6矩阵及各杆的参数l、、d，求关节变量1～6，其中3＝d3。
（1）求1
用
左乘式（3-10），得：
1T6=
T6=A2A3A4A5A6
将上式左右两边展开得：
（3-13）
根据式（3-13）左、右两边之第三行第四列元素相等可得：
-pxs1+pyc1＝d2
引入中间变量r及
，令
px=rcos
py=rsin
则式（3-13）化为：
利用和差公式，上式又可化为：
这里，
，0＜-＜，又因为：
故有：
所以：
（3-14）
这里，“+”号对应右肩位姿，“-”号对应左肩位姿。
（2）求2
根据式（3-12）左、右两边第一行第四列相等和第二行第四列相等可得：
（3-15）
故：
（3-16）
（3）求3
在斯坦福工业机器人中3=d3，利用sin2+cos2=1，由式（3-16）可解得：
（3-17）
（4）求4
因为1、2、d3已经求出，利用式（3-14）～式（3-16）可以求得矩阵A1、A2、A3，从而求得3T6＝A3-1A2-1A1-1T6。
由于3T6＝A4A5A6，所以：
（3-18）
将式（3-18）左、右两边展开后取其左、右两边第三行第三列相等，得：
所以：
（3-19）
（5）求5
取式（3-18）展开式左、右两边第一行第三列相等及第二行第三列相等，有：
所以：
（3-20）
（6）求6
采用下列方程：
（3-21）
展开并取其左、右两边第一行第二列相等及第二行第二列相等，有：
所以：
（3-22）
至此，1、2、d3、4、5、6全部求出。
从以上解的过程看出，这种方法就是将一个未知数由矩阵方程的右边移向左边，使其与其他未知数分开，解出这个未知数，再把下一个未知数移到左边，重复进行，直至解出所有未知数，所以这种方法也叫分离变量法。这是代数法的一种，它的特点是首先利用运动方程的不同形式，找出矩阵中简单表达某个未知数的元素，力求得到未知数较少的方程，然后求解。
还应注意到工业机器人运动学逆解问题的求解存在如下三个问题。
图3-20 工作域外逆解不存在
1.解可能不存在
工业机器人具有一定的工作域，假如给定手部位置在工作域之外，则解不存在。图3-20所示二自由度平面关节机械手，假如给定手部位置矢量（x，y）位于外半径为l1+l2与内半径为|l1-l2|的圆环之外，则无法求出逆解1及2，即该逆解不存在。
2.解的多重性
工业机器人的逆运动学问题可能出现多解。图3-21（a）表示一个二自由度平面关节机械手出现两个逆解的情况。对于给定的在工业机器人工作域内的手部位置A（x，y）可以得到两个逆解：1、2及'1、'2。从图3-21（a）可知手部是不能以任意方向到达目标点A的。增加一个手腕关节自由度，如图3-21（b）所示，三自由度平面关节机械手即可实现手部以任意方向到达目标点A。
图3-21 逆解的多重性
在多解情况下，一定有一个最接近解，即最接近起始点的解。图3-22（a）表示3R机械手的手部从起始点A运动到目标点B，完成实线所表示的解为最接近解，是一个“最短行程”的优化解。但是，如图3-22（b）所示，在有障碍存在的情况下，上述的最接近解会引起碰撞，只能采用另一解，如图3-22（b）中实线所示。尽管大臂、小臂将经过“遥远”的行程，为了避免碰撞也只能用这个解，这就是解的多重性带来可供选择的好处。
关于解的多重性的另一实例如图3-23所示。PUMA560工业机器人实现同一目标位置和姿态有四种形位，即四种解。另外，腕部的“翻转”又可能得出两种解，其排列组合共可能有8种解。
图3-22 避免碰撞的一个可能实现的解图
3-23 PUMA560机器人的四个逆解
3.求解方法的多样性
工业机器人逆运动学求解有多种方法，一般分为两类：封闭解和数值解。不同学者对同一工业机器人的运动学逆解也提出不同的解法。应该从计算方法的计算效率、计算精度等要求出发，选择较好的解法。
实际上，由于关节的活动范围的限制，机器人有多组解时，可能有某些解不能达到。一般来说，非零的连杆的参数越多，达到某一目标的方式越多，运动学逆解的数目越多。所以，应该根据具体情况，在避免碰撞的前提下，按“最短行程”的原则来择优，即使每个关节的移动量最小。又由于工业机器人连杆的尺寸大小不同，因此应遵循“多移动小关节，少移动大关节”的原则。