孫志毅，劉 松，王 銀，李 虹

（太原科技大學電子信息工程學院，山西 太原 030024）

1 引言

挖掘機作為一種功能典型，用途較為廣泛的工程機械之一，在建筑、工業(yè)、軍事等領域發(fā)揮著極其重要的作用［1］。但是復雜危險的工作環(huán)境以及對人的依賴使得傳統(tǒng)挖掘機在挖掘過程中給操作員帶來了太多的挑戰(zhàn)。

對挖掘機進行軌跡規(guī)劃研究作為其智能化發(fā)展的重要組成部分，對提高挖掘機的工作效率以及減少關節(jié)的磨損具有重要的意義。

在對6自由度的工業(yè)機器人以最優(yōu)時間和最優(yōu)脈動為目標的軌跡規(guī)劃方法中：文獻［2-3］采用七次B樣條插值的方法，規(guī)劃出了一條脈動連續(xù)，曲線較為平滑且跟蹤誤差較小的焊接軌跡，但是隨著插值曲線次數(shù)越高，曲線容易出現(xiàn)扭擺和發(fā)散問題；文獻［4］采用五次非均勻有理B樣條曲線插值的方法，以機器人的運行時間、能耗以及脈動為目標對其軌跡進行優(yōu)化得到一條關節(jié)驅動力矩變化較小的運動軌跡；在以挖掘機為對象的軌跡規(guī)劃研究中，文獻［5-6］采用3-3-5-3-3分段多項式插值法使挖掘機實現(xiàn)了平穩(wěn)連續(xù)的自主挖掘，但是得到的挖掘軌跡不能實現(xiàn)脈動連續(xù)；文獻［7］分別采用5次非均勻有理B樣條曲線和3-3-5-3-3分段多項式曲線插值法，通過對比仿真發(fā)現(xiàn)5次非均勻有理B樣條插值法得到的軌跡更加光滑平穩(wěn)且能夠減少計算量。

上述對挖掘機軌跡規(guī)劃的研究中，采用分段多項式插值的方法在連接點處關節(jié)加速度及脈動變化較大，并且低階次的曲線無法保證關節(jié)脈動的連續(xù)；在對軌跡規(guī)劃時沒有考慮關節(jié)脈動的性能指標以及鏟斗齒間的姿態(tài)角。

以某小型三關節(jié)挖掘機作為研究對象，以最短運行時間和最優(yōu)脈動的加權和作為目標函數(shù)，在滿足挖掘機動臂、斗桿、鏟斗的運動學約束，動作時間約束的條件下，同時在考慮鏟斗末端挖掘姿態(tài)角的情況下，采用C4連續(xù)的五次非均勻B樣條曲線來對離散點進行插值，并利用自適應粒子群優(yōu)化算法（APSO）進行優(yōu)化求解得到一條脈動連續(xù)且各關節(jié)啟停速度、加速度可分別設定的關節(jié)軌跡，最后通過MATLAB 仿真得到時間-脈動最優(yōu)的末端軌跡。

2 最優(yōu)軌跡規(guī)劃問題描述

2.1 五次B樣條函數(shù)插值軌跡

設定挖掘機的末端挖掘路徑點已經(jīng)給出，在滿足末端挖掘軌跡精度要求的前提下，將挖掘空間的挖掘路徑進行離散化，得到末端齒尖的位姿序列Ti和對應的時間序列ti，再進行逆運動學求解將路徑點的位姿矩陣Ti轉化為關節(jié)空間的角度值pi可得到每個關節(jié)的關節(jié)角度-時間節(jié)點序列{pi，ti}。

采用Ck-1連續(xù)的k次非均勻B樣條曲線構造鏟斗末端軌跡。所有關節(jié)軌跡曲線均可以描述為如下式所示［2］：

式中：dj，j=0，1，…，n—曲線的控制頂點；可通過移動某個或某幾個控制頂點來對曲線進行局部的修改，而不需改變曲線的其他部分，u∈[ui，ui+1]?[uk，un+1]?[u0，un+k+1]—節(jié)點向量。

根據(jù)B樣條曲線的r階導數(shù)pr(u)，r=1，2，3可依次求得各個臂的速度，加速度以及脈動曲線。根據(jù)德布爾算法，pr(u)可按照如下遞推公式計算：

根據(jù)B樣條曲線定義可知，要確定一條B樣條曲線，需要確定節(jié)點向量以及控制頂點序列。

在已知關節(jié)位置點序列為(ti，pi)，i=0，1，…，n，對時間序列ti進行參數(shù)化處理得到k次B 樣條的節(jié)點向量為：u=[u0，u1，…，un+2k]。其中，

將節(jié)點向量[uk，un+k]內(nèi)的n+1個節(jié)點值依次代入B樣條曲線方程讓其分別等于n+1個插值數(shù)據(jù)點pi，得到n+1個B樣條曲線方程如下：

對于k次B 樣條插值曲線總共需要n+k個控制頂點di(i=0，1，…，n+k-1)，因此還需要k-1 個由切矢條件來確定的附加方程。設初始點速度、加速度分別為v0，a0，終止點速度、加速度分別為vf，af。則由B樣條的r階導矢公式可得；

對于五次B樣條，最終聯(lián)立以上式子可以得到n+5個關于曲線控制頂點的方程：

2.2 目標函數(shù)

為了兼顧挖掘機機械臂運行效率和關節(jié)運動過程中軌跡的平滑性，參考文獻［9］，定義兩個性能指標S1，S2如下所示；

式中：S1—挖掘機關節(jié)軌跡運行總時間，用來衡量挖掘機的工作效率；n—末端插值點個數(shù)；j—插值點序號；S2—挖掘機關節(jié)的平均脈動之和，其中，N—關節(jié)個數(shù)，這里N=3；jerki(t)—第i個關節(jié)的脈動函數(shù)。

設定第i個關節(jié)的速度最大值為vimax、加速度最大值為aimax、脈動最大值為jimax。

根據(jù)B樣條的導矢方程可求得各關節(jié)速度，加速度和脈動的曲線方程，如式（12）所示：

根據(jù)式（9）、式（10）所給定的兩個性能指標以及根據(jù)式（12）所求得到的關節(jié)速度、加速度以及脈動，定義優(yōu)化目標和約束條件為：

關節(jié)約束：

2.3 自適應粒子群算法尋優(yōu)

采用自適應粒子群優(yōu)化算法對某個關節(jié)五段插值時間優(yōu)化的步驟為：

（1）根據(jù)經(jīng)驗初始化粒子種群規(guī)模N，初始及結束時的慣性權重ωstart，ωend，加速度因子c1，c2，最大迭代次數(shù)等，一般N=20；

（2）根據(jù)經(jīng)驗設定路徑點之間的最大運行時間，在最大時間限制內(nèi)，隨機產(chǎn)生N組初始粒子，并初始設定種群的位置Xi=(xi1，xi2，…，xiD)T和速度VI=(vi1，vi2，…，viD)T，其中，D為粒子的維度；

（3）對每組粒子進行時間節(jié)點歸一化處理，根據(jù)關節(jié)空間中的插值點反求B樣條曲線控制頂點后插值得到位置曲線，然后分別計算速度，加速度，加加速度并判斷其是否在約束條件范圍之內(nèi)，如果滿足，計算該組時間在當前位置的適應度值，否則將該組粒子適應度值置為較大值，最后將每個粒子的當前位置記為個體的最優(yōu)位置，種群最優(yōu)則在個體最優(yōu)中選取。

（4）通過式（20）更新粒子的速度向量、位置向量。依照式（21）更新慣性權重ω［10-11］，如果更新后粒子速度和位置超出其取值范圍，則用最大值代替。

（5）計算更新后每個粒子所在位置的適應度值，然后與歷史最優(yōu)值進行比較，如果當前值更優(yōu)，則更新個體歷史最優(yōu)位置為當前粒子位置，并再次與其他粒子的歷史最優(yōu)值進行比較，更新群體最優(yōu)粒子位置；

（6）當?shù)螖?shù)達到最大值時，則停止，得到一組最優(yōu)粒子(h1，h2，h3，h4，h5)，求得，軌跡在每個插值點的時間為：T=[t1t2t3t4t5t6]。否則返回步驟（4）。

3 仿真結果分析

首先在操作空間選取一組挖掘路徑點，其中包括鏟斗齒間在笛卡爾坐標系中的位置(x，y，z)以及鏟斗的姿態(tài)角ζ，然后采用幾何法求運動學逆解得到對應的關節(jié)空間角度值，如表1所示。各關節(jié)約束，如表2所示。

表1 關節(jié)位置序列Tab.1 Joint Position Sequence

表2 各關節(jié)的物理束條件Tab.2 Physical Constraints of Each Joint

采用APSO算法求解時間最優(yōu)和軌跡最優(yōu)問題，算法的參數(shù)設置為：種群規(guī)模為20，粒子位置的取值范圍［0.1，10］，粒子速度取值為［-1，1］之間，最大迭代次數(shù)200，慣性權重ωstart=0.9，ωend=0.4。

從大到小逐漸改變時間權重KT(KT+KJ=1)，在不同的權值下均采用粒子群算法進行優(yōu)化，軌跡優(yōu)化結果，如表3 所示。

表3 軌跡優(yōu)化結果Tab.3 Results of Trajectory Optimization

由表3可知：隨著時間加權系數(shù)KT從1到0逐漸減小，挖掘機各關節(jié)的動作時間從18.50s逐漸增大到40s；平均脈動從9.04逐漸減小到0.88。

當KT=1時，末端軌跡為時間最優(yōu)軌跡。當KT=0時，目標函數(shù)只含有脈動函數(shù)。當KT為1.0、0.5、0.3、0時得到挖掘機的各個關節(jié)軌跡，如圖1～圖4所示。

圖1 挖掘機關節(jié)角度變化曲線Fig.1 Variation Curve of Excavator Joint Angle

圖1中：a，b，c分別表示動臂，斗桿和鏟斗關節(jié)隨權重KT從1到0的角度變化曲線；關節(jié)速度和加速度變化曲線，如圖2、圖3所示；從圖中可以看出各關節(jié)的移動速度、加速度均滿足邊界條件，即啟停時刻均為0；且速度、加速度以及脈動曲線的變化均在運動學約束范圍之內(nèi)。由圖4可知，隨著動作時間的增加，脈動曲線基本趨于平滑，對關節(jié)的沖擊更小。

圖2 挖掘機關節(jié)角速度變化曲線Fig.2 Variation Curve of Joint Angular Velocity of Excavator

圖3 挖掘機關節(jié)角加速度變化曲線Fig.3 Variation Curve of Joint Angular Acceleration of Excavator

圖4 挖掘機關節(jié)角加加速度變化曲線Fig.4 Variation Curve of Joint Jerk of Excavator

從結果圖可以發(fā)現(xiàn)當權重系數(shù)KT從1變化到0.5時，各關節(jié)的動作時間以及速度，加速度和脈動變化基本相差不大，而當KT從0.5變化到0.3時曲線的運行時間卻有一個較大的提升，且從圖4鏟斗的加加速度曲線可以看出，平均脈動權重KJ由0增加到0.5時，鏟斗的最大加加速度值基本沒有較大的變化，僅略小于約束值10，這是因為約束條件中已經(jīng)有考慮加加速度約束，即曲線的脈動約束，在KJ取值較小時，約束對曲線的影響大于性能指標，所以KJ由0增加到0.5時，優(yōu)化得到的時間變化不明顯。

根據(jù)表三以及結果圖可知當KT從（1～0.5）變化時，脈動函數(shù)對軌跡影響不大，當KT從0.5變化至0.3時，關節(jié)軌跡運行時間S1由20.17s增大到24.96s，提升了23.75%。

關節(jié)平均脈動S2則減少了47.18%。當KT從0.3變化至0時，S1則達到最大約束時間，運行時間提升了60.26%，關節(jié)平均脈動S2減少了75%。

考慮在時間最優(yōu)的條件下，為了使挖掘機各個脈動最優(yōu)，選擇加權系數(shù)KT=0.3，KJ=0.7為挖掘機關節(jié)軌跡規(guī)劃的最佳取值。

在Matlab 中建立挖掘機機械臂模型，選擇加權系數(shù)KT=0.3，KJ=0.7時得到的挖掘機各個關節(jié)的軌跡進行正運動學求解最后得到的末端挖掘路徑，如圖5所示。

圖5 挖掘機鏟斗齒尖運動軌跡曲線Fig.5 Movement Trajectory Curve of Excavator Bucket Tooth Tip

由圖可知，對關節(jié)空間采用五次非均勻曲線插值所得到的末端軌跡經(jīng)過所有給定路徑點，能夠完成軌跡規(guī)劃任務；每兩個路徑點間的軌跡較為平滑。

綜上說明了以時間和脈動為優(yōu)化目標，采用五次非均勻B樣條曲線能夠實現(xiàn)經(jīng)過給定路徑點，動作時間較短，且軌跡平滑連續(xù)的要求。

4 結論

相比較多項式曲線而言，B樣條曲線擁有獨特的凸包性和局部支撐性的優(yōu)點；以最短運行時間和最優(yōu)脈動的加權和作為目標函數(shù)，綜合考慮了挖掘機的運行時間和關節(jié)脈動，實際操作時可以通過調(diào)節(jié)權重系數(shù)來平衡運動時間和關節(jié)脈動；考慮了鏟斗姿態(tài)角，可以保證挖掘機鏟斗在挖掘過程中更容易達到滿斗狀態(tài)；另外采用五次曲線可以保證關節(jié)初始速度，加速度均為0，且脈動連續(xù)，規(guī)劃出來的軌跡可以降低對機械臂的磨損和沖擊，提高挖掘機的使用壽命，