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
求 出所 有的 、 例如 ， 一百百丁 一 一 一 一 。 一 … “ 、 一 一 一 一 一 一 声 。 … 。 气护℃ 、 一 一 甲一口‘门 乡一 经 过以上 四步 ， 就可 以 用 进 行计算 例 已知 一 的 第四 杆 伪惯性 矩阵为 …… 一 一 一 “ 二 “ 一 卜 ‘ 二’ 丫 － 火 艺 ‘ 一 丫 丫 毛 一 、 一 么 ‘ 广 、 计算 。 旦黑止 、 廿 解 记 ‘ ， 。 · 几一 丫从叮一药一 口 ， ‘ ， ， ‘ 一 二万 口－ ， 由 知 ， 二 ， ， ， 二 会 ， 二 会 ， ‘ 一 ‘ “ ’ 一 据 及 ， 有 一 一 一 一 一 不 甘 、 、 万、了，‘ ‘ 、 ︸一 叶 ‘、 … 、、 一 … 峥 ，今 一 ’ ‘ ‘ 一 “ ’ “ ‘ 戈 一 一 ‘ ‘ ‘ ‘ 一 工 一 一 代 入 有
T…(股1,62)=D=4=等A,]=[6.司+ +x[6,e+a心，可]+a6,c]+6,]+be,]+ +A4b,©j=Aac4-s4=-1,C4-l5, 例2计算T…(（61:) 院 解记D4=T…(沿1) 由于B=C,故b:=c1(i,j=1,,4),有 Du=含6，b]=Y6,b]+:6,c可]+2a[6,6g] =入11+入3=I4yy 以上两例。仅是计算T…(沿1，)的两种类型(i*5i=j),对于 T…（,1,)只医把.看作B,把：看作C,同样可 用(S)来计算。 力 3 ROBOT一l动力学Lagrange方程的计算 据动力学方程(1)~(4)，由于D,k是产生哥氏力的惯性、只有机器人高速运 动时才有影响在计算ROBOT一1各杆转矩时省略，且公式中的广义坐标q应为各杆转 角0，有 F:=2D10+I0:+D: (6) j=1 D-总3 （01.) (7) D:m p 1 (8) Dssi 301 3.1计算F: F1=SD,9,+1,0+D (9 j=1 111
’ 。 · 会 ‘ ， 一 二 一 二 二 会 “ ， ， 不 ， 了 ， 〕 “ 不 ， 百〕 · 入一 一 ， 十 入二 ， 了〕 · “ 。 不 。 ， 乙〕 十 “ 尾 ， 苦〕 · “ 。 〔毛 ， 几 · 入 一 久 一 六 ‘ 一 ‘ ， 一 ‘ ， ‘ 一 叶叭 今 ‘今匕匕 沪广、 例 计算 ， 。 。 ‘ 口 ‘ 乡 解 记 ‘ 一 ‘ 二 ， ‘ 铁院学 一 由于 二 ， 故 ， 有 一 ‘ 二 ， 会 “ ， 入 久 。 尾 ， 石 ， 〕 〔不 ， 石〕 十 ‘ 。 〔 一乙 。 ， 几〕 “ 毛 ， 百 。 〕 ‘ ， 月七 以 上两 例 ， 仅 是计 算 ， 。 。 。 。 ， 口 。 、 ‘ 七二 ， ， ， 二 ， 、 一 动丁 一 ‘ ’ 丽万 户“ 阴 甲卜尖 笠 叹 ’ 牛 勺 ‘ “ ， 对 于 ， 。 ， ‘ ” “ ‘ 犷丽币 ‘ ， 用 来计算 。 丁 只要把 ， ， 。 。 二 。 。 二 ，二 。 ，， 二从 ， 不爹丫卜 ， 生 二石不 一 不丁 卜与， 川 个卜 电 口 一 动 力学 方程的计算 据动力学 方程 一 ， 由于 、 是 产生哥 氏力 的惯性 ， 只 有机器 人高速运 动时才有影响 在 计算 一 各杆转矩时省 略 ， 且公 式 中的广义坐 标 应为各 杆 转 角 ， 有 “ 乏 ， 。 、 ， ， 。 ， 万 － 石 一 一 一，人 ’ 一 一 一 艺 ， 艺 ‘ 计 算 勺一 ， 。 一︸
D1=D1,+D10,+…D10 s1 D=三T(616:)-三D: D=总T(沿，282)=D: D点…(0调)高 D点…(：）-点D D=T…（6:180) 利用例1与例2的方法，计算D1~D1,就可得D11（j=1,2,…,5)再计算 重力项 D1=-mgT。 p=1 a0: 这里的gF=（0,0,-g,0),”r,=（x,yp,z,1)是第P杆的质心在P坐标 系中的向量。 把以上结果代入(9)，即得F:。 3.2计算Fz 5 F2=D2101+1292+D2 (10) 含D0=D8+D8:++D8。 D点(8,8)-含（沿1，）》 p=2 点(08）点(0,8)D 一般的，由于对称性，有D:)=D: D=点…(18）点p =2 D点（器，）昌D 112
， ， 艺 … 一 。 经 … 一 兰姿 软 。 乡廿 ‘ 乡 艺 一 了旦里夕 ’ ‘ “ ‘ 口 ， 协 万 艺 一 片 一 ，‘ 艺 。 乡 。 ， 。 ‘ 二 一 石 一一 一 乡 ‘ ， 呈， ， 一 、了、了 。 ， 。 了月刁 擎‘ 艺 ’ ‘ ‘ 乏 叫产 － ， 一 飞一 、 乡廿。 利 用 例 与例 的方 法 ， 计算 ， 。 ， 就 可得 ， ， ， · 一 ， 再计算 重力 项 一 艺 一石白丁 叮 这里 的 ， 系 中的 向量 。 一 ， ， 二 ， ， ， 是第 杆的质心 在 坐 标 把以上结 果代 入 ， 即得 ，。 计 算 ，夕， 口 艺 一 艺 ，二 ， ， … 。 。 ， 一 口 ， 二 刀 乡 沙 ， 一 。 仁三工 竺 。 理军 生 、 ’ 、 乡 一 廿 ‘、 艺 盆 艺 一 ， ， 甲 ‘ ” “ ’ 一 石百 ’ 丁 ， 呈 一 甲 ‘ ’ “ 亡 ’ 丽压 ’ ， ， 一 一般 的 ， 由于 对称性 ， 有 ， ，二 。 。 ” “ ’ 舀百歹 ‘ “ 匀 艺 艺 一 一 刁 ， 。 “ 。 ’ 火 尸 动百 ‘ ’ 万瓦一 呈
D三T…(6:,0）点04 1== D=T(8.38:） D-三m6:, p==2 把以上结果代入（10)，即得F: 3.3计算F3 F=Di91+10,+D (11) D,8=D:+D,0:+…+D0 j1 由于对称性：，有 D31=DIs,D32=D23 D三T…(:,8）三D n=3 p:s3 D点…(8：）AD D=T…(8:18g） D,=-总m0:, p=3 将以上结果代入(11)，即得Fs 3.4计算F4 F4=D01+I0+D (12) j=】 D49=D:0+D4:0:+…+Ds06 1 由于对称性，有 D41=14,D42=D2,D4s=D34 D三下…（器）-三p p=4 113
‘ 生 ‘ ”一 里爵 ‘ 、 、 ， ‘ ‘ 司 二二 。 … 一 会 ‘ 二畜 皇 矛袭 二 把 以上结果代 人 ， 即得 了 计算 二 艺 、 口。 ‘ 艺 ， 。 口 … 。 ‘ 。 一 由 于对称性 ， 有 ‘ ，， ， 呀 、 ︸ 乡一口仔﹂叮八口一工二一、、刀 口月一 厂工口下， 一 芝 。 。 艺 。 。 二二 、 会 名箭 全 呈， 昌 。 。 二 一 会 ‘ 一 呈 。 矛 叮 将以上结果代 入 ， 即得 计算 一 艺 一 ， “ ‘ 一 污 艺 ‘ ， ， ‘ ‘ 口 干 … ‘ 由于对称性 ， ‘ ， 二 一 有 一 二 ‘ ， ‘ 。 、 矛 丁︸﹂ 一口 ︸工叮 ‘ 二 袅 ， 。 。 ， 丘 」 ‘ 一 丽了 ‘ ’ 万 ‘
Dt…（61g盼：) D三m,沿：， 5 将以上结果代入(12)，即得F4 3.5计算F6 F6=D6i0,+.606+D6 (13) j=1 是Di9,=Di01+Da92+…+D96 由于对称性，有 D61=D15,D。2=D25,D63=Dg5,D64=D6 D=T…(1g） D6=-m6gT7。 30g 代入(13)，即得F6 以上是用内积法计算我院机器人ROBOT一1动力学的方法及步骤，至于计算的具 体结果，此处从略。 4结果 用内积法计算机器人动力学方程是一种尝试。由于所得结果是准确解，可以看出以 下几点： 1,能确定各杆输入力矩大小的准确值F, 2,在惯性D:的表达式中，当i=时，D:1(P≥i)是第P杆对第杆有效惯性的 影响，当i+时，·D:)(P三>i)就是第P杆对第i与第j杆耦合惯性的影响。 3,能估计用〔1)的近似计算法所产生的误差。 4,对于目前使用的只保留伪惯性矩阵主对角线的元素的计算法，能在F:的具体表 达式中看出去掉的是哪些因子，误差有多大，从而能作出此法对那些机器人的动力学计 算适用的结论。 参考文献 (1 Richard,P.Paul:Robot Manipulator,1981 2 )Richard,H.Lathrop:Parallelism in Manipulator Dynamics,ADA 14 2515MIT Artificial Intelligence Laboratory,1985 3 Luh,J.Y.S.et al:J.Dynamic Systems,Measurement and Co ntrol, 102(1980),69 114
。 净 一一 ， ， 。 弓 一 一 万 。 将 以上结 果代入 ， 即得 ‘ · 计算 。 。 。 芝 口。 一 量 。 。 夕 。 … … 。 口 。 由于对称性 ， 有 。 二 ，。 ， 。 。 ， 。 。 ， 。 一 ‘ 。 一 · … 会 ， ‘ 一 戳 上 。 一 砷 ， 。 ‘ 一 ‘ 一 二 万 厂 一 。 沙 ‘ 代 入 ， 即得 。 以上 是用 内积 法计算我院机 器人 一 动力学的方 法及步骤 ， 至 于计算的具 体结 果 ， 此处 从略 。 结 果 用 内积法计算机器 人动力 学方程 是 一 种尝试 。 由于所得结果 是 准确解 ， 可 以看出以 下 几 点 能确定 各杆输入力 矩大小 的准确值 ，， 在惯性 ，的表达 式 中 ， 当 二 时 ， ‘ 以 之 是 第 杆对 第 杆有效惯 性 的 影响， 当 祷 时 ， ’ ‘ ， 苏 就是第 杆对 第 与 第 杆祸 合惯性的影响 。 能 估计 用 〔 〕 的近似计算法所 产生 的误 差 。 对于 目前 使用 的只保 留伪惯性 矩阵主 对 角线 的元素 的计算法 ， 能在 的具体 表 达 式中看 出去 掉的是哪些 因子 ， 误 差有 多大 ， 从而 能作 出此法对那些机 器人的 动力 学计 算适用 的结 论 。 参 考 文 献 ， ， ， ， ，