挖掘機自動挖掘軌跡規(guī)劃與動態(tài)優(yōu)化控制

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

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

0 前言

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

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

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

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

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。其工作原理為：通過控制各組液壓缸的運動即可完成各部件之間的相對運動，從而完成鏟斗挖掘、聯(lián)合挖掘等作業(yè)工況。在自動挖掘過程中，通過協(xié)同規(guī)劃各組液壓缸的動作，可以實現(xiàn)復雜的運動軌跡。

圖1 液壓挖掘機簡圖

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

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

(1)

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

(2)

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

表1 液壓挖掘機工作裝置連桿參數(shù)

3 液壓挖掘機的軌跡規(guī)劃

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

圖2 挖掘路徑點

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

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

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

(3)

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

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

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

(1)依次經(jīng)過給定的關節(jié)插值角度且保證連續(xù)：

(4)

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

(5)

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

(6)

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

4 液壓挖掘機動力學方程

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

由拉格朗日方程得：

(7)

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

(8)

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

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

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

表2 液壓挖掘機基本連桿參數(shù)

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

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

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

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

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

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

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

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

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

6 基于動力學模型的動態(tài)優(yōu)化控制

其中軌跡誤差表示為

e=qd-q

(9)

可以得到：

(10)

7 聯(lián)合仿真分析對比

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

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

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

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

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

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

8 試驗驗證

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

圖12 液壓挖掘機試驗臺

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

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

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

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

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

9 結論

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

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

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

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