化肥碼垛機器人的軌跡規劃及仿真試驗

郭瑞峰， 彭戰奎， 張文輝， 彭光宇

(西安建筑科技大學機電工程學院，陜西西安 710055)

目前化肥行業常見的碼垛方式有3種，分別是人工碼垛、傳統機械碼垛、碼垛機器人碼垛[1]。雖然化肥碼垛機器人的價格比前面2種要高，但在后期的使用過程中它更能節省企業的生產成本。因為碼垛機器人有以下7個特點：(1)化肥碼垛機器人工作8 h可代替人工3～4人，1年可節省人力成本數十萬元。(2)能耗低。傳統機械碼垛的耗能約 26 kW，而碼垛機器人的耗能僅需5 kW左右。(3)結構簡單、零部件少。因此其故障率低、魯棒性好、易于保養和維修[1]。(4)適應性好。當抓取物品的尺寸、形狀、空間位置發生改變或末端操作器更換時，只要修改控制程序或重新標定即可實現，而不會影響正常的生產效率[2]。(5)人機界面，便于操作。(6)占用空間少，便于流水化生產線的布置。(7)碼垛機器人碼垛精度更高，可達±3 mm，減少塌垛現象。

綜合以上因素，本研究以實驗室自主設計的四自由度化肥碼垛機器人為例，通過幾何法與Modified-DH模型組合求解的方式，推導該機器人正運動學方程。在逆運動學求解時，通過Atan2函數及各關節運動范圍，求出其可行解；針對逆解的多解問題，利用最短行程的原則進行優化，得到各關節角的最優解[3-4]。為了保證機器人精確、流暢、連續、平穩地碼放化肥包裝袋，采用“5-3-5”法進行軌跡規劃。最后通過ADAMS仿真分析驗證了理論方法的正確性及機械手設計的合理性，完成化肥碼垛機器人的試制，結果表明該碼垛機器人可以滿足設計要求。

1 化肥碼垛機器人結構分析

該機器人可碼垛化肥包裝袋尺寸參數為960 mm×600 mm，一次性抓取總質量達到160 kg，一次性抓取包裝袋的數量為2袋。其結構示意見圖1。

該化肥碼垛機器人有4個自由度，具有如下優點[5-6]：(1)四邊形機構具有誤差自補償作用，Ⅱ驅動軸的運動通過四邊形機構傳遞到末端執行器會更精確，在一定程度上保證了機器人末端執行器定位位置的準確性；(2)整機質量分布得到了優化，增強了碼垛機器人的穩定性；(3)伺服電機下移到回轉平臺上，增加了電機安裝位置的靈活性，減小了臂部關節的重力和轉動慣量，實現小臂輕量化；(4)在該機構的作用下，機器人的承載能力得以提高[7]。

2 Modified-DH模型

機器人運動學研究中，運動學正解的求解常使用 Denavit和Hartenberg在1955年提出的4參數DH模型[2]。Paul在1981年證明了其對機器人運動學分析的價值，并提出Paul系統，由于被機器人廣泛使用而發展成為了標準DH模型[2-3]。然而標準DH模型主要是針對串行結構的機器人進行運動學分析，當用標準DH模型來處理樹狀結構或者閉環結構機器人的時候會產生歧義[4，8]。1984年Craig提出一種Modified-DH模型[4]，其使用Craig連桿約束，因此又稱為Craig系統。該模型具有以下優點：(1)可以用統一的定義來處理串行結構、樹結構和閉環結構，具有更好的通用性。(2)其使用修改的DH參數，從機械結構的簡化模型方面來講，其參數更加清晰、真實。但由于國內機器人行業普遍采用標準DH模型處理串聯機器人，較少涉及樹結構和閉環結構的機器人，因此Modified-DH模型的論述較少。

Modified-DH模型的連桿坐標系描述如圖2所示，將它與標準DH模型進行對比得到其基本差異如下：(1)坐標系依附到連桿的位置不同。Modified-DH模型坐標系i-1固連于連桿i-1上，坐標原點Oi-1位于關節軸i-1上，而不像標準DH那樣坐標系i-1固連于連桿i上，坐標原點Oi-1位于關節軸i上。(2)所執行的轉換順序不同。根據Craig的連桿約定描述，變換矩陣由以下操作順序給出。

相鄰連桿坐標系Oi-1-xi-1yi-1zi-1到Oi-xiyizi的齊次變換步驟如下：(1)繞xi-1軸旋轉αi-1，使zi-1和zi平行；(2)沿xi-1軸平移ai-1，使zi-1和zi共線；(3)繞zi-1軸旋轉θi，使xi-1與Oi和Oi+1之間的連線重合；(4)沿zi軸平移di，使xi-1與xi重合。

因此Modified-DH模型的連桿坐標系Oi-1-xi-1yi-1zi-1到Oi-xiyizi的齊次變換通式為

i-1Ti=Rot(xi-1,αi-1)Trans(xi-1,ai-1)Trans(zi,θi)Rot(zi,di)；

(1)

(2)

3 化肥碼垛機器人運動學分析

3.1 化肥碼垛機器人正運動學計算

由于Modified-DH模型可以用統一的定義來處理該化肥碼垛機器人，因此本節采用Modified-DH模型求解的方式，推導該機器人正運動學方程。

機器人機構運動簡圖如圖3所示，圖中虛線所示為小臂處于水平位置時的機器人機構狀態。主傳動系統包括大臂前臂AF、短桿AD、大臂后臂DE和小臂EF組成的平行四邊形機構ADEF，以及短桿AD、連桿CD和曲柄BC組成的鉸鏈四桿機構ABCD2個并聯機構。為將這2個并聯機構串聯化，須要知道E點相對于B點的坐標[7-9]，具體做法如下。

首先如圖3所示建立局部坐標系B-xy。設各關節點坐標：A(300，300)，B(0，0)，C(xC，yC)，E(xE，yE)。根據幾何法可得：

(3)

(4)

(5)

聯立公式(3)～(5)，可得：

(6)

式中：α等于連桿CD與坐標系y的夾角，且α∈(0，90°)。

在鉸鏈四桿機構ABCD中可得γ角的正余弦如下：

(7)

根據三角函數關系，消去變量γ得：

(8)

根據公式(8)及A、D2點坐標，求出夾角α的正余弦：

(9)

通過公式(6)、(9)可以得出E點相對于B點的關系式僅與關節角θ2及θ3相關，因此可以將并聯機構串聯化。

根據圖3建立機器人Modified-DH連桿坐標系見圖4，對于Modified-DH模型，基坐標系O-x0y0z0可以任意設定[4]，通常為了簡化計算，設定z0軸沿D(xD,yD)關節軸1的方向。各關節Modified-DH模型連桿參數見表1。

表1 Modofied-DH模型連桿參數

將表1中Modified-DH模型連桿參數帶入公式(2)，可得0A1、1A2、2A3、3A4、4AG。將各連桿矩陣連乘得到0TG。

0TG=0A1·1A2·2A3·3A4·4AG。

(10)

所以，機器人末端執行器的位置為下列矩陣的第4列：

(11)

機器人末端執行器的姿態矩陣為

(12)

式中：R0為基坐標系的初始姿態矩陣。

聯立式(6)、(7)、(10)～(12)，可得化肥碼垛機器人正運動學方程如下：

(13)

式(13)中：

nx=-s(θ1+θ4)；

ox=ny=c(θ1+θ4)；

oy=s(θ1+θ4)；

(14)

(15)

(16)

3.2 化肥碼垛機器人逆運動學計算

運動學逆解時，文獻[7]采用單變量反正切函數，這樣不僅可能會造成解的丟失，而且角的精度也難以保證[9]。這里通過先確定各關節角的正弦和余弦，然后應用Atan2函數求關節角。

將公式(14)、(15)兩邊對應相除可得：

θ1=Atan2(Hy,Hx)。

(17)

將公式(14)、(16)移位化簡，可得：

(18)

對公式(18)利用三角函數關系，可以消去變量θ3。結合公式(9)、(17)可求出sθ2、cθ2：

θ2=Atan2(sθ2,cθ2)。

(19)

此時公式(18)只有θ3仍是未知量，因此可以求出sθ3、cθ3：

θ3=Atan2(sθ3,cθ3)。

(20)

假設工作過程中末端執行器的姿態保持不變，則：

θ1+θ4=0；
θ4=-Atan2(Hy0,Hx0)。

(21)

4 軌跡規劃及ADAMS仿真分析

4.1 軌跡規劃

碼垛作業如下：1 h碼放900袋，從生產線指定位置A(2 500，0，1 200)抓取化肥包裝袋、經提升位置B(2 500，0，1 200)、下降位置C(3 500，0，1 200)將化肥包裝袋碼放在位置D(3 500，0，800)，軌跡規劃按照“PTP”的運動形式，要求整個碼放過程連續、平穩運行。化肥碼垛機器人尺寸參數[7]見表2。

碼垛作業的時間分布如下：

ΓA-B=2 s、ΓB-C=4 s、ΓC-D=2 s。

通過Matlab編寫逆解程序，得到各關節角的可行解見表3。

對表3中各關節角的可行解，按照最短行程的原則(每個關節總的移動量最小)進行優化，可以得到各關節角的最優解見表4。

表2 化肥碼垛機器人尺寸參數

表3 各關節角的可行解

表4 各關節角的最優解

為了保證機器人精確、流暢、連續、平穩的碼放化肥包裝袋[5]，須要對表4中的數據進行連續化處理。本研究采用“5-3-5”法對軌跡進行插補運算[5，10]，最終得到各關節軸角位移曲線見圖5。

4.2 ADAMS仿真分析

(1)利用SolidWorks建立機器人三維模型；然后導入ADAMS軟件中進行裝配并添加相應的約束[10-11]。(2)將圖5中各關節角位移曲線的數據導入ADAMS中；利用AKISPL函數進行關節角位移擬合，然后添加到相應的關節角驅動中。(3)根據作業要求，設置求解器仿真時間為8 s，進行運動學仿真，仿真過程中的某一時刻狀態見圖6。(4)利用ADAMS的測量功能及后處理功能，得到機器人末端執行器的仿真位移曲線見圖7-a。(5)將圖5中的數據代入到公式(14)、(15)、(16)當中，計算得到理論位移曲線見圖7-b。(6)將ADAMS仿真位移曲線與理論位移曲線進行對比得到圖7-c。運用Modified-DH模型計算得到的理論位移曲線與ADAMS仿真位移曲線基本重合，驗證了該化肥碼垛機器人運動學求解的正確性。

5 結論

本研究以實驗室自主設計的四自由度化肥碼垛機器人為例，通過幾何法與Modified-DH模型組合求解的方式，推導該機器人正運動學方程。在逆運動學求解時，通過Atan2函數及各關節運動范圍，求出其可行解；針對逆解的多解問題，利用最短行程的原則進行優化，得到各關節角的最優解。為了保證機器人精確、流暢、連續、平穩地碼放化肥包裝袋采用“5-3-5”法進行軌跡規劃；通過ADAMS進行單周期仿真分析，驗證了理論方法的正確性及機械手設計的合理性。最后完成化肥碼垛機器人的試制，結果表明該碼垛機器人可以滿足設計要求。