六自由度蘋果采摘機械臂路徑規(guī)劃與仿真分析

孫 浩

（福建農林大學機電工程學院，福建 福州 350002）

0 引言

隨著機械智能化的迅速發(fā)展，機器人也越來越多地應用到農業(yè)生產中。農業(yè)果實（采摘）機器人在室外采摘作業(yè)時因為果樹大小高矮等情況而使作業(yè)工況復雜化，采摘機器人能夠實時判斷果實位置對于提高采摘效率、減小果實損傷率等至關重要。另外，農業(yè)機器人的移動與采摘大都同時進行，其作業(yè)路徑不是簡單的出發(fā)點與最終點距離，而是一個復雜的三維空間路徑。對于農業(yè)采摘機器人的設計進行分析，采摘機械臂是其關鍵部件，為了提高采摘機械臂快速準確地判斷目標果實位置的能力，減小采摘損傷率，針對采摘機械臂進行路徑規(guī)劃尤為重要。張程等（2017）在五自由度的機械臂搬運作業(yè)中，選取空間梯形路徑關鍵點，進行五次多項式插值，比傳統(tǒng)的五次多項式插值有效地縮短時間，提高了工作效率[1]。張廣超（2016）采用三次三角Bezier 樣條函數(shù)對機械臂進行插值計算。由約束條件求出函數(shù)的系數(shù)，再把時間等分，求出位移量、對應位移量的軌跡點的坐標值，即可求出各個時間點處的關節(jié)變量[2]。李東潔等（2009）把SCARA 機器人4 個關節(jié)的速度、加速度變化作為約束條件對機器人進行軌跡規(guī)劃[3]。譚冠政等（2003）把關節(jié)的位移、速度和加速度作為約束條件，利用二次多項式和余弦函數(shù)，基于時間最短原則進行關節(jié)空間的軌跡規(guī)劃，以提高作業(yè)效率[4]。

由于目前的軌跡規(guī)劃算法在應用過程中存在中間點停頓，并且有些算法存在時間長、加速度存在沖擊等問題，因此，筆者主要研究農業(yè)采摘機器人機械臂的軌跡規(guī)劃問題。根據作業(yè)要求設計機械臂，并用標準法建立其D-H 參數(shù)，機械臂末端確定采摘位置，通過蒙特卡洛法驗證其作業(yè)空間。運用五次多項式插值，確定平滑的加速度曲線，并將其應用到建立的機械臂軌跡規(guī)劃中，進一步完善機械臂軌跡規(guī)劃[5]。

1 機械臂模型建立與仿真

1.1 機械臂設計與D-H參數(shù)確定

本研究旨在建立蘋果采摘機械臂。通常果園的種植間距為3 m左右，行距約為3.5 m。蘋果在果樹上的生長位置距地面的最高高度一般為2.5 m左右，蘋果的冠幅在2.0 m～2.5 m之間，枝下高度在0.8 m～1 m。因此，采摘機械手目標水平工作空間為0～2.5 m，垂直工作空間為0.8 m～2.9 m，果實分布深度范圍0～0.4 m。根據作業(yè)空間確定機械臂參數(shù)，并由此建立機械臂D-H參數(shù)。D-H參數(shù)法分為標準D-H參數(shù)法與改進D-H參數(shù)法，本研究使用標準D-H參數(shù)法[6]。

標準D-H 參數(shù)法常用于建立關節(jié)型機器人的數(shù)學模型，D-H 參數(shù)法是一種對連桿的坐標描述，而關節(jié)機器人本質上就是一系列連桿通過關節(jié)連接起來而組成的空間開式運動鏈。對于連桿本身，其功能在于保持其兩端的關節(jié)軸線具有固定的幾何關系，連桿的特性由軸線決定，相鄰連桿坐標系變化如圖1 所示，通常用四個連桿參數(shù)來描述，即：連桿長度ai，連桿扭轉角αi，連桿偏移量di和關節(jié)角θi[7]。

圖1 相鄰連桿坐標系變化

假設一個關節(jié)只有一個自由度（多自由度關節(jié)可以看成連續(xù)的單自由度關節(jié)通過零長度連桿連接而成）。其中具有n 個關節(jié)的機械臂將有n+1 個連桿，因為每個關節(jié)通過兩個連桿連接起來[8]。從基座開始，將關節(jié)0 按照從1 到n 的順序進行編號，連桿按照從0 到n 進行編號。則共需要建立n+1 個參數(shù)坐標系，Z 軸的方向與各個關節(jié)的軸線重合，X 軸方向在連桿兩端的公垂線上，由Z 軸與X 軸則可以確定Y 軸方向。此機械臂共有六個自由度，分別X、Y、Z 的平移、旋轉。各個關節(jié)參數(shù)坐標系如表1所示[9]。

表1 機械臂DH 參數(shù)

在DH 約定中，其次變換矩陣Ai都可以表示為四個基本矩陣的乘積。如下式所示：

1.2 機械臂運動學仿真

在Matlab 環(huán)境RoboticsToolbox10.2.0 工具箱中，運用Link 函數(shù)和Serialink 函數(shù)構建一個六自由度機械臂模型。運動teach 函數(shù)可以通過改變各個關節(jié)角得到機械臂各關節(jié)姿態(tài)，可以更加直觀地觀察機械臂的運動軌跡和姿態(tài)，六自由度機械臂交互界面如圖2所示。

圖2 六自由度機械臂交互界面

1.3 機械臂作業(yè)空間模擬

工作空間作為一種重要的運動指標，常用于衡量機械臂活動的范圍。當機械臂各連桿長度和關節(jié)轉角的范圍確定后，采用相關算法可以求得機械臂的工作空間，其求解方法主要包括數(shù)值法和蒙特卡洛法。

蒙特卡洛法計算工作空間相較于數(shù)值化，優(yōu)勢在于用時較短，以100 000 個點為例，用時在2～3 min內（根據關節(jié)運動范圍和關節(jié)數(shù)不同而不同），劣勢在于蒙特卡洛法只能趨近于結果而不能完全得到真實的結果，但經過測試，只要數(shù)據的數(shù)量足夠大，也能獲得較為準確的結果。因此，采用大量數(shù)據可使其獲得較為準確的結果[11]。

三維工作范圍圖如圖3 所示，該機械臂作業(yè)空間云圖的形狀大體可看作橢球體，機械臂末端點在X 軸方向活動的最大范圍和最小范圍到坐標原點的距離分別為2 136 mm、1 833 mm，在Y 軸方向活動的最大范圍和最小范圍到坐標原點的距離分別為1 866 mm、1 849 mm，在Z 軸方向活動的最大范圍和最小范圍到坐標原點的距離分別為2 045 mm、1 698 mm。機械臂滿足設計需求，可以對果樹進行覆蓋采摘。

圖3 三維工作范圍圖

2 機械臂運動軌跡

械臂運動軌跡規(guī)劃既可以在笛卡爾空間坐標系中進行，也可以在關節(jié)空間中進行。本研究是在關節(jié)空間中進行的。本研究采用五次多項式軌跡規(guī)劃進行分析[12]。

2.1 五次多項式插值

五次多項式為：

其約束條件為：

這些約束條件確定了一個具有6 個方程和6 個未知數(shù)的線性方程組，其解為：

針對單點之間的軌跡規(guī)劃，應用三次多項式對機械臂運動軌跡進行仿真分析，當起始與終止速度均為0，且無中間點時：設置起始位置為0，終止位置為90，起始時間為0，終止時間為3，通過Matlab 編程仿真分析，五次多項式兩點位置、速度、加速度曲線圖如圖4 所示。由圖4 可知，兩個單點之間進行五次多項式插值，可以在約束位置與速度時間的前提下，約束加速度，減少機械臂運動到終點時的沖擊與抖動，提高機械臂運動的穩(wěn)定性。

圖4 五次多項式兩點位置、速度、加速度曲線圖

2.2 機械臂軌跡規(guī)劃運動學仿真

機械臂從初始關節(jié)角為q0=[0，0，0，0，0，0]的位置點運動到終止關節(jié)角為q1=[π/4，π/2，π/2，π/4，-π/4，π/2]的位置點，在Matlab 中運用五次插值多項式對其進行軌跡規(guī)劃，可得到機械臂各個關節(jié)位置、速度，五次多項式經過多點位置、速度、加速度曲線圖如圖5所示[13-14]。由圖5可知，當五次多項式插值經過多個連續(xù)點時，只要給定離散點位置、速度和時間，能夠通過插補得到一段連續(xù)平滑的曲線，由于加速度連續(xù)，所以這樣不會產生沖擊與抖動，有利于實際操作（不存在勻速階段，只有勻加速、勻減速）。

圖5 五次多項式經過多點位置、速度、加速度曲線圖

2.3 五次多項式軌跡規(guī)劃

通過上述分析對比可以知曉，D-H 參數(shù)進行五次多項式插值仿真。根據給定D-H 參數(shù)應用Matlab 建立六自由度機械臂，通過給定坐標點得到點的位姿，并通過位姿得到關節(jié)角，通過機器人工具箱進行五次多項式插值仿真分析可以得到各個關節(jié)位置、速度、加速度曲線。六自由度機械臂五次多項式位置、速度、加速度曲線圖如圖6所示。

圖6 六自由度機械臂五次多項式位置、速度、加速度曲線圖

通過機械臂與五次多項式插值聯(lián)合，由圖6 可知五次多項式能夠保證速度和位移連續(xù)，加速度連續(xù)?？梢詼p少實際應用過程中的抖動甚至沖擊，提高機械臂的采摘效率[15-16]。

3 結論

本研究設計了一種六自由度蘋果采摘機械臂，對機械臂進行了運動學分析和軌跡規(guī)劃研究，建立了機械臂的參數(shù)模型，由機械臂末端確定采摘手位置，并采用蒙特卡洛法驗證了機械臂抓取空間滿足設計需求。通過對五次多項式插值算法進行軌跡規(guī)劃，確定五次多項插值可以得到平滑的軌跡曲線，運動過程中無沖擊，更加有利于實際作業(yè)環(huán)境。