馮明豪，任志貴,2，魏萬行，孫浩然，李佳豪

1陜西理工大學(xué)機(jī)械工程學(xué)院 陜西漢中 723000

2陜西省工業(yè)自動化重點(diǎn)實(shí)驗(yàn)室 陜西漢中 723000

液壓挖掘機(jī)器人可以獨(dú)立工作，也可以遠(yuǎn)程控制，已成未來挖掘機(jī)發(fā)展的重要方向之一[1]。軌跡規(guī)劃在挖掘機(jī)器人的控制中有非常重要的作用，直接影響挖掘機(jī)器人作業(yè)的運(yùn)動平穩(wěn)性和準(zhǔn)確性[2]。挖掘機(jī)器人的操作過程就是通過軌跡規(guī)劃，將挖掘機(jī)的作業(yè)任務(wù)變成期望的運(yùn)動和力，由底層控制環(huán)節(jié)生成相應(yīng)的信號，產(chǎn)生相應(yīng)的控制，使挖掘機(jī)器人輸出實(shí)際的運(yùn)動和力，從而完成預(yù)期的作業(yè)任務(wù)[3]。一般來說，如果在沒有任何約束條件下，挖掘機(jī)器人的鏟斗齒尖作業(yè)起始點(diǎn)到終止點(diǎn)的軌跡有無數(shù)條，根據(jù)是否采用最優(yōu)化方法進(jìn)行最優(yōu)軌跡尋找，可將軌跡規(guī)劃分為一般軌跡規(guī)劃和最優(yōu)軌跡規(guī)劃[4]。李海虹等人[5]分別采用n次多項式法、傅里葉級數(shù)擬合法、分段多項式插值法對一條 T 型路徑進(jìn)行一般軌跡規(guī)劃，對規(guī)劃前后的動力學(xué)特性進(jìn)行對比評價，得出函數(shù)擬合法的軌跡規(guī)劃結(jié)果更平穩(wěn)連續(xù)、振動強(qiáng)度更低的結(jié)論。潘雙夏等人[6]采用最優(yōu)軌跡規(guī)劃方法優(yōu)化了速度梯度和液壓缸的總移動距離，使得挖掘機(jī)的規(guī)劃軌跡更加穩(wěn)定和節(jié)能，但軌跡規(guī)劃只應(yīng)用在挖掘機(jī)修整平面作業(yè)的路徑上，其路徑實(shí)際上是一條直線。目前，對液壓挖掘機(jī)器人軌跡規(guī)劃的研究多數(shù)停留在簡單路徑，如直線、圓弧等[7]，未考慮應(yīng)用在復(fù)雜路徑上，且挖掘工況主要集中在各液壓缸單獨(dú)挖掘的作業(yè)形式，復(fù)合挖掘的作業(yè)形式較少[8]。

筆者以減少液壓缸長度變化量作為優(yōu)化目標(biāo)，運(yùn)用粒子群優(yōu)化算法對一條真實(shí)復(fù)合挖掘路徑進(jìn)行軌跡規(guī)劃，更符合挖掘機(jī)真實(shí)作業(yè)情況，同時達(dá)到減少挖掘機(jī)器人能耗的目的。

1 液壓挖掘機(jī)軌跡規(guī)劃流程

根據(jù)液壓挖掘機(jī)器人的結(jié)構(gòu)，工作裝置可以簡要描述為位姿空間 [x，y，z，ξ]T、關(guān)節(jié)空間 [θ0，θ1，θ2，θ3]T和驅(qū)動空間 [θ0，λ1，λ2，λ3]T[9]。工作裝置連桿坐標(biāo)系如圖 1 所示，ξ為姿態(tài)角，是水平方向與鏟斗齒尖和鏟斗鉸點(diǎn)連線之間的夾角[10]。

圖1 工作裝置連桿坐標(biāo)系Fig.1 Coordinate system of linkage of working unit

基于機(jī)器人學(xué)，常用的液壓挖掘機(jī)軌跡規(guī)劃流程先規(guī)劃關(guān)節(jié)轉(zhuǎn)角，然后進(jìn)行關(guān)節(jié)空間變量和位姿空間變量的相互轉(zhuǎn)換，使鏟斗齒尖達(dá)到預(yù)定的運(yùn)動。而挖掘機(jī)在實(shí)際作業(yè)時，由于和工業(yè)機(jī)械手的作業(yè)對象不同，對姿態(tài)角的限制更為嚴(yán)格[11]。因此，筆者將以姿態(tài)角ξ作為控制變量，通過優(yōu)化方法計算姿態(tài)角ξ，減少 2 個相鄰位姿之間的液壓缸長度變化量。

在確定預(yù)設(shè)路徑之后，對路徑上的點(diǎn)進(jìn)行等步長分割離散化，確定每個離散點(diǎn)的 [x，y，z]T值。通過優(yōu)化方法計算各離散點(diǎn)對應(yīng)的姿態(tài)角ξ，確定位姿空間離散點(diǎn)坐標(biāo)的 [x，y，z，ξ]T值后，由式 (1) 解出各離散點(diǎn)的關(guān)節(jié)空間坐標(biāo)：

式中：θ1為zi-1軸從xi-1軸到xi軸的旋轉(zhuǎn)角度，規(guī)定逆時針方向?yàn)檎?；αi為xi軸從zi-1軸到zi軸的轉(zhuǎn)角，規(guī)定逆時針方向?yàn)檎?；li為連桿i的長度；di為xi-1軸與xi軸沿zi-1軸的垂直距離；i為連桿數(shù)量。

液壓挖掘機(jī)工作裝置機(jī)構(gòu)簡圖如圖 2 所示。根據(jù)得到的各離散點(diǎn)關(guān)節(jié)空間坐標(biāo) [θ0，θ1，θ2，θ3]T值設(shè)定作業(yè)時間后，可以通過多項式插值方法完成軌跡規(guī)劃。基于各離散點(diǎn)的關(guān)節(jié)空間坐標(biāo)值，還可以根據(jù)挖掘機(jī)的幾何結(jié)構(gòu)，由式 (2)[12]計算出各離散點(diǎn)的驅(qū)動空間坐標(biāo) [θ0，λ1，λ2，λ3]T值：

其中α1=∠BAC+∠FAx1，b1=AC2+AF2，c1=2AC·AF，如圖 2(a) 所示；

α2=π -∠GBH+∠ABD，b2=BD2+BH2，c2=2BD·BH，如圖 2(b) 所示；

α3′=π -∠JGL+∠BGN，b3′=GN2+GL2，α4=∠GNE，b4′=NG，b5=GL，如圖 2(c) 所示。

圖2 液壓挖掘機(jī)工作裝置機(jī)構(gòu)簡圖Fig.2 Mechanism sketch of working unit of hydraulic excavator

2 軌跡規(guī)劃數(shù)學(xué)模型

2.1 目標(biāo)函數(shù)

為了提高液壓挖掘機(jī)作業(yè)平穩(wěn)性，降低能耗，將動臂機(jī)構(gòu)、斗桿機(jī)構(gòu)和鏟斗機(jī)構(gòu)的相鄰位姿之間液壓缸長度變化量最小化作為優(yōu)化目標(biāo)。子目標(biāo)函數(shù)如下：

式中：n為軌跡曲線取點(diǎn)的數(shù)量。

根據(jù)挖掘機(jī)各工作裝置液壓缸長度變化范圍對挖掘機(jī)所有液壓缸長度之和的影響，經(jīng)過無量綱處理后采用權(quán)系數(shù)法，對各子目標(biāo)加上η1、η2和η3作為權(quán)重因子，滿足η1+η2+η3=1，將 3 個子目標(biāo)函數(shù)集合為一個通用目標(biāo)函數(shù)：

2.2 設(shè)計變量

將每一個離散點(diǎn)所對應(yīng)的姿態(tài)角ξi(i=1～n) 作為優(yōu)化的設(shè)計變量。

2.3 約束條件

(1) 關(guān)節(jié)轉(zhuǎn)角約束 動臂、斗桿、鏟斗關(guān)節(jié)轉(zhuǎn)角不應(yīng)超過各部分轉(zhuǎn)角限制。

(2) 液壓缸長度約束 動臂、斗桿、鏟斗液壓缸長度不應(yīng)超過各部分液壓缸長度限制。

(3) 姿態(tài)角約束 在挖掘機(jī)工作范圍內(nèi)，當(dāng)鏟斗齒尖的空間位置確定時，其姿態(tài)角不可能為 360°范圍內(nèi)的任意角，那么鏟斗齒尖所在的位置相對于其不能實(shí)現(xiàn)的姿態(tài)角范圍是盲位，這一不能實(shí)現(xiàn)的姿態(tài)成為該位置的盲角。由于盲位與盲角的存在，導(dǎo)致挖掘機(jī)鏟斗位于某個位置時，往往不能實(shí)現(xiàn)特定的姿態(tài)；反之，當(dāng)鏟斗處于某種姿態(tài)時不能到達(dá)所要求的位置，這意味著挖掘機(jī)在作業(yè)過程中，可能不能同時完成任意位置和姿態(tài)的要求。在某一點(diǎn)位姿空間坐標(biāo) [x，y，z]T值給定的情況下，姿態(tài)角允許的范圍為 (ξ0，ξf)，任意點(diǎn)姿態(tài)角范圍的計算[13]如下：當(dāng)θ1=θ1max，或θ2=θ2min、θ3=θ3max時，會出現(xiàn)ξ0；當(dāng)θ1=θ1min、θ2=θ2max，或θ3=θ3min時，會出現(xiàn)ξf。

3 不同規(guī)劃方法的對比分析

由于在真實(shí)復(fù)合挖掘路徑上驗(yàn)證各方法的節(jié)能效果比較困難，而 T 型路徑由D1～D6這 6 個點(diǎn)連成的直線組成，計算相對簡單，遂選用 T 型路徑進(jìn)行驗(yàn)證。以某 8.5 t 挖掘機(jī)在一條 T 型路徑 (見圖 3) 上作業(yè)為例，對 3 種規(guī)劃方法進(jìn)行對比分析，再選擇節(jié)能效果最好的方法對真實(shí)復(fù)合挖掘路徑進(jìn)行軌跡規(guī)劃。

圖3 T 型路徑Fig.3 T-shaped path

3.1 經(jīng)驗(yàn)法

在挖掘機(jī)軌跡規(guī)劃的經(jīng)驗(yàn)法中[14]，將切削后角β設(shè)為定值，當(dāng)后角出現(xiàn)盲角時，調(diào)整后角消盲。通過下式計算出每一點(diǎn)的姿態(tài)角：

式中：τ為路徑上某一點(diǎn)與x軸正方向的夾角，如圖4 所示。

再依據(jù)已知的 T 型路徑上各點(diǎn)的 [x，y，z]T值，根據(jù)第 1 節(jié)中的軌跡規(guī)劃流程得出液壓缸長度變化的計算結(jié)果，如圖 4 所示。

圖4 采用經(jīng)驗(yàn)法得到的液壓缸長度變化曲線Fig.4 Variation curves of length of hydraulic cylinders obtained with empirical method

3.2 MATLAB 優(yōu)化工具箱

將第 2 節(jié)所提到的目標(biāo)函數(shù)、設(shè)計變量和約束條件輸入 MATLAB 優(yōu)化工具箱，采用其中的 fmincon函數(shù)，以經(jīng)驗(yàn)法的各離散點(diǎn)的姿態(tài)角值作為初始值，得出優(yōu)化后各離散點(diǎn)的姿態(tài)角值，并根據(jù)第 1 節(jié)中的軌跡規(guī)劃流程得出液壓缸長度變化的計算結(jié)果，如圖5 中的 3 條實(shí)線曲線 (O 組曲線) 所示。相比于圖 4 中經(jīng)驗(yàn)法的規(guī)劃效果，采用 MATLAB 優(yōu)化工具箱后的液壓缸變向點(diǎn)明顯減少，即由于液壓缸閥門頻繁換向所產(chǎn)生的振動有所減小。

圖5 采用 MATLAB 優(yōu)化工具箱和粒子群優(yōu)化算法得出的液壓缸長度變化對比Fig.5 Comparison of length variation of hydraulic cylinders obtained with MATLAB optimization toolbox and PSO algorithm

3.3 粒子群優(yōu)化算法

采用粒子群優(yōu)化算法進(jìn)行軌跡規(guī)劃，設(shè)置粒子群數(shù)量為 1 500 個，粒子維數(shù)為每段直線離散點(diǎn)數(shù)量，最大迭代數(shù)為 200，慣性權(quán)重為 0.5，學(xué)習(xí)因子c1、c2均為 2，得到優(yōu)化后的各離散點(diǎn)的姿態(tài)角值，同樣根據(jù)第 1 節(jié)中的軌跡規(guī)劃流程得出液壓缸長度變化的計算結(jié)果，如圖 5 中 3 條虛線曲線 (P 組曲線) 所示。相比于圖 4 中經(jīng)驗(yàn)法的規(guī)劃效果，采用粒子群優(yōu)化算法得到的液壓缸變向點(diǎn)數(shù)量明顯減少，且相較于采用MATLAB 優(yōu)化工具箱的變向點(diǎn)數(shù)量更少，因而粒子群優(yōu)化算法比 MATLAB 優(yōu)化工具箱法的規(guī)劃效果更好。在接近時間末端時，采用粒子群優(yōu)化算法得到的液壓缸長度變化曲線更平滑，突變更小。采用 2 種優(yōu)化方法相對于經(jīng)驗(yàn)法的改進(jìn)效率如表 1 所列 (其中x為第2.1 節(jié)中目標(biāo)函數(shù)的計算結(jié)果，即液壓缸最小長度變化量，表 3 同)。從表 1 可以看出，采用粒子群優(yōu)化算法得到的總液壓缸長度變化量更少，能耗更低，優(yōu)化效果更好。

表1 采用 MATLAB 優(yōu)化工具箱和粒子群優(yōu)化算法相對于經(jīng)驗(yàn)法的改進(jìn)效果對比Tab.1 Improvement effects of MATLBA optimization toolbox and PSO algorithm compared with expirical method

4 基于常態(tài)挖掘路徑的軌跡規(guī)劃

4.1 常態(tài)挖掘路徑選取

測試機(jī)型為某 36 t 的反鏟液壓挖掘機(jī)，基本參數(shù)如表 2 所列。測試地點(diǎn)為湖州某試驗(yàn)場，挖掘?qū)ο鬄榛煊惺瘔K的Ⅲ級土壤，作業(yè)現(xiàn)場如圖 6 所示。采用傾角傳感器對挖掘機(jī)各關(guān)節(jié)角位移進(jìn)行測量[15]，取停機(jī)面最大半徑挖掘工況作為本次軌跡規(guī)劃路徑。由于受到客觀條件的限制，直接對此路徑進(jìn)行連續(xù)軌跡規(guī)劃比較困難，取路徑擬合結(jié)果所得路徑方程進(jìn)行軌跡規(guī)劃，結(jié)果如圖 7 所示。

圖6 作業(yè)現(xiàn)場Fig.6 Operation scene

表2 測試機(jī)型基本參數(shù)Tab.2 Basic parameters of test excavator

4.2 停機(jī)面最大半徑挖掘路徑軌跡規(guī)劃

選擇粒子群優(yōu)化算法按照第 1 節(jié)中的軌跡規(guī)劃流程進(jìn)行軌跡規(guī)劃，結(jié)果如圖 8 中 A 組 3 條虛線曲線所示。將其與試驗(yàn)得到的液壓缸長度變化曲線 (B 組 3條實(shí)線曲線) 進(jìn)行對比可知，兩者除動臂液壓缸長度變化接近外，采用粒子群優(yōu)化算法得到的液壓缸變向點(diǎn)數(shù)量減少，液壓缸長度變化時過渡更加平滑。采用粒子群優(yōu)化算法相對于試驗(yàn)結(jié)果的改進(jìn)效果如表 3 所列。

圖8 采用粒子群優(yōu)化算法與試驗(yàn)得到的液壓缸長度變化對比Fig.8 Comparison of length variation of hydraulic cylinders obtained with PSO algorithm and test

表3 采用粒子群優(yōu)化算法相對于試驗(yàn)結(jié)果的改進(jìn)效果Tab.3 Improvement effects of PSO algorithm compared with test data

由于對各關(guān)節(jié)在中間點(diǎn)時的速度有約束，所以采用 4-3-3-……-3-3-4 多項式插值方法[16]得到的軌跡規(guī)劃結(jié)果如圖 9～ 11 所示。由圖 9～ 11 可知：各關(guān)節(jié)角位移、角速度變化曲線平滑連續(xù)，角加速度變化曲線連續(xù)，保證了各關(guān)節(jié)角變化平穩(wěn)連續(xù)；起始點(diǎn)、終止點(diǎn)速度、加速度均為零，能夠完成連續(xù)路徑下的全過程工作，滿足挖掘機(jī)挖掘過程中的性能要求。

圖9 角位移變化曲線Fig.9 Variation curves of angular displacement

圖10 角速度變化曲線Fig.10 Variation curves of angular velocity

圖11 角加速度變化曲線Fig.11 Variation curves of angular acceleration

5 結(jié)語

在分析液壓挖掘機(jī)工作機(jī)理及運(yùn)動學(xué)建模基礎(chǔ)上，通過對 3 種方法的比較，以挖掘機(jī)器人工作平穩(wěn)性和節(jié)能作為重點(diǎn)，選擇采用粒子群優(yōu)化算法對挖掘機(jī)器人運(yùn)動軌跡進(jìn)行規(guī)劃，在一條實(shí)際復(fù)合挖掘路徑上加以驗(yàn)證，并與試驗(yàn)數(shù)據(jù)進(jìn)行對比，得出該方法效果良好的結(jié)論。對于復(fù)合挖掘來說，筆者對挖掘機(jī)器人動力學(xué)部分涉及不夠，針對這點(diǎn)，會在后續(xù)研究中進(jìn)一步深入。