基于旋量理論的模塊化機械臂動力學(xué)建模

吳 亮， 李憲華， 孫 青， 宋 韜,3

(1.安徽理工大學(xué) 機械工程學(xué)院， 安徽 淮南 232001； 2.上海大學(xué) 機電工程與自動化學(xué)院， 上海 200444； 3.上海機器人產(chǎn)業(yè)技術(shù)研究院有限公司， 上海 200063)

機器人動力學(xué)問題包括動力學(xué)正問題和動力學(xué)逆問題，正問題與機械臂的動力學(xué)仿真研究有關(guān)，逆問題則是為了實時控制的需要，利用動力學(xué)模型實現(xiàn)最優(yōu)控制，以達到良好的動態(tài)性能和最優(yōu)指標(biāo)。目前，機器人動力學(xué)的研究方法有很多，較為常見的有Lagrange方法和Newton-Euler方法。Lagrange方法建模過程相對簡單，系統(tǒng)性更強，但是計算量特別大，不適合實時計算；Newton-Euler方法則沒有多余信息，計算速度快[1]。在機器人動力學(xué)建模前需要對機械臂的參數(shù)進行描述，較為常見的是D-H參數(shù)法[2]。在D-H 參數(shù)法中，各連桿相對于前一連桿建立坐標(biāo)系，得到的是各個連桿相對于前一連桿的相對運動。D-H 參數(shù)法應(yīng)用相對成熟，在機器人實際應(yīng)用中也較為廣泛。近年來，旋量法[3-5]在機器人建模方面得到了很多重視。以旋量進行機器人運動描述的最大優(yōu)點是：各連桿是相對于機器人底座建立坐標(biāo)系的，同時各連桿的旋量具有明確的幾何意義，從而簡化了機器人。王紅旗等[6]基于旋量理論并利用Lagrange原理對移動機械手動力學(xué)建模。郭冰菁等[7]基于旋量理論對步態(tài)康復(fù)機器人進行了動力學(xué)的建模及仿真，驗證了動力學(xué)模型的正確性。

本文基于旋量理論對6自由度模塊化機械臂進行動力學(xué)建模與分析。首先，基于旋量理論建立機械臂動力學(xué)方程，通過逆動力學(xué)方程求解機械臂各關(guān)節(jié)的關(guān)節(jié)驅(qū)動力矩。然后，通過正動力學(xué)方程進行仿真，求解機械臂實際的關(guān)節(jié)位移、關(guān)節(jié)速度和關(guān)節(jié)加速度。最后，驗證了機械臂動力學(xué)方程的正確性。

1 旋量理論基礎(chǔ)

1.1 剛體運動的旋量表示

(1)

ξ=(vT，ωT)T

(2)

在旋量理論中，剛體的旋轉(zhuǎn)運動可由運動旋量指數(shù)積的形式表示：

(3)

將坐標(biāo)系{B}固定在剛體上，坐標(biāo)系{B}相對于基坐標(biāo)系{S}的位姿可以用gab表示，初始狀態(tài)下的位姿為gab(0)，當(dāng)剛體轉(zhuǎn)動θ后的位姿為:

(4)

1.2 速度旋量與力旋量

假設(shè)與剛體相聯(lián)的坐標(biāo)系{B}相對于固定參考系{A}的剛體運動，其軌跡曲線為gab(t)=SE(3)，由于

(5)

(6)

(7)

剛體運動的空間速度與物體速度之間存在如下關(guān)系：

(8)

上式將運動旋量由一個坐標(biāo)系變換到另一個坐標(biāo)系的矩陣稱為關(guān)于g的伴隨變換，記為：

(9)

剛體運動的空間速度為：

(10)

因此，對應(yīng)于該運動的空間速度即為由旋量產(chǎn)生的速度。

伴隨變換也能作用于力旋量，作用在剛體上的力旋量由純力f和作用在一點的純力矩τ組合而成：

(11)

在坐標(biāo)系{B}中表示的一個力旋量Fb可由一個坐標(biāo)系變換到另一個坐標(biāo)系{A}中，即：

(12)

2 機械臂動力學(xué)建模

本文以6自由度模塊化機械臂為研究對象，該機械臂由6個轉(zhuǎn)動關(guān)節(jié)串聯(lián)構(gòu)成，其三維模型以及簡化構(gòu)型如圖1所示。其中：坐標(biāo)系{S}為機械臂的基坐標(biāo)系或慣性坐標(biāo)系；坐標(biāo)系{T}為機械臂的工具坐標(biāo)系；ξi為各關(guān)節(jié)的運動螺旋。

圖1 機械臂三維模型和坐標(biāo)系

根據(jù)機械臂初始構(gòu)型及建立的坐標(biāo)系，可以列出各關(guān)節(jié)運動旋量的單位軸線向量ωi和軸線上的任意一點qi如表1所示。根據(jù)式(1)和式(2)即可求得初始時刻在基坐標(biāo)系下表示的各關(guān)節(jié)運動旋量ξi=(vi，ωi)，再根據(jù)旋量理論求得機械臂的運動學(xué)方程。

表1 關(guān)節(jié)軸線單位向量和軸線上的點

從基座到末端執(zhí)行器為各連桿設(shè)置固定在連桿的坐標(biāo)系{i}，基坐標(biāo)系可視為{0}。各連桿坐標(biāo)系相對于基坐標(biāo)系的位姿為T0,i(0)，并且坐標(biāo)系在初始時刻姿態(tài)都與基坐標(biāo)系相同，坐標(biāo)系原點位置則與qi點重合，關(guān)節(jié)坐標(biāo)系{i-1}相對于相鄰的坐標(biāo)系{i}的變換為：

(13)

設(shè)Φi為連桿坐標(biāo)系{i}中表示的關(guān)節(jié)運動旋量，則Φi可通過伴隨變換求得：

(14)

坐標(biāo)系{i-1}相對于相鄰的坐標(biāo)系{i}和之間的變換為：

(15)

(1) 向外遞歸。

由于串聯(lián)式機械臂連桿的特性，可以通過遞歸的方式從連桿1到連桿6逐次計算出機械臂各連桿的廣義速度。因此，在坐標(biāo)系{i}下表示的連桿i的廣義速度Vi為：

(16)

式中:當(dāng)i=1時，V0為基座的速度。

對式(16)進行微分可得廣義加速度為：

(17)

(2) 向內(nèi)遞歸。

求得各連桿的速度和加速度后，對連桿進行受力分析如圖2所示。連桿i將受到來自關(guān)節(jié)i和關(guān)節(jié)i+1的主動力，然后結(jié)合機械臂動力學(xué)參數(shù)(如表2所示)信息，可建立機械臂連桿i的Newton-Euler方程：

圖2 機械臂連桿的受力分析

表2 機械臂的各連桿的質(zhì)量參數(shù)

(18)

式中:Fi+1表示在坐標(biāo)系{i+1}中，其余均表示在坐標(biāo){i}中；連桿的廣義慣量Ii表示在坐標(biāo)系{i}中，而慣性張量Ic表示在質(zhì)心坐標(biāo)系中。因此，需要變換至坐標(biāo)系{i}中，可由下式求得：

(19)

(20)

式中:Ici為在連桿質(zhì)心坐標(biāo)系中表示的廣義空間慣量；E為3×3的單位矩陣。

由式(18)可得關(guān)節(jié)i對連桿i的廣義力為：

(21)

注意，當(dāng)i=n時(n為關(guān)節(jié)總數(shù))，Ti+1,i=Tn+1,n為連桿n相對于工具坐標(biāo)系的位姿變換；Fi+1=Fn+1為機械臂末端所受外界接觸力在工具坐標(biāo)系中的表示。由于在坐標(biāo)系{i}中的關(guān)節(jié)運動螺旋為Φi，所以，關(guān)節(jié)i的驅(qū)動力矩為：

(22)

通過向外遞歸求得各連桿的廣義速度和加速度，然后通過向內(nèi)遞歸求得關(guān)節(jié)力矩。

可以將上述遞歸Newton-Euler 算法表達成一個狀態(tài)空間方程：

(23)

(24)

(25)

(26)

3 仿真實驗

(a)(b)圖3 機械臂末端執(zhí)行器位置曲線圖

圖4 各關(guān)節(jié)力矩的變化圖

(a) (b)圖5 機械臂末端執(zhí)行器運動軌跡對比圖

圖6 仿真軌跡誤差圖

4 結(jié) 語

基于旋量理論對6自由度模塊化機械臂進行相關(guān)參數(shù)的描述，采用了遞歸的Newton-Euler方法求解了機械臂的逆動力學(xué)方程和正動力學(xué)方程，最后進行了動力學(xué)仿真實驗，首先設(shè)定機械臂關(guān)節(jié)軌跡，然后利用逆動力學(xué)方程求出理想狀態(tài)的關(guān)節(jié)力矩函數(shù)，再將關(guān)節(jié)力矩函數(shù)代入正動力學(xué)方程中進行仿真，得到關(guān)節(jié)運動軌跡，通過對比驗證了本文建立的機械臂動力學(xué)模型的正確性。