基于拉格朗日方程的Delta機器人動力學分析

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

（1.山東外事職業大學，濟南 264500；2.山東博爾特電梯有限公司，山東德州 253400）

0 引言

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

本文使用矢量法建立了并聯機器人運動模型，分析了系統進給與連桿夾角、動平臺空間位置之間的關系，獲得了運動學方程。基于拉格朗日方程，建立了系統動力學模型，分析了系統進給量、連桿夾角對系統受力的影響。

1 并聯機器人運動學分析

研究所用機器人如圖1所示，頂端靜平臺固定于支架上，3條直線導軌端接于靜平臺，呈對稱分布。連桿通過滑塊連接直線導軌與動平臺，端接于同一滑塊的兩根連桿互相平行，動平臺與靜平臺均呈等邊三角形。

圖1 Delta機器人結構模型圖

圖2 Delta機器人空間直角坐標系

建立如圖2所示的空間直角坐標系，坐標系原點O為靜平臺所在的等邊三角形的中心，也即是直線導軌在靜平臺投影的交點。直線導軌上端點與靜平臺交于點PA、PB、PC。O-PA所在直線為坐標系X軸，且X軸正方向為由O點指向PA點。將X軸沿逆時針方向旋轉90h為坐標系Y軸。Z軸正方向由O點出發，豎直向上。坐標原點O 到靜平臺端點PA、PB、PC的距離R=615 mm，直線導軌Lr=740 mm，連桿長度Lc=600 mm，動平臺中心O′到動平臺邊沿的垂直距離r=58 mm，A導軌在靜平臺的直線投影與X軸重合，B導軌在靜平臺的直線投影與Y軸正方向夾角θ1=30°，B 導軌在靜平臺的直線投影與X 軸負方向夾角θ2=60°，C 導軌在靜平臺的直線投影與X 軸負方向的夾角θ3=60°，C 導軌在靜平臺的直線投影與Y 軸負方向的夾角θ4=30°，各直線導軌與靜平臺的夾角均為α=45°。連桿與各直線導軌分別交于點A、B、C。

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

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

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

以連桿長度為定值，即|AA′ |=Lc， |BB′ |=Lc， |CC′ |=Lc聯立方程：

式（7）給出了電機進給量i1、i2、i3與動平臺空間位置x、y、z之間的關系，即給定了這兩組量中的任意一組，可以求解出另一組。

2 運動學驗證

設定動平臺空間位置，由式（7）求解電機進給量，借助機器人完成運動進給，將動平臺實際位置與設定位置做對比，結果如表1所示。

表1 數據采樣及誤差

空間位置關系如圖3 所示。考慮到測量的不準確性，系統誤差在可接受范圍之內。3臺伺服電機在相同位置同時以相同速度做勻速直線運動，動平臺運動軌跡如圖4所示。此運動軌跡與實際運行軌跡相一致。

圖3 空間位置關系折線圖

圖4 動平臺運動軌跡

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

式中：xA′為點 A′的 x 坐標，yA′為點 A′的 y 坐標，zA′為點 A′的 z 坐標，θax為連桿 AA′與 x 軸的夾角。

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

3 系統動力學分析

3.1 系統動能求解

由于直線導軌A在靜平臺的投影與坐標系X軸相重合，在位置上具有特殊性，動力學分析過程中選取B 導軌作為研究對象。由式（2）得點B的坐標（xB, yB,zB）。

使用位置平移的方法，求得連桿BB′的中點OB坐標（OBx,OBy,OBz）。令l代表連桿長度的一半，即。

點OB的廣義速度：

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

點OB的動能：

式中：EKB為連桿中點OB的動能；m為連桿的質量。

3.2 系統勢能求解

以靜平臺所在位置作為零勢能點，電機進給i2時的系統勢能表述為：

式中：EPB為連桿中點OB的勢能；Lr為直線導軌的長度。

3.3 建立拉格朗日函數

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

式中：L為系統拉格朗日函數。

3.4 建立動力學方程

電機進給在點OB產生的力矩：

式中：τ2為由于電機進給而在點OB產生的力矩；為電機進給加速度；為角θbx的角加速度；為θby的角加速度；為角θbz的角加速度。

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

同理可以求得各因素對連桿A和連桿C力矩產生的影響。

在勻速直線運動下，電機進給及各夾角對連桿力矩的影響如圖6所示。

圖6 中，τAi1為A 軸電機進給量對A 軸力矩產生的影響，τBi2為B 軸電機進給量對B 軸力矩產生的影響,τCi3為B 軸電機進給量對B軸力矩產生的影響。τAx、τAy、τAz、τBx、τBy、τBz、τCx、τCy、τCz分別為動平臺空間位置坐標 （x，y，z）對各軸力矩產生的影響。

圖6 各因素對力矩的影響

4 結束語

通過齊次坐標變換的方法搭建了運動學方程，建立了電機進給與空間坐標之間的對應關系，在此基礎上求取了機器人連桿與坐標軸夾角的大小。使用基于拉格朗日方程的方法，建立了機器人動力學方程，具體步驟為：（1）求解系統廣義速度；（2）求解系統勢能；（3）建立拉格朗日方程；（4）對拉格朗日方程求偏導，建立動力學方程，分析各變量對系統力矩的影響。