第六讲机器人运动学逆解

1、关节i前置模式：ilf 坐标系i o 仅涉及i杆件的参数，1、杆长:沿xi轴从zi-1到zi的距离。2、扭角：绕xi从zi-1转到zi的角度。3、平移量：沿zi-l轴从xi-1轴量至xi轴的距离。4、转角:绕zi-1轴从xi-1轴到xi的转角o第3章机器人运动学3.1机器人的位姿描述3. 2齐次变换及运算3. 3机器人运动学方程3.4机器人微分运动3.3机器人运动学方程332小节运动学方程的逆解3. 3机器人运动学方程人的逆运动学问题，或间接位置求解。逆运动学问题：对某个机器人，当给 出机器人手部在基座标系中所处的位置和 姿态时（即必h中各元素给定），求出其对 应的关节变量值彳。山东大学机械工

2、程学院机电工程研究所2010/09/022、运动学方程的逆解逆运动学问题的可解性：p勺 J ayPyn0apzzzJr z0 0 01M血=其中：® =下面以六自由度机器人PUMA为例, 研究其可解性。冬=_仏°), =5°z=C c?3(Q4C5C6 $4*6 ) *23$5。6 $1 CS4C5C6 + “4*6 ) $1上23 (。沱厶：一$4*6 ) $23$5“6 C1 S4C5C6 + 46 ) (C4C5C6 $4*6) C23S5C6= C1(23(©5$6 +3辰6)+ $23$5$6一®(S4GS6 + C4C6)C23C4

3、C5S6 +S4c6) + s23s5s6 + cl(-s4c5s6 +c4c6)=$23 (。4。5。6 + $4。6 ) + “23*5*6山东大学机械工程学院机电工程研究所2010/09/0219012、运动学方程的逆解ax = Cl （C23C4C5 +S23C5）ay = $1（C23C4C5 + 仏幺）+az = $23°4°5 +。23°5Px =。16（。23。4*5 + *23。）+ 4*23 +22 一 *1 （6*4$5 +2） Py = $1 6 （*23。4*5 + *23。5 ） + 423 + + C （dSS + ?） Pz = 6

4、 （。23。5 $234） + 4C23 2*2其中：C" =cos0cos&. sing sin 0. =cos© +0）JJJJ= sin（0 +0）Js 广 cosgsinq+singcosq山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解可见，我们有12个方程及6个未知数。 上述12个方程关系如何？我们先看看转动部分，它是3X3子矩阵 ,共有9个元素；我们知道，转动矩阵的每 列都是单位矢量，并且每列之间都两两正交 ；曲此，9个元素申仅三个是独立的，或则 说，12个方程中仅有6个是独立，对应6个 未知数。因此，一般情况下，单从数学的角

5、度看 ,方程组应该是有解的。山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解上述方程组是由一些非线性的、超越 、难解的方程组成。为了降低求解难度， 机器人的杆件参数应仅可能地取为o,如 常见的PUMA机器人那样。对于任何非线 性方程组，必须关心其解的存在性、多解 性和求解方法。山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解解得存在性：解是否存在与机器人的工作空间密切 相关，工作空间又取决于机器人的结构、 杆件参数，或手部（工具）的位姿。一般情况下，如果手部坐标系的位置 和姿态都位于工作空间内，则至少存在一 个解；询反，若挙部坐*示系白6位置

6、和姿态 都位于工作空间外，则无解。多解性问题： 解得数量不仅与机器人的关节数有关,111"如何选择？女口：路径最短、最近原则。山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程不像线性方程，不存在求解非线性方程 组的通用算法。非线性方程组的算法应能求出它的所 有解；因此，某些数值递推方法不适用。 逆解的形式：1)闭式解(Close-form solution):用解 析函薮式羌示解。特点：求解速度快。存在闭式解是机器人设计的目标，仅仅 在一些特殊情况下，机器人存在解析的闭式 解，女口：相邻的多个关节轴交与一点，杆件 扭角等于0或90度等。山东大学机械工程学

7、院机电工程研究所2010/09/023. 3机器人运动学方程2)数值解(Numerical solution): 牛审点：蓮推亲解。求解方法分类：代数法、几何法以及数值法，前两种 用于求闭式解，后一种用于数值解。下面我们结合几个实例，介绍机器人 闭式解析解的求解方法。山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程例1 :已知四轴平面关节SCARA机山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程器人如图所示,试计算:(1) 机器人的运动学方程；(2) 当关节变量取务二30。60。, - 120 f 90°T 时,机器人手部

8、的位置和姿态；机器人运动学逆解的数学 表达式。山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程解:1）运动学方程a.建立坐标系（前置模式）机座坐标系0杆件坐标系i 手部坐标系h 40030080077777山东大学机械工程学院机电工程研究所2010/09/02山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程解：（1）运动学方rli01800Oi40000120023000O23d3000d34-200040004程400 I 30077/77.山东大学机械工程学院机电工程研究所2010/09/023.3机器人运动学方程吩癖师制靖畴)

9、Trtz 715(400,0,0)s0t£&1_ 00£&1_ 00c 800s&ic乞00山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程程3. 3机器人运动学方程山东大学机械任桎学卩約他工程研究所2010/09/023. 3机器人运动学方程0 00 010 1解：（1）运动学方 程1 00 1=0 00 0山东大学机械任桎学卩約他工程研究所2010/09/023. 3机器人运动学方程程山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程山东大学机械工程学院机电工程研究所2010/09/023

10、. 3机器人运动学方程_ S04=00000Mh)祖邠折蜿 Rot(z $)s04C0400山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程解：（1）运动学方程d.建立方程1240M()力= Mn =-s0i24 0 400t+300t12 c&e 0 400sq+30(k&201600+£00 1式中：C&ix =COS® +&2+&4)9$&124 = Si 11 © + $ + &4 )c0i2

11、=cosd + &2)2&i2 =sinq +2)山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程解：（2 ）已知妒30。# -60° .-120 # 90°T # 则:1 - 273一 2 OV3-21 一 2 o O-3o O 1 O山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程19013. 3机器人运动学方程山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程解：（3）逆解数学表达式 已知运动学方程,用通式表示为:nx ox"1247 01240,C G

12、+ lc G2叫°yay PyC&1240hS G + l2S &12nz °zaz Pz001d +3 +40 00 10001关系己知分析：上述矩阵方程有4个未知量，由于第一行第一列元 素与第二行第二列元素相等，第一行第二列元素与第二 行第一列元素大小相等、符号相反；因此，仅4个元素相 互独立，与变量数相同。解：（3）逆解数学表达式联立方程：C$24 -nx-°y(d)SO24- °x(b)I©。、+I2c3i2 - px(c)lS0 +l2sOi2 - py（）右 + d3 + d4 = pz2)其中#加.心 片、Q和p是

13、已知的手的位姿 > 氐 02. d及丛是待求的未知量。解：（3）逆解数学表达式 由上面（a）, （b）两式可得:"124 = COS© + &2 + &4)= nx s &24 = sin(q + g + &4)=竹G + &2 + &4 =伽由上面(c). (d)两式：厶cosq + l2 cos( 3y +%)= Px I、siii q +12 sin( q + &2)= py 两边平方可得:I： COS2 0 +2仏 COS。 COS(q +。2)+琴 COS2( q +0)= P；I： sin2 3 +

14、2仏 sin 久 sin© + 0) + 乍 sin2 ( q + 0) =将两式相加得:C：2&C&2 一 C0S0S&2cosg =2仏则:&2 = ±COS“山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程人昭+/异务=I人(d)解：(3 )逆解数学表达式心心“(e)为了求鱼,由上面(C )、( d)两式展开可 得：lcOi +<2(C&iC&2 -S&P&2)= PxlS& +<2(S&1C&2 +C&1S&2)= Py

15、化简，得：这时&2已经求出。(A + lcOcOy(Z252)5 = px(gsg)。 +(/ + lCOrSO Py解：（3）逆解数学表达式由上面两式可得：_ Cl +l$02)P I应 EPx S 1= I1+l2c32)2l2s02)2仏 +?2C&2)P 汁心&2Py可得:1 （乍乞尸+厲+仏电尸十Q(泌)Man1COS&i'仏+/02）卩厂/&2卩（ll+l2CO2）p 汁/&2 耳丿山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程解：（3）逆解数学表达式 已知0 , &2后，由G +&am

16、p;2 + &4 = tan-1可得：仏）= tan 1 -x 2E丿最后由（e）式可得:3 = Pz _4山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程逆解数学表达式为:0i = tan"/ (l+l2C®2)P 厂-sEPx、 lx + l2cO2)p汁心力儿可见，四轴平面关节SCARA机器存在封闭式 逆解表达式。山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程结合实例介绍另一种求逆解的代数法。例2：已知PUMA机器人，如图所示，试用递 推逆变换法计算其运动学逆解（后置模式）。31山东大学机械工程学院

17、机电工程研究所2010/09/023. 3机器人运动学方程解：结构参数和关节变量表角11OO-2-90°OO3%阳4一90°g5go*00600Oa%31山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程31山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程接下来写出一些杆件间齐次变换阵，并 注意其中的一些元素。31山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程31山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程0 00 01 00 1一"如6 一

18、舉6Q Q 0 0S5C6 一$5%0 0 0 1“6 二3446 =(:他一哪63仔6 弹6031山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程1113 3 3 rlr2r3o 1112 2 2 rlr2r3o111 迈ho山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程rll = 2345 切兀一 *23号窃：厂21 = 一曲一网6，厂31 = 23tc4c5c6 屯* 一

19、切护5%, 厂12匕+$4% + $2?些卑, 广22 卧5$6 -唧6，r32 =+叼 + *23巧,r13 = C23C4S5 s23c523 =碍r33 = 畑切花一C23C5,Px = 口2辺十。3。23 如23$Py =(C)Px = 一呼23 一 a2s2 一 虫辺3山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解山东大学机械工程学院机电工程研究所2010/09/02手部相对基座坐标系的位姿矩阵:山东大学机械工程学院机电工程研究所2010/09/02山东大学机械工程学院机电工程研究所2010/09/02人2 心山东大学机械工程学院机电工程研究所2010/0

20、9/02山东大学机械工程学院机电工程研究所2010/09/02320r330r22丫23Pypz1山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程人2 Gq_込oooo100001PxPyPz1= Ti6 (1)山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程越靠近基座越简单注意到，弘的（2,4）元素为丛。让上式 中等号两边的（2,4）元素相等，得： SlPX+ClPy =d3人丄” 一个未知量"1牡 = W根据和差公式，得：sin(0 _%)=.取后：0_8i=Atan (%二Atan (卩/一 Atan (d界屈+片一碍

21、)求出了 0 1如果我们再令式等号两边(1, 4)和(3, 4)元 素相等，可得：clPx + $込=呼23 -心切 + 帔2，( 3)一卩x = °3$23 + ”4勺3 + °2切两个未知量02, 0,3将以上两式平方后相加，可得:(4)驹用一力4*3 = K、山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程其中:一呂 世 百山东大学机械工程学院机电工程研究所2010/09/021901幼3.3机器人运动学方程沪冲即曲-即6 -即-甲$ -斥号(h 邨6妙6c5血

22、一邨曲 - S4C5S5 -耶5°0 001(4)式与(2)式有相同的形式，可得：= Atan. (a0f4) - Atan2(4屈 + 曾-K?),、我们注意到，T36中的(1,4)和(2,4) 元素为常数，由：IT 03Tg =丁36吐勺 0§0 0 0 1512 耳3 Px切2223 Py31 02 巾3 Pz0 0 0 1(5)令(5)式等号两边(1, 4)和4)元素相等,可得：clc?3Px + sCPy " snPz J a2C3 二稣(6)一勺也3久一一切迅+邨3 "心 (91、。2已求出山东大学机械工程学院机电工程研究所2010/09/0

23、23. 3机器人运动学方程°2山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程（6）式中，仅c23和S23两个未知数，联立可解得:（1。3 - fl2c3）z +（clPx +吐$）2$3 巧+ （诃+讪2（切巾血）Pz 一佝+迥耳）©厶+ %竹）°23 = Atar（一勺一2巾典一（5如+旳Py）（血一。2$3）2旳 一 dp. 一 3 +耳十 hPyy°2山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动炜训1C怦6_邮6 一即少6-甲6-巾号 邨6即6c5-邨曲- C州吧$厂年6耶5 0 00这样

24、，式(5)左边矩阵中的所有元素都已知 To、为了求 触和05,令(5)式等号两边(1,3)和 (3,3)元素相等，可得：r13clc23 + r23Jlc23 " r33J23 -印与-讪+哪1 =些与设不等于零，得：% = Atan （一也必 + r23cl/r13clc23 23屮23 +.33$23）九等于零对应4轴与6轴共线的奇异结构，这时0的辕动效果相同，可任取山东大学机械工程学院机电工程研究所2010/09/02C5C6 -呼6 -号 03. 3机器人运动学方程円“茶打；2 0 0 0 1、为了求5和6,我们应用T46：?04人6 =T46(7)其中：宀clc23c4 +

25、 14S1C23C4 -中叼 一S23C4 仔2。3。4 + 3叼-仇3C4 c1c2354 +14 一$1巾3叼一 5C4巾3为 切勺叼+冷5 +说叼C1523_加23-C23Q说 一 4L 0001令(7)式等号两边(1, 3)和(3, 3)元素相 等，可得：r13(clc23c4 + 勺$4)+ r23(5lc23c4 -r33(J23c4)= -$5，厲3( 一 迪)+23呼 23)+33(-切)=与山东大学机械工程学院机电工程研究所2010/09/023. 3机器人运动学方程解得：,“勺旳切3"+呼4）+加胪235卍4）-%处3臥&5 = Atan2（），口3（-加

26、23）+尸23（-叩23）+耳3卜切）同样，令式等号两边（3,1）和仃，1） 元素相等，可得：% = Atan其中：/$6 = "rll（clc23J4 一 $15）一 厂21（$世如4 + clc4）+ 切（$2影4），% = 5©宓卬+$1$也5 一 %切$5】+21仙宓Q -加4屁 7疋四与F31CS23C4C5 + C23S5L3. 3机器人运动学方程% = Atan 5、卜為_ Atan站出卩殳+片一碍）.02 = 0" °23 A tar （一也 一。25）卫三 一 耳 + ElPy）（划 一 2$3） （边旳-dPZ 一（。3 +。25）1

27、 几 + hPy為=Atan. （口澈）Atan2（y士倨 + 出-K2）.°4 = Atan （/3$1 + r23cjAr13cic23 23$工23 +33$2» A人 切（勺切"+切4）+咖（加邠4一加4）一如$234 堀二 Atan2（）,厂口（一勺临 3）+ 广23（$1$23）+ 厂33（一如）兔=Atan 3,6）,$6 = 一巾1（勺辺护4 -迥卬）一21（$1包3口 +。工4）+呦（$2护4），% =厂11佝如切+吋»5 勺也则+加（归歸-屮4屁 7宓号巾1（切卯4。5 +切3巧丄山东大学机械工桎学険机电工桎研% Wr2010/09/023. 3机器人运动学方程从所得的各关节变量表达式可看岀：只 有01- 的式中有P“Py，Pz,故他们确定了 末杆坐标系原点的位置。而04- 三式中 有弘，它们确定了末杆坐标系的 姿态。这是后三个关节交与一点这种结构的 重要特点之一。3. 3机器人运动学方程递推逆变换法小结：1、原则：等号两端的矩阵中对应