闭链机构机器人的运动分析和动力分析

D0I:10.13374/j.issn1001-053x.1987.01.010 北京钢铁学院学报 第9卷，第1期 Journal of Beijing University Vol.9 No.1 1987年1月 of Iron and Steel Technology Jan,1987 闭链机构机器人的运动分析和动力分析 李德高 马香峰 (机器人研究所) 摘 要 本文作了单闭链结构机器人的运动分析和动力分析。作运动分析时，將单闭链 分为左右两路，建立某一中间杆的位姿.速度和加速度恒等式，求出了从动关节的 位移、速度和加速度关系式。在此基础上，导出了各杆速度和加速度计算公式。然 后，应用Kane方程导出了单闭链结构机器人的动力学逆问趣计算模型。 关键词：机器人.闭链机构.Kae方程，动力学逆问题。 Dynamic and Kinematic Analysis for Robots with Closed Kinematic Chains Li Degao Ma Xiangfeng Abstract This paper provides kinematic and dynamic analysis for robots with the structure of single-closed kinematic chain,The single-closed kinematic cha- in is divided into left and right paths for establishing the motion equations of an intermediate link,and then the displacement,velocity and acceleration of the passive joints are derived.On the ground of these equations,the formulas of calculating velocity and acceleration of each link are obtained.Finally, the inverse dynamics of the robots is modelled,using Kane's dynamical equa tions. Key words:robot;closed-chain mechanism;Kane/s equation;inverse dynamics 1986-03-26收稿 64

第 卷 ， 第 期 北 京 钢 铁 学 院 学 报 这 。 扮。 。 年 月 闭链机构机器人的运动分析和动力分析 李德高 马香峰 姿 〔 机器人研究所 摘 要 本文作了单闭链结构机器人的运动分析和动力分析 。 作运动分析时 ， 将单闭链 分为左右两路 ， 建立某一中间杆的位姿 速度和加速度恒等式 ， 求出了从动关节的 位移 速度和 加速度关系式 在此基础上 ， 导出了各杆速度和加速度计算公式 然 后 ， 应用 方程导 出了单闭链结构机器人的动力学逆问题计算模型 关键词 机器人 闭链机构 方程 动力学逆问题 公 ” 了 一 一 ‘ ， ， ， ， ， 只 竹 。 一 产 一 一 收稿 DOI ：10．13374／j ．issn1001－053x．1987．01．010

引 言 机器人动力学作为机器人技术和机器人学的一大课题，已经得到了广泛的研究。它在机 器人的机构和控制系统设计中具有十分重要的意义。 至今，机器人动力学研究主要集中在开链机构上，闭链机构研究得还不充分。Y.F, Zheng和J.Y.S.Luh在这方面做了有效的工作.他们在文c1)中采用拆副法，把闭链机构假想 切开成为树型开链结构，用开链机构动力学进行分析计算，假想切开的开链机构与原闭链机构 通过约束等式方程和拉格朗日乘子联系起来。在动力学研究中，牛顿一欧拉方法以其分析简 单、计算时间短的优点而被广泛采用。近年来，应用Kane动力学方程解算机器人动力学问 题也引起了广泛的重视(2)。我们将Kae方程与速度、加速度递推算法相结合而导出了开链 机构机器人的动力学方程的递推算法(3)，发现它计算速度更快，时间更短。 本文将用Kane方程来研究单闭链机构机器人的动力学逆问题。作运动学分析时，我们 把闭链机构分为左右两路，分别从两路建立某一中间杆的位姿、速度和加速度恒等式，从而导 出从动关节的位移、速度和加速度关系式，并利用中间计算结果求各杆的速度和加速度，最 后应用Kane方法作动力学分析。 1运动分析 设有一单闭链系统（图1），从基础杆O到某一中间杆M有左右两条路径。在基础上设一 固定坐标架，每一连杆上设一固连坐标架。R:,】为固连坐标架到固连坐标架的变换矩阵。 向量的表示规定为：如果坐标架i内某一向量表示为a,则它在固定坐标架内表示为1。显 然，a1=Ro,ia1o Joint L+ joint L+1 L joint L' ojoint L joint 3 ● o Joint 3 2 joint'21 joint 2 joint 1 0 joint 1 图1闭链机构 Fig.1 A Closed chain mechanism 65

引 言 机器人 动力学作为机器人 技术和机器 人学 的一 大课题 ， 已经 得到 了广泛 的研究 。 它 在机 器人 的机构和控制 系统设计 中具有十分重要 的意 义 。 至 今 ， 机器人 动力学研究主要 集 中在开链机构上 ， 闭链机构 研 究 得 还 不充分 。 夕 和 在这 方面做 了有效 的工 作 。 他 们在文 〔 〕 中采用拆副 法 ， 把闭链机构假想 切 开成为树型开链结 构 ， 用 开链机构动 力学进 行分析 一 卜算 ， 假想切 开 的开链机构与原 闭链机构 通过约 束等式方程和 拉格 朗 日乘子联 系起来 。 在动 力学研究 中 ， 牛顿一 欧拉方法 以其分析简 、 单 、 计算 时 间短 的优点而被 广泛采用 。 近 年来 ， 应 用 动 力学方程解算机器人 动 力 学 问 题 也引起 了广泛 的重视〔 “ 〕 。 我 们将 方 程与速 度 、 加速度递推算法相结 合而 导 出了 开 链 少 机构 机器人 的 动 力学方 程 的递推算法 〔 〕 ， 发 现它计算速 度更快 ， 时 间更短 。 本文将 用 方 程来研究单闭链机构 机器人 的动力学 逆 问题 。 作运动学 分析时 ， 我 们 把闭链机构分为左 右两 路 ， 分别 从两路建立 某 一 中间杆 的位姿 、 速度和加 速度恒等式 ， 从而 导 出从动关 节 的位移 、 速度和 加速度关 系式 ， 并利 用 中间计算结果求 各杆 的速度和加速度 ， 最 后应 用 方法 作 动力学 分析 。 运动分析 设 有一单 闭链 系统 图 ， 从基础 杆 到 某一 中间杆 有左右两 条路径 。 在基础上设一 ， 固定 坐标架 ， 每一 连 杆上设 一 固连坐标架 。 ， 为 固连 坐标架 到 固连 坐标架 的变换矩 阵 。 向量 的表示规定 为 如果坐标架、内某一 向量 表示茄 ， 则它 在 固定坐标架 内 表 示 为。 ‘ 。 显 然 ， ， ， 芝 七 ’ 七 工 二 图 闭链机构

连杆i的长度向量为，质心位置向昼为（图2a);连杆M有两个质心位置向量+： (=1+1)和，+1(=立：)（图2b),连轩0的长度向量为10（图2c)。关节i的轴线 方向的单位向量为e:。 joint i+1 C joint 1 joint 1 joint +7 jcint L+1 joint i a 0 图2连杆参数 Fig.2 Parameters of links 1.1位姿分析 设单闭链系统具有k个自由度。为了保证机构运动的唯一性，系统应该具有k个驱动关 节，其余m(=L+L'+2-k≤6)个关节为从动关节。把驱动关节变量记为q(j=1, 2,…,k),从动关节变量记为q（i=1,2,…,m)·。在不考虑系统瞬时自由度〔4)的情 况下，系统的运动由k个驱动关节变量唯一决定。这时，· qi=q引(qi,q8,…,q￥) (1) 其中，i=1,2,:,m。 (1)式为从动关节与驱动关节的位移关系式。对于简单的闭链机构，形如(1)的显 函数关系式或简单的隐函数关系式是可以直接得到的。但是，对于复杂的闭链机构，q与q分 之间的关系往往十分复杂，甚至不可能得到形如(1)的显式，只能得到一组复杂的隐函数关 系式。这组关系式可通过位姿约束等式面得到。 L+1 L'+1 Ro,-Ro,i=0 (2) i0 i±1‘. （R0,R,2…RL,M)（R0,R1,g…RL,n)=E (3) 。 其中，E为3×3单位阵。 (2)式为位置约束等式，从中最多可得出3个独立的标量方程；(3)式为姿势约束 等式，最多也只能得出3个独立的方程。这样，从位姿约束等式中最多能得出G个独立方 程。实际上，独立方程个数总是与从动关节个数m相等的，因而，从这m个独立方程中总可 解出m个从动关节变量q来。 闭链机构运动分析的关键在于位姿分析，在位姿分析的基础上作速度和加速度分析并不 困难。对于简单机构，如能导出(1)式，直接求导就能得到q和q;对于复杂机构，对 (2)和(3)式求导之后，解一线性方程组也可得到q和q?。但是，求导过程难于在计算 ·台上标“A”和“P”分别表示该元素为驱动关节元紫和从动关节元素。以下相同 66

连 杆 的长度 向量 为二 ， 质心 位置 向量为盯 图 连 杆 有两 个质心 位置 向量 万 十 ， 和 工 ， 十 ， 兀 图 ， 连杆 的长度 向量 为 。 图 。 关 节 的轴 线 方 向的单位 向量 为砚 。 门七 了 氛 、 七 工 一 图 连杆参数 位 姿分析 设单 闭链 系统具有 个 自由度 。 为 了保证机构运动 的唯一性 ， 系统应该具有 个 驱 动 关 节 ， 其 余 二 产 一 三 个关 节 为从动关节 。 把驱 动关节变量记为 全 二 ， ， 一 ， ， 从动关 节 变量记 为 丫 二 ， ， … ， ， ’ 。 在 不考虑系统 瞬时 自由度“ 〕的情 况下 ， 系 统 的运动 由 个驱动关节变量唯一决定 。 这 时 ， 丫 了 令 ， 只全 ， … … ， 会 其 中 ， ， ， 二 ，， 。 式为从动关 节与驱 动关 节 的位移关 系式 。 对于简单 的 闭链 机构 ， 形如 的显 函 数关 系式或简单的隐 函数关 系式是 可 以直接 得到 的 。 但是 ， 对 于 复杂 的闭链机构 ， 叮与 全 之 间的关 系往往十分 复杂 ， 甚至 不可 能得到 形如 的显式 ， 只 能得到一组 复杂 的稳 函数关 系式 。 这 组关系式可通过位姿约 束等式而 得到 。 士 乙 一 尹 。 ， 一 丁 ‘ 一 三 ， “ 。 ， 孔 一 ﹃、 了 ， ， … … 。 ， 、 ， ，‘ ，， ‘ … … ‘ ， 、 其 中 ， 为 单位阵 。 式为位置约 束等式 ， 从 中最 多可 得 出 个独立 的标量方程 式为姿势约束 等式 ， 最 多也 只能得 出 个独立 的方程 。 这 样 ， 从位姿约束等式 中最多能得 出 个 独 立 方 程 。 实 际上 ， 独立方 程 个数 总 是 与从动关 节 个数 相 等的 ， 因而 ， 从这 个独立方程 中总 可 解 出 个从动关 节变量 来 。 闭链机构运动分析的关键在于位姿分析 ， 在位姿分析 的基础上作速度和加 速度分析并不 困难 。 对于 简单机构 ， 如 能导 出 式 ， 直接求导就 能 得到 犷和 ， 对于 复杂 机 构 ， 对 和 式求导之后 ， 解一线 性方程组也可 得到 育和 了 。 但是 ， 求 导过程难于在计算 右上标 “ ， ， 和 “ ” 分别表示该元素为驱动关节元素和 从动关节元素 。 以 下相 同

机上实现，q和q的表达式必须在编程前导出，而且.很繁琐，这都不利于计算机程式化。 为了解决这一问题，我们提出了下面的速度和加速度分析方法。 1.2速度分析 连杆M的角速度可分别沿左右两路得到： +L+1→ ow=∑e:(1-5:)q:（右路) (4a) is1 L'+1 e;(1-s:)q:(左路) (4b) 由 oq (⊙-⊙)=0 (4c) 得到 x0q1+文=0 (j=1,2,…,k)· (4) q 式中 x,=e,(1-s,)h, (5) -( 第r个关节为旋转关节时， 第r个关节为移动关节时， =( 第r个关节位于右边路径时， 第r个关节位于左边路径时。 同理，根据M杆质心速度 L+1 L+1→ 文M=2,e:(1-s:)×p1q:+E,c1s:q:°（右路） (左路有类似的形式)可以得到 三g+9=0(1=1,2,…k) (6) 0q1 式中， -[,1-s)×日+s,小. (7) 苏芝(ia (7a) (5)和(7式中，对于从动关节，r=1,2,“,m,对于驱动关节，r=1,2, …yk。 综合(4)和(6)式，有 +〔U)=0 (8) ⑦qA 67

机上实现 ， 云和 云的表达 式 必须 在编 程前导 出 ， 而 且 很繁琐 ， 这 都 不利 于计算机程 式 手匕 为 了解决这 一 问题 ， 我们提 出了下面 的速 度和 加速 度分析方法 。 速度分析 连 杆 的 角速度可分别沿左右两 路 得到 月卜 二 十 名 一 右路 一 左 路 、 七 易护 一一 。 一 。 益 得到 皿 一卜 艺 式 中 口 全 十 今二 二 ， ， 。 二 ， 了 万 二 一 第 个关 节为旋转关 节时 ， 第 个关节 为移 动关 节时 ， 第 个关 节位于右边路径 时 ， 一 第 个关节位于左 ， 边路径 时 。 同理 ， 根据 杆质心 速度 。 一 右路 。 … “ ， ︸ ， 月卜 二人 名一一 。 左路有类似 的形式 一 只 可 以 得到 县 介 一 二 一鲜 一 ， 式中 、」已 二 贬 一 且 一 ， 乏 、 。 ，今一 和 式 中 ， 对 于 从动关节 ， 。 综合 和 式 ， 有 ， … ， ， 对 于 驱 动关节 ， 〔 〕 车 · 〔 二 。 刁 八

式中， (w)=()xa,U)=(A)x,〔g〕=(g)ax (9) 根据前面的分析，〔W〕的秩应为m。划去6一m个线性相关的行，得一新矩阵 、〔Wo〕mXm。〔Wo)为可逆的。同样划去〔U)中相应的行后，得〔Uo)mXk。于是， (8)式成为 〔Wo) 〔g)+(Uo)=0 (10) 0g4 ,求解(10)式，得 〔g〕=-(wo)-1〔Un)。 (11) 8qA 由(1)式可得到 af=之的d9,(i=1,2,,m) j=10q 写成矩阵形式并考虑到(11)式，有 〔q'〕=-〔Wo)1〔Uo)〔q〕。 (12) 式中， 〔q〕=〔q!)mX1,〔q门=(qf)kX1 1.3加速度分析 M杆的角加速度可以由右路得到 →L十1→ L十1i-1-→ eM=Σe:(1-s:)qi+∑e1(1-51)×e1(1-s1)g1g1 i=1 i=2j=1 由左路得到的M杆角加速度eM有类似的形式。根据ex和eM相等，可得到 ：qf+之x对9的+a+1-a'1=0 (13) i=1 j1 式中 L+1i-1→ a+1=空2e,(1-s1×ei（1-si)q1q (14) i=2j=1 上式中的求和上下限改为左路上相应的杆号之后，所得到的表达式即为L'+1。 根据M杆的质心加速度 应=营(1-s)×+〕i L+1 68

式 中 ， 〔 〕 〔 〕 二 冉 、， 根据前面 的分析 ， 〔 〕 的秩应为 。 划 去 多一 个 线 性 相 关 的 行 ， 得 一 新 矩 阵 〔 。 〕 。 二 。 〔 。 〕 为可逆 的 。 同样划 去 〔 〕 中相应 的行后 ， 得 〔 。 〕 。 于 是 ， 式成为 〔 〕 〔』琴 月 〕 十 〔 。 〕 求 解 式 ， 得 一 〔 〕 一 〔 〕 。 由 式可 得到 ， ， … ， 一 二 · ︸二 口︸日 一叉 一 。时 侧闷 写成矩 阵形式并考虑到 式 ， 有 〕 一 〔 〕 一 〔 〕 〔 式 中 ， 〔 〕 〔 ， 〔 〕 李 又 加 速度分析 杆 的角加速度可 以 由右路 得到 。 二 一 斗 艺 一 艺 艺 一 一 ， ， 一 一 由左路 得到 的 杆 角加速度 。 益有类似 的形 式 。 根据。 和 益相等 ， 可 得到 一，， 。 。 一 卜 。 。 一卜 刁卜 里 ， ‘ “ 了 十 三 令 今 “ 一 “ ‘ · “ 。 式 中 一 一 ， 艺 叉 一 ， 只 一 ， 上式 中的求和上下限改为左 路上相应 的杆号之后 ， 所得到 的表达式即 为 ‘ 。 根据 杆 的质心加 速度 色 士 艺 〔认 一 丫 认 认 〕 斌