D0I:10.13374/j.issn1001-053x.1987.01.018 北京钢铁学院学报 第9卷，第1期 Journal of Beijing University Vol,9 No.1 1987年1月 of Iron and Steel Technology Jan,1987 钢铁机器人一1操作手动力方程的建立 官心喜 冯德坤 (数学力学系机器人研究组) 摘要 本文应用拉格朗日方程和保罗近似方法(])来建立我院钢铁机器人一1操作手的 动力方程。为了突出动力方程的物理意义，而根据操作手机构的动能和势能，来找出 各关节力矩与各关节角加速度之间的简单关系式：为了实际应用，利用保罗近似方法 来简化求导计算过程。 关键词：机器人操作手.拉格朗日方程.保罗近似方法 Establish the Dynamic Equations of Steel Robot -1 Manipulator Gong Xinxi Feng Dekun Abstract In this paper,Using the Lagrangian equations,the dynamics equations of Steel Robot -1 manipulator of our university is established to explain the physical sense of dynamic equations.By means of the kinetic energy and the potential energy of the manipulator mechanism,the simple relation between the joint torques and the joint angular accelerations are found out. Paul's approximate method is used to simplify the process of the derivation for applying actually. Key words:robot manipulator;lagrangian equation,paul's approximate method 1986一02一28收稿 113
不 第 卷 ， 第 期 年 月 北 京 钢 铁 学 院 学 报 主主 七 钢铁机器人一 操作手动力方程的建立 宫心 喜 冯德坤 气 数学力学系机器 人研究组 摘 要 ，尽二 木文应用拉格朗 日方程和 保罗近似方法〔 〕来建立我院钢铁机器人一 操作手 的 动力方程 。 为了突出动力方程的物理意义 ， 而根据操作手机构的动能 和势能 ， 来找出 各关节力矩与各关节角加速度之间 的简单关系式 为了实际应用 ， 利用保罗近似方法 来简化求导计算过程 。 关键词 机器 人操作手 拉格朗 日方程 保罗近似方 法 一 ’ 石 “ 备 ， ， 一 沉 ， ‘ ‘ 一 一 收稿 DOI ：10．13374／j ．issn1001－053x．1987．01．018
序 言 关于建立多刚体系统的动力方程，目前有许多方法。美国等常用拉格朗日方程法。这种 方法的物理概念和运算步骤都很明确，但求导过程往往很复杂，难以实际应用。最近，美国 斯坦福大学的凯恩教授提出了凯恩方程法，其运算过程比拉格朗日方程法较简，也便于应 用，但其物理概念比较抽象，不如拉格朗日方程法那样明确。在苏联，常用高斯最小约束原 理法(2)，此法虽然运算过程比较简单些，但是要用到求泛函极值等变分方法，其物理概念也 不如拉格朗日方程法简明。目前这些方法各有其优缺点，都正在进一步发展。 本文为了保持拉格朗日方程物理意义明确的优点，克服其求导运算过程复杂的缺点，而 应用了保罗近似法。 1 钢铁机器人一1操作手运动分析 钢铁机器人一1换作手的机构简图和连杆坐标系如图1所示，其连杆参数见表1。以各 关节转角01、日2、6g、0、日5为广义坐标，a表示连杆扭角，a表示连杆长度，d表示连杆距 离。 由多关节操作手坐标变换矩阵的一般公式1).〔3)： X4,X5,y 7777 图1钢铁Robot-一1操作手坐标系 Fig 1 Coordinate frames for the stecl rodot-1 manipulntor 表1钢铁机器人一1操作手的连杆参数 Table I Link parameters for the steel robot-1 manipulator Link Variable Twist angle Link length Link distance 1 0=01’ 90 0 0 0:+90°=02’ 0° as 0 0a-88-90°=0，’ 0° 88 0 04-08十90°=04' 90 0 0 5 05十j0:=05’ 0° 0 0 114
序 国 ，肠 亡 关 于 建立 多刚 体系统的动力方程 ， 目前 有许多方法 。 美 国等常用拉格 朗 日方程法 。 这种 方法 的物理概念和运 算步 骤都很 明确 ， 但求 导过程往往很 复杂 ， 难 以实 际应用 。 最近 ， 美国 斯坦福 大 学的凯 恩 教授提 出 了凯 思 方程法 ， 其运算过 程 比拉格 朗 日方程法较 简 ， 也 便 于 应 用 ， 但共 物理概 念 比较抽象 ， 不如 拉格 朗 日方程法 那 样明确 。 在苏联 ， 常用 高斯最小约束 原 理法 卿 ， 此 法虽 然运 算过程比较简单些 ， 但是 要 用 到求 泛 函极值等变分方法 ， 其物理概念也 不 如拉格朗 日方程法 简明 。 目前这些方法 各有其优缺 点 ， 都正 在进 一步发展 。 本文为 了保持拉 格 朗 日方程物理意 义明确的优 点 ， 克 服其求导运 算过程 复杂 的缺点 ， 而 应用 了保罗近 似法 。 钢铁机器人 一 操作手运动分析 钢铁机器人一 操 作手的机构 简图和连 杆坐标 系如 图 关节转 角 、 、 。 、 ‘ 、 为广义坐 标 ， 表示连 杆扭 角 ， 离 。 所示 ， 其连 杆参数见表 。 以各 表示连 杆长度 ， 表示连 杆距 由多关节操作 手坐标变换矩 阵的一 般公式〔 〕 〔 “ 〕 三令 口 · 夕 夕 图 钢铁 一 操作手坐标系 一 表 钢铁机器 人一 操作手 的连杆参数 · 一 主 之 刀 ， 毛 一 ‘ ， 十 产 ， 。 一 一 ’ 一 。 ‘ ’ ‘ 一 。 十 尹 ‘ 产
c0% -s0ncan s0bsan aac0p A。= s0% c0ncan -c0isan a.sOn (1) 0 SQn caa d。 0 0 0 1 式中，s0:=sin,c:=cosn,saa=sina。,can=cosan,可得到各相邻连杆之间的坐 标变换矩阵： -52 C -a2S1 0 -c A2= C2 一52 0 A1= 且C2 0 1 0 0 0 1 0 0 0 0 0 0 1 S32 C32 83S81 0 C48 -c3 S32 -a3C32 A4= C43 S43 0 0 0 1 0 0 1 0 0 0 0 0 1 0 0 0 C54 -554 0 0 A3= s34' C54' 0 0 0 0 y 0 0 0 0 1 式中，s1=sin0,c:=cos0,s2=sin82,c2=cos02,cg2=cos(0,-02),5g2= sin(0,-02),s4g=sin(0:-0g),c4g=cos(04-03),s54'=sin(05+i04), c5'=cos(日5+i04),i为操作手手腕中齿轮传动比。 各相隔连杆之间的坐标变换矩阵T可由下列各式求得： TI=AL T3=A1A2A3, T5=A5,, 1T2=A2, 8T4=A4, 8T5=A4A5, T2=A1A2, :T4=A3A4, 1T5=A3A4A5, T3=A3, 1T4=A2A3A4, 1T5=A2A3A4A5, T:=A2A3, T=AA2A3A4,T5=AA2A3AA5o 限于篇幅，各T矩阵的内容就不写出了。 2 操作手动能与势能的计算1) 设连杆i上一点相对于坐标系T:=T:（0,02,,0,)的矢径为r,该点在静坐标 系上的矢径为： r-Tir (2) 该点的绝对速度为： 115
· 一 盆 。 盆 口盆 。 一 二 尸 ﹃ 口八口 ， 以 仪 。 盆 。 盆 。 式 中 ， ‘ 口孟 口盆 ， 盆“ 日盆 ， 。 。 ， 标 变换矩 阵 仅 二 口 可 得到 各相邻连 杆之 间的坐 灿 ， 一 … 一 一 “ 一 ‘ 一 一 盆 二 山 口 ‘ … 之 一 一 二 … 一 ‘ ‘ ‘ 。 ， ，︶ 一 式 中 ， 二… 口 ， 一 ， 一 “ “ ，， ， ‘ 一 ， ‘ 。 已， ， 一 ， 一 ， “ ‘ ， 也 。 ， ， 为操作手手腕 中齿轮 传动 比 。 各 相隔连 杆之 间的坐标变换矩 阵 可 由下列 各式求 得 ，， ， ‘ “ ， ‘ ， ‘ ‘ ， “ 。 二 一 ， ’ ‘ ， ’ ， ， 二 ， ‘ 二 ‘ ， ’ 二 。 ‘ ， ‘ 二 。 ， 二 ， 。 ， ， 。 限 于篇幅 ， 各 矩 阵的 内容就不 写 出了 。 操作手动能与势能的计算〔 〕 设连 杆 上一 点相对于坐标系 ，， ， 一 ， 的矢径 为 ’ ， 该 点在 静 坐 标 系 七的矢 径 为 、 少 该 点的绝对速 度 为
寸-正-（含船·） (8) dt 设该质点的质量为dm,则其动能dK:为： dK,=立.Vdm/2 (4) 或 （dm)86,a.〕 (5) 则连杆的动能为： ∫，dk分…含含船18器i,i.〕 (6) 式中J:为惯性矩矩阵，代表下式， J=∫ ir.irTdm (7) 设驱动器对于关节i的等效转动惯量为I.,则驱动器的动能为1./2，故操作手的 总动能为： K名点t…(器)，i+点 (8) 设用相对于连杆i坐标系T:的矢量表示连杆质心的矢径，用：表示连杆的质量， 用g表示重力加速度行阵，则5个连杆的总势能为： U=- migTir; (9) 11 3操作手动力方程的建立 设关节i处的驱动力矩为M:,则把操作手的动能方程(8)和势能方程(9)代入拉格 朗日方程： 00: (10) 可得我院钢铁机器人操作手的动力方程为： M=是D可+1+多2Dg,9+D: (11) is1 k (i=1、2、…、5) 式中，0，为连杆对于关节的角加速度，而 116
了‘了、了了、 、 、了、、产夕、，， 气 子 、产 子 主 ‘ 会 二 ‘ 厂 设该 质 点的质量 为 ， 则其动 能 、 为 ， · 或 二 ， 。 厂仓 宝 、 一塑匕 了 口日， 丁 丫终 刁翼 则连 杆 的动能 为 丁 ， 音 一 生主会 ， 癸 云 、 〕 式 中 为惯性矩 矩 阵 ， 代表下式 ‘ 一 丁 一 设 驱 动器 对于关 节 的 等效转动惯量 为 。 ， 总动能为 则 驱 动器 的动能为 履 ， 故操 作 手 的 尽 名 名 。 ‘ ， 孟 飞 尽 ， 占 ‘ 。 、 八 一 了 二 二 、立 ， ‘ ’ “ ’ 后万厂 ‘ 一丽丁 口 ’ 。 ‘ ’ 了 ，立 ， ‘ “ ’ “ ‘ 设用 相对于连 杆 坐标 系 的矢量 ‘ 表示连 杆 质心 的矢径 ， 用 表示连 杆 的 质量 ’ ， 用 ，表示重 力 加速 度行 阵 ， 则 个连 杆的总 势 能 为 一 卫 一 操作手动 力方程的建立 设关 节 处 的驱 动力 矩 为 、 ， 则 把 操作 手的 动能 方程 和势能方程 代人 拉 格 朗 日方程 二 一 日 、 卫警八 口 气 万 一 叫 十 口甘 可 得我院钢铁机器 人 操 作手的 动力方程 为 。 … ‘ 艺 、 ， ， 、 ‘ 艺 艺 、 口 口 ， 、 、 … 、 式 中 ， 为连 杆 对于 关 节 的 角加 速 度 ， 而
D,=2 …(8) (12) P=msxi,j 为关节i与关节j之间的耦合惯量，当j=i时，得在关节处的有效惯量D;: (13) p=j 另外，式中D:为关节j和k对于关节i所产生的离心惯量和氏惯量，即代表下式： p=maxi,j a90。,90) (14) 由于本操作手速度较低，所以D:1k≈0。D:为连杆i重力对于关节i的力矩，即代表下 式 D,=2(-m,g,) 5 (15) P=si 式中"r为连杆P质心在坐标系T,上的坐标列阵。下面用保罗近似方法对以上各式进行简化。 若引入坐标系T:对于坐标系1T的微分平移向量：和微分转动向量δ：以及惯量矩 阵K,则式(12)可写为： D1= p=msxi,j 〔K,6,+d,+(ix方，+i,x可，)](16) 式中， :=（-i1np,+1n1p)i+（--10xp +-10,-1p:）j+（-1ai-p,+1a,ip)k (17) 方：=1n+i1o方+la.k (18) 而其中的i1n,、lnx、i-lnp、i-l0px、ilop,、i0p、i-1apx、i1ap、11a,a 分别为坐标变换矩阵-1T,的第1、2、3、4列失量p、0p、a在xyz坐标系三轴上的 投影。 惯量矩阵K,的具体形式为： Kix: -Kix -Kx K,= -Ki:, Ki,, -K: (19) -Kx:。-K, Ki: 式中，Kx、K、K、K,x、Kpx、Kpy分别为连杆P对于其坐标轴x、y、z的回 转半径。由于Kx、Kv、Kpx比Kpxx、K,、K.要小得多，故式(19)可近似写为 117
艺 一 。 ‘ 。 口万， ， ， 口 了 一 一下， ， 一 万 、 ‘ 为关 节 与关 节 之 间的祸合惯量 ， 当 时 ， 得在关 节 处 的有效惯量 一 量 ， … 会 ， 尝 另 外 ， 式 中 ，、 为关 节 和 对 于关 节 所产生的离心惯量称耳 氏惯量 ， 即代表 下式 ， 艺 一 ， · … ， ‘ 二一 落一 刃 廿 由于本操作手速度较低 ， 所 以 、 、 。 。 、 为连 杆 重 力 对于关节 的 力矩 ， 即 代 · 表 下 式 垦 一 · 釜 一 式 中 ， 为连 杆 质心 在坐标 系 ， 上 的坐标列 阵 。 下面 用保 罗近 似方法 对以 上 各式进 行 简化 。 若引入坐标 系 、 对 于坐标 系 ‘ 一 ‘ ， 的微分平移 向量 ‘ 和微分转动 向量 以及惯 量 矩 阵 ， ， 则式 可 写为 ‘ 艺 口 ，， 〔 。 、 。 一矶一不 · 心 · 唁 · 唁 ，一不 〕 ，。 式 中 落 一 ’ 一 ‘ ， 二 ‘ 一 ‘ ， ‘ 一 ‘ ， · ‘ 一 ‘ ， 了 一 ‘ 一 ‘ ， 二 · ‘ 一 ‘ ， ， ‘ 一 ’ · 了 一 ， · 一 ， 一 ， · 一 ， 一 了 唁 一 ‘ 一 一 而其 中的 ‘ 一 ‘ ， 二 、 ’ 一 ‘ ， 二 、 ‘ 一 ‘ ， 、 ‘ 一 ‘ ， 二 、 一 ， 、 一 、 一 、 一 、 一 ， 分别 为坐标 变换矩 阵 ‘ 一 ’ 的第 、 、 、 列 矢量 ， 、 投 影 。 惯量矩 阵 的具体形式为 京 、 ，在 坐标 系三 轴 上 的 ， 君 二 一 赘 二 ， 一 熟 一 井 丈落 ， 一 幕 一 盆 二 一 攀 ， 孟 二 、 护 … 、 式 中 ， 转半径 。 ， 二 、 ， ， 、 、 ， ， 、 由于 二 ， 、 ， 、 比 ， 、 分别 为连 杆 对于其坐标 轴 、 、 的 回 ， ， 、 ， 要 小 得多 ， 故式 可近 似写 为
对角阵，从而可将式(13)简化为： D=,{〔k+ok+a.…〕 +〔p…p,〕+2，〔(p,·i,)i+（pom）j +(pa,)k)} (20) 并将式(16)简化为下式： 三m,(C20,k好.0+8，k5,8, D1=2 +8.k.6.+〔d·d〕 +,·〔xi+d×:)} (21) 又由于各连杆质心的分散程度较大，上式中第二项占优势，故第一、三项可略去，上式可进 一步简化为： 点m,〔过式，〕 (22) 利用坐标系之间的微分变化，可将式(15)简化为： 5 Di=i-1gT mri-irr (23) pai 式中，g"=〔-g·0g·n00〕为重力加速度在坐标系T:-中的行阵，g为重力加 速度向量。 根据以上所得近似式，可求得钢铁机器人一1的各个惯量和重力矩如下： D55=m5k5, D44=mk.+m5k号 Das=m3 (ki:+ai+a3xs)+m(kix+a) +ms (ki+a3+2a3Zs), D22=m2 (k:+a:+2x2a2)+m3Cki+ai+a:+2aaasaa +2x3 (as+a2ss2)]+m(kir+ai+a+2aza3s32) +m5〔k号x.+a:+2a2a3ss2+2z5（a3c4g-a2542)〕， D11=miki+m2(ck+sk+a)+m3 Sikixx+cikiv +a+ai+2a2aas32+2ysa2cs2)+m(ciki+siki.. +a8+a:+2aza3sa2)+m5〔ck号x+a:+a+2a2 asSs2 +2z(agc4g+a2s42)〕。 118
对角阵 ， 从而可将式 简化 为 〔 二 吞 幕 ， ， 十 。 ‘ 〕 艺 一 〔言 ， · 下 ， 〕 认〔 哀 · 言 ， 六 言 ， · 蕊 丁 衷 · 认 了 并将式 简化为下式 占 ， 井 艺 ， ， 护占，， ， ， ， 〔 ’ ‘ “ ’ · ’ 占” 域一 、尸 占， 吞 二 〕 〔斌 二 · “落 ， ‘ 眨 又 ， 占 ， 护占 又 由于各连 杆质心 的分散程度较大 ， 上式 中第二项 占优势 ， 故第一 、 三项可 略去 ， 上式可进 一步简化为 〔落 · 落 ， 〕 艺 一 位 ， 利用 坐标 系之 间的微分变化 ， 可 将式 简化 为 一 艺 一 式 中 ， ‘ 一 ’ 二 速度 向量 。 刁， 〕 卜 卜 刁卜 〔 一 · ’ 一刁卜 。 〕 为重 力加速度在坐标系 一 ，中的行 阵 ， 为重 力加 根据 以 上所 得近 似式 ， 可 求 得钢铁机器 人一 的各个惯 量 和重 力矩 如下 馨 ， “ ‘ 芸 二 ， 。 。 葺 轰 反 戈 ， ， 轰 言 ， 二 呈 呈 贾 。 〔 孟 若 。 灰 。 〕 重 ， ， 孟 呈 。 〔 呈 。 瓜 ‘ 。 一 〕 ， ， 莹 ， 一 卜 盆 呈 二 二 孟 呈 ， ， 置 孟 孟 二 二 孟 孟 ， ， 呈 一 卜 孟 丁 。 ‘ 。 乏 乏 二 乏 二 孟 二 。 。 一卜 〔 彗 二 二 呈 孟 。 。 ‘ 。 十 ‘ 〕
式中，x1、y:、z1为连杆i在坐标系T:中的质心坐标。 D23=D32=(m3+m4+m5)a3(a3+a2ssz) 其它耦合惯量皆为零。各重力矩为： 。 D5=0, D=-m5gc1c4Z5, Da=-m3gciss (x3+a3)-m5gc1(Z5s4+ass3), D2=-m2gcic2(x2+a2)-magciC xa+a3)s3+aac2 -m5gc 25s4+assa+a2c2), D,=-mg5x1+m2gs1s:（x2+a2)-mgC(元3+ag)cg -a2s23-m5gs(z5c4+asc3-a2s2)o 以上运算对于低速机器人操作手动力计算，有足够的精度1)。 另外，在以上计算中，由于未考虑连杆6和7，所以，可设连杆6和7对其关节轴的转 动惯量分别为I6和[？，作为对动力方程的修正量。由于连杆7的平移较慢，其影响可不计。 把所求出的各惯量和重力矩代入式(11)，可得我院机器人操作手动力方程为： M:=(D:1+I1)0,+D1 M:=(D22+1.a)02+D2+D2s0,+l702 M.=(Dss+I.3)0:+D:+D3202+Ic03 (24) M=(D44+I4)0:+D, M5=（D55+I5)5 根据上式，代入具体数据，就可以由操作手的各个连杆的惯量、重力矩和各关节角加速 度，求出各关节处的力矩，以供控制参考。 4结 论 本文中所用的保罗近似方法和拉格朗日方程，不但可以突出操作手动力方程的物理意 义，而且也克服了求导计算非常困难的缺点，因此，便于掌握和应用。 参考文献 C1)Richard,P.Paul:Robot Manipulators:Mathematics,Programming and Control,The MIT Press,1981 〔2〕波波夫，E.ΠI:操作机器人动力学与算法，遇立基等译，机械工业出版社，1983 〔3〕张启先：空间机构的分析与综合，上册，机械工业出版社，1984 119
式 中 ， 、 、 为连 杆 在坐标系 中的质心 坐标 。 “ 一 。 。 其它祸合惯量 皆为零 。 各重 力矩为 。 ， ‘ ‘ 一 ， ‘ ， 。 二 一 一 ， 一 ， 一 〔 。 〕 一 一 ， 一 ， ， ， ， 一 由 〔 一卜 。 。 气 一 〕 一 ， 一 。 一 。 以上运算对于低速机器 人操作手动力计算 ， 有足够的精度。 〕 。 另外 ， 在 以 上计 算 中 ， 由于未考虑连 杆 和 ， 所 以 ， 可设连 杆 和 对其关 节轴 的转 动惯量 分 别为 和 ， 作 为 对动力方 程 的 修正量 。 由于连 杆 的平移较慢 ， 其 影响可 不计 。 把所求 出的 各惯量 和重 力矩 代 入式 ， 可 得我院机器人 操作 手动 力 方程 为 。 。 、 、 廿， ‘ 产， 二 ‘ 一 ‘ ‘ 根据上式 ， 代入 具体数据 ， 就 可 以 由操作 手的各个连 杆 的惯量 、 重 力矩 和 各关 节 角加速 度 ， 求 出各关 节处 的力矩 ， 以供控制 参考 。 结 论 本文 中所用 的 保 罗近 似方法 和拉格 朗 日方程 ， 不 但可 以突 出操作 手动力方程 的 物 理 意 卜 、 义 ， 而且也克 服 了求 导计 算非 常 困难的缺 点 ， 因此 ， 便 于掌 握和应 用 。 参 考 文 献 〔 〕 五 ， 一 ， ， ， 〔 〕 波 波 夫 ， 操作机器 人动力 学 与算法 ， 遇立 基 等译 ， 机械工 业 出版 社 ， 〔 〕 张 启先 空间机构 的分析与综 合 ， 上册 ， 机械工业 出版社
