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

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 j<i 偏角速度： 0 R-:(oi1×i+d。1) j<i 关节偏速度：1= 0 ≥ +©：，× <t 质心偏速度：：：= ×d症 j=i 0 j>i 式中：上角标表示坐标系序号，下角标表示连杆序号；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

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