机器人学第6章动力学

1、第六章第六章 动力学动力学 Chapter Dynamics 6.1 引言引言 ( Introduction ) ii i q L q L dt d F (6.2) 6.2 拉格朗日力学拉格朗日力学 一个简例一个简例 ( Lagrangian Mechanics A Simple Example ) i q m2 d1 d2 m1 x y 2 1 图6.1 两连杆的机械手 2 2 1 mvK )( 1111 Cosgdmp (6.4) 2 1 2 111 2 1 dmK(6.3) )()( 212112 SindSindx (6.5) )()( 212112 CosdCosdy(6.6) 6.

2、 2. 1 动能和势能动能和势能 ( The Kinetic and Potential Energy ) 速度平方的值为 )()( 212121112 SindSindy (6.8) )()( 212121112 CosdCosdx (6.7) )2( 2 221 2 1 2 1 2 1 2 1 2 2 ddV )()(2 21 2 121121 SinSindd )(2)2( 21 2 1221 2 221 2 1 2 1 2 1 2 1 Cosdddd )()(2 21 2 121121 CosCosdd (6.9) )2( 2 1 2 1 2 221 2 1 2 22 2 1 2 12

3、2 dmdmK )( 21 2 12212 Cosddm (6.10) 质量的高度由式(6.6)表示，从而势能就是 )()( 21221122 CosgdmCosgdmp (6.11) )2( 2 1 )( 2 1 2 221 2 1 2 22 2 1 2 121 dmdmmL )( 21 2 12212 Cosddm )()()( 21221121 CosgdmCosgdmm(6.12) 6. 2. 2 拉格朗日算子拉格朗日算子 ( The Lagrangian ) 2 2 2 22 2 1 2 22 2 1 2 121 1 )( dmdmdmm L 2221212212 )()(2 Cos

4、ddmCosddm（6.13） （6.14） 12212 2 22 2 121 1 )(2)( Cosddmdmdmm L dt d 22212 2 22 )( Cosddmdm 2 22212212212 )()(2 SinddmSinddm )()()( 21221121 1 SingdmSingdmm L （6.15） 6. 2. 3 动力学方程动力学方程 ( The Dynamics Equations ) 12212 2 22 2 1211 )(2)( CosddmdmdmmT 22212 2 22 )( Cosddmdm 2 22212212212 )()(2 SinddmSind

5、dm )()()( 21221121 SingdmSingdmm（6.16） 122122 2 221 2 22 2 )( Cosddmdmdm L (6.17) 122122 2 221 2 22 2 )( Cosddmdmdm L dt d 212212 )( Sinddm(6.18) )()( 212221 2 12212 2 SingdmSinddm L (6.19) 用拉格朗日算子对 求偏微分，进而得到关节2的力矩方程 22 和 2 2 2212212 2 222 )( dmCosddmdmT 2 12212 212212)()sin(2 Sinddmddm )( 2122 Sing

6、dm （6.20） 将式(6.16)和(6.20）重写为如下形式 （6.21） 11212121112 2 2122 2 11112121111 DDDDDDDT 21222121212 2 2222 2 12112221122 DDDDDDDT （6.22） iii D jijiij DD 2 jijj D jkijkkjijk DD i q ii i q L q L dt d F 6.4 牛顿-欧拉方程 为杆件为杆件i-1对杆对杆i的作用力；的作用力； 为杆件为杆件i1对杆对杆i的作用力；的作用力； 为杆件为杆件i-1对杆对杆i的作用力矩；的作用力矩； 为杆件为杆件i1对杆对杆i的作用的作

7、用 力矩；力矩； 为杆为杆i的质心的质心; Vci杆质心的平移速度杆质心的平移速度 Wi杆的角速度向量杆的角速度向量 根据力、力矩平衡原理有：根据力、力矩平衡原理有： )616n)(1,2,.,(i0)(IIfpfpNN )606(n)1,2,.,(i0vmgmff iiiiii 1i ci 1i 1 i i ci i 1i i i 1i ciii1i i i 1i 上式：牛顿方程上式：牛顿方程 下式：欧拉方程下式：欧拉方程 Ii为杆为杆i绕其质心的惯性张量：绕其质心的惯性张量： dv)x(x)y(y dv)y)(yz(z dv)z)(zx(x dv)y)(yz(z dv)x(x)z(z dv

8、)y)(yx(x dv)z)(zx(x dv)y)(yx(x dv)z(z)y(y 2 ci 2 ci cici cici cici 2 ci 2 ci cici cici cici 2 ci 2 ci i I 由式（由式（660）和（）和（661）表示的牛顿欧拉方程没有明显地）表示的牛顿欧拉方程没有明显地 表示出关节位移与关节力间的关系，可以通过递推关系建立杆表示出关节位移与关节力间的关系，可以通过递推关系建立杆 件的递归方程。件的递归方程。 )n,.,2 , 1i (GqqhqM n 1j n 1k ikjijk n 1j jiji 惯性力项哥氏力和离心力重力项 【例【例31】解出左图所示

9、机械臂】解出左图所示机械臂 的的，和用关，和用关 节变量节变量 ， 和关节力矩和关节力矩 ， 表示的封闭动态方程。表示的封闭动态方程。 1 2 1 2 解：杆件解：杆件1的牛顿欧拉方程可的牛顿欧拉方程可 以表示为：以表示为： )656(0IfpN )646(0vmgmf 222 1 2c 1 2 1 2c222 1 )636(0IfpfpNN )626(0vmgmff 111 0 1c 0 2 1 1c 1 2 1 1 0 1c112 1 1 0 杆件杆件2的牛顿欧拉方程可以表示为：的牛顿欧拉方程可以表示为： 关节力矩和耦合力矩相等，有：关节力矩和耦合力矩相等，有： )666(N ii 1i

10、将式（将式（666）代入式（）代入式（665），消去），消去1f2，可得可得 )676(0Igmpvmp 2222c 1 2c22c 1 2 同样，消去同样，消去0f1，得到得到 )686(0Igmpgmpgmpvmpvmp 1121 0 11 0 11c 1 2c21c 0 1c11c 0 21 11 0 0 1001 0 0 11 11 11 11 1 1 1 1 1 0 1 0 sl cl sl cl c s s c Ap 11 0 0 1001 0 0 11 11 11 11 1 1 1 1 1 0 1 0 sl cl sl cl c s s c Ap c c c c cc 11 0

11、0 1001 0 0 22 22 22 22 2 2 2 2 2 1 2 1 sl cl sl cl c s s c Ap c c c c cc 在基坐标系中，杆在基坐标系中，杆1的重心为：的重心为： 1 11 11 1 cl sl J c c c 在基坐标系中，杆在基坐标系中，杆2的重心为：的重心为： 1 12211 12211 clcl slsl c c 杆杆1质心线速度、角速度为：质心线速度、角速度为： 1 111c 11 1 c 111c 11c 11c1c cl sl 1 cl sl Jr 杆杆2质心线速度、角速度为：质心线速度、角速度为： 21 2122112211 2122112

12、211 2 1 122 122 12211 12211 22 )( )( 11 clclcl slslsl cl sl clcl slsl Jr cc cc c c c c cc 2c J 从而从而 11 111 111 1 , cl sl v c c c 212 2122112211 2122112211 2 , )( )( clclcl slslsl v cc cc c 对上式进行时间微分，并将相关参数代入（对上式进行时间微分，并将相关参数代入（667）和（）和（668）式）式 )696(Gh2hMM Gh2hMM 221 2 2 2221212 121 2 2 2121111 式中：式中

13、： 12222 1112221111 2212 2 2 2 222 2 2 2 221212 2221 2 2 1 2 21 2 2 211 )( 2 2 cglmG clclgmgclmG sl lmh IlmM Ilmcl lmM Icl lllmIlmM c cc c c c c c cc 6.5 机器人的逆动力学 )716n)(1,2,.,(i)(IIfpfpNN )706(n)1,2,.,(iamgmff iiiiii 1i ci 1i 1 i i ci i 1i i i 1i ciii1i i i 1i 6.5.2 动坐标系 s dt ds tsins tsinAOACAB f 1i i i 0 1i 0 ppp f 1i i f i 0 f 1i 0 dt pd dt pd dt pd 1i 0p i 0p 1i i p 1i i r 1i i f 1i i p dt pd dt pd rf dt d dt d 1i i iir 1i i i1i i ir 2 1i i2 i1i f 1i i i1i i f i fr 1i i i1i 1i i r 1i i i1i p dt pd 2p dt pd aa dt pd p dt d dt pd dt d dt dv dt dv p dt pd vv 1i 1i i1i i1i i1i bq i1i bq i1ii