机器人操作手的动力学计算

D0I:10.13374/j.issn1001-053x.1985.03.023 北京钢铁学院学报 1985年第3期 机器人操作手的动力学计算 机器人研究宝马香峰 摘 要 本文在凯思动力学方程的基出上导出了一个计算关节型礼器人操作手动力学 方程的公式。它简练、直观，特别易于计算机辅助设计计算。文中还以简例说明 了该公式的计算步骤和方法。 前 言 日前在进行关节型机器人操作手的动力学计算中，常用的有拉格朗日方程法，牛顿一 饮拉法，高斯原理法，阿贝尔方程法。其中用的最多的是拉格朗日方程法，该法虽然在求 拉氏函数时，只需求广义速度，但求广义力时，却要对广义速度和广义坐标求偏导数，最 后还要对t求导。我们在对关节型操作手进行动力学计算时，在凯恩质点系动力学方程1) 的基础上，导出一个公式，利用求操作手各杆关节及质心处的线速度和线加速度以及求各 杆角速度和角加速度的递推公式(5)，不用求导运算，即可方便的求出操作手的动力学方程 一、机器人操作手动力学方程计算公式 1·关于凯恩方程 美国斯坦福大学的凯恩教授，借助于他所提出的“偏速度”、“编角速度”和“广义 速率”等概念，建立了质点系统动力学新方程，人们就把这些方程称为凯思方程。简单导 出如下： 设质点系统有1个自由度，对于完整系统，我们有： r:=ri(q1yq2…gyt） (1) 式中 q1…q1-…一是系统的广义坐标。 山式(1)可得： dt ,i1dg…taq: + (2) 令： =u, d0=u2…+d9=u1 dt dt dt 96

北 京 钢 铁 学 院 学 报 年 第 期 机器人操作手的动力学计算 机 器人 研 究 室 马香峰 摘 要 木文 在 凯 恩 动 力 学方 程 的 基础 土 导 出 了一 个 计 算 关 节型机器人 操作 手动 力 学 方 程 的公 式 。 它 简练 、 直 观 ， 特别 易于计 葬机 辅 助 设 计计 葬 。 文 中还 以 简例 说 明 了该公 式 的计弄 步骤 和 方 法 。 前 言 目前在进行 关 节型机器人操作手的动力 学计算中 ， 常用 的 有拉格 朗 日方程法 ， 牛顿一 欧拉法 ， 高斯原 理法 ， 阿 贝尔方程法 。 其 中用 的最 多的是拉格朗 日方程法 ， 该法 虽然在求 拉 氏函数时 ， 只需求 广义速度 ， 但求广义力时 ， 却要 对广 义速度和 广 义坐标求偏导 数 ， 最 后还要对 求导 。 我 们 在对关 节型操作手进行动力 学计算时 ， 在凯恩质点系动力学方程 〔 〕 的基础上 ， 导 出一个公式 ， 利用求操作手各杆关 节及 质心处的线 速度和线加速度以及求 各 杆角速度和 角加 速度的递 推公式〔 ” ， 不用求导运算 ， 即可方便的求 出操作手的动力学方程 一 、 机器人操作手动力学方程计算公 式 关于凯恩方程 美国斯坦福大学的凯 恩 教授 ， 借助 于他所提 出的 “ 偏速度 ” 、 “ 偏 角速 度 ” 和 “ 一 义 速率 ” 等概念 ， 建立 了质点系统动 力学新方程 ， 人们 就把这些 方程称为凯 思 方程 。 简单 导 出如 下 设 质点系统 有 个 白由度 ， 对于完整系统 ， 我们有 ， … … ，， 式 ‘布 ， 尸 ‘ · … 一是 系统 的广 义坐标 。 由式 可 得 日 十 杯 二 ， 令 … 十 ， · 一 一 迄互 一生红 、 ， 逆 二 一 一 些 些妞 DOI ：10．13374／j ．issn1001－053x．1985．03．023

8r1=v11,6q2 aq1 ⊙ri=Ui2… 8q1 dri=vio dt 则式(2)可以改写成： .=u1+0,2u2…+0, (3) dt 式中 u1,2…u1-称为广义速率， 小 v;1,V:2v;1一称为相应于u1,u2…u,的偏速度。当广义速率与线速率相 当时，称为偏线速度，当与角速率相当时，称为偏角速度。 根据动力学普遍方程： (京：-ma),81=0 (4) i=1 式中 F；一作用于质点i的主动力： m,a:一质点i的惯性力。 由式(4)，凯恩导出了如下的方程式： n 马Ed,+图Q,d=0 小 j=1,2,…l (5) 写成简式则是： F,+F"1=0 (6) n 其中P:=三可，心，称作质点系统相应千，的广义主动力，F1=三心， 称作系统相应于u1的广义惯性力。 公式(6)就叫凯恩方程，它同样适用于非完整系统，这时只须把“当作准速率就可 以了。 2.关节式操作手动力学方程计算公式 设有杆联接的机器人操作手，如图1。暂设各关节（图中表示为“”）为回转副， 日：为关节转角，可取作广义坐标0：为广义速率。各杆（图中表示为“L”)的角速度和 角加速度分别为⊙：和⊙：。各关节点和各杆质心处的线速度和线加速度分别是：U:,v1 VeiyVcio 97

巫二 芍 ， 旦立 若 一彝 芍 ‘ 日 一 “ ’ 。 一 。 了 万亡 一 ‘ “ 则式 可 以 改写 成 豆 一 之 叭 “ ” 二 ” ’ ” ， ” 式 中 ，， … … ， 一一 称 为广 义速率 言 ，若 … …二 一称 为相应于 ，， … … 的偏速度 。 当广 义速率与线 速 率 相 当时 ， 称 为偏线 速度， 当与角速率相 当时 ， 称为偏 角速度 。 根 据动力学普遍方程 三‘ 一 ‘ “ ‘ 。 占 二 式 中 一 一作 用于 质点 的 主动力 “ 一 质 点 的惯性力 。 由式 ， 凯 恩导 出 了如 下 的 方程 式 三 ， · ” “ ， 。 司 写成简式 则是 一 二 其 中 二 鱿 一通 。 ， 称作质点 系统 相应于 ” 的 广 义 主动力 ， ‘ 二 ，三 ， · ” 』， 称 作系统 相应于 的广义惯性力 。 公式 就 叫凯恩方程 ， 它 同样适用 于非完整系统 ， 这 时只须把 当作准 速率 就 可 以 了 。 关节式操作手动力学方程 计算公式 设有 杆联接 的机器人操作 手 ， 如图 。 暂设 各关 节 图 中表示 为 “ ” 为回转副 ， 为关 节转 角 ， 可取 作广义坐标 为广义速率 。 各杆 图 中表 示 为 “ ” 的 角速度和 角加 速度分别 为。 和 。 。 各 关节点和 各杆质心 处 的线 速度和线加 速度分 别 是 。 ， 。 ，， ” 。 一”

对于关节j:和杆1有： o:=0101+0ii02+…+0:in0。 心：=010：+0620a+tu0. da:=0ei101+dei282+…+ui8 将凯恩方程推广到这一刚体系统，机器人操作 手的动力学方程可以表示为： M,=∑m;Ru:;·Ruii+ i=1 .R(l,o+oxl,w)·Roi (7) 图1 式中 M:,一·是相应于0：的广义主动力，对回转副操作手，就是第】个关节需加的转矩 R!-一第i个关节到第j个关节的3×3阶旋转变换矩阵； U-第i个连杆上质心C处的线加速度表示在第i个坐标系中的3×1阶列阵； (注意：此时的左上角标不是乘方数，下同) v一一第i个连杆上质心C处相应于广义逃率0，的表示在第i个坐标系中的偏线速 度，是8×1阶列阵， I:一第i个连杆的惯性张量，是3×8阶方阵， ⊙：一一第i个连杆的角加速度，是表示在第i个坐标系中的3×1阶列阵， ⊙0：一第i个连杆相应于9，的偏角速度，是表示在第i个坐标系中的3×1阶列阵。 和号工的意义是由最末杆或手爪算起一直累加到相应于，的第j个连杆。 关于偏线速度和偏角速度，可用下式计算： 0,=1 Ui1=U:( (8a) 0=0 ⊙1=0：（ 01=1 (8b) 0=0 式中v( 0,=1 )和回( 0时=1 )的意义是：在速度列阵u和o中，令第j个广义 0;=0 8;=0 速率，=1，其他非的广义速率0；=0时，所得到的列阵。 二、实 例 我们以最简单的两杆操作手（图2)为例，说明应用公式(7)求解操作动力学方程的 08

对于 关 节 和 杆 有 。 口 乙 曰 石 ，· · … 十 。 石 。 。 二 节 ” 己 … … 言 。 。 。 。 占 ” 。 毛 … … 将凯恩方程 推广到这 一刚 体系统 ， 手的动力 学方程可 以表示 为 ” 占 。 。 机器人操 作 子 了 ， 艺 。 二 · 之 之 ‘ 名 、 一 八 轰 式 中 。 。 。 · 。 ’ 图 ‘ 一 一 是 相 应于 ‘ 的 广义主动力 ， 对回转 副操 作手 ， 就 是第 个关 节需加 的 转矩， 一第 个关节到第 个关 节的 阶旋 转变换矩 阵 。 二 一第 个连 杆上质心 处的线加 速度表示 在第 个坐 标 系中的 、 阶 列 阵 注意 此 时的左 上 角标 不是 乘方数 ， 下同 诚 。 。 ，一一第 个连 杆上质心 处相应于广义 速率 的表示在第 个坐 标系中的偏线 速 度 ， 是 阶列阵， 一第 个连杆的惯性张量 ， 是 阶方阵， 。 － 第 个连 杆的 角加 速度 ， 是表示 在第 个坐标系中的 又 阶列阵， 。 。 － 第 个连 杆相应于 的偏 角速度 ， 是表示在第 个坐标系中的 阶列阵 。 和 号 名 的意 义是 由最末杆或手爪算起一直 累加到 相应于 的第 个连 杆 。 关于偏线 速度和 偏角速度 ， 可用 下式计算 。 ， 。 吞丁二 ” 。 。 。 生二 廿 二 式 中川 和 。 叭 七 ’ 的意义 是 在速 度 列 阵” 和。 中 ， 令第 个 厂 又 口下 ︸ 一一 ︺口口 矛 ‘ 、 速率 二 ， 其他非 的广义速率先 二 时 ， 所得到的列阵 。 二 、 实 例 我们 以最简单的两 杆操作 手 图 为例 ，说 明应用 公式 求解操作动 力学 方程的

步骤和方法。 已知条件如下表 杆号 杆长重心位置 质量 惯性张量 1 d Lt mi I 2 d2 L2 m2 I2 3 0 0 ms 0 解： 1,设坐标系，求坐标变换矩阵 坐标系的设立如图2所示，关节转角为 日1,02，坐标之间的变换关系是： 图2 C1 -S1 0 S1+S0 T:= S1 Cr 0 0 0 1 0 0 0 0 C2 -S2 d S2+S1 T!= S2 C2 0 0 0 1 0 0 0 1 (1 0 0 d2 S1+S2 T}= 0 1 0 0 0 0 0 0 0 1 标架之间的旋转变换矩阵则是： CL -S1 0 C2 -S2 R9= S1 Ci 0 R:= S2 C2 0 0 0 1 、 0 0 1 0 0 C12 -S12 0 Ri=I=0 1 0 R:=R:=RR:= S12 C120 0 0 0 1 上述矩阵中的原素： S1=sine1,S2=sin02, S12=sin(01+02) C1=c0s91, C2=c0s62,C12=c0s(01+02) 2,求速度和加速度 99

步骤和方法 。 已知 条件如下表 杆号 杆长 国今竺置 性张量 几 卜 山 解 设坐标 系 ， 求 坐标变换矩 阵 坐标系的设立 如 图 所示 ， 关 节转角为 ， 口 ， 坐标之间的 变换关系是 图 一 ， ‘ 卜 … ‘ 一 、 山 ‘ 、 头 、 ， 二 … 一 ，工 ‘ 、 二 … 一 乳。 标架 之 间的旋 转变换矩 阵则 是 一 兮 曹二 二 处 上述矩阵中的原 素 姿 二 盆 兮 尘 一 口 曰︵ 厂 二 口 ， ， 二 ， ，口 ， 二 一 口 求速度和加速度 电 八了‘

①基本公式 @=Ri+i+0K =R:kRx0k U+}=R*1（v+o×Pi+1) u+:=R*1(U+o×P!+1+o×(o×Pi+:)) vct:兰U+}+wt}×Pcti ct:=U:+ot:×Pcti+oi×(o:×Pc:） 式中各符号的意义是 云ti,o:,o,0是第i+1杆和第杆的角速度和角加速度表示在第i+1个和第 个坐标系中的列阵，下角标表示杆号，上角标表示坐杆系号， 0,◆，日1+1是第1+1杆的旋转角速率和角加速率， k:表示第i+1杆转轴的单位向量在第i+1坐标中的列阵，即k}=〔001)T R*1表示第坐标系相对于i+1坐标系的旋转变换矩阵； v:,U:,u,心c:是第i+1杆上坐标系原点O:+:和质心C:+处的线 速度和线加速度表示在第i+1坐标系中的列阵， P◆1是由坐标系到坐标系i+1原点间的距离向量在坐标系i中的列阵； P。}是由i+1坐标系原点0：+1到第i+1杆质心C:+1的距离向景在坐标系i+1中的 列阵。 为了考虑各连杆和被取物件的重心影响，取第○杆（基座）有向上的，即与重力加速 度方向相反而大小相等的加速度，对于本例，则有： u8=〔-g00〕T ②计算各杆的速度和加速度 对于连杆1，取i=0 ⊙i=〔000，〕r 0:=〔0081)r v=0 =(-c1gs1g0〕T ve=〔0L1010〕T 100

①基本公式 。 卜 ‘ 。 卜 ‘ ， 李 。 丰 ‘ 。 才 ‘ 。 乡，， 幸全 ” 令 ‘ ” 。 。 李 ” 丰 屯 。 丰 ’ 。 。 ， 。 。 又 ， 二 川 丰 十 。 李 。 享 川 享 十 。 李 。 李孟 。 丰 。 李 又 。 享 式中各符号的意义 是 。 李 ， 。 拿多 。 ， ， 是第 一 卜 杆和 第 杆 的 角速度和 角加 速度表示在第 个 和 第 个坐标系中的列 阵 ， 下角标表示杆 号 ， 上 角标表示坐 杆系号 ， ， ， ，是第 杆的旋 转角速率和 角加速率， 京 表示第 杆转轴的单位向量在第 十 坐标中的列阵 ， 即 丰 二 〔 〕 ’ 今 ‘ 表示 第 坐标系相对于 坐标系的旋转变换矩 阵 李 ， 。 李 ， 。 。 李 ， 。 。 才 是第 杆 上坐标 系原 点 ，十 和质心 ， 处 的 线 速度和线加 速度表示在第 坐标系中的列阵， ， 是 由坐标系 到坐标系 十 原 点间的距 离向量在坐标系 中的列阵， 。 支 是 由 十 坐标 系原点 到第 杆质心 的距 离 向量在坐标 系 十 中 的 列 阵 。 为 了考虑各连 杆和被取物件的重 心影响 ， 取 第 杆 墓座 有 向上的 ， 即 与重 力加 速 度方向相反而 大小相等的加 速度 ， 对于本例 ， 则有 “ 〔 一 〕 ， ②计算各杆的速度和加 速度 对于连 杆 ， 取 口 〔 〕 ， ” 二 。 〔 ” 〔 一 〕 ， 〕 丁 。 。 卜 〔 〕

c=〔-c1g-L1(01)2s1g+L190〕r 对于连杆2，取i=1 0 0 }= 0 o;= 0。 01+日2 0,+92 dI s2 u=· dI c2 = -c12g+d1s21-d1c2(d1)2） s12g+d1c201+d1s2(01)2 0 d1 s2 Vci= d1c2i,+L2(i1+2) 0 -c12g+d:S28:-d1c2(81)2-L2(61+02)2 s12g+d1c201+d1s2(81)2+L2(8,+日2) 0 对于连杆3，取ⅰ=2。对于本例，并没有第三杆，只是用以考虑加在第二杆末端的 质量为m:的负荷（重物）。 0}=⊙=oi 0}=w}=w好 d1s261 v=Uc3= d1ci.·.(01+62) 0 0=e= -c12g+d1s201-d1c2(6:)2-d2(61+02)2 s12g+d1c201+d1s2(1)2+d2(月+82) 0 (③求偏速度 根据公式(8)，得： 偏角速度 oii:=〔001)T ⊙i1=0 ⊙i:=〔001〕7 oii2=〔001〕T @3i1=@ii1 ⊙ii:=0iig 偏线速度 vci,=〔0d10)T Vclis=0 vcii1=〔d1s2d1c2+L?0〕r uci:=〔0L20)T 101

。 。 “ 〔 一 一 ， 对 于连 杆 ， 取 。 。 一 。 万 一 乡 ， 、 … 一 、 、了 一 ，口 、 、 ， 厂 口 一 一一 ， 勺 一 一 一 ， 一 一 一 么 … 吕 卜 一 对 于连 杆 ， 取 。 对于本例 ， 并没 有第三杆 ， 只 是用 以 考虑加在第二 杆末 端 的 质量 为 的负荷 重 物 。 。 聋二 。 孟二 。 口 璧二 。 孟二 。 若 、厂、 八 十 。 “ 了、 ‘ 。 ‘ 。 里二 。 。 聋 。 ‘ 、 。 二 。 一 一 一 一 莎 ③求 偏速 度 根据公式 ， 得 偏 角速度 ， ‘ 〔 〕 了 。 ‘ 。 。 〔 〕 。 若‘ ， 〔 〕 。 璧‘ 。 二‘ 。 ‘ 口 ‘ 偏线 速度 ” 。 ‘ 〔 〕 ” 。 ‘ 。 。 〔 〕 丁 ” 。 。 二 〔 、

vei1=〔d1s2d1c2+d20〕T vci:=〔0d20)T 4,求动力学方程式 相应于0：的广义主动力，实际上就是促使i杆相对于i-1杆发生角位移的关节力矩， 利用公式(7)和上面的相应值并取： I1:: 0 0 I= 0 I￥ 0 012yy 0 0 0 0 I2 Mi,=ms〔d2512g+d1d2c201+d1d2s2(01)2+d(81+02)〕 +m2〔L2s12g+d1L2c261+d1L2s2(01)2+L:(0,+92)) +12.(01+02) Mi=m3〔(ds1+d2s12)g+(di+d1d2c2)01+d1d2s2(0:)2 +(d+d1d2c2)(8i+02)-d1d2s2(01+02)2) +m2〔(d1s1+L2s12)g+(d1+d1L2c2)81+d1L2s2(日1)2 +(d:+dL2c2)(0,+62)-d1L2s2(01+02)2) +m1(L1s1g+L,)+12.(61+02)+11.6: 经过整理可得： M=J(0)9+V(0,0)+G(0) 式中 M=〔Mi1Mi:〕T 日=(61日2)r Ju J12 J(0)= (J21J22 Ji1=di ms+m2)+2did2c2 (m3+m2)+dims +Lim2+Lim+I1:+12:, J12=J21=dic2 doms+L2m2)+dims+2.. J22=di ms+Lim2+I2.. V(0,0)=〔V1V2)r V1=-2d1s2(d2mg+L2m2)802-d2s2(L2m3+L2m2)(02)2 V2=d1s2(d2mg+L2m2)(0:)2 G(0)=〔G1G2)T 102

互‘ 〔 ， 〔 〕 甲 。 〕 求动力学方程 式 相应于 的广义 主动力 ， 实际上就是促使 杆相对于 一 杆发生 角位移的关节 力 矩 ， 利用 公式 和上面的相应值并取 一 ， ， 二 ， ， … ‘ 〔 ， ， “ 尝 〕 〔 “ 委 一 卜 〕 … 二 ‘ 二 〔 ， 璧 一 、 至 … 二 ， 一 ， 〕 〔 一 一 二 一 “ 〕 … ， ， 、 经 过 整理可 得 ， 式 ‘朴 〔 ‘ ， ‘ 〕 〔 一 〕 ， 二 竺 ， 委 十 ， 十 二 一 二 ， 盆 二 二 委 了 ， 〔 〕 一 一 ， ， 〔 〕 了

GI=((dis+d2s12)ma+(dis+L2s2)m2+Lim]g G2=(d2ms512+L2512m2）g 四、结 语 本文在凯恩方程的基础上所导出的解算多杆开式链由转动副组成的机器人操作手动力 学方程的公式(8)，计算比较直观，简练，比之现有的计算公式，可简少一部分计算 量。特别适合于计算机辅助解算。何既然操作手是一复杂的刚体系统，故所求得的结果依 ” 然是复杂的。 在对具体的实用关节型机器人操作手求解过程中，还有很多省时的计算技巧和方程式， 同时还可以把公式(8)推广到具有移动副操作手的动力学计算中去。限于篇幅，待另文 讨论。 参考文献 〔1〕吴镇，《分析力学》，上海交通大学出版，（1984).9。 2 Richard P Raul,Robot Manipulators:Mathematics,Programm- ing,and Contro1》MIT Press,(1981)。 〔3〕M.Vukobratovic',V.Potkonjak,《Dynamics of Manipulation Robo- ts>,Springer-Verlag Berlin Heidelberg New York,(1982) 〔4)Roth,来华（西南交通大学）讲学资料(1984) 〔5〕马香峰，《确定机器人操作手的位姿及关节力矩的实用算法》，北京钢院学报 机器人系统专辑，1985。 A Method to Calculate Dynamics Eguations of Robot Manipulator Ma Xiangfeng abstracts This paper presents a formula which can be used to calculate dynamic eguations of goint-typical robot manipulator.This formula is simple,direct, especially is suitable for calculation with computer.In this paper the process to use this formular is also introduced with a simple example. 103

翻 一 二 〔 ， ， 一 一 〕 一 四 、 结 语 本文在凯 恩方程 的基础上所导 出的解算多杆开 式链 由转动 副组成的 机器 人操作手动力 学方程 的公式 ， 计算 比较直 观 ， 简练 ， 比 之现有的计算公式 ， 可简少一 部 分 计 算 量 。 特别适合 于计算机辅助解算 。 但既 然操作 手是 一 复杂的 刚体 系统 ， 故 所求 得的结果依 然是 复杂的 。 在 对具体的实用关 一 ，’ 型 机器 人操 作 手求解过程 中 ，还 有很 多省时 的计算 技巧 和 方程式 ， 同时还可 以把公式 推广到具 有移动副操 作手的动 力学计算 中去 。 限于篇幅 ， 待另文 讨 论 。 参 考 文 献 〔 〕 吴镇， 《 分析力 学 》 ， 上海交通 大学 出版 ， 。 〔 〕 ， 《 ， ， 》 ， 。 〔 〕 。 ‘ ， ， 《 》 ， 一 ， 〔 〕 ， 来华 西 南交通大学 讲学资料 〔 〕 马香峰， 《 确 定机器 人操作 手 的位姿 及关 节力矩 的实用算法 》 ， 北京钢院学报 机器 人 系统 专辑 ， 。 一