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

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

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