液壓挖掘機軌跡規劃及運動仿真

胡海浪，殷晨波，周俊靜，賈文華

(1. 南京工業大學 車輛與工程機械研究所，江蘇 南京 211800; 2. 南京工程學院，江蘇 南京 211167)

0 引言

液壓挖掘機作為土石方施工機械，在各種工程建設領域所起到的作用越來越明顯，在工程機械中占有十分重要的地位[1]。為實現挖掘機的智能化，提高挖掘機自助挖掘能力，需確定合理的挖掘作業軌跡規劃。國內外學者圍繞插值函數，提出諸多軌跡規劃方法。如朱世強等7次樣條曲線插值函數，該方法減振效果明顯，但計算復雜，效率低[2]；劉涼等提出采用5次多項式插值法，能夠保證加加速度平穩，但必須已知路徑點速度及加速度[3]；Macfarlane提出最小脈動和脈動連續軌跡規劃方法[4]。對于挖掘機工作裝置的運動學分析，國內外學者一般采用作圖法、矢量代數法和桿組分析法等。運動學仿真多采用MATLAB/SimMechanics、Pro/E和ADAMS等工具[5]，但其建模方法較為復雜。

考慮到挖掘機工作裝置與機械手較為相似[6]，本文采用機械手運動仿真用的MATLAB/Robotics Toolbox工具箱對某21t級挖掘機工作裝置進行軌跡規劃及運動仿真，得出挖掘機工作裝置姿態角、液壓油缸長度以及鏟斗齒尖位移變化曲線。

1 建立運動坐標系

為了描述相鄰桿件之間平移和轉動的關系，Denavit和Hartenberg提出了一種為關節鏈中的每一桿件建立附加坐標系的方法[7]。D-H矩陣法是為每個關節處的桿件建立四階齊次變換矩陣，表示其與前一桿件坐標系的關系。本文運用D-H方法，在圖1中建立挖掘機工作裝置運動坐標系。

圖1 挖掘機工作裝置運動學坐標系

每個桿件分別用參數ai、θi、di、αi描述。其中ai表示zi-1軸與zi軸沿xi軸方向的距離；di表示xi+1軸與xi軸沿zi軸方向的距離；αi表示從軸zi到軸zi+1繞軸xi+1旋轉的角度;θi表示從xi+1軸到xi軸繞zi旋轉的角度。本文所選挖掘機結構參數如表1所示。

表1 SY215挖掘機工作裝置D-H參數

2 運動學分析

2.1 正運動學

挖掘機工作裝置具有4個自由度，需要在四維空間才能唯一表示出來。按選取描述變量不同可分為：以回轉馬達轉角和三組液壓缸長度所表示的驅動機構空間；回轉平臺與底座之間、動臂與回轉平臺之間、斗桿與動臂之間、鏟斗與斗桿之間夾角所表示的關節空間；鏟斗齒尖在基坐標系中的位置以及鏟斗姿態角所表示的位姿空間。

如圖1所示，建立挖掘機工作裝置運動學坐標系。根據機器人運動學可知，空間任意坐標系中向量均可以通過齊次變換矩陣T轉換到另一坐標系中，如坐標系i相對于坐標系i-1的變換矩陣[8]為：

(1)

將連桿坐標系變換矩陣相乘，可得鏟斗齒尖的坐標系中相對于基坐標系的變換矩陣為:

(2)

其中：c0=cosθ0；c123=cos(θ1+θ2+θ3)；s0=sinθ0；s123=sin(θ1+θ2+θ3)，依次類推。

令P4為鏟斗齒尖在坐標系O4x4y4z4中的坐標，則鏟斗齒尖在極坐標系中坐標P0為:

(3)

(4)

鏟斗姿態角β為:

β=θ1+θ2+θ3

(5)

由此便得出關節向量空間向鏟斗位姿空間的正向變換。

不考慮挖掘機回轉運動時，根據式(4)可得出如圖2所示挖掘機工作裝置包絡圖，鏟斗齒尖理論上可以到達包絡圖邊界內的所有區域。

圖2 挖掘機工作裝置包絡圖

2.2 逆運動學

對于運動學逆問題求解，主要有3種方法：迭代法、解析法和幾何法。根據挖掘機工作裝置的結構特性本文應用幾何法逆向求解液壓缸長度與挖掘機工作裝置各關節角度的關系。

以鏟斗液壓缸為例，建立圖3所示的鏟斗結構簡圖。

圖3 鏟斗結構簡圖

圖3中A為動臂與斗桿鉸點；B為鏟斗油缸與斗桿鉸點；C為鏟斗油缸、連桿和搖桿鉸點；D為連桿與鏟斗鉸點；E為搖桿與斗桿鉸點；F為鏟斗與斗桿鉸點；G為鏟斗齒尖。則液壓缸長度為：

(6)

∠BEC=∠BEF-∠CED-∠DEF

(7)

(8)

(9)

(10)

∠DFE=π-θ3-∠DFG-∠AFE

(11)

式中，λ3為鏟斗油缸長度，θ3為鏟斗關節角度，lBC為BC兩點之間的距離，同理，其余各物理量幾何參數如表2所示。

表2 鏟斗逆運動學分析中各桿件參數

根據式(6)-式(11),可求得鏟斗油缸長度與鏟斗關節角的逆向運動學關系。同理可得動臂油缸長度λ1與動臂關節角度θ1、斗桿油缸長度λ2與斗桿關節角度θ2的對應關系，如圖4所示。

圖4 油缸長度與關節角度關系圖

3 運動學仿真

3.1 Robotics Toolbox運動學建模

Robotics Toolbox是澳大利亞科學家Corke開發的基于MATLAB的機器人學工具箱，其提供了機器人學研究中許多函數，如正運動學、逆動力學和Jacobi矩陣。同時該工具箱還可以建立機器人桿件三維模型，并對其進行軌跡規劃仿真[9]。

根據表1中挖掘機D-H參數，利用link([alpha A theta D sigma ,‘standard’])函數建立挖掘機工作裝置運動學模型。其中，link函數中standard代表采用標準的D-H參數;參數alpha代表扭轉角αi;參數A代表桿件長度ai;參數theta代表關節角θi;參數D代表橫距di;參數sigma代表關節類型：0代表旋轉關節，1代表移動關節。挖掘機4個關節均為旋轉關節，所以本文中sigma均為0。

L0=link([pi/2,90,0,1877,0],'standard')；

%回轉平臺模型

L1=link([0,5700,0,0,0],'standard')；

%動臂關節模型

L2=link([0,2925,0,0,0],'standard')；

%斗桿關節模型

L3=link([0,1334,0,0,0],'standard')；

%鏟斗關節模型

然后用link.qlim函數限定各關節角度變化范圍，Li.qlim=[θimin,θimax],θimin與θimax分別表示表1中θi的最小角和最大角所對應的的弧度值。再用robot函數excavator=robot({L0L1L2L3})建立模型，運行drivebot(excavator)程序，得出如圖5所示該挖掘機模型工作裝置的運動調節界面和圖6所示挖掘機工作裝置三維視圖。

圖5 挖掘機工作裝置運動調節界面

圖6 挖掘機工作裝置三維結構簡圖

本文不考慮挖掘機的回轉運動，即令θ0恒等于0，基于此，為了觀察更直觀，可以用view(0,0)函數將圖6三維視圖轉化為基坐標系xoz平面內的二維視圖，用axis函數限定x軸與z軸坐標范圍。

3.2 軌跡規劃

挖掘機軌跡規劃是根據具體的作業要求，計算出預期的運動軌跡以及各關節角度變化趨勢。軌跡規劃主要包括2種方法：點到點規劃PTP(point to point)、連續路徑規劃CP(continuous path motion)。Robotics Toolbox提供的ctraj、jtraj和trinterp函數可以實現笛卡爾規劃、關節空間規劃和變換插值。

本文采用jtraj函數來實現挖掘機各關節連續路徑規劃仿真。首先選取軌跡規劃起點與終點，起點選用最大挖掘高度A，終點選用最大挖掘深度B。使用view(0,0)將工作裝置三維結構圖轉為xz平面二維結構簡圖，通過調節圖5中關節角度滑塊可以直觀得出最大挖掘高度點與最大挖掘深度點對應的關節角變量，分別為qA=[0，1.033 2，-0.607 4，0.907 6]，qB=[0，-0.795 8，-0.775 2，0]。

使用plot命令進行運動仿真，命令如下:

t=[0:0.05:10] %產生時間向量

qA=[0,1.0332,-0.6074,0.9076]；

qB=[0,-0.7958,-0.7752,0]；

%輸入起止點關節坐標.

[q qd qdd] =jtraj(qA,qB,t);

%軌跡規劃命令

再運行程序plot(t,q(：，i))，i=2、3、4，就可得出各關節角度變化曲線，如圖7所示。

根據已規劃的關節角度軌跡以及逆運動學分析，可以在Matlab中求解出挖掘機各工作裝置相應液壓缸的長度變化曲線，如圖8所示。

再根據軌跡規劃好的角度變化，采用正運動學求解函數S=fkine(excavator，q)，可求得挖掘機模型鏟斗齒尖在x、z方向上位移量，如圖9所示。

圖7 關節角度變化曲線

圖8 油缸長度變化曲線

圖9 鏟斗齒尖在x、z方向的位移

4 仿真結果分析

通過調節圖5中滑塊位置，可以直觀看出該型號挖掘機最大挖掘高度約為9.3 m，最大挖掘深度約為6.5 m，最大挖掘半徑約為9.5 m，滿足中型挖掘機作業空間要求，說明了該型號挖掘機工裝裝置尺寸參數設計的合理性。

在運動仿真過程中，從圖6中觀察到挖掘機鏟斗由最大挖掘高度運動到最大挖掘深度時各個關節的運動情況(運動過程是動態的，文中無法顯示這一過程)，由圖7可以看出各個關節運動情況正常。圖8得出在運動過程中，動臂油缸由3.15 m縮為1.87 m，斗桿油缸由2.075 m伸長到2.17 m，鏟斗油缸由1.68 m伸長到2.08 m，油缸伸縮速度穩定。圖9得出挖掘機模型末端在作業空間內位置變化平緩，且在運動起始和終止位置速度和角加速度均趨向于零，理論上沒有較大的沖擊。

5 結語

1) 在機器人理論基礎上，對某21t級中型挖掘機進行了正、逆運動學分析。在MATLAB 2010a環境下，運用Robotics Toolbox編寫程序語句，對挖掘機工作裝置進行運動學建模和作業空間測定，該方法建模簡單，可直觀獲取挖掘機作業空間。

2) 對挖掘機工作裝置進行點到點軌跡規劃，得出各關節姿態角、油缸長度以及鏟斗齒尖平滑連續的運動曲線。

3) 運動仿真結果表明所得出的挖掘機各關節運動速度和角加速度變化平穩，油缸伸縮速度穩定，鏟斗齒尖運動變化平緩，挖掘機在工作過程中沒有較大的沖擊。驗證了該型號挖掘機各關鍵部位桿件參數設置的合理性，為挖掘機軌跡規劃和運動學仿真提供了一種簡便的方法，并為挖掘機自主挖掘研究奠定了運動學基礎。

參考文獻:

[1] 張啟君,劉偉.國內挖掘機行業發展的探討[J]. 筑路機械與施工機械化, 2005,22(2):1-6.

[2] 朱世強,劉松國,王宣銀,等. 機械手時間最優脈動連續軌跡規劃算法[J]. 機械工程學報, 2010, 46(3):47-52.

[3] 劉涼,陳超英,趙新華. 考慮關節摩擦的并聯機器人平滑軌跡規劃[J]. 機械工程學報, 2014，50(19):9-17.

[4] Macfarlane S,Croft E.Jerk-bounded manipulator trajectory planning: design for real-time applications[J]. IEEE Transactions on Robotics & Automation, 2003, 19(1):42-52.

[5] 馬肖麗,周志鴻. 基于ADAMS的液壓挖掘機工作裝置的運動學仿真[J]. 建筑機械, 2011(5):102-105.

[6] 黃德銀,嚴世榕,許鎖林. 基于Robotics Toolbox挖掘機工作裝置的運動仿真[J]. 建筑機械,2013(17):91-94.

[7] 陳支,鄒樹梁,唐德文,等. D-H坐標系下挖掘機工作裝置運動學建模與仿真[J]. 機械設計與制造, 2014(11):188-190.

[8] 蔡自興. 機器人學[M]. 北京：清華大學出版社，2009.

[9] 左富勇,胡小平,謝珂,等. 基于MATLAB Robotics工具箱的SCARA機器人軌跡規劃與仿真[J]. 湖南科技大學學報(自然科學版),2012,27(2):41-44.