基于凯恩方程的机器人动力学递推算法

D0I:10.13374/i.issn1001-053x.1988.02.030 北京钢铁学院学报 第10卷第2期 Journal of Beijing University Vol,10 No.2 1988年4月 of Iron and Steel Technology Apr.1988 基于凯恩方程的机器人动力学递推算法 马香峰 徐向荣 李德高 (机器人研究所) 摘 要 本文在凯恩动力学方程的基础上，提出了一种新的机器人动力学递推算法。这种方 法直观简练，可以不经拆链就能较方便地解决带有局部闭链结构的操作手动力学何题， 尤其适用于计算机编程计算，本文用这种算法对2自由度4杆机构进行了计算，列出了它 的动力学公式。 关键词：机器人，凯题方程，动力学算法 A Recursive Algorithm of Robot Dynamics Based on the Kane's Dynamical Equation Ma Xiangfeng Xu Xiangrong Li Degao Abstract This paper presents a new recursive algorithm of robot dynamics based on the Kane's dynamical equation.The algorithm is simple and illustrative,and can be used for solving dynamic problem of robots con- taining closed-chain without cutting the closed-chain open.Also,it is especial available to cad of robot because it can be easily realized on 1987一02一12收稿 193

第 卷 第 期 年 月 北 京 钢 铁 学 院 学 报 。 基于凯恩方程的机器人动力学递推算法 马香峰 徐 向荣 李德高 机器人研究所 摘 要 本文在凯恩动力学方程的基础上 ， 提出了一种新的机器人动力学递推算法 。 这种方 法直观简练 ， 可以 不经拆链就能较方便地解决带有局部闭链结构的操作手动力学问题 ， 尤其适用于计算机编程计算 本文用这种算法对 自由度 杆机构进行了计算 ， 列 出了它 的动力学 公式 关键词 机器人 ， 凯 恩方程 ， 动力学算法 尸 ，夕 夕 “ ，夕 夕 爪 ， 犷 ， 一 一 ， 一 一 收稿 DOI ：10．13374／j ．issn1001－053x．1988．02．030

computer.In the end of the paper,an example of computation of dyna- mics of robot with 2-DOF and one closed-chain is given to demonstra- te computing stages and methods of establishing dynamic equation of robot using the new algorithm. Key words:robot,Kane's equation,algorithm of robot dynamics 前 言 目前在进行机器人操作手（以下简称操作手）的动力学计算中，最流行的是拉格朗 日(Lagrange)方法C1和牛顿一欧拉(Ne wton-Enler)方法C2),其他还有高斯 (Gauss)最小约束原理法(3)，阿贝尔(Appell)方程法C4),凯恩(Kane)方程法() 等。为了减少运算量，提高运算速度，继递推的牛顿一欧拉法之后，又出现了递推的 拉格朗日方法〔6)。也有一些作者运用上述方法研究了带有局部闭链结构的操作手动力学 计算问题〔)。我们为了求得自行研制的含有局部闭链结构的操作手动力学方程，在凯，恩 动力学方程的基础上，与〔5)不同，导出了一种递推的动力学算法，它直观简练，从 而加快了计算迅度，还可不经拆链就能较方便地解决带有局部闭链结构的操作手动力学 计算问题。 1 速度、加速度、偏速度的递推算法 设有如图1所示之开链操作手，具 - 有n个旋转关节(J1,J2…Jm), dn-1 n个杆连构件(L1,L2…Ln),各 n-l 构件的杆长，为11,12…1n,质心为C1, 12 C2…Cm,第号杆的质心C:到关节J: d, 的距离为d:。当以各关节之转角9：为广 义坐标，各转动速度之大小9，为广义速 率时，其速度、加速度的递推算法如下： 图1开链操作手 Fig.1 An open chain robot 角速度：@=R-⊙+0： 角加速度：o=R-o+R-:o×0:R+0: 关节速度：=R:-,(o×1:+0{) 199

。 ， 一 一 皿 。 了 ， ， ， ‘ ‘ 月 吕 目前在进行机器人操作手 以下简称操作 手 的动 力学计算 中 ， 最流行 的 是拉格 朗 日 方法 〔 〕和牛顿一 欧拉 一 方法 〕 ， 其 他 还 有 高 斯 最小约 束原理法 〔 〕 ， 阿 贝尔 方程法 〔 〕 ， 凯 恩 方 程 法〔 〕 等 。 为 了减少运算量 ， 提高运算速 度 ， 继递推 的牛顿一 欧拉法 之后 ， 又 出现 了 递 推 的 拉格朗 日方法〔 〕 。 也有一些作者运用 上述 方法研究 了带有局 部闭链结构的操作 手动力 学 计算 问题〔 〕 。 我们为 了求得 自行研制的含有局 部闭链结构的操作手动 力 学方程 ， 在凯恩 动 力学方程的基础上 ， 与 〔 〕 不 同 ， 导 出了一种递推的动 力 学算法 ， 它直观简练 ， 从 而加快 了计算迅度 ， 还可 不经 拆链就 能较 方便地解 决带有局部 闭链结构的操作 手动力 学 计算 问题 。 速度 、 加速度 、 偏速度 的递推算法 设有如 图 所示 之开链操作 手 ， 具 有 个 旋转关节 ， · “ … 。 ， 个 杆 连构件 ，， “ 一 。 ， 各 构 件的杆 长 ， 为 ， … … 。 ，质心 为 ， … … 。 ， 第泣号杆 的质心 到 关节 的距离为 。 当以各关 节之转角 为广 义坐标 ， 各转动速度 之大小 为广义速 率时 ， 其速度 、 加速度的递 推 算法如下 图 开链操作手 角速 度 。 卜 侧 一 。 至十 欲 一 二委 秃 十 川二 角加速度 。 二 一 一︸卜 、富 关 节速度 此 刀

质心速度：del=i+o×d 关节加速度：=R-1(=+⊙}×i=:+⊙×（⊙：xi=:)) 质心加速度：c=+oxd+o×(oxd） 偏速度的递推算法是： RI-1@l1ij ji 式中：上角标表示坐标系序号，下角标表示连杆序号；R-:一表示坐标系一1到坐标 系中的旋转变换，是3×3方阵，亦即姿势矩阵；一表示第杆旋转角速度单位向 量，亦即坐标系的：轴单位向量，通常取k!二〔001)T。 2应用凯恩刚体动力学方程解算操作手关节转矩 取末杆L。为研究对象，在无外力作用，并在质 心C处附加-9加速度来考虑其重力影响时，相对于 速率9的计算简图如图2所示。 应用凯恩刚体动力学方程： F,+F*;=0 (1) M 式中：F,-R。·立1本M·o1 图2末杆计算管图 Fig.2 A sketch of the end link F-C名，m:d:e+(r@+axf@).@〕 200

质心速度 。 卜 此 。 武 关 节加速度 心 一 川 二 。 二 又 二 。 二 。 二圣 量二圣 势 质心加速度 二 二 心 十 。 义 心 偏速 度 的递推算法是 ， 龙，二 、 偏角速 度 。 歹 一 似 李咬 招 截端 ‘ ，、 六 二卜 六 二 。 ， 关 节 偏速 度 “ ’ 炭 。 ， 寸 、 、 畜吮 才 ‘ 万 曰 俪 宋 蔓 口 是乞 万气 夭 、 义 了 户 式 中 上 角标表示坐标系 序 号 下角 标表示连杆序 号 一 一表示坐 标系 一 到 坐 标 系，中的 旋转变 换 ， 是 义 方阵 ， 亦 即 姿势矩 阵 蕊 一表 示第 杆旋转角速 度 单 位 向 量 ， 亦 即 坐标 系 的 二 轴 单位 向量 ， 通常取 二 〔 〕 。 应 用 凯恩 刚体动力学方程解算操作手 关节转矩 取末杆 为研究对 象 ， 在无 外力 作 用 ， 并在质 心 。 处附加 一 加速 度 来考虑其重 力影响 时 ， 相 对 于 速 率 。 的 计算 简图如 图 所示 。 应用 凯 恩刚体动 力学方程 眷 式 中 二 · 。 汀， 艺， 言， 图 末杆计算简图 五 、 一洛 洛 、 。 。 。 ， ’ 。 干 后扁 目 阴 。卜叉 二口 厂叹

对末杆可以得出： Fcin+(M+F:×d:)oin=maUenvenin+(1.回+o8 x In@R)·①ngn 式中：mn一末杆Ln的质量；In一末杆Ln的惯量张量(3×3) 利用前节的偏速度公式，因为j=n,故 Fa.venia=Fa·(xd:) (F:×d)·o:in=(F:×d:).=-F(xd) 故得： Mi-ORin=maveivein+NaORio (2) 式中： N:=In@+o×In: 因为O:m=,所以关节转矩rn的计算公式是： Tia=Mn.kn=mavchvenon+Nn-oBin (3) 取n一1到n杆的两杆系统为研究对象，相对0。-1的计算简图如图3所示： In Ln-1 Ja RaI MA 人时 dn-1 人叶 删 图3二杆计算简图 Fig.3 A sketch of the laat two links 应用凯恩刚体动力学方程，得： F,n-;=F=ie二am-4+（F=×d=)o-n-:+Ma-o=an-4 -RB-IMB.B=1i8-,+MBBin-1 利用上节的偏速度公式将上式展开，并考虑到： 201

对末杆可 以得 出 ‘ 盏 口 盆 · 刀 ‘ 盈歹 。 十 乃 盆 旦 及盆 访盆歹 切 。 口 。 。 一 盆 又 一止、 。 式 中 一末杆 。 的 质量 一末 杆 。 的 惯量 张量 利 用前节 的偏速 度公式 ， 因为 二 。 ， 故 盆 盆 。 一刀 一一 凡一 一 一 一几 一粼 一 君 又 刀 一二盆 〕 又 盆 全又 故 得 夕 一 石、 二、 、 〕 阴 。 口 。 只 口 。 。 乍 。 。 一 。 只’ 。 式 中 歹 甲 二、 二、 石、 一 。 盆 。 盆丫 。 盆 、 、 因 为 黑 。 二 且 ， 所 以关节 转矩 『。 的 计算公式 是 、 龙 。 。 盆 盆犷 。 公 。 又扩 下 粼 护 二 盈 。 取 一 到 杆的两杆系统 为研究对 象 ， 相对 一 的 计算简 图如 图 所 示 图 二杆计算简 图 应 用凯恩 刚体动力 学方程 ， 得 。 ‘ 王 盆二 一 ‘ 器 全 盆二至 、 · 口 盆二 。 。 一 十 一 一 一 口 一 盆二乏荡 一 口 一 盆 一 盆 、 一 一 口 一 卜 只日 一 利用上节 的偏速 度公式将上式 展开 ， 并考虑到

F-⑦=g。-4=0 Fai.(@B-ion-xdB)=-(Fn=ixd1)ni Ra-MB-0n-iin-1=MB-RB-16-1in- 可以求得： Fin-1=MHio1in-1=MB-i-kRi 而 Fin-1=-(mnveRvehin-1+Nmomia-1 +mn-1de=ve=a-,+N二oi。-:) 于是关节转矩tn-:是： xia-1=MA二a8二=Fin-1=-Fi。-4 =mnvcR-Venin-1+Na-@Bea-1 +mn-ivcB-ivch=iin-1+NR.on-iin-1 (4) 式中各符号的意义同前。 由式(4)可以看出：关节Jm处的主动力矩Mn对关节Jn-:处的转矩x;。-:不产生 影响。 仿上面的推导，即可得出一般的计算关节转矩的公式： r,=含m:.i+8.@ (5) 当在杆L:上作用有主动外力和外力矩时，应向质心简化求出合力F:和合力矩 i,于是公式(5)变为： ti=(m vv+N-Fv-M) (6) 下面给出一组进行符号推导时，求解关节转矩的计算公式，它既可用于开键操作 手，也可用于具有局部闭链结构的操作手。对于后者，必须令主动关节变量为广义坐标。 o1=R-1o+0:k (a) ①j=①( 0i=1) (b) 05=0 202

声只二受 。 盆二 。 。 声 · 。 盆二 ， 。 一 又 盆二 二 盘二 义 盆二至 。 。 含 扩 。 一 。 。 盆二全扩 一 盆 。 尝 二二受扩 一一 可 以求得 犷 。 一 只二 。 。 盈二 二 一 二 盆二圣 · 粼 ‘ 、 一、 一 切 口 。 盆 一 盆扩 君 一 。 盆歹 。 十 一 。 只二卜 牡 了 一 于是 关 节转矩’ 。 一 是 只二受 · 。 盆二受’ 。 一 子 。 一 盈二 减 扩 一 一 言 。 一 阴 口 盆 一 。 只子 。 、 盆 一 。 盈 ， 。 阴 一止‘ 。 沙 。 伙 圣’ 。 一 十 翌 一 ‘ 、 旧 一 口 一 式 中各符号的意义 同前 。 由式 可 以看 出 关 节 处的主动 力 矩 。 对关 节 一 处的 转矩’ 。 一 不 产 生 影响 。 仿上面的推导 ， 即 可得 出一般的计算关 节 转矩的 公 式 ， 弓 ， ‘ · ‘ · “ ， 十 月 · 。 。 ， 当在杆 上作用 有主 动 外力和 外力矩 时 ， 应 向质心简化 求 出 合 力 。 。 ， 于是 公 式 变 为 八 叉 。 。 卜 。 。 ， 十 · 。 ， 一 户 ‘ · 。 ‘ 二 一 含 和合 力 矩 · 。 二， 下面给 出一 组进行符号推导时 ， 求解关节 转矩的计算公式 ， 它既 可用 于开 链 操 作 手 ， 也可用 于具 有局部 闭链结构的操作手 。 对于后 者 ， 必须令主动关 节变量 为广义坐标 。 。 卜 一 ， 二兰 乞 。 了

wi=R-:(o×1二+v) (c) ve=v+o×d:: (d) ,=1) (e) 0了=0 0=R-1o=}+R-1oe×0:k+6:k (f) =R-1(!+o=:×1:+o}×(o:×1!)) (g) ve=v+o×d;+o×(o×d) (h) N=I;o+o×I:0 (i) M1fi=m:ve·iii+N·oi-Fe·etj-M·o1 (j) T615 ∑M:ei (k) 式中各符号的意义同前，除r,0,6:为标量；R-1,1:为3×3方阵，其他皆 是1×8列阵，是向量。j表示非j,0于：0，即非j号关节速率等于零。 在使用上组公式时，标架应按图4 L 所示的规则设立。 为了计及重力作用，可设基座具有 一g加速度 如果用计算机作动力学数值计算， 则公式组中的式(b)到(e)可以换用 前节中的偏角速度、关节偏速度和质 心偏速度三个递推公式，并令初始条件 为： 图4标架设立规则 Fig.4 The cartesian coordinate based on link Li 08=0 o81=08:1=0 -G 3 计算举例 图5是比较流行的带有单闭链五自由度关节型弧焊机器人（如MOTOMAN和AS EA)的闭链系统示意图。对于这一局部结构，可以看作是平面4杆两自由度操作手， 驱动器通过传动机构将动力矩分别作用在O,'和O:处，以产生角位移9，‘和81。今以该 简化操作手为例，求其动力学方程式，即计算关节转矩τ：，τ：′。 203

城 二 一 。 派 乏 城二 。 。 二 训 。 武 二 ， ，、 一 。 ， 、 ‘ 一 、 、 了 二 。 二 刀 一 ， 二圣 一 。 二 、 秃 牛 砚 以 一 、 。 口 二 。 。 二全 二圣十 。 二 。 拜 兰 二 十 。 “ 。 〕 又 口， 义一卜 。 。 又 、 。 、 ‘ 、 。 、 二 · ，乏 ‘ · 。 言， 一 · 二 二 一 卜 。 艺 。 屯一 式 中各符号的意义 同前 ， 除百 ， ， 是 、 列阵 ， 是 向量 。 表示非 ， 丁 二 在使 用上组公式 时 ， 标架应 按图 所 示的 规则设立 。 为 了计及 重 力作用 ， 可设基座具 有 一 加速度 如 果用计 算机作 动 力学 数值计算 ， 则公式 组 中的 式 到 可 以换 用 前 省中的偏 角速 度 、 关 节 偏 速 度 和 质 心偏速 度三个递推 公式 ， 并令 初始 条 件 为 为标量 一 ， ， 为 方 阵 ， 其他皆 ， 即非 号关 节速率等 于零 。 图 标架设立规则 ， 卜 忿二 ‘ 计 算举例 图 是 比较流行 的带 有单 闭链五 自由度关 节型弧焊机 器人 如 和 的 闭链系统示意 图 。 对 于这一局 部结 构 ， 可 以看 作是 平面 杆两 自由度操作手 ， 驱动 器通过传动 机构将动 力矩分别作 用 在 ， 产 和 处 ， 以产生 角位移 产 和 。 今 以该 简化操作手为例 ， 求 其动 力学方程式 ， 即计算关节 转矩 ， ‘ ‘

9C 图5闭链操作手计算简图 Fig.5 A sketch of a manipulatorwith a closed chain 3.1建立坐标系，求变换矩阵 现将闭链分作两路（右、左路），各杆分别命名为L,L2,L:',L:',L,(即 L:)。各关节坐标系的设立、结构参数和关节变量如图5所示。 由图5可得各坐标变换矩阵（略）。 对于右路(L,L:) -S12 -C12 0 c.1 RT=TT!= C:2 -S12 0 SI 0 0 1 0 0 0 0 1 其中： C:=cos01 Si=sine C:=cos02 S2=sin02 C12=cos(0,+0,)S1z=sin(0,+0:) 对于左路(L,',L',L,'=L:) LTS=TS TU -S31-C30-S31,+Cl,+S1l T/T'= C9-S30C31a,+S1,-C1 01 0 0 0 0 1 3.2计算主、从转角关系 设主动杆L1,L:'的转角为主动转角0，0：'，从动转角分别为，'，0，‘m0。 根据几何关系，有RT:=LT,即： 204

“ 。 户夕 了 己 公 ， 子一一丫 少夕。 ， 一 ’ ， 之 些 岁 ‘ 才厂 一 毛 一下 …』 、 恤 、 止了 护 尹。 ‘ ，一 。 。 。 厂 ， 拱 声犯 牡一 习 孔 图 闭链 操作手计算简图 建立坐标系 ， 求变换矩阵 现将 闭链分作两 路 右 、 左 路 ， 各杆分别命名 为 ， 产 ， 产 ， 产 即 。 各关 节坐标系的 设立 、 结 构参数和关节变量如图 所 示 。 由图 可得 各 坐标变换矩阵 略 。 对 于右路 ， 、… 呈二 ’ 一 ， 一 火 其 中 对 于左 路 工 了 ， ， 一一︷一 一，‘ ﹄ ， 二 ， 二 ，、 刁 、 呈 至 ， 一 工 ， ， ， ， ， ， 一 ， ， ， ， ， ， ， ， ， ， ， ， ， ， ， ， ， ， ， 一 ， ， ’ 、忆 电 ， 计 算主 、 设主 动 杆 ， 根据几何关 系 ， 从转 角关系 尹 的 转角为主 动 转角 ， 尹 从动 转角分别为 ‘ ， 产 。 有 呈 呈 ， 即 到

sin(0:+02)=sin(0,'+0z+0,) c0s(0,+02)=cos(0,十0.十0，) -1ssin(01,+62r+03)+12cos(0+62,) +11,sin01,=11c0s0, 13,cos(01,+02,+03,)+12sin(01,+02) -11,cos0,=1isin0 根据结构条件又有 0g,=0213,=1,12,=1 由此得到： 103=02=01,-0: 02,=01-0: 所以： 0g,=02=61,-61 02=01-01 3.3利用公式(a)~(k)求左二杆：M:1 为计及重力影响，取8=〔g00〕T 对于杆L,有： o=Ro8+01,=〔0001，〕T @91,=0( ,0,=1)〔001)T 01=0 0=o(0:=1)=〔0003r 0:'=0 v:=R:'(o8×18+8)=〔000]T e=+o×d1,=〔0d1,01,0〕T 91=,（ 6=1)〔0d,0) 01=0 i=.(日=1）=〔000) 01=0 ⊙=R8⊙8+Ro8×01,+01,=〔0001，)1 7=R'(8+®8×18+@8×(o8×18)) ,=Cg51'.9c,0〕T v,:=v:+o×d+o:×(o:×d) 205

产 产 ， ， ， ， 十 ， 十 ， 一 ， ， ， ， ， ， ， ，， ， ， ， ， ， ， ，， ， 一 ， ，， ， 根据结构 条件又 有 ，， 由此得到 ， ， 所 以 ， 二 ， ， 主 ， ， 一 一 ， ， 一 一 ，， 目八︸ 、广飞气了 ︺性 吸 利用 公式 求左二杆 言 、」卫 、砂 ︸ 为 计及重 力 影响 ， 取 川 二 〔 对 于杆 ， ， 有 。 孟。 ， 丁二 〔 ， ， 乡 ， 一 。 丁 ， ， 〔 〕 八口 孟， 八门︺口﹄ 〔 生 产 〔 产 〕 尸 孟 ‘ 。 盆 此 毗 〕 刀 二 。 。 二丫 ， ， ， ，， 〕 讨 毛忿 ， ， 夕 二 〔 。 ， 〕 ，， 川” ， 一一 〕 、， ，山 飞电 一 口 · 蛛 口 ，’ 。 ， 川 二 。 。 里二 者 ‘ 。 名 孟 ‘ 。 君 ，， 丁 ， ‘ 〔 川 丁 孟 。 。 义 。 〔 ， 一 〕 圣二 。 二又 二 。 口 十 尹口 。 一一

=〔9s1-d(01)2gc1+d,0,01,0)T N/=I1o+①7×I1,⊙=I12.01 Mia1=miivizevicol+Niz0ii =m1（9d1c,+di,01,)+11.01 M11=m1/ee1+N@/g1=0 对于杆L2有： o=〔006，+0，]t=〔00‘01〕r 011=0, @,1=〔001〕寸 v=〔1，c2,0, -11S2,010〕T =〔11c1-1,01,-1s1-101, 0)T e=〔11c1-01v-11s1-01v+d2,010〕T 0e可1v=〔11vc1-1-118:-110)T e61=〔0d210〕r o=〔0001)r vi, 〔gc1+1s-a(0i,)2+l,c1-10, -g51+11c1-1（01,)2-1154-1,8, 0 .=gc1+1s1-a,(i,)2+lvc-9,d,(9)2 g1+11c-(0,)2-1s1-10,+d2,01 0 M2,61,=m2〔gl1c,+li,01,-1d2c1-(6:)3-1d2-481〕 M201=m2〔-gds1+1d2c1-(01,)2-1d2s1-,8,+di,61〕 +12.01 3.4利用公式(a)~(k)求右二杆：M0 过程与求左二杆相似，今直接给出结果： 对于杆L: M0:=m:〔-gd1s1+d0,)+I1z0: M101=0 对于杆L: M281=m2〔-gl151+101-1d2c1-:(0,)2-11d2s1-18,) M201=m2〔-gd2c1v+1d2c1-:(0,)2-11d2s1-16, +d28〕+1201， 206

〔 夕 ， 一 ，， ， 夕。 ， ， ， ， ， 〕 了 至 ，， 。 二 ① 二只 ， 。 二 ，， 几了 ，， ， 水 ， 以 二 。 · 以 二 。 兹，， 十 类二 ， ， ， 习 ， 优 ，， ， ， ， ， ， ， ， ， · 川 二 。 · 时 。 蔚 全丁 · 二扩 。 对 于杆 ， 有 。 置二 ， ， 〕 〔 〕 产 人 一 圣二扩 月 ， ， 呈今 〕 刀 里二 〔 ， ， ， ， 一 ， ， ，， 〕 〔 ， ，一 ，， ， 一 ， 一 ， ， ， 〔 ， ， 一 ， ， ， 一 ，， ， 一 ， ，， ， 〕 二 委二 〕 武 二 ‘ 扩 ， 二 ， 〔 ， 一 ， 一 ， 泞 ， 〕 口 夕 〔 〕 石呈 〔 〕 、 … 置 夕 ， 二 ， ， ， 一 ， ， 二 ， ， ， 一 ， ， 一 ， 一 ， 盯 ‘ 、 户， 孟 口 了、 二口 ，八曰 · 二 口 ， 一 口 一 卜 一 ， ，， 工， 一 ， 一 ， 嵘 ， 、 ， 、 一 ， ， ， 少 ， ， 二 阴 〔 ，， 犷 ， 咨 厂 “ 一 ， ， 一 ， 〕 、尹 几 ‘ 竹︸ 盆 十 八” ﹄ 一 ， ， ， 一 工 ， 工 阴 〔 一 ， ，， ， 一 工 ， 工， 一 ， ， ，一 工 ， 干 ， 利 用公式 一 求 右二 杆 过程 与求 左 二 杆相似 ， 今 直接 给 出结果 对 于杆 ， 二 阴 〔 一 ， 一 卜 工 〕 ， ， 对于杆 二 。 阴 〔 一 量 ， 一 工 ， ， ， ， 一 ， ， 〕 ，， 阴 〔 一 ， ，， 一 一 ， 一 二， “ 伟 一 百 ， 〕

3.5两自由度平行4杆系统动力学方程 令x:表示驱动杆L的关节J:处的所需转矩，τ：表示驱动杆L,的关节J1,处的 所需转矩，有： t91=M:i1+Mzi:+M91,+M2,92 =GM1+IM+CM ti1=M:g1,+M291+M1,g:+M26g =GM3+IM+CM 其中：GM:、GM:,一表示静力（杆自重）产生的力矩；IM:,IM。1,一表示由惯性 力产生的力矩；CM,CM:1,一表示离心力、哥氏力产生的力矩。它们分别是： GM=-g (midisin+m2lisin:+m:,da,sin) GM=g(midicose,+m2,11,cos01,-m2d2cos01,) 1M,=（m1d+m2,l+m2d,)0,+（11:+12a)0, -〔m2l2d2sin(0v-0,)+mald2,sin(0,-01)〕6， IMa1,=(m,d,+m2l,+m2d2)6,+（1+I2:)0, 〔-m2ld2sin(01-0)+m2ld2sin(0-0,)〕0， CM01=〔-mzl1d2cos（0,-0:)+m211,d2cos(01-01,))(0:,)2 CMg1=〔-m211d2co5(0,-0,）+m2l1d2cos(0,-0,))〔0，〕2 至此便完成了一个闭链结构两自由度操作手动力学方程建立的全过程。而且没有进 行拆链处理。 4结 语 本文在凯恩动力学方程的基础上导出了一种新的递推的机器人操作手动力学算法， 并以一个具有闭链结构的两自由度4杆操作手为例，说明了使用该算法进行符号计算的 方法和步骤。从导出的公式(a)~(k)和例题可以看出该算法具有如下优点： (1)它具有递推性质。所以该算法可以很容易地在计算机上实现数值计算或符号 推导。 (2)与目前通用的牛顿一欧拉递推算法比较，它只须一步向前递推，无须先向前 递推求出各杆的速度、加速度之后，再向后递推才能求出各关节转矩，从而避免了使用 两种旋转矩阵(R-1和R),提高了计算效率。 (3)本文推出的算法，既可适用于开链操作手，也可不经拆链直接完成闭链结构 操作手的动力学计算。 参考文献 1 Paul R P.Robot Manipulators,Mathematics,Programming and control The MIT Press,1981 207

两 自由度平行 杆系统动力学方程 令端 表示驱动杆 的关节 处的所 需转矩 ， 二 ， 表示驱 动杆 ， 的关节 ， 处的 所需转矩 ， 有 下 忿 ， 落 ， 喜 ， ， ， 二 、 二 二 ， ， ， ， ， ， ， 二 ， ， ， ， 对 二 ， 其 中 、 ， 一表 示静力 杆 自重 产生 的力 矩 二 ， ， ， 一表示 由惯性 力产生的 力矩 落 ，， 落 ， 一表示离心 力 、 哥 氏 力产生的 力 矩 。 它们 分别是 二 一 夕 川 。 ， ， ， 二 ， 阴 ， ，， ， ， ， 一 阴 ，， 忿 ， 。 阴 ， 全 。 ， 孟 ， ， ， ， 一 〔 优 ， 一 ， 阴 ， ， ， ， 一 ， 〕 ， 二 ， 川 ， 圣 ， 二 全 ， 优 ， ，， ， 〔 一 阴 ， ， ， 一 ，， ，， 一 ， 〕 ， 答 〔 一 ， ， 一 ， ， ， ， ， 一 ， ， 〕 ， 二 ， 〔 一 ” ， ， ， 一 ， ， 一 〕 〔 〕 至此便完成 了一 个 闭链结构两 自由度操作手 动力学方程建立的全过程 。 而且没 有进 行拆链处理 。 结 语 本文在凯 恩 动力学方程的 基础上导 出 了一种新的 递推的机器人操 作手 动 力学算法 ， 并 以一个具 有 闭链结 构的 两 自由度 杆操 作手为例 ， 说 明 了使用 该 算法进 行符号计算的 方法和步骤 。 从导 出的公式 和 例题可 以看出该算法具 有如下 优点 它 具有递推性质 。 所 以该算法可 以很容 易地在计算机上实现数值计算或符号 推导 。 与 目前通用的 牛顿一欧拉递推算法 比较 ， 它 只须 一步 向前递推 ， 无须先 向前 递推求 出各杆的速度 、 加速度之后 ， 再 向后递推才能求 出各关 节 转矩 ， 从而避免 了使用 两 种旋转矩阵 一 、 和 一 ‘ ， 提高 了计算效率 。 本文推 出的算法 ， 既可适用 于开链操 作 手 ， 也可不经 拆链直接完 成闭链结构 操作手的动力学计算 。 参 考 文 献 〔 〕 ， 川 ， 川沉 ， ， ，