基于拉格朗日方程的Delta機(jī)器人動(dòng)力學(xué)分析

孫志偉，李亞洲※，武志華

（1.山東外事職業(yè)大學(xué)，濟(jì)南 264500；2.山東博爾特電梯有限公司，山東德州 253400）

0 引言

機(jī)器人動(dòng)力學(xué)分析描述了機(jī)器人關(guān)節(jié)力矩與機(jī)器人關(guān)節(jié)位置、速度、加速度之間的關(guān)系。動(dòng)力學(xué)分析包含正向動(dòng)力學(xué)分析與逆向動(dòng)力學(xué)分析兩類。正向動(dòng)力學(xué)分析指已知關(guān)節(jié)驅(qū)動(dòng)力，求解各相應(yīng)部分瞬時(shí)運(yùn)動(dòng)，多應(yīng)用于機(jī)器人運(yùn)動(dòng)仿真；逆向動(dòng)力學(xué)分析指已知機(jī)器人關(guān)節(jié)的位移、速度和加速度，求相應(yīng)的關(guān)節(jié)力矩矢量，是實(shí)時(shí)控制的需要[1]。常用的動(dòng)力學(xué)分析方法有虛功原理法、牛頓—?dú)W拉法、拉格朗日法等[2-10]。虛功原理的思想最早來(lái)源于歐洲的伯努利家族，后來(lái)在拉格朗日的《分析力學(xué)》一書中得到體現(xiàn)。虛功原理又叫虛位移原理，指的是外力在虛位移上做的功等于內(nèi)力在虛位移上做的功。當(dāng)系統(tǒng)勢(shì)能不唯一時(shí)，依據(jù)最小勢(shì)能原理，在平衡條件下總勢(shì)能最小，所以其變分形式為0，可以據(jù)此條件聯(lián)立方程。當(dāng)用虛功原理處理力學(xué)問(wèn)題的時(shí)候，實(shí)際上就是在進(jìn)行動(dòng)能為0時(shí)的E-L方程的推導(dǎo)，因此其推導(dǎo)過(guò)程較為復(fù)雜，而且虛功原理僅僅可以處理靜力學(xué)問(wèn)題，所以應(yīng)用范圍較窄。牛頓-歐拉方程針對(duì)剛體的平移和旋轉(zhuǎn)問(wèn)題，將兩個(gè)關(guān)于剛體的運(yùn)動(dòng)方程組合在一起。一般情況下，牛頓-歐拉方程使用矩陣或向量進(jìn)行運(yùn)算。從本質(zhì)上來(lái)講，牛頓-歐拉方程描述了物體重心的運(yùn)動(dòng)與物體所受力或力矩之間的關(guān)系。牛頓-歐拉法通過(guò)平衡方程來(lái)求解問(wèn)題，思路清晰，方法簡(jiǎn)單，不失為一種高效的動(dòng)力學(xué)建模方法。但該種方法需要對(duì)機(jī)器人每一連桿分別建模，加之并聯(lián)機(jī)器人結(jié)構(gòu)復(fù)雜，所以計(jì)算量巨大，求解困難，運(yùn)算效率低下，難以保證實(shí)時(shí)性。拉格朗日方法的理論基礎(chǔ)是能量守恒原理，通常分別求取系統(tǒng)動(dòng)能和勢(shì)能，然后建立拉格朗日函數(shù)。使用拉格朗日法求解系統(tǒng)動(dòng)力學(xué)，優(yōu)越性體現(xiàn)在以下幾方面：（1）求解過(guò)程中，避免了對(duì)系統(tǒng)速度和加速度的求解，因此計(jì)算量小、運(yùn)算簡(jiǎn)單；（2）能滿足實(shí)時(shí)控制的需求；（3）以矩陣的形式表示動(dòng)力學(xué)模型，便于對(duì)系統(tǒng)進(jìn)行動(dòng)力學(xué)控制。

本文使用矢量法建立了并聯(lián)機(jī)器人運(yùn)動(dòng)模型，分析了系統(tǒng)進(jìn)給與連桿夾角、動(dòng)平臺(tái)空間位置之間的關(guān)系，獲得了運(yùn)動(dòng)學(xué)方程?；诶窭嗜辗匠?，建立了系統(tǒng)動(dòng)力學(xué)模型，分析了系統(tǒng)進(jìn)給量、連桿夾角對(duì)系統(tǒng)受力的影響。

1 并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)分析

研究所用機(jī)器人如圖1所示，頂端靜平臺(tái)固定于支架上，3條直線導(dǎo)軌端接于靜平臺(tái)，呈對(duì)稱分布。連桿通過(guò)滑塊連接直線導(dǎo)軌與動(dòng)平臺(tái)，端接于同一滑塊的兩根連桿互相平行，動(dòng)平臺(tái)與靜平臺(tái)均呈等邊三角形。

圖1 Delta機(jī)器人結(jié)構(gòu)模型圖

圖2 Delta機(jī)器人空間直角坐標(biāo)系

建立如圖2所示的空間直角坐標(biāo)系，坐標(biāo)系原點(diǎn)O為靜平臺(tái)所在的等邊三角形的中心，也即是直線導(dǎo)軌在靜平臺(tái)投影的交點(diǎn)。直線導(dǎo)軌上端點(diǎn)與靜平臺(tái)交于點(diǎn)PA、PB、PC。O-PA所在直線為坐標(biāo)系X軸，且X軸正方向?yàn)橛蒓點(diǎn)指向PA點(diǎn)。將X軸沿逆時(shí)針?lè)较蛐D(zhuǎn)90h為坐標(biāo)系Y軸。Z軸正方向由O點(diǎn)出發(fā)，豎直向上。坐標(biāo)原點(diǎn)O 到靜平臺(tái)端點(diǎn)PA、PB、PC的距離R=615 mm，直線導(dǎo)軌Lr=740 mm，連桿長(zhǎng)度Lc=600 mm，動(dòng)平臺(tái)中心O′到動(dòng)平臺(tái)邊沿的垂直距離r=58 mm，A導(dǎo)軌在靜平臺(tái)的直線投影與X軸重合，B導(dǎo)軌在靜平臺(tái)的直線投影與Y軸正方向夾角θ1=30°，B 導(dǎo)軌在靜平臺(tái)的直線投影與X 軸負(fù)方向夾角θ2=60°，C 導(dǎo)軌在靜平臺(tái)的直線投影與X 軸負(fù)方向的夾角θ3=60°，C 導(dǎo)軌在靜平臺(tái)的直線投影與Y 軸負(fù)方向的夾角θ4=30°，各直線導(dǎo)軌與靜平臺(tái)的夾角均為α=45°。連桿與各直線導(dǎo)軌分別交于點(diǎn)A、B、C。

假定原點(diǎn)的空間位置坐標(biāo)為O=[0 0 0 1]T，通過(guò)齊次變換中的位置平移方法求得連桿與導(dǎo)軌交點(diǎn)A、B、C的坐標(biāo)：

式（1）～（3）中：i1、i2、i3分別為附著于導(dǎo)軌A、B、C 上的滑塊與導(dǎo)軌上端點(diǎn)之間的距離，也即代表伺服電機(jī)的進(jìn)給量。

由于動(dòng)平臺(tái)空間位置是未知的，在此用未知量x、y、z表示動(dòng)平臺(tái)中心點(diǎn)的坐標(biāo)，所以，以齊次坐標(biāo)形式表示的動(dòng)平臺(tái)空間位置為：O′=[xy z1]T。使用平移的方法表示出各連桿與動(dòng)平臺(tái)交點(diǎn)A′、B′、C′的坐標(biāo)：

以連桿長(zhǎng)度為定值，即|AA′ |=Lc， |BB′ |=Lc， |CC′ |=Lc聯(lián)立方程：

式（7）給出了電機(jī)進(jìn)給量i1、i2、i3與動(dòng)平臺(tái)空間位置x、y、z之間的關(guān)系，即給定了這兩組量中的任意一組，可以求解出另一組。

2 運(yùn)動(dòng)學(xué)驗(yàn)證

設(shè)定動(dòng)平臺(tái)空間位置，由式（7）求解電機(jī)進(jìn)給量，借助機(jī)器人完成運(yùn)動(dòng)進(jìn)給，將動(dòng)平臺(tái)實(shí)際位置與設(shè)定位置做對(duì)比，結(jié)果如表1所示。

表1 數(shù)據(jù)采樣及誤差

空間位置關(guān)系如圖3 所示?？紤]到測(cè)量的不準(zhǔn)確性，系統(tǒng)誤差在可接受范圍之內(nèi)。3臺(tái)伺服電機(jī)在相同位置同時(shí)以相同速度做勻速直線運(yùn)動(dòng)，動(dòng)平臺(tái)運(yùn)動(dòng)軌跡如圖4所示。此運(yùn)動(dòng)軌跡與實(shí)際運(yùn)行軌跡相一致。

圖3 空間位置關(guān)系折線圖

圖4 動(dòng)平臺(tái)運(yùn)動(dòng)軌跡

空間中，2 條已知的直線，可以求出2 條直線的夾角。以連桿AA′為例，假定電機(jī)進(jìn)給量已知，由式（7）可知，連桿的2 個(gè)端點(diǎn)A、A′也確定，所以直線AA′的空間位置方程是確定的，可以分別求出連桿與坐標(biāo)系各坐標(biāo)軸的夾角：

式中：xA′為點(diǎn) A′的 x 坐標(biāo)，yA′為點(diǎn) A′的 y 坐標(biāo)，zA′為點(diǎn) A′的 z 坐標(biāo)，θax為連桿 AA′與 x 軸的夾角。

由式（8）同理可得連桿AA′與y 軸的夾角θay，連桿AA′與 z 軸的夾角θaz；點(diǎn) B′的 x 坐標(biāo)xB′，點(diǎn) B′的 y 坐標(biāo)yB′， 點(diǎn)B′的z 坐標(biāo)zB′；連桿BB′與 x 軸的夾角θbx，連桿 BB′與y 軸的夾角θby，連桿BB′與z軸的夾角θbz；點(diǎn)C′的x坐標(biāo)xC′， 點(diǎn)C′的 y 坐標(biāo)yC′，點(diǎn) C′的 z 坐標(biāo)zC′；連桿 CC′與 x 軸的夾角θcx，連桿CC′與y軸的夾角θcy，連桿CC′與z軸的夾角θcz。在勻速直線運(yùn)動(dòng)條件下，各軸夾角隨時(shí)間變化曲線如圖5所示。

3 系統(tǒng)動(dòng)力學(xué)分析

3.1 系統(tǒng)動(dòng)能求解

由于直線導(dǎo)軌A在靜平臺(tái)的投影與坐標(biāo)系X軸相重合，在位置上具有特殊性，動(dòng)力學(xué)分析過(guò)程中選取B 導(dǎo)軌作為研究對(duì)象。由式（2）得點(diǎn)B的坐標(biāo)（xB, yB,zB）。

使用位置平移的方法，求得連桿BB′的中點(diǎn)OB坐標(biāo)（OBx,OBy,OBz）。令l代表連桿長(zhǎng)度的一半，即。

點(diǎn)OB的廣義速度：

B為滑塊在B 直線導(dǎo)軌上的速度；為連桿 BB′與X 軸方向所成夾角的角速度；為點(diǎn)OB在y 軸方向的廣義速度；為連桿BB′與y 軸方向所成夾角的角速度；為點(diǎn)OB在z 軸方向的廣義速度；為連桿BB′與z軸方向所成夾角的角速度。

點(diǎn)OB的動(dòng)能：

式中：EKB為連桿中點(diǎn)OB的動(dòng)能；m為連桿的質(zhì)量。

3.2 系統(tǒng)勢(shì)能求解

以靜平臺(tái)所在位置作為零勢(shì)能點(diǎn)，電機(jī)進(jìn)給i2時(shí)的系統(tǒng)勢(shì)能表述為：

式中：EPB為連桿中點(diǎn)OB的勢(shì)能；Lr為直線導(dǎo)軌的長(zhǎng)度。

3.3 建立拉格朗日函數(shù)

由式（12）～（13）可知，系統(tǒng)拉格朗日方程可以表述為：

式中：L為系統(tǒng)拉格朗日函數(shù)。

3.4 建立動(dòng)力學(xué)方程

電機(jī)進(jìn)給在點(diǎn)OB產(chǎn)生的力矩：

式中：τ2為由于電機(jī)進(jìn)給而在點(diǎn)OB產(chǎn)生的力矩；為電機(jī)進(jìn)給加速度；為角θbx的角加速度；為θby的角加速度；為角θbz的角加速度。

同理可得因θbx角度變化而在點(diǎn)OB產(chǎn)生的力矩τbx，因θby角度變化而在點(diǎn)OB產(chǎn)生的力矩τby，因θbz角度變化而在點(diǎn)OB產(chǎn)生的力矩τbz。

同理可以求得各因素對(duì)連桿A和連桿C力矩產(chǎn)生的影響。

在勻速直線運(yùn)動(dòng)下，電機(jī)進(jìn)給及各夾角對(duì)連桿力矩的影響如圖6所示。

圖6 中，τAi1為A 軸電機(jī)進(jìn)給量對(duì)A 軸力矩產(chǎn)生的影響，τBi2為B 軸電機(jī)進(jìn)給量對(duì)B 軸力矩產(chǎn)生的影響,τCi3為B 軸電機(jī)進(jìn)給量對(duì)B軸力矩產(chǎn)生的影響。τAx、τAy、τAz、τBx、τBy、τBz、τCx、τCy、τCz分別為動(dòng)平臺(tái)空間位置坐標(biāo) （x，y，z）對(duì)各軸力矩產(chǎn)生的影響。

圖6 各因素對(duì)力矩的影響

4 結(jié)束語(yǔ)

通過(guò)齊次坐標(biāo)變換的方法搭建了運(yùn)動(dòng)學(xué)方程，建立了電機(jī)進(jìn)給與空間坐標(biāo)之間的對(duì)應(yīng)關(guān)系，在此基礎(chǔ)上求取了機(jī)器人連桿與坐標(biāo)軸夾角的大小。使用基于拉格朗日方程的方法，建立了機(jī)器人動(dòng)力學(xué)方程，具體步驟為：（1）求解系統(tǒng)廣義速度；（2）求解系統(tǒng)勢(shì)能；（3）建立拉格朗日方程；（4）對(duì)拉格朗日方程求偏導(dǎo)，建立動(dòng)力學(xué)方程，分析各變量對(duì)系統(tǒng)力矩的影響。