六自由度機(jī)械臂軌跡規(guī)劃研究與仿真

曾 波，吳欽木，鄭 飛

（貴州大學(xué) 電氣工程學(xué)院，貴陽 550025）

0 引 言

近年來，機(jī)械臂在各個領(lǐng)域得到廣泛的應(yīng)用與認(rèn)可。現(xiàn)如今的機(jī)械臂為適應(yīng)各種特殊與復(fù)雜的工作狀況，自由度的數(shù)目也越來越多［1］，而機(jī)械臂數(shù)目的提高帶來的是整個機(jī)械臂控制系統(tǒng)復(fù)雜度的增加。軌跡規(guī)劃是整個機(jī)械臂運(yùn)動系統(tǒng)中十分重要的組成部分［2］，因此合理有效的軌跡規(guī)劃算法，是保證機(jī)械臂準(zhǔn)確高效運(yùn)行的關(guān)鍵所在。軌跡規(guī)劃通常又可分別在笛卡爾空間以及關(guān)節(jié)角度空間中進(jìn)行［3］，根據(jù)軌跡規(guī)劃的初始條件，兩種規(guī)劃方法又可以互相轉(zhuǎn)換。相比笛卡爾空間中的規(guī)劃方法，前者計(jì)算相對簡單并且不會發(fā)生機(jī)構(gòu)的奇異性問題［4］。在笛卡爾規(guī)劃空間中最明顯的特點(diǎn)就是末端軌跡能直接確定，但需要進(jìn)行龐大的求逆解運(yùn)算工作，這也是笛卡爾空間規(guī)劃下出現(xiàn)角速度失控的主要原因。因此，該方法主要用于特殊的工況作業(yè)，而如今機(jī)械臂的控制系統(tǒng)大多都采用關(guān)節(jié)空間軌跡規(guī)劃的方法［5］。

本文以六自由度機(jī)械臂PUMA560為研究對象，在Matlab下通過Matlab Robots Toolbox中的函數(shù)進(jìn)行編程，創(chuàng)建該機(jī)器人的運(yùn)動學(xué)模型［6］。在關(guān)節(jié)空間中利用三次均勻B樣條插值算法，完成經(jīng)過多個中間節(jié)點(diǎn)的始末兩點(diǎn)間的軌跡規(guī)劃，得到各個關(guān)節(jié)角度變化曲線［7］。通過運(yùn)動學(xué)求解，可以由機(jī)械臂6個關(guān)節(jié)的角度變化函數(shù)，計(jì)算得到機(jī)械臂末端的運(yùn)動軌跡。將角度變化曲線映射為時間變量的函數(shù)，得到關(guān)節(jié)與時間對應(yīng)的函數(shù)關(guān)系曲線表達(dá)式，機(jī)械臂的角速度和角加速度可由關(guān)節(jié)時間函數(shù)依次求導(dǎo)得到［8］，實(shí)現(xiàn)機(jī)械臂的軌跡規(guī)劃與仿真。

1 Puma560機(jī)器人模型建立

本文首先建立機(jī)械臂的運(yùn)動學(xué)仿真模型，以完成機(jī)械臂仿真和運(yùn)動軌跡規(guī)劃工作。仿真研究所采用對象是PUMA560型工業(yè)機(jī)器人，該機(jī)械臂是由Unimation公司生產(chǎn)的一種各關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié)的6R結(jié)構(gòu)機(jī)器人［9］。該機(jī)器人的工業(yè)結(jié)構(gòu)如圖1所示?？紤]到該機(jī)器人未知參數(shù)過多，以及本文仿真需求，采取一種更為簡便且全面的描述方式，用于表示該機(jī)器人的位置、姿態(tài)以及該機(jī)械臂各個關(guān)節(jié)之間的連桿關(guān)系［10］。本文根據(jù)標(biāo)準(zhǔn)D-H參數(shù)法建立機(jī)械臂運(yùn)動學(xué)模型，由PUMA560機(jī)器人的工業(yè)結(jié)構(gòu)模型，建立如圖2所示的連桿坐標(biāo)系。

圖1 PUMA560工業(yè)結(jié)構(gòu)圖Fig.1 PUMA560 industrial structure

圖2 PUMA560實(shí)際模型連桿坐標(biāo)系圖Fig.2 Connecting rod coordinate system diagram of PUMA560 actual model

該機(jī)械臂模型的D-H連桿參數(shù)值和6個關(guān)節(jié)的最大變化范圍見表1。在表中包含了描述相近兩連桿關(guān)系的參數(shù)變量。其中，θi表示連桿之間公垂線的夾角；αi表示其軸線之間的夾角；di為相對位置；ai是兩連桿公共法線之間的距離。PUMA560采取同大多數(shù)工業(yè)使用機(jī)器人類似的結(jié)構(gòu)，其后3個關(guān)節(jié)4、5和6的關(guān)節(jié)軸線相交于同一點(diǎn)，并且這3個關(guān)節(jié)之間相互垂直，這種設(shè)計(jì)方法被廣泛應(yīng)用在工業(yè)機(jī)器人中［11］。

表1 PUMA560機(jī)器人連桿參數(shù)Tab.1 Connecting rod parameters of PUMA560 robot

根據(jù)表1中的各連桿參數(shù)值，在Matlab下調(diào)用Matlab Robots Toolbox中的Link函數(shù)，分別組建6個連桿，創(chuàng)建好各連桿之后再調(diào)用SerialLink函數(shù)將6個連桿串聯(lián)，以形成連桿坐標(biāo)系。也可根據(jù)PUMA560其余參數(shù)做進(jìn)一步設(shè)置。調(diào)用Teach函數(shù)可進(jìn)入示教模式，該模式下可以手動更改機(jī)械臂位姿。由上述方法得到最終機(jī)器人運(yùn)動學(xué)模型如圖3所示。

圖3 PUMA560仿真模型圖Fig.3 PUMA560 simulation model

2 三次B樣條曲線構(gòu)造

B樣條曲線可用于進(jìn)行機(jī)械臂的一般軌跡規(guī)劃，該方法是在關(guān)節(jié)空間中進(jìn)行的。其需要已知始末兩點(diǎn)間的多個中間節(jié)點(diǎn)，該方法被廣泛應(yīng)用于曲線插值擬合中，具有可分段處理、導(dǎo)數(shù)連續(xù)以及局部支撐性等優(yōu)點(diǎn)［12］。根據(jù)機(jī)械臂運(yùn)動過程中的速度以及加速度等大小的需要，可進(jìn)行相應(yīng)某段軌跡的調(diào)整，而不用更改其余段軌跡曲線，這些優(yōu)點(diǎn)使B樣條曲線作為機(jī)械臂控制系統(tǒng)中的軌跡規(guī)劃部分，具有一定的使用價值［13］。

2.1 均勻B樣條基函數(shù)定義

均勻B樣條基函數(shù)的定義為：

式中：t為參數(shù)，i=0，1，2，…，n-k，k＞1，由該式可以推導(dǎo)出k階k-1次均勻B樣條的基函數(shù)。

2.2 三次B樣條函數(shù)計(jì)算

設(shè)曲線中第i段三次均勻B樣條函數(shù)的表達(dá)式為：

式中：t為時間參數(shù)，取值為0≤t≤1，Vi為第i段曲線上的多個控制點(diǎn)，可見某一段B樣條曲線共由相鄰的4個控制點(diǎn)共同決定。（控制點(diǎn)的具體求解過程與方法將在后續(xù)給出）Xi（t）為參數(shù)多項(xiàng)式，在此假設(shè)三次多項(xiàng)式的表達(dá)式為：

式中：Ai、Bi、Ci、Di為待求多項(xiàng)式系數(shù)。根據(jù)整段B樣條曲線的連續(xù)性以及兩分段曲線在連接處值相等，即：

同時根據(jù)B樣條曲線的一階導(dǎo)和二階導(dǎo)函數(shù)在相鄰兩分段曲線的連續(xù)性可得：

根據(jù)坐標(biāo)變換的不變性，即基函數(shù)的規(guī)范性可得：

根據(jù)式（5）～（7）可解得Xi（t）的相應(yīng)系數(shù)。再根據(jù)式（2）可解得第i段三次B樣條函數(shù)為：

式（9）給出了三次均勻B樣條函數(shù)的矩陣表達(dá)式。根據(jù)此式，只需計(jì)算出各控制點(diǎn)，即可計(jì)算出函數(shù)表達(dá)式。而為求取控制點(diǎn)，就需要已知機(jī)械臂始末兩點(diǎn)間的至少4個中間節(jié)點(diǎn)，在已知中間路徑節(jié)點(diǎn)下，可通過逆運(yùn)動學(xué)求解，由笛卡爾坐標(biāo)獲得各關(guān)節(jié)的中間角度值［14］，進(jìn)而求解各控制點(diǎn)。中間各關(guān)節(jié)變量值見表2。

表2 各中間節(jié)點(diǎn)關(guān)節(jié)角度值Tab.2 Joint angle of each intermediate node

設(shè)Pi為中間路徑點(diǎn)，則Pi點(diǎn)相鄰兩曲線滿足表達(dá)式：

在機(jī)械臂實(shí)際運(yùn)動中，始末兩點(diǎn)間通常滿足速度為零的條件。加入該邊界條件再由式（10）可以得到表達(dá)式：

由式（11）可以求得各控制點(diǎn)，將求得的各個控制點(diǎn)的值代入式（8）可求得各段B樣條函數(shù)表達(dá)式。

3 仿真實(shí)例

由上述求取的B樣條函數(shù)表達(dá)式，計(jì)算得到機(jī)械臂關(guān)節(jié)函數(shù)表達(dá)式，再通過運(yùn)動學(xué)求解，得到笛卡爾坐標(biāo)下運(yùn)動軌跡。運(yùn)動學(xué)求解可借助Matlab Robots Toolbox中的fkine函數(shù)進(jìn)行求解，求解出機(jī)器人的位姿奇次變化矩陣，即笛卡爾坐標(biāo)下機(jī)器人末端坐標(biāo)系相對于基坐標(biāo)系的位姿［15］。根據(jù)上述三次均勻B樣條曲線，最終得到笛卡爾坐標(biāo)下機(jī)械臂末端運(yùn)動軌跡曲線如圖4所示。

圖4 末端運(yùn)動軌跡曲線圖Fig.4 Trajectory curve of terminal motion

為進(jìn)行機(jī)械臂運(yùn)動仿真，在本仿真后續(xù)模塊中，分別對角速度以及角加速度進(jìn)行處理。軌跡規(guī)劃通常指機(jī)器人的位置和姿態(tài)等隨時間變化的函數(shù)。在關(guān)節(jié)空間規(guī)劃中可以很容易的獲取到各關(guān)節(jié)的角速度和角加速度，各關(guān)節(jié)仿真結(jié)果如圖5～圖10所示。

圖5 第一關(guān)節(jié)仿真結(jié)果圖Fig.5 Simulation results of the first joint

圖10 第六關(guān)節(jié)仿真結(jié)果圖Fig.10 Simulation results of the sixth joint

圖6 第二關(guān)節(jié)仿真結(jié)果圖Fig.6 Simulation results of the second joint

圖7 第三關(guān)節(jié)仿真結(jié)果圖Fig.7 Simulation results of the third joint

圖8 第四關(guān)節(jié)仿真結(jié)果圖Fig.8 Simulation results of the fourth joint

圖9 第五關(guān)節(jié)仿真結(jié)果圖Fig.9 Simulation results of the fifth joint

從以上仿真結(jié)果圖可以看出，機(jī)械臂各關(guān)節(jié)角位移曲線光滑且平穩(wěn)，變化率較小，能有效提高機(jī)器人的工作效率，避免機(jī)器人末端出現(xiàn)抖動。角速度曲線以及角加速度曲線在整個運(yùn)行過程中無不連續(xù)和不平穩(wěn)情況發(fā)生，且各曲線在連接處過度平穩(wěn)。仿真結(jié)果表明，機(jī)械臂在各個分段區(qū)間運(yùn)動過程中穩(wěn)定無突變，能有效避免軌跡規(guī)劃不合理造成對機(jī)器人的沖擊。

4 結(jié)束語

本文以六自由度機(jī)器人為研究對象，建立機(jī)器人的運(yùn)動學(xué)模型并采用三次均勻B樣條插值法軌跡進(jìn)行規(guī)劃。首先在matlab中借助Matlab Robots Toolbox建立PUMA560機(jī)器人的運(yùn)動學(xué)模型，隨后通過本文方法得到關(guān)節(jié)位移函數(shù)曲線及機(jī)器人的末端軌跡。仿真結(jié)果驗(yàn)證了該方法的合理有效性，同時整個機(jī)械臂運(yùn)動過程平穩(wěn)且無突變，各關(guān)節(jié)的角位移、角速度以及角加速度曲線平滑，能夠有效避免運(yùn)動過程中對機(jī)器人的沖擊和振動，同時該方法的局部支撐性可以進(jìn)行后續(xù)的軌跡優(yōu)化處理。