挖掘機自動挖掘軌跡規劃與動態優化控制

周有明，劉凱磊，3，殷鵬龍，康紹鵬，強紅賓，3

(1.江蘇理工學院機械工程學院，江蘇常州 213001；2.國機重工集團常林有限公司，江蘇常州 213136；3.江蘇大學流體機械工程技術研究中心，江蘇鎮江 212013)

0 前言

液壓挖掘機作為一種功能強大且使用最為普遍的傳統工程機械，在工業建設領域中扮演著極其重要的角色[1-2]。隨著人類社會對挖掘機需求的不斷提高，使得挖掘機的應用環境也越來越多樣化，人們對挖掘機的操作要求也越來越高，傳統的液壓挖掘機在一些領域凸顯出越來越多的問題[3]。例如液壓挖掘機在不可視環境下挖掘施工或者復雜折線工程進行作業時，對挖掘精度往往會有很大的要求；另外，對駕駛員腦力和體力消耗較大，長時間工作易造成駕駛員疲勞，甚至事故的發生。因此，對工程機械領域為代表的液壓挖掘機進行自動化改造，實現自動挖掘運動軌跡規劃控制技術的應用已經越來越迫切和必要。

目前，國內外眾多的科研人員對挖掘機自動化進行了相關研究。HAGA等[4]探究了挖掘機的智能挖掘控制系統，采用了激光測距儀，利用位置閉環控制實現對挖掘作業的自動控制。LEE等[5]針對挖掘機器人，基于運動學和雅克比矩陣進行輪廓控制研究。SU和CHENG[6]提出了一種基于梯度分析的運動優化方法，使得自動挖掘機可以獲得更好的軌跡規劃。郭曉光等[7]針對挖掘機軌跡規劃控制系統設計，采用有限狀態機和模糊邏輯的思想，通過MATLAB中的Stateflow(狀態流)和Fuzzy Logic(模糊邏輯)工具箱對挖掘機軌跡規劃控制系統進行了設計并獲得可靠的結果。付榮等人[8-9]采用分段多項式插值對挖掘機進行時間最優軌跡規劃并取得良好的成效。代瑞恒等[10]通過引入正態分布概率密度函數進行改進量子遺傳算法，以最短時間為目標函數，對挖掘機器人關節運動軌跡進行了優化。然而多數學者是從運動學角度研究挖掘機器人軌跡規劃，未考慮由于負載力波動而引起各個驅動液壓缸輸出力的變化過大，導致液壓缸運動控制精度過低，從而影響復雜運動軌跡的實現。因此，如何在液壓挖掘機軌跡規劃中，將各驅動液壓缸輸出力與運行軌跡相聯系，從而便于軌跡運動控制，是液壓挖掘機自動挖掘的關鍵因素之一。

針對挖掘機器人的動力學特性，李海虹、林貞國[11]運用了n次多項式法、傅里葉級數擬合法以及分段多項式插值法對一條T形路徑進行一般軌跡規劃，將規劃前后的動力學特性進行對比，得出函數擬合法的軌跡規劃結果分析結論。何經良[12]將對數螺旋曲線軌跡應用和機器人學相關理論相結合，建立了挖掘機的運動學和動力學方程，分析動力學特性的變化影響。王勇等人[13]采用奇異攝動復合控制策略，將挖掘機器人系統分為快、慢2個獨立子系統，分別為其設計控制器，加和后作為整個系統的控制輸入，進而分析關節柔性及其大小對關節軌跡的影響。文獻[14-16]運用了機器人技術以及理論力學對液壓挖掘機的鏟斗末端建立相關的動力學數學模型，基于液壓控制技術、模糊控制理論完成對液壓挖掘機鏟斗末端軌跡控制試驗的研究。

綜上所述，本文作者以液壓挖掘機為研究對象，建立液壓挖掘機工作裝置運動學和動力學數學模型。通過五次多項式插值方法將運動軌跡離散，采用MATLAB編寫基于動力學模型的離散運動軌跡程序，并將計算結果與ADAMS仿真結果進行對比，最終確定動力學模型參數；其次，建立基于動力學模型的Simulink和ADAMS聯合仿真模型，在動態控制中，采用PD控制方法進行運動軌跡聯合仿真，研究液壓挖掘機自動挖掘軌跡規劃權重對穩定性、精確性的影響，從而為后續試驗提供理論依據。

1 液壓挖掘機模型的建立

液壓挖掘機的基本結構如圖1所示。液壓挖掘機由底座、動臂、動臂液壓缸、斗桿、斗桿液壓缸、鏟斗、鏟斗液壓缸等組成。該液壓挖掘機底座1固定于地面，動臂2底端鉸接于底座1上點O，動臂液壓缸3底端鉸接于底座1上點A，動臂液壓缸3活塞桿一端鉸接于動臂2上點B，斗桿液壓缸4底端鉸接于動臂2上點C，斗桿液壓缸4活塞桿一端鉸接于斗桿5上點D，動臂2另一端鉸接于斗桿5上點E，鏟斗液壓缸6底端鉸接于斗桿5上點F，鏟斗液壓缸6活塞桿一端鉸接于搖臂上點H，斗桿5另一端鉸接于鏟斗7上點J。其工作原理為：通過控制各組液壓缸的運動即可完成各部件之間的相對運動，從而完成鏟斗挖掘、聯合挖掘等作業工況。在自動挖掘過程中，通過協同規劃各組液壓缸的動作，可以實現復雜的運動軌跡。

圖1 液壓挖掘機簡圖

2 液壓挖掘機的運動學模型

根據機器人理論，將液壓挖掘機的工作裝置建立在D-H坐標系中，采用齊次變換描述挖掘機工作裝置各連桿坐標系之間空間的幾何關系時，其連桿變換矩陣i-1iT為

(1)

式中：θi表示為第i個關節轉角；ai-1表示為第i-1個桿件長度；di表示為第i個關節距離；sαi-1表示sinαi-1；cαi-1表示cosαi-1；sθi表示sinθi；cθi表示cosθi。

(2)

其中：θ1、θ2和θ3分別為動臂關節變量、斗桿關節變量、鏟斗關節變量。

表1 液壓挖掘機工作裝置連桿參數

3 液壓挖掘機的軌跡規劃

根據液壓挖掘機的一般工作情況，在液壓挖掘機工作區域內進行軌跡規劃，圖2所示為液壓挖掘機挖掘路徑點。

圖2 挖掘路徑點

以圖3所選的挖掘路徑點為例，對液壓挖掘機機械臂進行軌跡規劃研究。對于給定的路徑點，分成對應的挖掘過程中的5段軌跡，為了使整個運動過程中關節角度、角速度、角加速度以及關節脈動滿足平滑連續的要求，對各關節的5段路徑均采用5次多項式插值。

圖3 MATLAB(a)、ADAMS(b)液壓挖掘機運動軌跡

設挖掘機第i個關節五次分段多項式表達式為

(3)

式中：θik(t)為第i個關節的第k(k=1,2,3,4,5)段多項式插值函數，其中i=2，3，4分別表示動臂、斗桿、鏟斗關節；aikw為第i個關節、第k段的多項式系數，w=0，1，2，3，4，5。

選取圖3所示的鏟斗末端液壓挖掘機路徑所對應的3組關節角度(n=5)Pi=[pi0,pi1,pi2,pi3,pi4,pi5]。

則五次分段多項式軌跡連續并且經過插值點所需要滿足的條件如下：

(1)依次經過給定的關節插值角度且保證連續：

(4)

(2)始末點關節角速度為0且中間插值點處關節角速度連續以及始末點關節角加速度為0且中間插值點處關節角加速度連續：

(5)

(3)中間插值點處關節角加加速度連續以及四階導數連續：

(6)

根據上式求得五段五次多項式的表達式，可得關節運動時的速度和加速度。

4 液壓挖掘機動力學方程

液壓挖掘機進行軌跡規劃時，不僅要得到液壓挖掘機運動學的軌跡規劃值，還要盡量減少運動過程中液壓缸輸出力的突變。為此，在軌跡規劃中應用拉格朗日法求解液壓挖掘機的動力學方程。

由拉格朗日方程得：

(7)

針對液壓挖掘機的非線性系統，為了更加直觀地表述動力學方程，整理成如下的動力學模型：

(8)

當系統以一定的速度和加速度驅動液壓挖掘機時，可通過動力學方程估計出各個關節需要提供的動力。

5 運動學和動力學擬合曲線分析

根據液壓挖掘機的運動學五次多項式插值方法將運動軌跡進行離散，采用MATLAB編寫了基于動力學模型的離散運動軌跡程序，同時將該模型導入到ADAMS虛擬樣機里進行仿真。液壓挖掘機基本參數如表2所示。

表2 液壓挖掘機基本連桿參數

令液壓挖掘機末端沿預設軌跡移動，軌跡上兩點之間運動時間設置為0.1 s，仿真時間為20 s，圖3所示為MATLAB、ADAMS液壓挖掘機運動軌跡。

根據MATLAB程序以及ADAMS虛擬樣機得到各個關節的軌跡，對其運動軌跡求一階導和二階導得到各個關節角速度和角加速度變化曲線并進行對比分析。

由圖4可知：從整體上看，MATLAB 在理想狀態下得出的各個關節角速度和角加速度均低于ADAMS仿真結果；對于動臂而言，由于傳統液壓系統不穩定因素難以控制，導致軌跡路徑的初始角速度和初始角加速度出現負值；相對斗桿而言，在剛柔耦合的作用下尤為突出，角加速度在始末位置的突變顯著，ADAMS仿真角速度達到的峰值高于MATLAB且超前1 s；對于鏟斗而言，在空載的情況下，前10 s軌跡運動比后10 s移動快并趨于穩定。

圖4 液壓挖掘機角速度(a)、角加速度(b)對比

利用計算機對液壓挖掘機動力學方程進行推導，并將各個關節的運動軌跡轉化為各個液壓缸的輸出位移，限于篇幅，詳細的動力學方程不在此給出。

如圖5所示，MATLAB理論公式基于理想狀態，因此在剛柔耦合的作用下，液壓缸輸出力要遠遠大于理想值。在傳統的液壓系統中，輸出力難以穩定控制，尤其是動臂液壓缸，故動臂液壓缸輸出位移要大于期望值。為了讓液壓挖掘機能夠到達預設軌跡，斗桿、鏟斗液壓缸輸出位移隨之做出相應的補償修正。

圖5 液壓挖掘機液壓缸輸出位移對比

綜合角速度、角加速度以及液壓缸輸出位移變化曲線可知，圖6所示的各個液壓缸輸出力變化規律符合理論。MATLAB理論公式基于理想狀態，因此ADAMS仿真結果均大于理論值；其次，動臂液壓缸在后半段軌跡中出現抖動，可為后續優化控制器提供切入點。

圖6 液壓挖掘機液壓缸輸出力對比

6 基于動力學模型的動態優化控制

其中軌跡誤差表示為

e=qd-q

(9)

可以得到：

(10)

7 聯合仿真分析對比

在Simulink里建立PD反饋控制律模型，通過接口設置與ADAMS模型進行聯合仿真，圖8所示為搭建的部分動力學模型。

將基本參數以模塊參數的形式從MATLAB工作區里封裝讀取，設置交互時間為0.01 s，仿真時間為20 s，仿真結果如圖9—11所示。

圖9 液壓挖掘機液壓缸輸出位移對比(不同Kp、Kv組合)

通過適當地設定權重Kp和Kv可使軌跡誤差更小、更精確。文中采用經驗法采集了12組數據，將誤差過大的數據剔除，保留6組數據與第5節中的性能曲線對比。動態控制下的聯合仿真，可以實時動態反饋液壓挖掘機各關節的狀態，能夠讓液壓挖掘機到達期望軌跡，其中，Kp和Kv組合為200-5和250-5時更接近期望軌跡，因此還需進一步對比分析。

圖10所示為上述6組數據的液壓挖掘機液壓缸運動收斂趨勢?？梢钥闯觯篕p和Kv組合為200-5和250-5時，雖在整個仿真過程中，最終收斂趨勢都趨于0，但組合為200-5時，相較于組合250-5起始過程要收斂近50%，有利于液壓挖掘機穩定精確控制。

如圖11所示，綜合液壓缸輸出位移變化曲線對比以及運動收斂趨勢對比，動態控制下的聯合仿真結果表明液壓挖掘機軌跡運行過程中各個液壓缸輸出力變化規律不僅與理論相等，且能穩定運行，其誤差穩定在5%附近。相較于未優化仿真，角加速度在始末位置趨近于0，有效地削減了抖動的影響，提升了軌跡的可執行性，使得液壓挖掘機自動挖掘軌跡穩定精確。

8 試驗驗證

如圖12所示，在2T液壓挖掘機試驗臺的基礎上開展液壓挖掘機運動軌跡控制試驗，以驗證上述動態控制下PD控制方法的有效性。

圖12 液壓挖掘機試驗臺

此試驗將Simulink中建立的PD反饋控制律模型通過CAN通道與試驗臺控制器BODAS進行實時通信，控制效果如圖13、14所示。

圖13 液壓挖掘機液壓缸目標與試驗位移對比

從圖13可以看出：軌跡試驗中，動臂、斗桿和鏟斗液壓缸輸出位移基本與目標位移一致，軌跡曲線存在波動，雖波動頻率較高，但波動幅度較小。結果表明：Kp和Kv組合為200-5的PD控制中各組液壓缸輸出位移的誤差明顯減小。

如圖14所示，Kp和Kv組合為200-5的PD控制軌跡試驗驗證了上述各個液壓缸在起始時運動收斂趨勢的優勢，保證了液壓挖掘機各個液壓缸輸出力穩定并貼近目標值。雖然軌跡過程存在一定波動，但Kp和Kv組合為200-5的PD控制能夠有效削減抖動帶來的影響，而且在實際運動過程中波動并不明顯，其誤差穩定在5%附近，能夠使液壓挖掘機自動挖掘軌跡穩定精確，提升了軌跡的可執行性，滿足挖掘機的工作要求。

圖14 液壓挖掘機液壓缸目標與試驗輸出力對比

9 結論

(1)MATLAB程序以及ADAMS虛擬樣機求得的角速度、角加速度、液壓缸輸出位移以及液壓缸輸出力曲線變化規律均一致，且較好地貼合期望運動軌跡。

(2)由于閥控液壓系統的非線性，液壓挖掘機軌跡運動中的加速度在始末位置不為0，使得部件啟停時各關節會產生較大的沖擊，從而引起部件的振動、加劇關節的磨損、降低部件的使用壽命。分析軌跡過程中突變因素，為后續優化控制器提供切入點。

(3)在聯合仿真的動力學模型中加入動態控制中的PD控制律，有效地削減了軌跡中突變因素。調整權重得到的收斂趨勢相較于未優化的要收斂近50%，各個液壓缸輸出力變化穩定并貼近理論值，其誤差穩定在5%附近，緩解了各關節產生的較大沖擊，提升了軌跡的可執行性，使得液壓挖掘機自動挖掘軌跡穩定精確。

(4)Kp和Kv組合為200-5的PD控制軌跡試驗驗證了聯合仿真下各個液壓缸的運動收斂趨勢起始時的優勢，保證了液壓挖掘機各個液壓缸輸出力穩定并貼近目標值。雖然軌跡過程存在一定波動，但Kp和Kv組合為200-5的PD控制能夠有效削減抖動的影響，而且在實際運動過程中波動并不明顯，能夠滿足挖掘機的工作要求。