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

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

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

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

1 旋量理論基礎(chǔ)

1.1 剛體運(yùn)動(dòng)的旋量表示

(1)

ξ=(vT，ωT)T

(2)

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

(3)

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

(4)

1.2 速度旋量與力旋量

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

(5)

(6)

(7)

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

(8)

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

(9)

剛體運(yùn)動(dòng)的空間速度為：

(10)

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

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

(11)

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

(12)

2 機(jī)械臂動(dòng)力學(xué)建模

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

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

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

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

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

(13)

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

(14)

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

(15)

(1) 向外遞歸。

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

(16)

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

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

(17)

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

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

圖2 機(jī)械臂連桿的受力分析

表2 機(jī)械臂的各連桿的質(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對(duì)連桿i的廣義力為：

(21)

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

(22)

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

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

(23)

(24)

(25)

(26)

3 仿真實(shí)驗(yàn)

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

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

(a) (b)圖5 機(jī)械臂末端執(zhí)行器運(yùn)動(dòng)軌跡對(duì)比圖

圖6 仿真軌跡誤差圖

4 結(jié) 語

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