林木聯(lián)合采育機自主作業(yè)的虛擬仿真系統(tǒng)1)

葛桃桃 鄭一力 劉晉浩

(北京林業(yè)大學(xué)，北京，100083)

林木聯(lián)合采育機自主作業(yè)的虛擬仿真系統(tǒng)1)

葛桃桃 鄭一力 劉晉浩

(北京林業(yè)大學(xué)，北京，100083)

針對林木聯(lián)合采育機自主作業(yè)路徑規(guī)劃與控制的研究需求，采用SolidWorks軟件對林木聯(lián)合采育機進行了三維實體建模，將模型導(dǎo)入到圖形開發(fā)引擎OpenSceneGraph(OSG)，結(jié)合Visual C++編程語言，完成了一套林木聯(lián)合采育機作業(yè)三維虛擬仿真系統(tǒng)。闡述了該虛擬仿真系統(tǒng)的架構(gòu)、功能以及系統(tǒng)的開發(fā)流程。在對機械臂運動學(xué)建模的基礎(chǔ)上，給出了伐木頭直線、圓弧和激光反饋的路徑規(guī)劃和控制方法。在三維虛擬仿真系統(tǒng)中通過采伐虛擬立木實驗，驗證了自主作業(yè)路徑規(guī)劃與控制方法的有效性，該研究為實現(xiàn)林木聯(lián)合采育機的無人自主采伐作業(yè)打下基礎(chǔ)。

林木聯(lián)合采育機；自主作業(yè)；虛擬仿真

For the path planning and control simulation for the autonomous task of a logging harvester, we built three-dimensional solid models of all parts of the logging harvester via SolidWorks. Introducing these models to the OpenSceneGraph (OSG) by Visual C++ programming language, we performed the virtual simulation system. We introduced the system architecture of the virtual simulation system, together with its function, and system process. Kinematics modeling of the hydraulic boom was obtained for the control purpose. The straight, circular and the laser measurement feedback path planning and control methods were given for the harvesting head to capture trunks. The felling trunks experiments in the virtual simulation system showed the validity of the proposed path planning and control method.

林木聯(lián)合采育機是一種高性能現(xiàn)代林業(yè)裝備，可大大提高林木采伐的效率和安全性，但將伐木頭對準(zhǔn)和捕獲目標(biāo)立木的環(huán)節(jié)，由于視線被擋以及車底盤和機械臂的振動，需要操作員做出大量的觀測判斷和反復(fù)手動對準(zhǔn)，造成不必要的時間和燃料，有必要對機械臂的自主作業(yè)路徑規(guī)劃與控制方法進行研究。

針對林木聯(lián)合采育機這種大型和復(fù)雜機電系統(tǒng)，對其自主作業(yè)進行研究的前期，由于存在危險性和費用高等問題，必須要對關(guān)鍵控制技術(shù)進行虛擬仿真驗證[1]。

在農(nóng)林機械虛擬仿真方面，芬蘭Mevea公司[2]和美國JohnDeere公司[3]開發(fā)的林業(yè)訓(xùn)練模擬機，對林木聯(lián)合采育機和集材機的液壓裝置、機身、變速器以及伐木環(huán)境等精確的仿真，可以完成采伐、打枝、去皮、造材、集材等連續(xù)作業(yè)，并通過顯示屏從不同的視角觀察伐木點周圍環(huán)境；U. Mettin等[4]針對集材機液壓臂的路徑規(guī)劃與控制問題，綜合考慮了液壓臂的動作時間、關(guān)節(jié)速度、最大角度和動力學(xué)等約束，通過OSG對機械臂進行三維路徑規(guī)劃，提出了時間運動分解的運動控制算法，實現(xiàn)了集材機液壓臂抓取原木的自主規(guī)劃和控制。賀繼林等[5]針對液壓挖掘機機械臂的軌跡控制，將基于SOPC的軌跡控制器與基于OpenGL的仿真軟件共同組成虛擬仿真環(huán)境，使液壓挖掘機的機械臂模型按照設(shè)定軌跡運動，達到了良好控制效果；趙滿全等[6]、張紅彥等[7]應(yīng)用SolidWorks和ADAMS對雙圓盤割草機進行仿真和動力學(xué)分析，研究其運動軌跡，計算重割率；蘇明明[8]采用SolidWorks和Creator建立了挖掘機的三維模型和虛擬場景，用Lynx Prime對仿真系統(tǒng)進行配置，在VC++平臺上調(diào)用Vega Prime實現(xiàn)交互功能，用鍵盤控制挖掘機的挖掘動作。

三維實體建模軟件SolidWorks具有仿真效果逼真、實時交互性好等諸多優(yōu)點，圖形開發(fā)引擎OSG具有可擴展性、可移植性和快速開發(fā)性。本文選用SolidWorks為三維建模軟件，在Visual C++開發(fā)環(huán)境中調(diào)用OSG進行虛擬仿真系統(tǒng)的研發(fā)，對機械臂的自主作業(yè)路徑規(guī)劃與控制方法進行研究。

1 仿真系統(tǒng)的建立

虛擬仿真系統(tǒng)包含林木聯(lián)合采育機、林地背景和目標(biāo)立木的虛擬模型和數(shù)學(xué)模型，可輸入控制參數(shù)和指令，并實時顯示和保存運動信息，進行林木聯(lián)合采育機自主作業(yè)路徑規(guī)劃與控制的參數(shù)化仿真與同步動態(tài)虛擬顯示。

1.1 仿真系統(tǒng)的架構(gòu)

針對于林木聯(lián)合采育機的虛擬仿真任務(wù)需求，本系統(tǒng)分為任務(wù)管理、數(shù)據(jù)管理、路徑規(guī)劃和界面顯示4個模塊，系統(tǒng)架構(gòu)見圖1。

圖1 系統(tǒng)架構(gòu)

任務(wù)管理模塊：完成對林木采育作業(yè)任務(wù)的管理，以及任務(wù)運行過程中對數(shù)據(jù)管理模塊和路徑規(guī)劃模塊的協(xié)調(diào)與同步。

數(shù)據(jù)管理模塊：管理路徑規(guī)劃數(shù)據(jù)和狀態(tài)數(shù)據(jù)，實現(xiàn)數(shù)據(jù)共享和同步更新。林木聯(lián)合采育機的運動控制需要處理大量的路徑規(guī)劃數(shù)據(jù)和狀態(tài)數(shù)據(jù)，且存在多個模塊處理相同數(shù)據(jù)的情況，故將整個系統(tǒng)所需要處理的數(shù)據(jù)分離出來形成一個獨立的模塊。

路徑規(guī)劃模塊：調(diào)用相關(guān)路徑規(guī)劃庫函數(shù)，根據(jù)作業(yè)任務(wù)需求完成林木聯(lián)合采育機作業(yè)的直線、圓弧和自主路徑規(guī)劃與控制算法計算，并更新數(shù)據(jù)管理模塊中的相關(guān)內(nèi)容。

界面顯示模塊：由仿真模型顯示模塊和信息更新模塊兩部分構(gòu)成。仿真模型顯示模塊可實時顯示林木聯(lián)合采育機的運動及其操作環(huán)境的仿真模型。信息更新模塊將數(shù)據(jù)管理模塊和路徑規(guī)劃模塊傳輸?shù)臄?shù)據(jù)進行實時更新和實現(xiàn)，有數(shù)據(jù)變動時，發(fā)送更新信號，視圖類窗口即會讀取對應(yīng)的數(shù)據(jù)進行更新，確保仿真圖像的實時性。

1.2 林木聯(lián)合采育機三維實體建模

采用SolidWorks軟件，以CFJ30型林木聯(lián)合采育機為原型，根據(jù)其真實尺寸，對各零件進行三維實體建模，如圖2所示。

1.后推油缸;2.底座;3.主油缸;4.立柱;5.副油缸;6.連桿;7.大臂;8.小臂;9.伸縮臂。

圖2 林木聯(lián)合采育機結(jié)構(gòu)示意圖

林木聯(lián)合采育機的模型由輪式底盤、伐木頭和多自由度機械臂組成。輪式底盤為所有部件提供支撐，為車輛行駛、機械臂和伐木頭的運動提供動力；伐木頭安裝在機械臂的末端，用來完成伐木、打枝、去皮、造材等一系列工序；機械臂是林木聯(lián)合采育機的主要執(zhí)行機構(gòu)，決定了其作業(yè)能力和范圍，主要由立柱、連桿、大臂、小臂、伸縮臂等組成。后推油缸伸縮控制底座小角度擺動，最大傾角為20°，底座的回轉(zhuǎn)機構(gòu)使機械臂可相對于底座做150°轉(zhuǎn)動，大臂和小臂通過主油缸和副油缸的伸縮進行變幅和升降運動，伸縮臂從小臂中伸出，小臂內(nèi)部有伸縮油缸控制伸縮臂的伸出量[9]，本文系統(tǒng)中設(shè)定伸縮臂的最大伸長量為2 m。

機械臂各機構(gòu)之間通過旋轉(zhuǎn)關(guān)節(jié)和滑動關(guān)節(jié)連接，共有5個自由度：立柱俯仰旋轉(zhuǎn)和朝向旋轉(zhuǎn)，大臂俯仰旋轉(zhuǎn)，小臂屈伸和伸縮臂伸縮。伐木頭通過銷軸安裝在機械臂末端，呈懸掛狀態(tài)。

1.3 三維場景建立

林木聯(lián)合采育機作業(yè)在三維場景中完成，以增強虛擬仿真的沉浸性。本文三維場景主要包括林木聯(lián)合采育機本體、林地背景圖和目標(biāo)立木。林地背景圖和目標(biāo)立木通過貼圖導(dǎo)入到OSG中，將SolidWorks建立的林木聯(lián)合采育機實體模型，通過stl格式導(dǎo)入到OSG中，完成各虛擬零件的組裝，并添加紋理、顏色。完成林木聯(lián)合采育機虛擬仿真系統(tǒng)界面(見圖3)。本系統(tǒng)中目標(biāo)立木的方位數(shù)據(jù)來源于項目先前在白楊樹林中做的激光測量實驗[10]。

圖3 虛擬仿真系統(tǒng)界面

本系統(tǒng)在旋轉(zhuǎn)軸位置建立了Denavit-Hartenberg(DH)坐標(biāo)系[11](如圖4)，以坐標(biāo)軸為參考，通過一系列數(shù)據(jù)的指令驅(qū)動各坐標(biāo)系使實體產(chǎn)生運動。同時，林木聯(lián)合采育機組裝過程中，使DH坐標(biāo)系與零件本身位姿坐標(biāo)系重合，可以精確快速的完成組裝。

2 運動學(xué)建模分析

運動學(xué)建模分析是對林木聯(lián)合采育機機械臂的自主作業(yè)路徑規(guī)劃和控制方法進行研究的基礎(chǔ)。運動學(xué)正解是機械臂關(guān)節(jié)空間到末端操作空間的映射，在已知機械臂各關(guān)節(jié)角時，可求得機械臂末端相對于基坐標(biāo)系的位姿；運動學(xué)逆解是位姿空間向關(guān)節(jié)空間的轉(zhuǎn)換，通過液壓缸運動可以實現(xiàn)對機械臂末端執(zhí)行器位姿的控制。

圖4 機械臂的DH坐標(biāo)系

2.1 坐標(biāo)系建立

林木聯(lián)合采育機的機械臂可看成是具有多個關(guān)節(jié)的空間結(jié)構(gòu)，為了描述機械臂末端執(zhí)行器在空間的位置，利用坐標(biāo)系之間的關(guān)系來描述機械臂末端執(zhí)行器的位置。林木聯(lián)合采育機的機械臂有4個旋轉(zhuǎn)關(guān)節(jié)和一個平動關(guān)節(jié)。每一個關(guān)節(jié)對應(yīng)一個坐標(biāo)系{i}(i=1，2，…，5)，其對應(yīng)的DH參數(shù)見表1。

表1 機械臂的DH參數(shù)

注：表中“-”表示機械臂逆時針轉(zhuǎn)動。

2.2 運動學(xué)正解

則機械臂末端的位姿為：

(1)

2.3 運動學(xué)逆解

(2)

(2)式中的雅克比矩陣J不能直接變換，用5×6的廣義逆矩陣J+=JT(JJT)-1表示[13]。機械臂的運動學(xué)逆解為：

(3)

通過(3)式可以實時計算出各關(guān)節(jié)轉(zhuǎn)動的角速度。

3 路徑規(guī)劃與控制方法

機械臂的路徑規(guī)劃是指給定機械臂的初始位姿及機械手末端的目標(biāo)位姿，根據(jù)作業(yè)任務(wù)的要求，計算出預(yù)期的運動軌跡[14]。本文主要研究3種路徑規(guī)劃與控制方法：直線路徑規(guī)劃、圓弧路徑規(guī)劃和基于激光反饋的自主路徑規(guī)劃。

3.1 直線路徑規(guī)劃

機械臂末端按直線運動規(guī)劃時，機械臂末端執(zhí)行器的線速度和角速度都采用按照帶拋物線過度圓弧梯形法規(guī)劃。設(shè)起點和終點時刻為0和tz，加速段(或減速段)時間為ta，勻速段時間為ts，加速段和減速段曲線的過渡點時刻為ta/3、2ta/3、tz-2ta/3和tz-ta/3，在過渡點時刻ta/3或(tz-ta/3)、2ta/3或(tz-2ta/3)處的速度為v1和v2，最大速度為vm，終點對應(yīng)的位移為df，其位移-時間關(guān)系如圖5所示。

a=ta/3,b=2ta/3,c=ta,d=ta+ts,e=tz-2ta/3,f=tz-ta/3,g=tz。

3.2 圓弧路徑規(guī)劃

林木聯(lián)合采育機的圓弧路徑規(guī)劃是將圓弧路徑當(dāng)做空間圓弧曲線處理?？臻g圓弧規(guī)劃一般分兩步處理，第一步把三維問題轉(zhuǎn)化為二維問題，即在圓弧平面內(nèi)規(guī)劃，第二步利用二維平面插補算法，求出插補點坐標(biāo)，然后再把這個點坐標(biāo)值轉(zhuǎn)變成基坐標(biāo)系下的值。同樣采用按照帶拋物線過度圓弧梯形法的線性插值方法，與直線路徑規(guī)劃不同的是，這里規(guī)劃的是末端執(zhí)行器沿圓周的旋轉(zhuǎn)角度和角速度。

空間圓弧規(guī)劃坐標(biāo)系如圖6所示。已知點p1、p2、p3，設(shè)機械臂末端執(zhí)行器沿著從始點p1將中間點p2到終點p3的圓弧運動[15]。

圖6 空間圓弧規(guī)劃坐標(biāo)系

3.3 基于激光反饋的自主路徑規(guī)劃

自主控制時，激光測量系統(tǒng)參與運動控制，激光測量系統(tǒng)實時計算出目標(biāo)立木相對伐木頭的位姿，進行路徑規(guī)劃與控制。

設(shè)基坐標(biāo)系為I(OI-XIYIZI)，機械臂末端坐標(biāo)系為T(OT-XTYTZT)，目標(biāo)點坐標(biāo)系為W(OW-XWYWZW)，自主路徑規(guī)劃的坐標(biāo)系如圖7所示。具體步驟如下：

1)確定跟蹤目標(biāo)點W相對于機械臂末端T和基坐標(biāo)系I的當(dāng)前位姿；

2)考慮到直接跟蹤目標(biāo)物，可能會發(fā)生機械臂末端和目標(biāo)物碰撞的問題，因此先以與目標(biāo)物固連的坐標(biāo)系的X軸上距離目標(biāo)物ds(設(shè)ds=0.1 m)的點作為跟蹤中間點pW；

3)計算pW的位姿坐標(biāo)，然后以pW為目標(biāo)物，計算pW相對于機械臂末端T在基坐標(biāo)系I下的位姿坐標(biāo)差D，并計算機械臂末端與pW之間的距離dv；

以實際目標(biāo)物W為跟蹤對象進行精確跟蹤。其規(guī)劃跟蹤方法是重復(fù)前4個步驟。

圖7 自主路徑規(guī)劃坐標(biāo)系

3.4 路徑規(guī)劃的虛擬仿真流程

4 實驗驗證

本系統(tǒng)采用林木聯(lián)合采育機伐木頭連續(xù)捕捉并采伐目標(biāo)立木的實驗，驗證自主作業(yè)路徑規(guī)劃與控制方法的有效性。

圖8 程序的架構(gòu)簡圖

在圖9(a～h)中，白線代表伐木頭的運動軌跡。圖9(a)為車體先運行到一個通過機械臂可以捕獲目標(biāo)立木的位置；圖9(b)伐木頭從A點到B點進行自主路徑規(guī)劃，并在B點將伐木頭對準(zhǔn)捕獲目標(biāo)立木；圖9(c)伐倒目標(biāo)立木；圖9(d)將目標(biāo)立木沿直線路徑B點拖至C點進行打枝、造材；接著如圖9(e～f)伐木頭通過圓弧軌跡到達靠近下一個目標(biāo)立木的D點；在圖9(g)中，從D點到E點是自主路徑規(guī)劃，并將伐木頭對準(zhǔn)捕獲下一棵目標(biāo)立木；圖9(h)將目標(biāo)立木伐倒。

表2 各關(guān)節(jié)角的運動范圍約束

注：表中“-”表示關(guān)節(jié)角逆時針運動。

圖9 實時動態(tài)仿真系統(tǒng)

將圖9實驗過程的數(shù)據(jù)導(dǎo)出并分析，在此過程中，俯仰液壓缸保持靜止，即關(guān)節(jié)1沒有發(fā)生變化，角速度為0，關(guān)節(jié)2～5位移的數(shù)據(jù)隨時間變化如圖10～13。

圖中縱坐標(biāo)負值表示轉(zhuǎn)動裝置逆時針運動。

圖中縱坐標(biāo)負值表示轉(zhuǎn)動裝置逆時針運動。

在圖10～13中，時間0-7.9 s是基于激光反饋的自主路徑規(guī)劃方法捕獲目標(biāo)立木過程，對應(yīng)圖9(b)；時間7.9-11.1 s，各關(guān)節(jié)位移沒有變化，對應(yīng)圖9(c)，伐木頭伐倒目標(biāo)立木；時間11.1-18.7 s對應(yīng)圖9(d)，將目標(biāo)立木從B點拖至C點；時間18.7-29.2 s是伐木頭進行打枝、造材作業(yè)；時間29.2-42.2 s對應(yīng)圖9(e～f)，伐木頭運動到靠近下一個目標(biāo)立木的D點；時間42.2-49.7 s伐木頭捕獲下一個目標(biāo)立木，對應(yīng)圖9(g)；第49.7 s開始，再次對該目標(biāo)立木伐倒，對應(yīng)圖9(h)。

圖中縱坐標(biāo)負值表示機械裝置逆時針轉(zhuǎn)動。

圖13 關(guān)節(jié)5的位移

實驗過程中，關(guān)節(jié)2的位移-33.8°～0°，最大速度為8.5°/s(<20°/s)；關(guān)節(jié)3的位移-23.0°～0°，最大速度為3.6°/s(<15°/s)；關(guān)節(jié)4的位移-6.1°～29.8°，最大速度為8.0°/s(<15°/s)；關(guān)節(jié)5的位移0～1.16 m，最大速度為0.15 m/s(<0.2 m/s)。各關(guān)節(jié)運動范圍沒有超出約束設(shè)置，最大速度沒有超過設(shè)定的最大速度，且運動曲線平滑，實驗結(jié)果顯示本文的自主作業(yè)路徑規(guī)劃和控制方法有效。

5 結(jié)束語

本文采用三維實體建模軟件SolidWorks和圖形開發(fā)引擎OSG，結(jié)合VC++編程語言，建立了林木聯(lián)合采育機參數(shù)化可視化虛擬仿真系統(tǒng)。完成了對林木聯(lián)合采育機三維實體建模，構(gòu)建了包括林木聯(lián)合采育機模型及其周圍環(huán)境的仿真平臺，并對林木聯(lián)合采育機的機械臂建立了運動學(xué)模型，對其進行直線、圓弧、自主路徑規(guī)劃與控制。同時對機械臂作業(yè)路徑規(guī)劃進行仿真實驗，實現(xiàn)了仿真數(shù)據(jù)的實時傳輸和圖形界面的實時顯示，驗證了自主作業(yè)路徑規(guī)劃與控制方法的有效性。

林木聯(lián)合采育機無人自主采伐作業(yè)的研究可進一步提高其作業(yè)效率和安全性，同時采用虛擬仿真技術(shù)可節(jié)省實驗成本，保證實驗安全，也可用于林木聯(lián)合采育機駕駛員的培訓(xùn)。

[1] 臧宇，朱忠祥，宋正河，等.農(nóng)業(yè)裝備虛擬試驗系統(tǒng)平臺的建立[J].農(nóng)業(yè)機械學(xué)報，2010，41(9)：70-74.

[2] LAPOINTE J F, ROBERT J M. Using VR for efficient training of forestry machine operators[J]. Education and Information Technologies,2000,5(4):237-250.

[3] BAHARUDIN M E, ROUVINEN A, KORKEALAAKSO P, et al. Real-time multibody application for tree harvester truck simulator[J]. Proceedings of the Institution of Mechanical Engineers Part K Journal of Multi-body Dynamics,2014,228(2):182-198.

[4] METTIN U, LA HERA P X, MORALES D O, et al. Path-constrained trajectory planning and time-independent motion control: Application to a forestry crane[C]//ICAR. 2009 14th International Conference on Advanced Robotics (ICAR) Proceedings, Munich: Germany IEEE,2009.

[5] 賀繼林，劉鵬飛，彭燦，等.液壓挖掘機軌跡控制策略的半實物仿真研究[J].華中科技大學(xué)學(xué)報(自然科學(xué)版)，2008，36(S1)：19-22.

[6] 趙滿全，張寧，楊鐵軍，等.雙圓盤割草機切割器虛擬樣機設(shè)計與試驗[J].農(nóng)業(yè)機械學(xué)報，2014，45(8)：101-105.

[7] 張紅彥，于長志，趙丁選，等.基于虛擬現(xiàn)實的液壓挖掘機視景仿真[J].華中科技大學(xué)學(xué)報(自然科學(xué)版)，2013，41(3)：87-91.

[8] 蘇明明.基于Multigen Creator/Vega prime的液壓挖掘機的可視化虛擬仿真[D].成都：西南交通大學(xué)，2011.

[9] 郭秀麗.采伐聯(lián)合機機械手運動分析與控制系統(tǒng)研究[D].哈爾濱：東北林業(yè)大學(xué)，2011.

[10] ZHENG Y, LIU J, WANG D, et al. Laser scanning measurements on trees for logging harvesting operations[J]. Sensors-Basel,2012,12(7):9273-9285.

[11] DENAVIT J, HARTENBERG R S. A kinematic notation for lower-pair mechanisms based on matrices[J]. Trans of the ASME Journal of Applied Mechanics,1955,22(2):215-221.

[12] CRAIG J J. Introduction to robotics[M]. Beijing: China Machine Press,Pearson/Prentice Hall,2005.

[13] ZHENG YILI, GE TAOTAO, LIU JINHAO. Kinematics modeling and control simulation for a logging harvester in virtual environments[J]. Advances in Mechanical Engineering,2015,7(10):1-10.

[14] 熊有倫，丁漢，劉恩滄.機器人學(xué)[M].北京：機械農(nóng)業(yè)出版社，1993.

[15] 任秉銀，梁兆東，孔民秀.機械手空間圓弧位姿軌跡規(guī)劃算法的實現(xiàn)[J].哈爾濱工業(yè)大學(xué)學(xué)報，2012，44(7)：27-31.

Autonomous Task of A Logging Harvester in Virtual Simulation System//

Ge Taotao, Zheng Yili, Liu Jinhao

(Beijing Forestry University, Beijing 100083, P. R. China)//Journal of Northeast Forestry University，2017,45(1)：71-76.

Logging harvester； Autonomous task； Virtual simulation

1)國家自然科學(xué)基金項目(31300596)。

葛桃桃，女，1990年12月生，北京林業(yè)大學(xué)工學(xué)院，碩士研究生。E-mail：getaotao@bjfu.edu.cn。

鄭一力，北京林業(yè)大學(xué)工學(xué)院，副教授。E-mail：zhengyili@bjfu.edu.cn。

2016年8月2日。

S78

責(zé)任編輯：王廣建。