挖掘機時間最優與脈動最優軌跡規劃

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

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

1 引言

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

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

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

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

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

2 最優軌跡規劃問題描述

2.1 五次B樣條函數插值軌跡

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

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

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

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

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

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

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

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

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

2.2 目標函數

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

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

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

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

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

關節約束：

2.3 自適應粒子群算法尋優

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

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

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

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

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

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

（6）當迭代次數達到最大值時，則停止，得到一組最優粒子(h1，h2，h3，h4，h5)，求得，軌跡在每個插值點的時間為：T=[t1t2t3t4t5t6]。否則返回步驟（4）。

3 仿真結果分析

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

表1 關節位置序列Tab.1 Joint Position Sequence

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

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

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

表3 軌跡優化結果Tab.3 Results of Trajectory Optimization

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

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

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

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

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

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

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

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

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

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

考慮在時間最優的條件下，為了使挖掘機各個脈動最優，選擇加權系數KT=0.3，KJ=0.7為挖掘機關節軌跡規劃的最佳取值。

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

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

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

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

4 結論

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