机器人控制技术动力学

第 六 章 动 力 学Chapter Dynamics 6.1 引 言 ( Introduction ) iii qLqLdtdF (6.2) 6.2 拉 格 朗 日 力 学 一 个 简 例( Lagrangian Mechanics A Simple Example ) iq m2d1 d2m1 xy 21图 6.1 两 连 杆 的 机 械 手 221mvK )( 1111 Cosgdmp (6.4)212111 21 dmK (6.3) )()( 212112 SindSindx (6.5)()( 212112 CosdCosdy (6.6) 6. 2. 1 动 能 和 势 能 ( The Kinetic and Potential Energy ) 速 度 平 方 的 值 为 )()( 212121112 SindSindy (6.8)()( 212121112 CosdCosdx (6.7)2( 2222212121212122 ddV )()(2 212121121 SinSindd )(2)2( 212122122222121212121 Cosdddd )()(2 212121121 CosCosdd (6.9) )2(2121 22222121222212122 dmdmK )( 21212212 Cosddm (6.10)质 量 的 高 度 由 式 (6.6)表 示 ， 从 而 势 能 就 是 )()( 21221122 CosgdmCosgdmp (6.11) )2(21)(21 22222121222212121 dmdmmL )( 21212212 Cosddm )()()( 21221121 CosgdmCosgdmm (6.12)6. 2. 2 拉 格 朗 日 算 子 ( The Lagrangian ) 22222212222121211 )( dmdmdmmL 2221212212 )()(2 CosddmCosddm （ 6.13）（ 6.14）1221222221211 )(2)( CosddmdmdmmLdtd 22212222 )( Cosddmdm 222212212212 )()(2 SinddmSinddm )()()( 212211211 SingdmSingdmmL （ 6.15） 6. 2. 3 动 力 学 方 程 ( The Dynamics Equations ) 1221222221211 )(2)( CosddmdmdmmT 22212222 )( Cosddmdm 222212212212 )()(2 SinddmSinddm )()()( 21221121 SingdmSingdmm （ 6.16） 12212222212222 )( CosddmdmdmL (6.17)12212222212222 )( CosddmdmdmLdtd 212212 )( Sinddm (6.18) )()( 2122212122122 SingdmSinddmL (6.19) 用 拉 格 朗 日 算 子 对 求 偏 微 分 ， 进 而 得 到 关 节 2的 力 矩 方 程 22 和 2222122122222 )( dmCosddmdmT 212212 )( Sinddm )( 2122 Singdm （ 6.20）将 式 (6.16)和 (6.20） 重 写 为 如 下 形 式 （ 6.21）1121212111222122211112121111 DDDDDDDT 2122212121222222212112221122 DDDDDDDT （ 6.22） iiiD jijiij DD 2jijjD jkijkkjijk DD 02 0 2221122 DDT 122122 DD于 是代 入 方 程 (6.21)有 122122111 DDDT (6.36) 1111 DT 1122 DT (6.35)(6.34) 021 2 表 6.1 m1 = 2 , m2= 1 , d1 = 1 , d2 = 1D11 D12 D22 IL If Cos 20 1 6 2 1 6 290 0 4 1 1 4 3180 -1 2 0 1 2 2 270 0 4 1 1 4 32 表 6.2 m1 = 2 , m2=4 , d1 = 1 , d2 = 1D11 D12 D22 IL If Cos 20 1 18 8 4 18 290 0 10 4 4 10 6180 -1 2 0 4 2 2270 0 10 4 4 10 6表 6.3 m1 = 2 , m2=100 , d1 = 1 , d2 = 1D11 D12 D22 IL If Cos 20 1 402 200 100 402 290 0 202 100 100 202 102180 -1 2 0 100 2 2270 0 202 100 100 202 10222 6. 3 机 械 手 动 力 学 方 程 ( The Manipulator Dynamics Equation )iq iii qLqLdtdF 于 是 ， 它 的 速 度 就 是速 度 的 平 方 rrdtdr 2)(或 者 用 矩 阵 形 式 表 为rTr ii （ 6.37）)()( 2 TrrTracedtdr （ 6.39）rqq Tdtdr iij jji )( 1 （ 6.38）6. 3. 1 机 械 手 上 一 点 的 速 度 ( The Velocity of a Point on the Manipulator )z0 zi yx ir yixiTi ik Tikkiiij jji rqqTrqqTTracedtdr 112 ij ik kjkTiTiiji qqqTrrqTTrace 1 1 （ 6.40） 于 是 ， 连 杆 i的 动 能 就 是 ij ik kjkTilink Tiijilink ii qqqTdmrrqTTracedKK ii 1 1 )(21 (6.42)dmqqqTrrqTTracedK ij ik kjkTiTiijii 1 121 ij ik kjkTiTiiji qqqTrrdmqTTrace 1 1 )(21 (6.41)6. 3. 2 动 能 ( The Kinetic Energy ) i iiii iiii iiii iiiilink linklink ilink ilink i link ilink ilink iilink ii link ilink iilink ilink ii link ilink iilink iilink iTiii dmzdmydmxdm zdmdmzzdmyzdmx ydmzdmydmyydmx xdmzdmxydmxdmxdmrrJ 222 （ 6.43） dmzyIxx )( 22 dmzxIyy )( 22 dmxyIzz )( 22 xydmIxy xzdmIxz yzdmIyz xdmxm ydmym zdmzm dmxydmzxdmzydmz )(21)(21)(21 2222222 2/)( zzyyxx III (6.46) dmxydmzxdmzydmx )(21)(21)(21 2222222 2/)( zzyyxx III (6.44) dmxydmzxdmzydmy )(21)(21)(21 2222222 2/)( zzyyxx III (6.45) iiiiiii iiizziyyixxiyzizz iiiyzizziyyixxizy iiixzizyizziyyixxi mzmymxm zmIIIII ymIIIII xmIIIIIJ 222 （ 6.47）机 械 手 的 总 动 能 就 是 ij ik kjkTiiiii i qqqTJqTTraceKK j1 16161 21 （ 6.48） 221 iaactuator qIK ii 把 Trace运 算 和 求 和 运 算 相 互 交 换 一 下 ， 再 加 上 传 动 机 构 的 动 能 部 分 ，最 后 得 到 机 械 手 的 总 动 能 为在 棱 柱 形 滑 动 关 节 的 情 况 下 ， Ia成 为 一 个 等 价 质 量 。 61 1 61 21 21)(21 i ij i iakTiiiik qIqqqTJqTTraceK ikjj (6.49) 例 如 ， 在 重 力 场 中 ， g = 0i + 0j 32.2k， = 10i + 20j + 30k ， 位 于 r处 的质 量 m就 有 势 能 966 nm。 rrmgp (6.51)6. 3. 3 势 能 ( The Potential Energy )r ir其 中从 而 ， 机 械 手 的 总 势 能 就 是 iiiTii rTgmP (6.52) 61i iiiTi rTgmP (6.54) 0zyxgggg (6.53) 应 用 欧 拉 拉 格 朗 日 方 程 我 们 就 可 求 得 动 力 学 方 程 。 61 1 6161 21 21)(21 i ij i iiiTii iakTiiiik rTgmqIqqqTJqTTraceL ikjj (6.55)iii qLqLdtdF )( (6.56) 6. 3. 4 拉 格 朗 日 算 子 ( The Lagrangian ) 把 上 式 第 二 项 中 的 脚 标 j 变 为 k ， 把 第 一 项 中 Trace运 算 换 成 pajpTiijiijikkTiipiikip qIqqTJqTTraceqqTJqTTraceqL p )(21)(21 161161 (6.57)()()( pTiikiTkTiipikTiipi qTJqTTraceqTJqTTraceqTJqTTrace 我 们 就 得 到 pak pTiikiikip qIqqTJqTTraceqL p )(161 (6.58) 6. 3. 5 动 力 学 方 程 ( The Dynamics Equations ) 0 piqT现 在 求 式 (5.59)对 于 时 间 t 的 微 分 pakpTiijii ikp qIqqTJqTTraceqL p )(61 1 (6.59) pakpTiikipi ikp qIqqTJqTTraceqLdtd p )(6 1 mkpTiimk ipi ik im qqqTJqq TTrace )( 26 1 1 (6.60)mkkTiimp ipi ik im qqqTJqq TTrace )( 26 1 1 把 式 (6.61)第 二 项 中 求 和 运 算 的 脚 标 j 换 成 k ， 再 把 第 二 项 与 第 一 项合 并 ， 就 得 到 6 1 21 )(21 pi ij kjkTiipiikp qqqTJqq TTraceqL j 6 1 621 )(21 pi ij pi iipiTikjjTiipk iik rqTgmqqqTJqq TTrace (6.61) 6 1 21 )(pi ij kjkTiijp iikp qqqTJqq TTraceqL 6pi iipiTi rqTgm (6.62) 最 后 ， 把 求 和 运 算 的 脚 标 p 和 i 换 成 i 和 j ， 就 得 到 动 力 学 方 程 6 1 )(pi pakpTiikiikpp qIqqTJqTTraceqLqLdtd p 6 1 621 )(pi ik pi iipiTimkpTiimk iim rqTgmqqqTJqq TTrace (6.63) 61 1 )(j iakiTjjkjjki qIqqTJqTTraceF i 621 )( ij jjijTjmkiTjjmk jjm rqTgmqqqTJqq TTrace 6 1pj jk (6.64) 其 中 6 1 6 16 1 j k ikjijkiaj jiji DqqDqIqDF i (6.65) 6 ip ppipTpi rqTgmD 6 ),max( )(jip iTppjpij qTJqTTraceD (6.66) 6 ),max( 2 )(kjip iTppkj pijk qTJqq TTraceD (6.67)(6.68)