六自由度機械臂軌跡規劃研究與仿真

曾 波，吳欽木，鄭 飛

（貴州大學 電氣工程學院，貴陽 550025）

0 引 言

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

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

1 Puma560機器人模型建立

本文首先建立機械臂的運動學仿真模型，以完成機械臂仿真和運動軌跡規劃工作。仿真研究所采用對象是PUMA560型工業機器人，該機械臂是由Unimation公司生產的一種各關節都是轉動關節的6R結構機器人［9］。該機器人的工業結構如圖1所示。考慮到該機器人未知參數過多，以及本文仿真需求，采取一種更為簡便且全面的描述方式，用于表示該機器人的位置、姿態以及該機械臂各個關節之間的連桿關系［10］。本文根據標準D-H參數法建立機械臂運動學模型，由PUMA560機器人的工業結構模型，建立如圖2所示的連桿坐標系。

圖1 PUMA560工業結構圖Fig.1 PUMA560 industrial structure

圖2 PUMA560實際模型連桿坐標系圖Fig.2 Connecting rod coordinate system diagram of PUMA560 actual model

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

表1 PUMA560機器人連桿參數Tab.1 Connecting rod parameters of PUMA560 robot

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

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

2 三次B樣條曲線構造

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

2.1 均勻B樣條基函數定義

均勻B樣條基函數的定義為：

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

2.2 三次B樣條函數計算

設曲線中第i段三次均勻B樣條函數的表達式為：

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

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

同時根據B樣條曲線的一階導和二階導函數在相鄰兩分段曲線的連續性可得：

根據坐標變換的不變性，即基函數的規范性可得：

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

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

表2 各中間節點關節角度值Tab.2 Joint angle of each intermediate node

設Pi為中間路徑點，則Pi點相鄰兩曲線滿足表達式：

在機械臂實際運動中，始末兩點間通常滿足速度為零的條件。加入該邊界條件再由式（10）可以得到表達式：

由式（11）可以求得各控制點，將求得的各個控制點的值代入式（8）可求得各段B樣條函數表達式。

3 仿真實例

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

圖4 末端運動軌跡曲線圖Fig.4 Trajectory curve of terminal motion

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

圖5 第一關節仿真結果圖Fig.5 Simulation results of the first joint

圖10 第六關節仿真結果圖Fig.10 Simulation results of the sixth joint

圖6 第二關節仿真結果圖Fig.6 Simulation results of the second joint

圖7 第三關節仿真結果圖Fig.7 Simulation results of the third joint

圖8 第四關節仿真結果圖Fig.8 Simulation results of the fourth joint

圖9 第五關節仿真結果圖Fig.9 Simulation results of the fifth joint

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

4 結束語

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