《机器人导论》机器人逆运动学

1、 逆运动学逆运动学: 已知工具坐标系相对于工作台坐标系的期望位置已知工具坐标系相对于工作台坐标系的期望位置和姿态，计算一系列满足期望要求的关节角和姿态，计算一系列满足期望要求的关节角 为求出要求的关节角以放置相对于工作台坐标系为求出要求的关节角以放置相对于工作台坐标系S的工的工具坐标系具坐标系T，可将这个问题分为两部分，可将这个问题分为两部分: 首先，进行坐标变换求出相对于基坐标系首先，进行坐标变换求出相对于基坐标系B的腕部坐标的腕部坐标系系 W. 应用逆运动学求关节角应用逆运动学求关节角. TTTTTTT56453423120106T06101TTTTTTTT5645342312061011

2、112TTTTTTTT5645342306101112211121321222300123456112233445566313233()()()()()()0001xyzrrrprrrpTTTTTTTrrrp1. 解的存在性解的存在性 解是否存在的问题完全取决于操作臂的工作空间解是否存在的问题完全取决于操作臂的工作空间. 灵巧工作空间灵巧工作空间: 机器人的末端执行器能够从各个方向到达机器人的末端执行器能够从各个方向到达的空间区域的空间区域.可达工作区间可达工作区间:机器人至少从一个方向上有一个方位可以达机器人至少从一个方向上有一个方位可以达到的空间到的空间. 例例: 考虑一个两连杆操作臂考虑

3、一个两连杆操作臂. 如果如果 , 则可达工作空间是半径为则可达工作空间是半径为 的圆，而灵巧工作空间仅是单的圆，而灵巧工作空间仅是单独的一点，即原点。如果独的一点，即原点。如果 ，则不存在灵巧工作空间，而可达工作空，则不存在灵巧工作空间，而可达工作空间为一外径为间为一外径为 ，内径为，内径为 的圆环。在可达工作空间内部，末端执的圆环。在可达工作空间内部，末端执行器有两种可能的方位，在工作空间的边界上只能一种可能的方位。行器有两种可能的方位，在工作空间的边界上只能一种可能的方位。12ll12ll12l12ll12ll 当一个操作臂少于当一个操作臂少于6自由度时，它在三维空间内不能达到自由度时，它

4、在三维空间内不能达到全部位姿全部位姿. -操作臂的工作空间是一个子空间操作臂的工作空间是一个子空间. -更简单的操作臂的工作空间是这个子空间的子集更简单的操作臂的工作空间是这个子空间的子集. 对于少于对于少于6个自由度的操作臂来说，给定一个确定的一般个自由度的操作臂来说，给定一个确定的一般目标坐标系，什么是最近的可达目标坐标系？目标坐标系，什么是最近的可达目标坐标系？ 一般来说，工具坐标系的变换与操作臂的正逆运动学无一般来说，工具坐标系的变换与操作臂的正逆运动学无关，所以一般常去研究腕部坐标系关，所以一般常去研究腕部坐标系W的工作空间。对于一个的工作空间。对于一个给定的末端执行器，定义工具坐标

5、系给定的末端执行器，定义工具坐标系T，给定目标坐标系，给定目标坐标系G，去计算相应的腕部坐标系，去计算相应的腕部坐标系W。 例例: 试着描述三连杆操作臂试着描述三连杆操作臂 的子空间的子空间. 利用连杆参数求得操作臂的运动学方程为利用连杆参数求得操作臂的运动学方程为: 这里这里 和和 是满足约束的任意变量是满足约束的任意变量,因此，子空间就因此，子空间就建立了建立了.连杆长度和关节的限位决定了操作臂的工作空间连杆长度和关节的限位决定了操作臂的工作空间. 1231231 12121231231 121200000010001000010001BWcsxcsl cl cscyscl sl sTBW

6、T, x y 例例: 试描述下图两自由度操作臂试描述下图两自由度操作臂 的子空间的子空间. 已知已知: 这里这里 可以取任意值可以取任意值. 它的方位是确定的，因为它的方位是确定的，因为 的方向取决于的方向取决于 它的姿态受限，它的姿态受限， 总是向下，而总是向下，而 的方向是叉乘求得。的方向是叉乘求得。020ORGxPy 02T, x y, x y02Y000222XYZ22220222220001000001yxxxyxyxyyTxyxy02Z 2. 多重解多重解 一个具有一个具有3个旋转关节的平面操作臂，由于从任何方位均可到达工作个旋转关节的平面操作臂，由于从任何方位均可到达工作空间内的

7、任何位置，因此在平面中有较大的灵巧工作空间（给定适当的空间内的任何位置，因此在平面中有较大的灵巧工作空间（给定适当的连杆长度和大的关节运动范围）连杆长度和大的关节运动范围）. 系统最终只能选择一个解，比较合理的选择应当是取系统最终只能选择一个解，比较合理的选择应当是取“最短行程最短行程”解解. 最短行程的确定：最短行程的确定： 计算最短行程需要加权，使得选择侧重于移动小连杆而不计算最短行程需要加权，使得选择侧重于移动小连杆而不是移动大连杆是移动大连杆. 在存在障碍的情况下，最短行程发生干涉，这时选择较长在存在障碍的情况下，最短行程发生干涉，这时选择较长行程。行程。 解的个数取决于操作臂的关节数

8、量，它也是连杆参数和关节运动范解的个数取决于操作臂的关节数量，它也是连杆参数和关节运动范围的函数围的函数. 例子例子: PUMA 到达一个确定目标有到达一个确定目标有8个不同的解个不同的解. 图中给出了其中的图中给出了其中的4个个解解.它们对于末端手部运动来说具有相同的位姿。对于图中所示的每一个它们对于末端手部运动来说具有相同的位姿。对于图中所示的每一个解存在另外一种解，解存在另外一种解， 其中最后三个关节变为另外一种位形：其中最后三个关节变为另外一种位形： 由于关节运动的限制，由于关节运动的限制， 这这8个解中的某些解是不能实现的个解中的某些解是不能实现的.04455066180180 通常

9、，连杆的非零参数越多，达到某一特定目标的方式也越多通常，连杆的非零参数越多，达到某一特定目标的方式也越多. 以一个具有以一个具有6个旋转关节的操作臂为例，解的最大数目与等于零的连个旋转关节的操作臂为例，解的最大数目与等于零的连杆长度参数的数目相关。非零参数越多，解的最大数目就越大杆长度参数的数目相关。非零参数越多，解的最大数目就越大. 3. 解法解法 与线性方程组不同，非线性方程组没有通用的求解方法，与线性方程组不同，非线性方程组没有通用的求解方法，我们把操作臂的全部求解方法分成两大类：我们把操作臂的全部求解方法分成两大类：封闭解封闭解: 封闭解是指基于解析形式的算法，或者指对于不封闭解是指基

10、于解析形式的算法，或者指对于不高于四次的多项式不用迭代便可完全求解。可将封闭解的高于四次的多项式不用迭代便可完全求解。可将封闭解的求解方法分为两类：代数解法和几何解法求解方法分为两类：代数解法和几何解法.数值解法数值解法: 数值解具有迭代性质，所以比封闭解法的求解数值解具有迭代性质，所以比封闭解法的求解速度慢得多。通常，数值解的计算也依赖于解的解析形式，速度慢得多。通常，数值解的计算也依赖于解的解析形式，一般不用数值解来求解运动学问题，对运动方程的数值迭一般不用数值解来求解运动学问题，对运动方程的数值迭代本身已形成一个完整的研究领域代本身已形成一个完整的研究领域. 关于运动学逆解的几个结论关于

11、运动学逆解的几个结论: 所有包含转动关节和移动关节的串联型所有包含转动关节和移动关节的串联型6自由度操作臂都自由度操作臂都是可解的，但这种解一般是数值解是可解的，但这种解一般是数值解. 对于对于6自由度操作臂来说，只有在特殊情况下才有解析解。自由度操作臂来说，只有在特殊情况下才有解析解。这种存在解析解（封闭解）的操作臂具有如下特性：存在这种存在解析解（封闭解）的操作臂具有如下特性：存在几个正交关节轴或者有多个几个正交关节轴或者有多个 为为0或或 . 具有具有6个旋转关节的操作臂存在封闭解的充分条件个旋转关节的操作臂存在封闭解的充分条件（sufficient condition）是相邻的三个关节

12、轴线相交于一）是相邻的三个关节轴线相交于一点点. i090 为了介绍运动学方程的求解方法，这里用两种为了介绍运动学方程的求解方法，这里用两种不同方法对一个简单的平面三连杆操作臂进行求解不同方法对一个简单的平面三连杆操作臂进行求解. 1. 代数解法代数解法 该操作臂的运动方程为该操作臂的运动方程为: 1231231 12121231231 1212030000100001BWcsl cl cscl sl sTT 研究的是平面操作臂研究的是平面操作臂,通过确定三个量通过确定三个量 就可以容易确定目标点就可以容易确定目标点的位置的位置 : . 所有可达目标点均位于上式描述的子空间内所有可达目标点均位

13、于上式描述的子空间内. 0000100001BWcsxscyT, ,x y1231231 12121231231 12120300000010001000010001BWcsxcsl cl cscl sl sscyTT 得到四个非线性方程得到四个非线性方程: 上式有解的条件是上式有解的条件是 的值必须在的值必须在-1和和+1之间。在这个之间。在这个解法中，可用这个约束来检查解是否存在。如果约束条件不解法中，可用这个约束来检查解是否存在。如果约束条件不满足满足 ,则操作臂离目标点太远则操作臂离目标点太远. 1231231 12121 1212ccssxl cl cyl sl s2222121 2

14、222221221 2121 21 2121 21 222,xylll l cxyllcl lcc cs ssc ss c2c 假设目标点在工作空间内，则假设目标点在工作空间内，则: 上式是多解的，可以选择正或者负上式是多解的，可以选择正或者负.2222221,tan2(,)scAs c 为便于计算引入新的变量引入新的变量: 式中式中: 为了求解这种形式的方程，进行变量代换为了求解这种形式的方程，进行变量代换:令令 那么那么 于是有于是有 1 12121 1212xl cl cyl sl s1 1211 121xk ck syk sk c1122222kll ckl s221221,tan2(

15、,)rkkAk k 12cos,sinkrkr1111cos cossin sincos sinsin cosxryr11cos()sin()xryr 得到得到 最后，我们解出最后，我们解出 : 总之，用代数解法求解运动学方程是求解操作臂的基本方法之一，在求解方程时，总之，用代数解法求解运动学方程是求解操作臂的基本方法之一，在求解方程时，解的形式已经确定。可以看出，对于许多常见的几何问题，经常会出现几种形式解的形式已经确定。可以看出，对于许多常见的几何问题，经常会出现几种形式的超越方程。的超越方程。 注：超越方程：等号两边至少有一个含有未知数的初等超越函数式的方程。如指注：超越方程：等号两边至

16、少有一个含有未知数的初等超越函数式的方程。如指数方程、对数方程、三角方程、反三角方程等。具有未知量的对数函数、指数函数方程、对数方程、三角方程、反三角方程等。具有未知量的对数函数、指数函数、三角函数、反三角函数等的方程。超越方程一般没有解析解，而只有数值解数、三角函数、反三角函数等的方程。超越方程一般没有解析解，而只有数值解或近似解，只有特殊的超越方程才可以求出解析解来。或近似解，只有特殊的超越方程才可以求出解析解来。1121tan2(,)tan2( , )tan2( , )tan2(,)y xAAy xAy xAk krr123312tan2(,)As c 2. 几何解法几何解法 为求出操作

17、臂的解，将操作臂的空间几何参数分解成为平面几何参数，为求出操作臂的解，将操作臂的空间几何参数分解成为平面几何参数，然后应用平面几何方法求出关节角度。对于例子中的然后应用平面几何方法求出关节角度。对于例子中的3自由度操作臂，有自由度操作臂，有于操作臂是平面的，因此利用平面几何关系直接求解。于操作臂是平面的，因此利用平面几何关系直接求解。 We have:2222121 222cos(180)xylll l2222221221 2cos(180)cos()2xyllcl l 为了使该三角形成立，到目标点的距离为了使该三角形成立，到目标点的距离 必须满足小于等于两个连杆长度之和必须满足小于等于两个连

18、杆长度之和 , 12ll22xy 应用反正切公式: 应用余弦定理: Here, the arccosine must be solved so that , in order that the geometry which lead to the equation will be preserved. Then we have: 平面内的角度是可以相加的，因此三个连杆的角度之和即为最后一个连杆的姿态: This equation is solved for to complete our solution.00180tan2( , )Ay x222212221cos2xylllxy11233

19、3. 通过化简为多项式的代数解法 万能公式: 22212sintan,cos,sin,tan21121 cosuuuuu 例子: 求解超越方程 的 . 利用: 得到: 取u的幂函数形式: 得到: 如果 , 那么 . 0accossinabc018022(1)2(1)aubucu22212cos,sin11uuuu2()2()0ac ubuca2222221,2tan ()bbacbbacuacac 尽管一般具有6自由度的操作臂没有封闭解，但在某些特殊情况下还是可解的.PEIPER研究了3个相邻的轴相交于一点的6自由度操作臂。PEIPER的方法主要针对6各关节均为旋转关节的操作臂，且后面3个轴相

20、交。该成果广泛应用于产品化的机器人中。 当最后3根轴相交时，连杆坐标系4、5、6的原点均位于这个交点上，这点的基坐标如下： 3134323001230120141234123124333()()()111ORGORGafxd sfyPT T T PT T TT Tzd cf 111111111100001 iiiiiiiiiiiiiiiiiiicsas cc cssdTs sc sccd 3134323001230120141234123124333()()()111ORGORGafxd sfyPT T T PT T TT Tzd cf 1333323234332322234323334332

21、3222343()0()()1100011 facsaafd ss cc cssdd sTfd csscsccdd c13 343 32232 3432 342332332 3432 342332fa cd ssafa csd sccd scd sfa ssd sscd ccd c 利用 ,得到: 0112,TT1101322110100012321211120421211121001101331112111230()0()()0001000111 ORGcsafcsas cc cssdfscccssdPsscsccds scsccdfc gs gs gc gg121121221 121213

22、21321 12121321gc fs gags cfc cfsfd sgs sfc sfcfd c We now write an expression for the squared magnitude of , which we will denote as : We now write this equation, along with the Z-component equation, as a system of two equations in the form: where: 2222222212312312231212222 ()rgggfffadd fa c fs f04O

23、RGP( ) 1 22 2131 22214()2()rk ck sakzk sk c sk222rxyz11222222231231223431212kfkfkfffadd fkf cd c Equation * is useful because dependence on has been eliminated and because dependence on takes a simple form. Now consider the solution of for : 1. If , then , where r is known. is a function of only. Af

24、ter the substitution, a quadratic equation in may be solved for . 2. If , then we have , where z is known. Again, after substituting, a quadratic equation arises that can be solved for . 3. Otherwise, eliminate and , the equation results in an equation of degree 4, which can be solved for .3121 22 2

25、131 22214()2()rk ck sakzk sk c sk310a 3rk3k33tan(/2)310s4zk32s2c222234122211()()4rkzkkkas Having solved for , we can solve equation * for and * for . To complete our solution, we need to solve for . These axes intersect, so these joint angles affact the orientation of only the last link. We can comp

26、ute them from nothing more than the rotation portion of the specified goal, . 456, 1 22 2131 22214()2()rk ck sakzk sk c sk32111120111243*1ORGc gs gs gc gPg06R 得到 之后 , 我们还需要求 ，由于这些轴相交，所以这些关节角只影响最后连杆的方位，我们只需计算指定目标的方向: 求出 后，当 时，可以由连杆坐标系4相对于基坐标的方位计算出 。坐标系6的期望方位与连杆坐标系4的方位差别仅在于最后三个关节的作用。 对于大多数操作臂来说，完全可以将之

27、前介绍的 Z-Y-Z欧拉角解法应用于 . 123, 404040|R4440106040 6|RRR456, 06R123, 4460|R Example: Z-Y-Z 欧拉角的求法 描述一个坐标系 B : 与参考坐标系 A重合，首先绕 旋转 然后绕 旋转 ，最后绕 旋转 . 000( , )001000010001 ABZ Y ZcscscsRscscscc c cs sc c ss cc ss c cc ss c sc cs ss cs scBZBYBZ 解: 如果 或 , 解就简化为: 111213212223313233 c c cs sc c ss cc srrrs c cc ss

28、c sc cs srrrs cs scrrr018002231323323133231tan2(,)tan2(/,/)tan2(/,/)ArrrArsrsArsrs121100tan 2(,)Arr012111800tan 2(,)Arr 例子：puma560机器人逆解: 将含有 的部分移到方程的左边 11121321222300123456112233445566313233( )()()()()()0001xyzrrrprrrpTTTTTTTrrrp101 0123451162233445566( )()()()()()TTTTTTT 转置 : 令元素 (2,4) 相等，得到: 进行三角恒

29、等变换: 其中1112131121222311163132330000001 0000 10001xyzrrrpcsrrrpscTrrrp113xys pc pd01Tcos,sinxypp22,tan(,)xyxyppApp We obtain If equate both the (1,4) and (3,4) elements: We can obtain113/s cc sd3 34 3a cd sK2233331122tan2(,1),tan2(,)tan2(,1)yxddddAAppA3 234232 2xpa sd ca s113 234 2322xyc ps pa cd sa

30、c233112sin(),cos()1dd2222222233422xyxpppaaddKa Solved by the same kind of trigonometric substitution: Rewrite again (left-hand side is a function of only knowns and ): 222233434tan2(,)tan2(,)Aa dAKadK01 0345326445566()()()()TTTTT1112131 231 23232 32122231 231 23232 336113313233000010001xyzrrrpc cs c

31、sa crrrpc ss sca sTscdrrrp Equating both the (1,4) and (2,4) elements: We can get We can solve2332 31142 32 3432 311tan2()()(),()()()zxyzxyAaa cpc ps pda sa sdpaa cc ps p2 3432 311232211()()()()zxyzxya sdpaa cc ps pcpc ps p1 231 23232 33xyzc c ps c ps pa ca32 3112 34232211()()()()zxyzxyaa cpc ps pa

32、sdspc ps p1 231 23232 34xyzc s ps s pc pa sd2233 Equating both the (1,3) and (3,3) elements: As long as , we can solve for as: When , the manipulator is in a singular configuration in which joint axes 4 and 6 line up and cause the same motion of the last link. In this case, all that matters (and all that can be solved for) is the sum or difference of and . This situation is detected by checking whether both arguments of the Atan2 are near zero. If so, is chosen arbitrarily, and when is computed