机器人操作臂动力学

1、机器人学第五章操作臂动力学课程的基本要求：3掌握空间开链机构动力学建模的牛顿-欧拉递推 动力学方程和拉格朗日方程V 5.1连杆的速度和加速度分析一、刚体的旋转矩阵及其导数刚体变换是旋转和平移的组合为了描述刚体B的位置（Position）和姿态（Orientation）, 通常在B上固接另一坐标系B（简称B）,相对于参考 坐标系A。刚体B的位置和姿态（简称位姿Location） 可由B相对于A的位置和姿态来描述，如图21所示。 B的位置可用B的坐标原点在A中表示的n维位置矢量 p来描述.如果Po是刚体上的任意一点，在局部坐标系中表不。Po在 全局坐*示系的位置p(t)是使#0首先绕原点旋转，然后

2、平移它 得到的计算结果：p(t)= R(t) p0 + x(t)(6-1)其中，双”矢量表示刚体移动，3X3矩阵7?表示刚体旋转。刚体B的姿态可用坐标系B的个主轴的方向矢量组成 的必啷介旋转矩阵R表示，当心3时，旋转矩阵R为：R r21r31r!2ri3(6-2)其中，每一列向量表示B的一个主轴在A中的三个方 向余弦。因此也把旋转矩阵7?称为余弦矩阵。/量 外向 另轴向我们可以把物理含义赋予尺。考虑局部空间的X(1, 0, 0),在时刻t,该向量在世界空间具有方丁R 0(6-3)0(6-4)参照式(6-2)中R(t)的元素，有丁R0厂210丿31_是R(t)的第一列。R(t)的物理含意是当在时

3、刻/转换到世界空间的时候，R(t) 第一列给出刚体的x轴的方向。同样地。R(t)的第二列和第 三列，是在时刻t到世界空间中刚体y轴和z轴的方向。r=/ y zir21rn厂22和厂 13尸23J3_32_卡3.Ml)刚体不仅能平移，也能旋转。想象我们在空间中固定质心位 置，因此刚体上的任何点的运动都是由刚体绕某个穿过质心 的轴旋转造成的（除非质心本身被移动）。我们可以把旋转 描写为向量秋亠 忒/）的方向给出旋转轴的方向（图6.3）, 忒f）量被称做角速度。A线速度：和v（r）的关系为(r)x 厂21帑对于R的其它两列是同样的。这意味着 我们可以记x r2269 x r23r32那么oxb是矢量

4、、(apzbyS笑为了简化上述表达式，使用下述算子。 如果和b是3维矢量，- Q + W(axby -bxay够给定矢量a，定义a*是矩阵- yy0ayaxayax0/(0一 a ?Q、(b)zy役0_ aXb0yCayJ/、I z /a bb cy zyzci b+ baXza b一 b cI.x yXy丿= axb冷使用符号”，我们可以重新把简化为)= cd7Sa)R = e*RR(t) =a)(t) * R=S(e)R1、刚体的速度和加速度P=PBo +bRbp+bRbpR(t) =a)(t)R(t) = S(a)R、氐理比0C如欲2丿筲=%+：叭 +2S(A扰+ s(a 方扰r+ s(

5、as(a；rI0二、刚体的速度和加速度；A固定，冈IJ体与B固结Bp = O,Bvp=O,Bvp=O5产 母* SC）號=人 +S（%侬花 +张卩亠2兴吟）個 * s（霸J叙+ S（%b）S（%b）欲J% + S （人 RBp + S （人 ）S （人 ）；RBp1、刚体的速度和加速度AB相对于A移动，姿态不变a（x）b = Oj = 0+ S如紬叫+叽A B相对于A纯滚动，APb0 = const, av5q =0,% =0花七+侬5 +S（伽）侬加张兌+s（伽）强p%产气;+侬+ 2SCX）侬S + S（%B）欲+ SCX）SCX）获WR5+2S（AB）：R%p+S（A 九汇RBp + s

6、（A%）s（A%）；RBp3若已知C相对B的转动角速度为贝VC相对A的转动角速度和角加速度 矢量为A(bc=A(bB+RB(bc + SCbbB(dc二刚体的速度和加速度线速度、线加速度*/%+於 Vp + S(AB)；RBp%+；叭 + 2S(ARBvp + S(aRbp + S(acdb)S(aRbp角速度、角加速度ACOc-ACOB+RB cocAd)c=A d)B+RB d)c + S(acdb)Rb(x)cXi三、旋转关节的连杆运动的传递如图所示，连杆Z+1相对连杆Z 转动的角速度是绕关节Z+1 运动引起的，A角速度两关节角速度向量转换到同一坐标系中，有0+1=0+i+；R+Q+i在

7、卩+1坐标系中表示-和+ Q+1化+iA线速度A角加速度，线加速度+i+；RoZ+1 ：%=+和 x&J+如 + &畀如叫+1-叫+xb+iz+1 畑和（V +X叽+0 xCxlpM）四、移动关节的连杆运动传递Ai+1为移动关节即，连杆i+1相对于连杆i移动，Z+1没有转动，有-+R，coi + 0+1+乜+1=*尺J+i =和（+ x，pm）+4+i,+1 乙+1+i =和（乜 +伽 x，A+i + x（z x,+i） +2中 x厶严如+厶仔如五、质心的速度和加速度必=%+只化汁 X（X爲）V计算操作臂各连杆运动传递的公式利用这些公式即可依次从基座开始递推各连杆的速度、加速度 基座：方0 =

8、0,%=% =0转动关节：%冲=；欠+0如_冲=+：疋心+号尺 xJ+匕+| +匕+i ，+1 岭+和(5 + xp+i) ，+1 %+1 =和( + xp/+1 + x( a)i xpI+1)移动关节:i+T i+ln叽=叽1+1 v/+i -+iRC vi+卩 5+i)+4+i1+1 zm MM-+Ri +6 X，A+1 + XC XPi+l) +2McoMxdMMZM+dMi+lZi+l质心：c尸V+e/iPci# X(i O)XPc)I六、雅可比矩阵的速度递推法够通过逐次递推，可以得到末端连杆速度，匕记末端连杆的笛卡尔广义速度为匕 A X9L令注意到它是各关节变量和速度的函数，写为矩

9、阵方程的形式，则可得X严Jg広1 5.2连杆静力学分析A建立连杆z的力和力矩平衡方程计算每个关节的驱动力或力矩翳各个连杆所承受的力向量和力矩向量中，某些 分量由操作臂本身的连杆结构所平衡，一部分 量则为各关节的驱动力或力矩所平衡。A对转动关节，关节驱动力矩平衡力矩的z分量i T iA对转动关节，关节驱动力矩平衡力矩的Z分量方法：利用运动递推和力的递推建立操作 臂动力学方程，建立动力学逆问题的求 解方法牛顿-欧拉方程/ =叽ncT(b +i1相对于质心坐标转动惯量L = Ico系的惯性张量动力学逆问题递推算法已知：关节位移、速度、加速度 求解：关节力矩或力逆问题算法步骤：1）向外递推 计算各连杆

10、的速度和加速度, 由车顿欧拉公金计算容连杆的（贯生力和 力矩。2）向内递推计算各连杆相互作用力和力矩, 以及关节驱动力和力矩。（）向外递推som,=3 -4-氛】+刁门 对于转动关节宀 1厂尺匕，（对于移动关节n-lR 3 + 广ig x 9+ 宀一 十 & ,. 17 ：5尬 尸卫伉+ Q x /卄i十g x 3 x /宀）=2 广LR仏 + 远 X 加 1 + 3 X S X 卫“）+計如订X N十厂長十T”乩+严%叫E =宀%+1X +乜冲I十十亠1 X （十心/+!J亠1 - 1 1 K/ + L T3.1卜+十x d+a3计ix丿川十L -i 亠打 和T 1 JT ，+!：_/；nC

11、i+ | 一If_(2)向内递推(/：-!)7. =+虻了7 + 咒nt =:亠冲叭 + !nt. 4- ., X . + X ；_代 丁 =於2（转动关节），：斗 1）.；+ 1）5（转动关节）.（移动关节）;转动关节），（移动关节）；X I）. ） QJ f-1 T门= 1/7七（移动关节）。 I r 丨 wk-l I I AU I I nw I 1帑注意，应将操作臂末端与外界的接触力和力矩包含在 平衡方程之中，作为向内递推的初值；如果操 作臂在自由空间运动，则n+1+1=n+i = oA如果考虑连杆的重力，则取0%| = 8且方向向上.即机器人的基座以加速度g作向上 加速运动，从而抵消相

12、当于重力造成的影响。5.4关节空间和操作空间动力学5 =曲2呂佰 1 + 2)+加2“啟2(2% + 2)+ (删1 +加2”筲一血2川2仓 -2也心22 + m2cl2 +( + 加 2”1&5匕2 = m2/1/2c?261 十啊2【1如2呼 + 2l2Sel2 + 帼2(% + 玄)t =十 hq.q)+ G(g)/操作臂的福性 离心力和哥氏力向量 重力矢量F妇丁 =!， g =“，wF24也+叫S十E t 2你/迢）叫斗皿业）J杭2（心4- 2/忆匕）M心-hq.q)=-4F加/虽爲2打1&心川/仏、*2碎5.7拉格朗日动力学笑拉格朗日动力学描述则基于系统能量的 概念。对于任何机械系统

13、，拉格朗日函 数厶定义为系统总的动能瓦与总的势能坊 之差，即式中,厂如，2,，久是表示动能和势能的 广义坐标，么，Q是相应的广义速度。连杆j具有的动能为Eki 二1 i T 卜一。2 ，N 连杆质心线速度引起的动能连杆角速度产生的动能操作臂所具有的动能是个连杆的动能之和71E严工Ekji=lnn 11E严乞 Eki=$匸 mivva + 才呵 ClJi ) z=li=l 乙乙曲由前面的速度分析可知，和切是操作臂关节变量g和关节速度4的函数.够因此操作臂的动能是关节变量和关节速 度的标量函数，记为E M。帑操作臂的动能为1 丫EkG小=二4 D(q)q奇连杆j具有的势能为奇操作臂所具有的势能为各

14、连杆势能之和ne =y e .p 厶piA由于位置矢量c是关节变量g的函数，因此势能也为g的标量函数，记为Ep(q).e利用拉格朗日函数厶 系统的动力学方程（称 第二类拉格朗日方程）为d dL dLT dt dq dqA 7是“X 1的关节驱动力矩矢量。由于势能不显含0，因而动力学方程变为/ / TJ. d dEk, dEPdt dq / dq dq不含Ep作业5.1, 5.3例站1平而2R机械手如图32所示用递推法求出末端连杆的速度和角速度，雅可比。平而2*机械手各连杆塑标系如图中所示，其中【初固结在手臂末端即有久=乩优=0。因而各连杆变换为L舟01Ik出0?7T = 门0o!I00i o0

15、10 i00101! o l001JL.O001na009 JFW-01000010LO00V/+1i+ui , b i+i= iR e + Q+i z+iz+1z+1 F)/ - i i 气+1= iR( vz+.xp/+1)运用武(525)和(5.Z7)依次算出各连杆的速度和角速度。由于基坐标系 而定不动，因其余各连杆的速度和角速度为二X = i()i r LoJ39)01J 0L旷II0卩 o i loJ(乩 40)1 0 0亠厶 $2 0 乙厶$：1J10 1 01/1 %屍+10 lx1 计L、占3寸仏（必T-久）Lo 0 1_ 0 d宀仇”：-oJ _ 0 因而为了求出相对于基坐标

16、系冷表示的。和计算旋转变换 心 一 50；R =毬店込=严5 0卜.00 13转换到基坐标系下表示必）i。3R Robof Examplerji Cl+ ri OIIi oIo1+1 HZS1V rjeiBL oorcreo1 o1o19II1 o1oii+L o1oi oi1 oor-Ho90 +JFL00(SE 0 丄)UOHeboEdoJd A=oojp 自 nODuvLinear VelocityPropagation (110 io 3)II3 oX o 亠+ oII11o o h*1s1-o1 cs1Ao J/11oo1 1 o1X11oo1 o1+11 、oo1 o .1yII1

17、1oO1 oJrrm$3c30s30 n520!L2 0 1c300 01JeJL0-ZIG0严L.0Z2.s300 Lie.Z =Z2c301-Z2c201-Z161IJ-J(-Zl-Z2c2)9；c3召+2= s3010o,s239； _L3L2.s392申临乂+社=010c23eX0十L2c3B2001e；十 83 JJ0(-Zl-Z2c2)01L2s3Q2(2q3 + 3)0；+厶 30；(-Zl-Z2r2-Base frame representation of*%Jacobian of 3R RobotL2s3O1(厶 2q3 + Z3)&+03& (厶 1 厶 2c2厶 3c23

18、)q s238c23&4 + &ccsCl77+CQCl377o oIIII寸寸IIIIIIX J z qX 誉IIr寸ii寸Transformation of Jacobian Reference5X5j（e）ebR月bR$RB % _0 0 0sR ooo0 0 00 0 o-0 0 0 级0 0 05j（e）ex=j（e）eJacobian of 3R Robof in Base Frame2C23 CIS 2 35.1C23 5.1523 s23 c23.0 0 0.0 0 00 0 01.1oOIILOo-1 o1L Ooo1OooOoo1o1oo1O ooJclc23 C1S23

19、slc23 S1S23 s23 c23Z2s3Z2C3+Z30000L3000例52如图5-2所示，令3/是水平向左作用在机械手的末端执 行器（即坐标系3原点上）的外力，那么各关节力的表达式可 反向递推如下：o o O 1 o O 1 O clo O 矶51O O 一=TO 1JM-O o 1 o O 1 O- 包o O 矽0 OO 1 o O 1 O O 1 o O 1 o o O1 =i+；R&%+i +Q+i x 咒(T01.77/,-0-77兀=丨,0_gfr + Cf,，- 0 - 0 -i0 z/1ro-%=0+ Ai x /i 0丄 h + hsfy +写成矩阵的形式:： r;

20、+厶门无4 JL/J仁升人+ (Z2 +T2 = I訂汕写成矩阵的形式:于是得到在討中表示的力雅可比为将旋转矩阵衣電右乘vr(),得到在0表示的力雅可比叮丁（g） =T（g”RT =IK j + /2G2/2G2例5.3为了简单起见、 中在连杆末端, 力学参数为假定平面2R机械手的两个连杆的质量集 分别为和m?。机械手的运动参数和动%=牍-G /i = 6 2f2 =0.斥=Q=巧=adie = o.Vo =连杆1的正向递推一 0 一如=0烷1 =0、0 匕=旺吃=0臨冷r叮L+0+10JQL o_皿/診：+血靑$1 呗佚 +mlegCj 0舛2 =e20七S 00 0 1 T 年 + g$i

21、 畅 + gCl- 0 .?1恥2 哈2 +阿2耳紅2 + “昨2 +阿20 1论2 = ?2（歹1 + 2）01+一俎（& + 2）?0000”十 ”20044til“+饥连杆2的正向递推厶毎 $2 M含2 + Ss2+ /證问+人呼七+ 12 10皿2人恥2 一耕2%年匸2 +也2跖2 一皿2b（l + 2卩 mlhc2 + 也 2人儈 2 + m2&cM + 加 2勺（% + 2）0厶反向递推o.o_ m2iil2c2l +也2*/丹% +胡2滋勺2 +也2呂 +朗+皿2 缶2 代 + 2c12 + 叫】2 + 2)0?2 一先0 T附2治2玄_利2Z2%十叫的12 _ rniWl十毎)

22、2A = s2 C2 0_历詁1年十宙庐1 0Q-. 0m占加I +也2滋2 +叫此輕十禺）.0（ 0 ）_加占毎+吋1阿J2片百-叫占匕庇+ 2尸+ m21討2$12 +fn2tll2c2(i + 62)十 mvgc2c2 .最后，将Z轴分量列岀，即得到两关节力矩Tj =叫*（0十禺）+也如旳（2%+2）+（叫& -朋2*2七电 一2加2心心22 + m2cH +（加1 + 叫1旳匕2 =也十 m2lli2s2 + m2l2Sc2 + 叫弓（ +%）上式即是关节驱动力矩作为关节位移、速度 剂加速度的显函数表达式。即为平面2A机械手动力学方程的封闭形式。F or ward and Inverse Dynamics Forward Dynamics Given: Joint torques Find: Joint/Cartesian motion Solving diffe