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

曾 波，吳欽木，鄭 飛

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

0 引言

近年來(lái)，機(jī)械臂在各個(gè)領(lǐng)域得到廣泛的應(yīng)用與認(rèn)可。現(xiàn)如今的機(jī)械臂為適應(yīng)各種特殊與復(fù)雜的工作狀況，自由度的數(shù)目也越來(lái)越多［1］，而機(jī)械臂數(shù)目的提高帶來(lái)的是整個(gè)機(jī)械臂控制系統(tǒng)復(fù)雜度的增加。軌跡規(guī)劃是整個(gè)機(jī)械臂運(yùn)動(dòng)系統(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ì)算相對(duì)簡(jiǎn)單并且不會(huì)發(fā)生機(jī)構(gòu)的奇異性問(wèn)題［4］。在笛卡爾規(guī)劃空間中最明顯的特點(diǎn)就是末端軌跡能直接確定，但需要進(jìn)行龐大的求逆解運(yùn)算工作，這也是笛卡爾空間規(guī)劃下出現(xiàn)角速度失控的主要原因。因此，該方法主要用于特殊的工況作業(yè)，而如今機(jī)械臂的控制系統(tǒng)大多都采用關(guān)節(jié)空間軌跡規(guī)劃的方法［5］。

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

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

本文首先建立機(jī)械臂的運(yùn)動(dòng)學(xué)仿真模型，以完成機(jī)械臂仿真和運(yùn)動(dòng)軌跡規(guī)劃工作。仿真研究所采用對(duì)象是PUMA560 型工業(yè)機(jī)器人，該機(jī)械臂是由Unimation 公司生產(chǎn)的一種各關(guān)節(jié)都是轉(zhuǎn)動(dòng)關(guān)節(jié)的6R 結(jié)構(gòu)機(jī)器人［9］。該機(jī)器人的工業(yè)結(jié)構(gòu)如圖1 所示?？紤]到該機(jī)器人未知參數(shù)過(guò)多，以及本文仿真需求，采取一種更為簡(jiǎn)便且全面的描述方式，用于表示該機(jī)器人的位置、姿態(tài)以及該機(jī)械臂各個(gè)關(guān)節(jié)之間的連桿關(guān)系［10］。本文根據(jù)標(biāo)準(zhǔn)D-H 參數(shù)法建立機(jī)械臂運(yùn)動(dòng)學(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 個(gè)關(guān)節(jié)的最大變化范圍見(jiàn)表1。在表中包含了描述相近兩連桿關(guān)系的參數(shù)變量。其中，θi表示連桿之間公垂線的夾角；αi表示其軸線之間的夾角；di為相對(duì)位置；ai是兩連桿公共法線之間的距離。PUMA560采取同大多數(shù)工業(yè)使用機(jī)器人類似的結(jié)構(gòu)，其后3個(gè)關(guān)節(jié)4、5 和6 的關(guān)節(jié)軸線相交于同一點(diǎn)，并且這3 個(gè)關(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 個(gè)連桿，創(chuàng)建好各連桿之后再調(diào)用SerialLink 函數(shù)將6個(gè)連桿串聯(lián)，以形成連桿坐標(biāo)系。也可根據(jù)PUMA560 其余參數(shù)做進(jìn)一步設(shè)置。調(diào)用Teach 函數(shù)可進(jìn)入示教模式，該模式下可以手動(dòng)更改機(jī)械臂位姿。由上述方法得到最終機(jī)器人運(yùn)動(dòng)學(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)間的多個(gè)中間節(jié)點(diǎn)，該方法被廣泛應(yīng)用于曲線插值擬合中，具有可分段處理、導(dǎo)數(shù)連續(xù)以及局部支撐性等優(yōu)點(diǎn)［12］。根據(jù)機(jī)械臂運(yùn)動(dòng)過(guò)程中的速度以及加速度等大小的需要，可進(jìn)行相應(yīng)某段軌跡的調(diào)整，而不用更改其余段軌跡曲線，這些優(yōu)點(diǎn)使B樣條曲線作為機(jī)械臂控制系統(tǒng)中的軌跡規(guī)劃部分，具有一定的使用價(jià)值［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í)間參數(shù)，取值為0 ≤t≤1，Vi為第i段曲線上的多個(gè)控制點(diǎn)，可見(jiàn)某一段B 樣條曲線共由相鄰的4 個(gè)控制點(diǎn)共同決定。（控制點(diǎn)的具體求解過(guò)程與方法將在后續(xù)給出）Xi（t）為參數(shù)多項(xiàng)式，在此假設(shè)三次多項(xiàng)式的表達(dá)式為：

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

將式（4）代入式（2）中可以解得：

同時(shí)根據(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 個(gè)中間節(jié)點(diǎn)，在已知中間路徑節(jié)點(diǎn)下，可通過(guò)逆運(yùn)動(dòng)學(xué)求解，由笛卡爾坐標(biāo)獲得各關(guān)節(jié)的中間角度值［14］，進(jìn)而求解各控制點(diǎn)。中間各關(guān)節(jié)變量值見(jiàn)表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ù)式（4）和式（9）可求得：

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

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

3 仿真實(shí)例

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

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

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

圖5 第一關(guān)節(jié)仿真結(jié)果圖Fig.5 Simulation results of the first 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

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

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

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

本文以六自由度機(jī)器人為研究對(duì)象，建立機(jī)器人的運(yùn)動(dòng)學(xué)模型并采用三次均勻B 樣條插值法軌

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