负载与操作机分开的机器人动力学算法

D0I:10.13374/i.issm1001-053x.1990.03.009 第12卷第3期 北京科技大学学报 Vol.12 No.8 1990年5月 Journal of University of Science and Technology Beijing May 1990 负载与操作机分开的机器人动力学算法 孙昌国·马香峰 摘要：基于Ka口c动力学方程，导出了负戡与操作机分开，并具有封闭形式的机器人动 力学通用算法。该法适用于开、闭链机构的机器人动力学研究，便于用计算机推导机器人操 作机的动力学数一符方程。给出了该算法及其证明，并讨论了该算法的性质及特点。 关键词：操作机，动力学，年法，数一符 The Robotic Dynamical Algorithm by Divided Load-Manipulator Sun Changguo Ma Xiangfeng ABSTRACT:Bascd on the Kane's equation,a ncw general robotic dynamical algorithm,in which load-manipulator is divided,has been presented.The algo- rithm with closed-form is efficicnt in solving the dynamical problems of mani- pulator with closed-chaiu or opened-chain,and generating the dynamical nume- ric-symbolic model of manipulator by computcr.The algorithm has been derived and the characters of the algorithm has been discussed. KEY WORDS:manipulator,dynamic,algorithm,numeric-symbolic 机器人动力学算祛，一般是根据多刚体动力学原理和方法，经过适当的推演和变换得到 的。基于不问的力学原理和方法，可以得到不同的算法1~5)。虽然各种力学原理和方法都互 相等价【)，但由于推导出的算法不同，在实际应用这些算法时，却有运算量的大小、运算 过程的繁简及运算表达式的物理意义明显与否的差别。 作者在应用Kane方程求解机器人动力学问题的基础上，最近采用求运动参数的方式， 又导出了新的，将负载与操作机分开的、封闭形式的机器人动力学通用算法。 1989-04-25收稿 ·机器人研究所(Robotic5 Instilule) 250

第 卷第 期 年 月 北 京 科 技 大 学 学 报 犷 丁 一 ， 口 ， 负载与操作机分开的机器人动力学算法 孙 昌 国 ’ 马香峰 ’ 尹声 摘 要 基 于 。 。 动 力学 方程 ， 导 出 了负载 与操作机 分 开 ， 并具 有封闭 形 式 的机 器人 动 力学 通用 算法 。 该法 适用 于开 、 闭 链机 构 的 机器人 动 力学 研究 ， 便于用 计算机 推导 机 器人 操 作 机 的动 力学数 一符 方程 。 给 出 了该算法及其证 明 ， 并 讨论 了该算法 的性质及 特点 。 关 健词 操 作机 ， 动 力 学 ， 算法 ， 数 一 符 一 夕。 ” 尸产 、 一一。 。 ，、 。 ， 叉 ， 一 、 · 一 一 ， 一 · 一 一 一 了 。 一 、 认 一 一 一 ， 一 · ， 。 ， ， 一 。 机器人 动力 学算法 ， 一般是根 据多刚体动力 学原理 和 方 法 ， 经 过适 当的推演和 变换得到 的 。 基于 不同 的 力 学原理 和 方 法 ， 可以 得到 不同 的算法 〔 ‘ 一 ’ 。 虽然各种 力学原理 和 方 法都互 相等价 〔 “ 〕 ， 但 由于推导 出的算 法 不同 ， 在 实际应用这些 算法时 ， 却有运 算量 的大 小 、 运 算 过 程的 繁简及 运 算 表达 式的物理 意 义明显 与否的差别 。 作者在应 用 方程 求解 机器人 动 力学问题 的基础上 ， 最近采 用求运 动参数 的 方 式 ， 又导 出 了新的 ， 将 负载与操作 机分开 的 、 封闭形 式的机器 人 动力学通 用算 法 。 一 一 收 稿 机 器人 研究所 DOI ：10．13374／j ．issn1001－053x．1990．03．009

1算法 负载与操作机分开的，封闭形式的机器人动力学方程，可由(1)式表示： n-2ai+22i+a+i+7+ =1 含+i+，+. (1=1,2,…，p) (1) 式中的GF,为操作机主动关节的广义驱动力。(1)式的求解步骤如下： ①确定操作机的主动关节数p,运动连杆数n,广义坐标q。 ②列出操作机连杆系统的约束方程： P (r=p+1,p+2,…,n,w) (2) s=1 式中的A,,为常数，w表示负载号。 ③由操作机各杆的角速度、质心速度及角加速度、质心加速度(3)~(6)式，求偏角速 度⊙i,1,质心偏速度0，：及角加速度和质心加速度中的速度偶合系数Mk、V1t。 角速度： d=0.194(=1,2,…m,w,ji) (3) P 质心速度： (4) j=1 角加速度： -209i+22di9 (5) =1k= 质心加速度： =209+22i (6) 1=1 i=l k=1 (3)~(6)式中上角标为坐标号，下角标为连杆号。 ④ 求操作机连杆的惯性力系数H:。 a1=26m41+u,00 (7) (1=1,2,…p) 式中m.为S杆质量，I.为S杆对其质心的惯量张量。 251

算 法 负载与操作机分开 的 ， 封闭形式的机器人 动力学方程 ， 可由 式表示 九︸ 育。 冬 六 令 冬 介 二 含 。 厂 七与 月 ， ‘ 盆 盆匀 三与 廿 二 。 全方 ， 百 全全霖 ， 子 』 ， 民 ， · 式 、 ， ， 一 ， ， 式 中的 为操作机主 动关 节的广 义驱动力 。 式的 求解步骤 如下 ① 确定操作机的 主 动关节数 ， 运动连 杆数 ， 广义坐标 。 ② 列 出操作机连 杆 系统的 约束方程 。 万 夕 ， 夕 ， … ， ， 二 二 式 中的班 ， 为常数 ， 。 表示负载 号 。 ③ 由操作机各 杆 的 角速度 、 质 心速 度及 角加 速 度 、 质心加 速度 一 式 ， 求偏 角速 、夕、、 、工、少， 任勺 夕一 、吸了‘了、 了、 ， ， ︸、了、 质心 偏速度叫 ， 及 角加 速度和 质心加 速度 中的速 度偶 合系数 、 升 。 一尹 、 ，、 一 ‘ 一 ， 。 乡 一 、 用 迷发 山 。 夏 ， 君 ， 艺 ， ， 砂 一 飞， 质 心速度 灵 ， 全几 ，， 砚 二 角加 速度 或 全歇 ， 名 习 二 决 名 二七 ‘ ， 矶 十 勺 质心加速度 。咋 一 习同 全 万 式 中上 角标 为坐标号 ， 下 角标 为连杆 号 。 ④ 求操作 机连 杆 的惯性力系数 ， 。 万 、 名〔 · “ 一 刀 忿 。 ， 。 。 ， ， ， ， … 式 中 。 。 为 杆质量 ， 。 为 杆对 其质 心 的惯 量 张 量

⑤求负载的惯性力系数Hw1。 月.1=m.,,1+(山0)0t (8) 式中mw为负载的质量，Iw为负载对其质心的惯量张量。 ⑥求操作机连杆的离心力及科氏系数hk。 -∑{m,d1+山，at+@xu,00{(9) ⑦求负载的离心力及科氏力系数hw1k。 hw1k=m.NWIk:w,1+CwM01k+00,1+1n0:.k)门@，1 (10) $求作用在操作机连杆质心的广义外力F。1。 F1=-∑c+:@ (11) i=1 式中F、M:为作用在i杆质心，且在坐标系表示的外力和外扭矩。 ⑨求负载上的广义外力F1。 Fn1=Fg,1+i:0. (12) 式中F:m、M:为作用作负载质心，L在负载坐标系表示的外力、外扭矩。 心求操作机连杆上的广义摩擦力f:。 fi=-(f开.，1+Mo.1) (13) 式中，M:,为作用在1杆质心且在1坐标系表示的摩擦力、摩擦扭矩。 ①求操作机连杆的重力G,。 g,=-∑. (14) i=1 G=m,R。g,9=C0,0,-9) 式巾R:为基础坐标系相对i坐标系的旋转变换矩阵。9为重力加速度。 2求负载的重力Gw1。 252

⑤ 求负载 的惯 性 力系数 ， ， ， 。 禅 、 ， 八、 月、 ‘ 门、 口 产、 目 ， 川 曹 ， · 尸 梦， ， ， 。 二 ， 。 一 。 岑 ， 式 中 阴 为 负载 的 质 量 ， 了 为 负载对 其质 心 的惯 量张 量 。 功 求 操 作机连 杆的 离心 力及 科 氏系数 、 。 ， 全卜 ‘ 分 · 几 ，， · 〔，诬寿 』 ， ， ‘ 言， ， · 〕 · 言‘ ， ‘ ” ， ⑦ 求 负载 的离心 力及 科 氏 力系数 』 。 气、 、 口 翻、 、 ‘ ，， 气 ， 姜 · 芝 ， 〔 牛一、 田 器 ， 求 作 用 在操 作机连 杆 质心的广 义 外 力 。 。 。 影 ， 、 〕 · 。 二 ， 武 一 全 〔成 ， · 砍 ， 裔 · 动 ， 〕 二 式 中 上 、 二为作用在 ‘ 杆质心 ， 且 在 坐 标 系表示 的外 力和 外扭矩 。 ⑨ 求负载上 的广义外 力 ， 。 户、 一今 声、 口 一扣 户、 门 ， ， 乳 。 口 丁 ， ， 乳 。 。 ， 式 中 九 、 九 为作川在 负载质心 ， 且 在 负载坐 标系表示 的 外力 、 外 扭矩 。 ⑩ 求操 作机连 杆上 的广 义摩擦 力 。 八、 碑 一七 内、 夕 今 八、 一 卜。 ‘ ， · ， ， 式 中 ， 菇 ，为 作 用在 杆质心且 在 坐标 系 表示 的摩擦 力 、 摩擦扭 矩 。 护 求 操 作机连 杆 的 重 力 子 二 一 名动 · 几 。 ， 二 今 一 今 。 ， 夕， 夕 〔 ， ， 一 夕〕 式 中 为 基 础坐 标 系相 对 坐 标 系的旋 转变换矩 阵 。 为 重 力加 速度 。 画 求负载的 重 力子 ，

G1=-G,1 (15) G=mRgg 式中R。为基础坐标系相对负载坐标系的旋转变换矩阵。 将(2)~(15)式的结果代入(1)式，即可得到负载与操作机分开的、封闭形式的机器人 动力学方程。 2算法的导出 机器人操作机由一系列连杆组成。一般地，若设连杆为刚体，则操作机的连杆构成完整 的刚体系统。取关节变量为广义坐标q,若运动的连杆数为n,独立的广义坐标数为p(也就 是操作机主动关节的广义坐标数)，则可列出系统m=n-p个如(2)式的约束方程。 在D-H坐标系【4'5]下，操作机第i杆在i坐标系中的速度、加速度可表示为(3)~(6) 式。从而可确定偏角速度⊙，1、质心偏速度：，、角加速度的偶合系数Mk、质心加速 度的偶合系数N:1k。 基于Kane方程[7的基本概念和功率公式8)，推导得到多刚体的Kanc方程(91为： 1+K:=0(1=1,2,,p) (16) N K,=2(,1+元0.） i=1 N =∑(-m,)01+0, i=1 i=-1e-0×la 式中：及，：为第1刚体上的广义主动力和广义惯性力；F,为给定力系的主关 和主矩；-m,a:,广为惯性力系的主失和主矩；1：为刚体对质心的惯量张量，m:为刚 体的质量；N为系统的刚体总数。上角标为坐标系号，下角标为刚体号。 设在操作机第（任=1,2，…n)杆上，作用有向质心简化后的摩擦力f,外力F。:, 驱动力F.,重力所组成的广义力；以及摩擦扭矩，外扭矩M.,驱动扭矩M.所 组成的广义扭矩，即： Fi-fi+Fi+Fi+G (17) Li=Mi+Me+M. (18) 同理设在负载w上，作用有向质心简化后的外力F。,重力G”所组成的广义力F以及 253

声、 碑 一加 ， 一 岑 晋 ， 一知 》 ‘ 要 。 ， 尸了 式 中刀育为基 础坐标 系相 对 负载坐标 系 的旋 转变换 矩 阵 。 将 一 式 的结果代 入 式 ， 即可得到 负载 与操作 机分 开 的 、 封 闭形式 的 机器人 动 力学 方程 。 算 法 的 导 出 、 、 机器人操作机 由 一系 列连 杆组成 。 一般 地 ， 若 设连 杆 为刚体 ， 则操 作机的连 杆构 成完 整 的 刚体 系统 。 取关节 变量为广义坐标 ， 若 运 动的连 杆数 为 。 ， 独 立的广 义坐标数 为 也就 是操作 机主 动关 节 的广 义坐标数 ， 则 可 列 出系统， 儿 一 个如 式 的约 束方程 。 在 一 坐标 系 〔 ’ ” ’ 下 ， 操 作机第 杆在 犷坐标 系 中的 速度 、 加 速度 可表示 为 一 八、 产、 沪 声、 曰 式 。 从而 可 确定 偏 角速度叫 ， 』、 质 心偏 速度 以 ，， 、 角加 速度 的偶 合系数 、 质 心 加 速 度的偶 合系数 。 基于 方程 〔 ’ 的基本概念和功率公式 〔 日 〕 ， 推导 得到 多刚体 的 方程 〔 。 〕 为 产、 ‘ 、 喇 尤 宁 ， ， … ， 气从 又 名 咸 · 燕 》 尸 、 曰 。 。 式 ， 石 〔 一 水 急 ， · 急』， ， ， 叫 知 内、 砂 份 ‘ 一 。 全 ， 〕 。 。 式 中 和 主矩 尤 ， 兮为第 刚体上 的广 义 主 动力和广 义惯性 力 ， 为给定 力 系 的 主矢 一 入︸无 一 。 、 ， 为惯 性 力 系的 主矢 和 主 矩 为 ￡刚体对 质心 的惯 量 张 量 ， ” 、 为 刚 、 体的质量 为 系 统 的刚 体总 数 。 上 角标 为坐标 系 号 ， 下 角标 为 刚体号 。 设在操作 机第 ’ ， ， “ · 杆上 ， 作 用有 向 质心简化后 的摩 擦 力 ， 外力 。 驱 动力 ， 重 力 所组 成的广 义力 尸 以及 摩擦 扭矩 ，外扭矩 。 ， 驱 动扭矩 ‘ 所 组 成的广义扭矩川 ， 即 一 盏 ‘ 知 一 八 。 ‘ 十 粼 。 飞十 。 同 理 设在 负载 上 ， 作 用 有 向质心简化后 的 外力 。 ， 重 力 幕所组成的广义力 ，以及

外扭矩M所组成的广义扭矩乙：，即： F:=F:+G: (19) L:-M.: (20) 将(17)~(20)式代入(16)式，并将广义驱动力GF:广义外力F。1,广义摩擦力f,重 力G:用(21)~(24)表示， GF1=Fl1+M.⊙，1 (21) 户=-公(E.01+io） (22) i=1 =-(f.+M,@) (23) G=-G (24) i=1 再将负载所受的广义外力F,1,重力G,1用(25)~(26)式表示， F1=-(F0:.1+M0:.) (25) G1=-G.1 (26) 则可得： 含2mdi…th+ +宫会会.itu立i+ +@.1×(I,@.)门oi.1}99+ +G+户1+fi+(m,1+ P j=1 j=1 k-1 +c1,i:k+心：.1×（山，0：）门0.1}g19k+G+F1 (27) 交换(27)式q项的下角标→s,j→i;考虑当>s时：.1=0：.，=0，则可将9项的系 254

一 告 吞 外扭 矩 。 二所组 成的广义扭矩 ， 即 二 。 二 。 ︸ 一 二 将 式 代入 式 ， 用 表 示 ， 并将 广 义驱 动力 广 义 外 力 。 八、 广 义摩擦 力 ， 重 、 力 口、 、 尸、 曰 · 。 。 卜。 广 。 一 名 歹 。 卜 筑 ， 一 卜 ， 十 言 ， 一 名动 · 之 ， 矛尸 。 卜。 ， 门、 曰 卜。 卜 再 将 负载 所 受的广 义 外力 尸， ， 内、 重 力 ， 用 一 式表 不 ， 一 。 》 ，、 习‘ 口 、 、 ， 。 。 一 。 二一 一 二 。 飞 洲声 育 ︸、， 则可 得 碑、 、 口 二 艺 艺 ， 。 二 ， ，。 ， 叫、 。 』 十 讯十 名 名 艺 气、 。 ， 、 。 。 ， ， 〔 。 二 一 二 ，。 、 〕 。 、 十 口 护‘ 一 。 用 ， ， ， 脚名 十 八、 八、 名 习 沪 ， 。 二 卜益 田 奋一 ， 方 、 ， 二 二 碑 、 曰 产、 砂 〔 ， 淤 ， ， 。 二 二 ， 〕 介 、 ， 一 ， 产、 砂 八， 交换 式 项的 卜角标 、 ， 了 八 考虑 当 时 。 二 。 ， 则可将 项 的 系

数分别用(7)(8)两式表示：再将99项的系数分别用(9)(10)两式表示，即可得到(1) 式。 (1)式右边从第一项起，顺序表示操作机的惯性力、离心力（当：=时）和科氏力（当 牛时)、重力、除驱动力之外的外力（如弹性力、电磁力等）、摩擦力和负载的惯性力、 离心力（当到=时）和科氏力（当≠时）、重力及负载所受的外力。 3讨论 (1) 将(7)式中的1.0，)0：.1与(1.⊙.1)o:.1展开后有： (.0.i)-1=(1.o.1)o:.t (28) 再由矢量性质得： 8:a1=.1g (29) 所以：月4=百1 (30) 以及同理有： 立1i=产1 (31) 由(5)式得： 时 j-1 j=1k=1 k=1i=1 所以： 0qx 0q 即应1k-应k (32) 同理，由(6)式可证得： Niy =Niks (33) 将0.1×(1⊙.)与©.k×(1:0.)展开后有 a.1×(1o.k)=.k×(1,a.) (34) 所以： Bixhx (35) 255

数分别 用 式 。 两 式表示 再 将 项 的系数分别用 两 式表 示 ， 即 可 得 到 等 时 离 心力 式右边 从第 一项 起 ， 顺序表示操 作机的惯 性 力 、 离 心力 当 时 和 科氏 力 当 、 重 力 、 除驱 动 力之 外的外力 如弹性力 、 电磁 力等 、 摩擦 力和 负载 的惯性 力 、 当 二 时 和 科氏力 当 笋 寿时 、 重 力及 负载 所 受的外力 。 讨 论 声、 目 户 ‘ 、 目 产、 口 口 、 丫 将 式 中的 。 。 ， 。 与 。 。 ， ， 。 展开后 有 多净今心 、 户妇了、划了、 、 。 。 言 ， 。 。 二 ， 。 再 由矢 量性质得 。 刀 。 。 ， 刀 。 所以 二 以 及同理 有 ， ， ， 由 式得 。 。 习 叫 。 』 习二 、 、 ，、 石 书 一 一 户满犷 一 ’ “ 全决一 粼 伙 竺丝这 户代 忿 口 ‘ 一 孟 所以 。 言 口一尧 ︸刀甘 口 即 老』 兰 同理 ， 由 式可证 得 万 万 将 歇 ， 决 ， 与截 裁 展开后 有 截 ， 砚 、 一 言 ， 、 ， 歇 ， 所以 ， ， ‘

同理： 不1=h1t1 (36) (30),(31),(35),(36)式表明可将(7)~(10)式的运算量减半，以及操作机和负载的 惯性力、离心力与科氏力的系数的性质。 (2)文中的算法表达式(1)由两部分组成，一部分是操作机连杆的动力学贡献，另一部分 是负载的动力学贡献。对于具体的机器人来说，操作机的惯性参数是已知且确定的，负载的 惯性参数是随负载的种类、大小而变的。在机器人动力学的控制中，把动力学方程中的操作机 的动力学贡献以数一符方程的形式给出，由传感器检测负载的动力学贡献，不仅可以大大降 低动力学方程的计算量，而且大大方便了在控制中解决负载动力学贡献问题。 (3)文中的算法在求机器人动力学方程的过程中，不需要求导数运算，也不需要求中间 变量，而且，由于运算过程都是项数较少的欠量运算，所以很少出现数十项的多项式之间的 运算。本算法的另一方便之处是，可只对感兴趣的某项力进行计算，避免了多项力混合运算 的麻烦。 (4)(1)式中包含了摩擦力的影响。由于对摩擦力进行定量分析的研究尚有待于进一步 发展，所以(1)式中的摩擦力表达式未以定量的表达形式给出。但(1)式列出摩擦力项，为 定性分析摩擦力的影响，及采用实验方法确定摩擦力的数值提供了方便。 4结论 文中的算法经实际应用于YG-1型机器人动力学的研究！)表明： (1)该算法是推导或上算开、闭链机构的机器人动力学方程的实用算法。 (2)该算法是便于朔定机器人动力学控制方案的算法。 参考文献 1 Kane T R and Levinson D A.Intl.J.Robotics Res.,1983;2(3):3~21 2马香峰，徐向荣。首届全国机器人学术讨论会论文集，北京，1987 3 Faessler H.Intl.J.Robotics Res.,1986;5(3):129~141 4 Fu K S,Gonzalez R C and Lee C S G.Robotics:Control,Sensing,Vision and Intelligence,Mcgraw-Hill,USA,1987 5 Craig J J.Introduction to Robotics:Mechanics and Control,Addison-Wesley, Reading,Mass,USA,1986 6武斐。机器人动力方程的等价性和改进算法，北京大学研究生论文，1986 7 Kane T R and Levinson D A.Dynamics:Theory and Applications,New York,McGraw-Hill,USA,1985 8陈滨。力学学报，1984：16(3)：312~315 9孙昌国。含闭链机构的机器人操作手动力学研究，北京科技大学研究生论文，1988 256

同理 ， ， ，、 ， 一 ， ， ， 式 表明 可将 一 式 的运 算量减 半 ， 以及操作机和 负载 的 惯 性力 、 离 心力与科 氏力的 系数 的性质 。 文 中的 算法 表达 式 由两 部分 组 成 ， 一 部分 是 操 作 机连 杆 的动力学 贡献 ， 另 一部分 是 负载的 动力 学 贡献 。 对 于 具 体的机 器人 来说 ， 操 作机 的惯 性参数 是 已 知 日确定 的 ， 负载的 惯 性参数是随 负载的种 类 、 大 小而 变的 。 在机 器人 动力 学 的 控 制中 ， 把 动力 学 方程 中的 操 作机 的 动 力学 贡献 以数 一符 方程 的 形 式给 出 ， 由传感 器检测 负载 的 动力学贡献 ， 不 仅可以大 大 降 低 动 力学 方程 的计算量 ， 而 且大 大 方 便 了在控 制 中解 决 负载 动力学 贡献 问题 。 文 中的算法在求 机器 人 动力学 方程 的 过 程 中 ， 不需 要求导 数运算 ， 也 不需 要求 中间 变 量 ， 而 且 ， 由于运 算过 程 都是项 数较少 的 欠 量 运算 ， 所 以很少 出现数十项 的 多项 式之 间的 产 运 算 。 本算 法的 另 一方便之 处 是 ， 可 只对感 兴趣的某项 力进行 计算 ， 避 免 了多项 力混 合运算 的 麻烦 。 式中包含 了摩擦 力的影 响 。 由于 对摩擦 力进 行定量分析的 研究 尚有待于进 一步 发 展 ， 所 以 式 中的摩擦 力 表 达式未 以定 最的 表达 形 式 给 出 。 但 式 列 出摩擦 力项 ， 为 定性分析摩擦 力的影响 ， 及 采 用 实验方法确定摩擦 力的数值提供 了方便 。 结 论 文 中的算法经 实际 应 用于 一 型 机器人 动力学的研究〔 〕 表 明 该 算 法是 推导 或 计算开 、 闭 链 机构 的机 器人 动 力学 方程的 实用算 法 。 该 算 法是便 于 吻定 机 器人 动 力学控 制 方案 的算 法 。 参 考 文 献 。 ， 马 香峰 ， 徐 向荣 首 届全 国机 器人 学 术 讨 论 会论文 集 ， 北京 ， 。 。 。 ， ， ， ， ， ， 、 一 ， ， · 〔 ， ， 一 ， ， ，， ， 武 望 机 器人 动 力方 程 的 等价性和 改 进算 法 ， 北 京大 学研 究生 论文 ， 了 ，， 、、 · ， 从一 ， ， 陈 滨 。 力学 学 报 ， 一 孙 昌国 含 闭链 机构的 机器人操作 手动 力学研 究 ， 北 京科技大学研究生 论 文