D0I:10.13374/j.is8m1001053x.1986.03.014 1986年9月 北京钢铁学院学报 NO.3 第3期 Journal:of.Beijing University Sept.1986 of Iron and steel Technology 机器人动力学Lagrange方程 计算的内积法 冯德坤 (数学第一教研盆) 摘要 用拉格朗日动力学第二类方程建立机群人动力学算法，是一种常用的，行之有效的 方法，但是，计算起米很繁。Pa1引入平移和旋转微分向量进行化简，最后得到了 近似解1). 本文在用拉氏方程得到机器人动力学算法的基础上，引进线性空间的内积概念 得，到内积法，并用它来计算我院讥器人R0B0T一1的动力学方程，可使计算大 为简化. 关键词， 机器人，Lagrange方杜，内积法 The Law of Inner Product of Computation for Robot Dynamics in Lagrange Equation Feng Dekun Abstract Using Lagrange the second type equation of mechanics to set up com- putation of robot dynamics is an effective method that used frequent- ly.But it is quite acomplicatcd one.It must be simplified being intro- duced differential translation and differential rotation by pual,and an approximate solution is obtained at last. In this paper,on the base of the Lagrange equation of the dynami- 1986一01一08收到 106
年 月 第 期 北京 钢 铁 学 院 学 报 称 一 乒牡 。 。 门 机器人动力学 ” 方程 计算的内积法 刁才，卜口刃 冯德坤 数学第一教研室 摘 要 用 拉格朗 日动力学 第二类方 程建立机器 人动力学算法 ， 是一 种常用的 ， 行之有效的 方 法 ， 但是 ， 计算起来很焦 。 “ 玛 入平移和旋转微分 向量进厂化 简 ， 最后得到 了 近似解〔 〕 。 本文在 用拉氏方程得到机器 人动 力学算法的她础 上 ， 引进线性空 间的 内积概念 得 到 内积法 ， 并用它来计算我院机器人 一 的动力学方程 ， 可使计算大 为简化 。 关桩词 机器人 ， 。 方程 ， 内积法 七 ， 七 ， ， ， 一 一 收 到 毛 DOI ：10．13374／j ．issn1001－053x．1986．03．014
cs in the computation method,the concept of inner product in the lin- ear space can be introduced and the law of inner product can be obta- ined too.The quite a simplicated equation can be apply to compute the dynamic equation of robot-1 in our university Key words:robots Lagrange equation;method of inner product 1动力学方程及内积公式 机器人第杆的动力学方程为 F1=D0,+L9+2, 5 Diqiq+D (1) j1 j-1 k=1 D盘ac(,) 5 p=mex(i,j)aqi (2) 5 Diik=之Trace. 9、，） P-mex(i,j,k)aqi aqk (3) D点，： 5 (4) p=i 以上公式及各符号的意义，见〔1)第六章 由动力学方程看山。要计算力矩，关键是要求出T(1,沿；）)及Trace （。,-).但是，它们的展开式晨复杂。例蜘我院机得人R0B0T-1 的Tac©（沿：1,2）、就有188项。每一项均有七八个因子相乘，因此， 直接计算是很困难的。下面，用内积法来建立它们的直接计算公式。 对于1rac(,).令 T=B=(b:)4x4=(b.,ba,ba,ba),bi=(bi,b2ibjb) aqi aT-C(c:)4X4=(c3c),c=(ccaci) aqi 这里的5，与c分别是矩计，与矩阵：的列向量。它们是广义坐标的确数。 对于任意的qi,所有的b;与c1必构成实数城上的线性空间。如果取b;与c;前三个儿 素组成列向量，且记 107
。 亡室 ， 王 五 室 一 动力 学方程及 内积公式 机器人第 杆的动 力学方程 为 ‘ 艺 二二 艺 艺 ，‘ 、 十 “ 叠 。 里里狂 。 旦工扩、 云， 一 、 艺二 口 ， ，丘 ， ， 落 、 少里 、 口马 合 ， ， 夕 ， 扩﹁ 月 乏 一一 ︸ 以上公式及各符号的意 义 ， 见 〔 〕 第六章 ， ，、 、 ， ‘ ， ， 。 、 ‘ 。 ‘ 上 ‘ … ， 。 了 ， ， ， ， 、 ， ， 田 动 刀 子 力 往石 山 ， 女 汗 异刀 犯 ， 大 健足女 水 山 ， 飞石二， 广石万一 ，仪 七 、 “ 悦 ， ， ， 口 丁 ， 乡 。 。 但是 ， 它’ 的 展开式，复杂 。 例如我院机器人 。 ” 一 ‘ ， 口 玉 乡 、 直接计算是 很困难 的 口 。 ， 口 ‘ 。 下 面 ， ， 就有 项 ， 每一 项均有七 、 个 因 子 相 乘 ， 因 此 ， 用 内积法 来建 立它 们 的直接计 算公式 。 对 于 二 会 ， · 寄 令 二 二 ‘ ， ， ， ‘ ， 二 ， ，， ， 、 ， 加 翻今 一 十 今 〕 二 二 ， 、 ‘ 大 ‘ 二 ，， ， 。 ， ， ，二 ， ，， ， ， ， ， 一乡刁一 这 里 的 与成 ’ 分 别 是 矩阵 ， 刁 ， 与 矩阵 ， 乡 的列 向量 ， 它 们 是 广 义 坐 标 的 函 数 。 对于任意的 ， 所有 的 素组 成 列 向量 ， 跳记 与 必 构 成实 数 域上 的线性 空 间 。 如果取 与 了 三 个 元
则这些向量的全部就会构成三维的向量空间，它们的内积是存在的。 现在，耳令伪惯性矩阵为 J,(入1)4X4 则有 TJ,=w=(o1)4X401k=之b1s入k aq S=1 .8=w2-(d)x aqi aqi d1=分w1,c,=三01，=交之b1入6c1: 4， 40 r=1 fl =1S=1 由于以上矩阵使用齐次坐标，故有 ma(,）盒4-点点盒w -名总（含bs,） rl S=1 总名〔6，门 =11 为了方便，以换S,.换r,省略向量的左上标，则有内积公式为 Trace ar ,eT▣含之AC,c (5) 上式说所.Tac(对产，：)停于知阵B=对与矩阵C-沿中每 aqi aqi 列前三个元素所构成的列向量作内积，再乘上相应的入值(J,矩阵的元荣)，然后求其和。 公式(5)的意义是明显的，它把三个矩阵(B、Jp和C)相乘后取其积，化简为 两个矩阵B与C列向量的内积。同时，由于TP矩阵是刻画机器人关节的平移和旋转的， 而旋转矩阵的正交矩阵，其列向量的内积等于零。TP求导之后，列向量仍然保持某些 正交性质，因此，内积公式（5)大大地简化了计算 下面，以我院机器人为例，说明(5)式的用法。 2举 例 2.1 ROBOT一1计算简图及标架选取如下 108
叶 一 冷 、苍 台 二， 去匕 则这些 向量 的 全部就会构 成三维 的 向量空 间 ， 它们的 内积是 存 在的 。 现在 ， 再令伪惯性矩阵为一 ‘ 卜 ， ， 入 ， 则有 、 一二 。 乡 ， － 二 返 。 ‘ 。 ， 、 艺 入 、 口 ， 口 、 二 竺些兰 乡 一火 勺一 ， 二 艺 艺 ， 入 ， 口 艺 由于以上矩 阵使用 齐次 坐标 ， 故有 乡 ， 艺 勺白 艺 一 － 护 卫工亡 、 二 刁 玉 ， 艺 ， 是 一 乏 几 吸 乏 ，。 一 ， 生 五 、主 ‘ 吕 〔 “ 订二〕 为 了方便 ， · 斗 烨， 一 换 ， 省略 向量的左上标 ， 则有内积公 式为 一 ” ， ， 么 么 ， 广 常 二 通 二丁－ 补 － ‘ 币 六 一 ， 七 、 口 ， 仑 一 一 、 ， 二 。 ‘ ，，，， ， 乡 诊 丫 口 ， 、 赫 ， 。 ，。 ， ， ， ， 户 · ， ，， 、 仁 不、 民 妇 ， 以 七 气 万 二一一 下二了 一一 ‘石 〕 口 一 为三 工 一 沙 刃 一一 刁 为三 牛 灿 二 口 ， 甲 遥尽一 列前三个元素所 构成的列 向量作 内积 ， 再乘上相应的入值 ， 矩 阵的元素 ， 然 后 求其和 。 公 式 的意 义 是 明显 的 ， 它 把三个矩 阵 、 和 相 乘后取其 积 ， 化简为 两 个矩 阵 与 列 向量 的 内积 。 同时 ， 由于 矩 阵是 刻 画 机器人关 节的平移和旋转的 ， 而旋 转矩阵的正 交 矩阵 ， 其 列 向量 的 内积 等 于零 。 求导之后 ， 列 向量仍然 保持 某 些 正 交性质 ， 此 ， 内积 公 式 岛 大 大地简化 了计算 下面 ， 以 我 院机器 人为 例 ， 说 明 式 的用 法 。 举 例 气 一 ， 、 一 计算 简 图及标架选 取 如下
2e0 0,=500 X. 0-0: 0s+B4. y Xo X 1 5 2.2An矩阵 使用以下记号 S:=sin0:,C=cos0:,Six=sin (0:-0) C1k=cos(0,-0x),S's1=Sin(05+i0:),C's4=cos(0,+i0:) 其中，i为传动比。 据ROBOT一1结构图有 c10S:0 -5-C20 A= S10C10 A?= C2 a2C: .0100 0 01 0 0001 0 00 1 -5430C43 0 Aa 0 aac3 A4= C430S43 0 0 0 0 0 10 0 0 0 0 1 0 0 0 1 -564 0 0 0 As= s4 c's+0 0 0 10 0 01 2.3T,矩阵 T。=A1A…,(P=1,2,…,5)。例如 S1 cIc ci(aaca-a2s2) T-A…A-5s4c155y5:(aca-a25:) el o sa as3+a.c. GO 1 2.4导数矩阵 109
丫 ‘ 雄 一 一 视哺 研 一研 ， 本 。 一 之 入 坦吕““ ‘ 、 ， 习 〕 八 矩 阵 使用以 一 下记 号 ， 二 ， ， 一 、 二 ， 一 ， ‘ 。 、 二 ， 。 ， ‘ 。 二 、 其 中 ， 为传 动 比 。 据 一 结 构 图有 一 二 二 一 厂尹、 一 一 ，刀。 ‘ 、 一 召 二 么 一 连 ， ， 一 ， 斗 ， ，矩 阵 二 ， … 八 ， ， ， ， ， … 。 例如 、 二 八 尸 二 里 工 ， 。 。 一 一 、 誓 一 之 导数矩 阵
求出所有的册(P、j=1,”,5)例如 S1S4 C1 -S1C4 -S1(a3c3-a2S2) -C1s451 CIC4 c(aac3-a2s2) a01 …(5.1) 0 0 0 0 0 0 0 0 -c1c40-C1S40 4- -51c40-5:540 0 …(5.2) -54 0 c40 0 0 0 0 经过以上四步，就可以用（5)进行计算 例1.已知ROBOT一1的第四杆伪惯性矩阵为 号x+lw+1） I4x￥ I.xz m4X4 J4= 14x￥ }(山x-lm+la) I4￥z m4y4 I4x石 1-(Iaxx+I4YY-I4zz) 14Yz2 m4Z4 maX m4y4 m4Z ma 计算工…（沿6） 解记 D4T(沿） 由(5)知，P=4,i=1,j=4,q=0 B=器，C=沿，()4 a01 据（5.1)及(5.2)，有 -C1S4 0 -51(aC-a2s2) ci(aaca-a2S2) 0 代入(5)有 110
求 出所 有的 、 例如 ， 一百百丁 一 一 一 一 。 一 … “ 、 一 一 一 一 一 一 声 。 … 。 气护℃ 、 一 一 甲一口‘门 乡一 经 过以上 四步 ， 就可 以 用 进 行计算 例 已知 一 的 第四 杆 伪惯性 矩阵为 …… 一 一 一 “ 二 “ 一 卜 ‘ 二’ 丫 － 火 艺 ‘ 一 丫 丫 毛 一 、 一 么 ‘ 广 、 计算 。 旦黑止 、 廿 解 记 ‘ ， 。 · 几一 丫从叮一药一 口 ， ‘ ， ， ‘ 一 二万 口－ ， 由 知 ， 二 ， ， ， 二 会 ， 二 会 ， ‘ 一 ‘ “ ’ 一 据 及 ， 有 一 一 一 一 一 不 甘 、 、 万、了，‘ ‘ 、 ︸一 叶 ‘、 … 、、 一 … 峥 ，今 一 ’ ‘ ‘ 一 “ ’ “ ‘ 戈 一 一 ‘ ‘ ‘ ‘ 一 工 一 一 代 入 有