馮明豪，任志貴,2，魏萬(wàn)行，孫浩然，李佳豪

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

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

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

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

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

根據(jù)液壓挖掘機(jī)器人的結(jié)構(gòu)，工作裝置可以簡(jiǎn)要描述為位姿空間 [x，y，z，ξ]T、關(guān)節(jié)空間 [θ0，θ1，θ2，θ3]T和驅(qū)動(dòng)空間 [θ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)動(dòng)。而挖掘機(jī)在實(shí)際作業(yè)時(shí)，由于和工業(yè)機(jī)械手的作業(yè)對(duì)象不同，對(duì)姿態(tài)角的限制更為嚴(yán)格[11]。因此，筆者將以姿態(tài)角ξ作為控制變量，通過(guò)優(yōu)化方法計(jì)算姿態(tài)角ξ，減少 2 個(gè)相鄰位姿之間的液壓缸長(zhǎng)度變化量。

在確定預(yù)設(shè)路徑之后，對(duì)路徑上的點(diǎn)進(jìn)行等步長(zhǎng)分割離散化，確定每個(gè)離散點(diǎn)的 [x，y，z]T值。通過(guò)優(yōu)化方法計(jì)算各離散點(diǎn)對(duì)應(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ī)定逆時(shí)針?lè)较驗(yàn)檎沪羒為xi軸從zi-1軸到zi軸的轉(zhuǎn)角，規(guī)定逆時(shí)針?lè)较驗(yàn)檎?；li為連桿i的長(zhǎng)度；di為xi-1軸與xi軸沿zi-1軸的垂直距離；i為連桿數(shù)量。

液壓挖掘機(jī)工作裝置機(jī)構(gòu)簡(jiǎn)圖如圖 2 所示。根據(jù)得到的各離散點(diǎn)關(guān)節(jié)空間坐標(biāo) [θ0，θ1，θ2，θ3]T值設(shè)定作業(yè)時(shí)間后，可以通過(guò)多項(xiàng)式插值方法完成軌跡規(guī)劃?；诟麟x散點(diǎn)的關(guān)節(jié)空間坐標(biāo)值，還可以根據(jù)挖掘機(jī)的幾何結(jié)構(gòu)，由式 (2)[12]計(jì)算出各離散點(diǎn)的驅(qū)動(dòng)空間坐標(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)簡(jiǎn)圖Fig.2 Mechanism sketch of working unit of hydraulic excavator

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

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

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

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

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

2.2 設(shè)計(jì)變量

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

2.3 約束條件

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

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

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

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

由于在真實(shí)復(fù)合挖掘路徑上驗(yàn)證各方法的節(jié)能效果比較困難，而 T 型路徑由D1～D6這 6 個(gè)點(diǎn)連成的直線組成，計(jì)算相對(duì)簡(jiǎn)單，遂選用 T 型路徑進(jìn)行驗(yàn)證。以某 8.5 t 挖掘機(jī)在一條 T 型路徑 (見(jiàn)圖 3) 上作業(yè)為例，對(duì) 3 種規(guī)劃方法進(jìn)行對(duì)比分析，再選擇節(jié)能效果最好的方法對(duì)真實(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)盲角時(shí)，調(diào)整后角消盲。通過(guò)下式計(jì)算出每一點(diǎn)的姿態(tài)角：

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

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

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

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

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

圖5 采用 MATLAB 優(yōu)化工具箱和粒子群優(yōu)化算法得出的液壓缸長(zhǎng)度變化對(duì)比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 個(gè)，粒子維數(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ī)劃流程得出液壓缸長(zhǎng)度變化的計(jì)算結(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ī)劃效果更好。在接近時(shí)間末端時(shí)，采用粒子群優(yōu)化算法得到的液壓缸長(zhǎng)度變化曲線更平滑，突變更小。采用 2 種優(yōu)化方法相對(duì)于經(jīng)驗(yàn)法的改進(jìn)效率如表 1 所列 (其中x為第2.1 節(jié)中目標(biāo)函數(shù)的計(jì)算結(jié)果，即液壓缸最小長(zhǎng)度變化量，表 3 同)。從表 1 可以看出，采用粒子群優(yōu)化算法得到的總液壓缸長(zhǎng)度變化量更少，能耗更低，優(yōu)化效果更好。

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

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

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

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

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

表2 測(cè)試機(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)得到的液壓缸長(zhǎng)度變化曲線 (B 組 3條實(shí)線曲線) 進(jìn)行對(duì)比可知，兩者除動(dòng)臂液壓缸長(zhǎng)度變化接近外，采用粒子群優(yōu)化算法得到的液壓缸變向點(diǎn)數(shù)量減少，液壓缸長(zhǎng)度變化時(shí)過(guò)渡更加平滑。采用粒子群優(yōu)化算法相對(duì)于試驗(yàn)結(jié)果的改進(jìn)效果如表 3 所列。

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

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

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

圖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é)語(yǔ)

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