林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，包括工業(yè)控制計算機、車載嵌入式計算機、車載PLC控制器；車載嵌入式計算機連接有激光測量系統(tǒng)，車載PLC控制器連接有操控手柄、方向盤、剎車踏板、油門踏板；工業(yè)控制計算機用于完成林木聯(lián)合采育機的自主采伐作業(yè)任務的實時動態(tài)虛擬顯示；車載嵌入式計算機包括控制輸入采集模塊、輸出參數(shù)顯示模塊、立木激光測量算法模塊和軌跡規(guī)劃與控制算法模塊；車載PLC控制器用于采集處理操控手柄、方向盤、剎車踏板、油門踏板控制輸入量，同時輸出實體林木聯(lián)合采育機的液壓油缸控制信號。本發(fā)明能驗證林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與智能控制方法、數(shù)據(jù)量處理能力大、交互能力高、成本低。
【專利說明】林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種林業(yè)采育裝備智能控制技術(shù)，尤其涉及一種林木聯(lián)合采育機作業(yè) 軌跡規(guī)劃與控制半實物仿真系統(tǒng)。

【背景技術(shù)】
[0002] 林木聯(lián)合采育機是一種高性能現(xiàn)代林業(yè)裝備，可在人工干預下完成采伐、打枝、去 皮、造材等連續(xù)作業(yè)。在對林木聯(lián)合采育機這種大型和復雜機電系統(tǒng)進行實驗時，由于存在 危險性和花費高等問題，在開發(fā)階段需要對關(guān)鍵控制技術(shù)進行仿真驗證。純粹的軟件仿真 很難真正模擬實際林木聯(lián)合采育機的特性。為保證林木聯(lián)合采育機在樣機實驗初期的安全 性和經(jīng)濟性，需構(gòu)建半實物仿真的實驗平臺進行實驗。
[0003] 現(xiàn)有技術(shù)中還沒有一種能驗證林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與智能控制方法、數(shù) 據(jù)量處理能力大、交互能力高、成本低的實驗平臺。


【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的是提供一種能驗證林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與智能控制方法、 數(shù)據(jù)量處理能力大、交互能力高、成本低的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿 真系統(tǒng)。
[0005] 本發(fā)明的目的是通過以下技術(shù)方案實現(xiàn)的：
[0006] 本發(fā)明的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，包括工業(yè)控制計 算機、車載嵌入式計算機、車載PLC控制器，所述車載嵌入式計算機連接有激光測量系統(tǒng)， 所述車載PLC控制器連接有操控手柄、方向盤、剎車踏板、油門踏板；
[0007] 所述工業(yè)控制計算機用于完成林木聯(lián)合采育機的自主采伐作業(yè)任務的實時動態(tài) 虛擬顯示，包括虛擬林木聯(lián)合采育機、虛擬立木和地面、虛擬動態(tài)顯示模塊、實驗數(shù)據(jù)保存 模塊、人機交互模塊；
[0008] 所述車載嵌入式計算機包括控制輸入采集模塊、輸出參數(shù)顯示模塊、立木激光測 量算法模塊和軌跡規(guī)劃與控制算法模塊；
[0009] 所述車載PLC控制器用于采集處理所述操控手柄、方向盤、剎車踏板、油門踏板控 制輸入量，同時輸出實體林木聯(lián)合米育機的液壓油缸控制信號。
[0010]由上述本發(fā)明提供的技術(shù)方案可以看出，本發(fā)明實施例提供的林木聯(lián)合采育機作 業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，由于包括工業(yè)控制計算機、車載嵌入式計算機、車載 PLC控制器，所述車載嵌入式計算機連接有激光測量系統(tǒng)，所述車載PLC控制器連接有操控 手柄、方向盤、剎車踏板、油門踏板，能驗證林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與智能控制方法， 可完成林木聯(lián)合采育機自主作業(yè)的運動學計算、軌跡規(guī)劃和通訊等復雜任務，同時可完成 林木聯(lián)合采育機作業(yè)任務的實時動態(tài)顯示和人機交互，具有大數(shù)據(jù)量處理能力、高交互能 力和較低成本的特點。

【專利附圖】

【附圖說明】 toon]圖1為本發(fā)明實施例提供的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系 統(tǒng)的結(jié)構(gòu)框圖；
[0012] 圖2為本發(fā)明實施例中林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制算法的流程框圖。

【具體實施方式】
[0013] 下面將對本發(fā)明實施例作進一步地詳細描述。
[0014] 本發(fā)明的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其較佳的具體實 施方式是：
[0015] 包括工業(yè)控制計算機、車載嵌入式計算機、車載PLC控制器，所述車載嵌入式計算 機連接有激光測量系統(tǒng)，所述車載PLC控制器連接有操控手柄、方向盤、剎車踏板、油門踏 板；
[0016] 所述工業(yè)控制計算機用于完成林木聯(lián)合采育機的自主采伐作業(yè)任務的實時動態(tài) 虛擬顯示，包括虛擬林木聯(lián)合采育機、虛擬立木和地面、虛擬動態(tài)顯示模塊、實驗數(shù)據(jù)保存 模塊、人機交互模塊；
[0017] 所述車載嵌入式計算機包括控制輸入采集模塊、輸出參數(shù)顯示模塊、立木激光測 量算法模塊和軌跡規(guī)劃與控制算法模塊；
[0018] 所述車載PLC控制器用于采集處理所述操控手柄、方向盤、剎車踏板、油門踏板控 制輸入量，同時輸出實體林木聯(lián)合米育機的液壓油缸控制信號。
[0019] 所述虛擬林木聯(lián)合采育機包括虛擬采伐臂、虛擬伐木頭和虛擬車體，所述虛擬動 態(tài)顯示模塊能將虛擬林木聯(lián)合采育機的作業(yè)運動和立木三維模型信息實時顯示到液晶顯 示器上，所述實驗數(shù)據(jù)保存模塊能實時保存虛擬林木聯(lián)合采育機的采伐臂、車體和伐木頭 的運動數(shù)據(jù)，所述人機交互模塊能完成自主作業(yè)任務的控制參數(shù)輸入。
[0020] 所述控制輸入采集模塊用于處理車載PLC控制器的輸入量，所述輸出參數(shù)顯示模 塊完成伐木頭的工作狀態(tài)參數(shù)和立木的胸徑、方位、間距參數(shù)的輸出顯示，所述立木激光測 量算法模塊用于采集激光掃描的數(shù)據(jù)，獲得立木的胸徑、方位和間距參數(shù)，所述軌跡規(guī)劃與 控制算法模塊能基于激光測量數(shù)據(jù)完成虛擬林木聯(lián)合采育機的作業(yè)軌跡規(guī)劃與控制算法。 [0021 ] 所述操控手柄用于完成虛擬采伐臂和虛擬伐木頭運動的人工控制量輸入。
[0022] 所述激光測量系統(tǒng)能獲得采伐目標立木的激光點云數(shù)據(jù)，經(jīng)車載嵌入式計算機上 的立木激光測量算法模塊計算獲得立木的胸徑、方位和間距參數(shù)，為采伐臂的軌跡規(guī)劃與 控制提供基本的測量反饋數(shù)據(jù)。
[0023] 所述方向盤用于完成虛擬林木聯(lián)合米育機車體的方向控制輸入。
[0024] 所述剎車踏板用于完成虛擬林木聯(lián)合采育機車體的減速和制動控制輸入。
[0025] 所述油門踏板用于完成虛擬林木聯(lián)合采育機車體的加速運動控制輸入。
[0026] 所述工業(yè)控制計算機與車載嵌入式計算機之間通過雙冗余的外部CAN總線連接， 所述車載嵌入式計算機與車載PLC控制器間通過雙冗余的內(nèi)部CAN總線連接，所述激光測 量系統(tǒng)與車載嵌入式計算機間通過串口連接。
[0027] 本發(fā)明的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，能驗證林木聯(lián)合 采育機作業(yè)軌跡規(guī)劃與智能控制方法，可完成林木聯(lián)合采育機自主作業(yè)的運動學計算、軌 跡規(guī)劃和通訊等復雜任務，同時可完成林木聯(lián)合采育機作業(yè)任務的實時動態(tài)顯示和人機交 互，具有大數(shù)據(jù)量處理能力、高交互能力和較低成本的特點。
[0028] 本發(fā)明充分利用了分布式控制系統(tǒng)設(shè)計和半實物仿真系統(tǒng)設(shè)計的方法，將林木聯(lián) 合采育機、立木、地面等采用虛擬仿真方式實現(xiàn)，而車載嵌入式計算機、車載PLC控制器、方 向盤、剎車踏板、油門踏板等采用與實驗樣車完全相同的電子部件實物實現(xiàn)。采用這種半實 物仿真系統(tǒng)，軌跡規(guī)劃和控制算法以及車載控制系統(tǒng)的動態(tài)特性、靜態(tài)特性和非線性因素 等能更接近實際地反映出來，可滿足林木聯(lián)合采育機作業(yè)任務的實驗需求，保證了實驗過 程的低成本、高安全性以及實驗結(jié)果的高可信度。
[0029] 本發(fā)明中一部分采用與樣車相同的實物，其他部分則采用計算機仿真形式。采用 半實物仿真系統(tǒng)代替純粹的軟件仿真和實際樣車系統(tǒng)，作業(yè)軌跡規(guī)劃和控制算法以及車載 控制系統(tǒng)的動態(tài)特性、靜態(tài)特性和非線性因素等能更接近實際地反映出來，同時具有良好 的人機交互性，實驗次數(shù)不受限制，可以簡化林木聯(lián)合采育機的開發(fā)周期，節(jié)省開發(fā)成本， 提高實驗安全性，也為林木聯(lián)合采育機的駕駛員培訓提供基本的實驗條件。
[0030] 具體實施例，如圖1和圖2所示：
[0031] 在圖1中，每個方框表示一個功能單元，林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半 實物仿真系統(tǒng)按其功能劃分為：工業(yè)控制計算機、車載嵌入式計算機、車載PLC控制器、操 控手柄、激光測量系統(tǒng)、方向盤、剎車踏板、油門踏板。
[0032] 工業(yè)控制計算機和車載嵌入式計算機之間通過雙冗余的外部CAN總線連接，車載 嵌入式計算機與車載PLC控制器間通過雙冗余的內(nèi)部CAN總線連接，激光測量系統(tǒng)和車載 嵌入式計算機間通過串口連接。
[0033] 其中，在工業(yè)控制計算機上運行采用Visual Studio 2008和Open Scene Graph 開發(fā)了的虛擬仿真軟件，該軟件包含虛擬林木聯(lián)合采育機、虛擬立木和地面、虛擬動態(tài)顯示 模塊、實驗數(shù)據(jù)保存模塊、人機交互模塊；其中虛擬林木聯(lián)合采育機由虛擬采伐臂、虛擬伐 木頭和虛擬車體構(gòu)成；虛擬動態(tài)顯示模塊可將虛擬林木聯(lián)合采育機的作業(yè)運動和立木三維 模型信息實時顯示到液晶顯示器上，實驗數(shù)據(jù)保存模塊可實時保存虛擬林木聯(lián)合采育機的 采伐臂、車體和伐木頭的運動數(shù)據(jù)；人機交互模塊可完成自主作業(yè)任務的控制參數(shù)輸入，如 自主作業(yè)指令、人工操控指令等。
[0034] 車載嵌入式計算機發(fā)出虛擬林木聯(lián)合采育機的車體、采伐臂、伐木頭的運動參數(shù) 指令，車載嵌入式計算機采用工業(yè)嵌入式CorteX-A81. 2GHz雙核處理器，5. 7寸TFT液晶屏， 具備1GB DDR2內(nèi)存和16GB Flash固態(tài)硬盤，配備，CAN接口，支持CAN V2. 0B，具備RS232 串口和RS485串口，USB2. 0端口，超強的安全穩(wěn)定可靠性，確?？稍趷毫拥淖鳂I(yè)環(huán)境下穩(wěn) 定，不間斷運行。
[0035] 車載嵌入式計算機采用WinCE操作系統(tǒng)，通過軟件完成控制輸入采集模塊、輸出 參數(shù)顯示模塊、立木激光測量算法模塊和軌跡規(guī)劃與控制算法模塊，其中控制輸入采集模 塊主要負責處理車載PLC控制器的輸入量；輸出參數(shù)顯示模塊完成虛擬伐木頭的工作狀態(tài) 參數(shù)和立木的胸徑、方位、間距等參數(shù)的輸出。
[0036] 立木激光測量算法模塊主要負責采集激光掃描的數(shù)據(jù)，經(jīng)聚類算法和最小二乘擬 合算法獲得采伐目標立木的胸徑、方位和間距等參數(shù)，為采伐臂的軌跡規(guī)劃和控制提供基 本的測量參數(shù)。
[0037] 軌跡規(guī)劃與控制算法模塊主要基于激光測量數(shù)據(jù)或人工輸入數(shù)據(jù)，完成虛擬林木 聯(lián)合采育機的多自由度采伐臂捕獲立木的軌跡規(guī)劃與控制算法。
[0038] 整個軌跡規(guī)劃與控制算法的流程如圖2所示，軌跡規(guī)劃與控制算法首先由激光獲 取實際立木的方位數(shù)據(jù)，在工業(yè)控制計算機上顯示立木虛擬模型，后續(xù)流程分為自主作業(yè) 和人工操控兩部分。
[0039] 在自主作業(yè)時，為保證伐木頭到達期望的捕獲位置，需要進行多次規(guī)劃。本發(fā)明采 用了"規(guī)劃一次、走十步"的循環(huán)控制方法，實現(xiàn)伐木頭對目標立木的逐步逼近。首先，虛擬 車體自主移動，使虛擬采伐臂和虛擬伐木頭處于接近目標立木的位置；然后，根據(jù)虛擬伐木 頭相對立木的方位數(shù)據(jù)，在笛卡爾空間進行軌跡規(guī)劃，并進行運動學反解，獲得各規(guī)劃點虛 擬采伐臂的運動參數(shù)；然后車載嵌入式計算機分別通過外部CAN總線和內(nèi)部CAN總線將各 規(guī)劃點虛擬采伐臂的運動參數(shù)值傳送給工業(yè)控制計算機上虛擬林木聯(lián)合采育機和車載PLC 控制器；虛擬采伐臂運動前10個規(guī)劃點，并同步動態(tài)顯示；完成前10個規(guī)劃點運動后，后 續(xù)的規(guī)劃點舍棄；經(jīng)10個控制周期后，如虛擬伐木頭未到達可捕獲立木區(qū)域則返回開始新 的規(guī)劃周期，直至逐步逼近目標立木。如虛擬伐木頭到達可捕獲立木的區(qū)域，車載嵌入式計 算機分別通過外部CAN總線和內(nèi)部CAN總線向虛擬伐木頭和車載PLC控制器發(fā)送指令完成 捕獲立木和采伐任務。
[0040] 在人工操控時，首先人工操控方向盤、剎車踏板和油門踏板，完成虛擬車體移動； 然后人工操作操控手柄，控制虛擬采伐臂的運動，然后車載嵌入式計算機分別通過外部CAN 總線和內(nèi)部CAN總線將虛擬采伐臂的運動參數(shù)傳送給工業(yè)控制計算機上的虛擬林木聯(lián)合 采育機和車載PLC控制器；然后虛擬采伐臂的運動，并同步動態(tài)顯示，如伐木頭到達可捕獲 立木的區(qū)域，車載嵌入式計算機分別通過外部CAN總線向虛擬伐木頭和內(nèi)部CAN總線向車 載PLC控制器發(fā)送指令完成捕獲立木和采伐任務。
[0041] 車載PLC控制器用于采集處理所述操控手柄、方向盤、剎車踏板、油門踏板控制輸 入量，通過內(nèi)部CAN總線接收車載嵌入式計算機發(fā)出的運動參數(shù)，同時在實體林木聯(lián)合采 育機測試時，可以輸出實體林木聯(lián)合米育機的液壓油缸控制信號。
[0042] 操控手柄主要通過安裝在上面的各個開關(guān)量按鍵、模擬量按鍵和操作桿的前后左 右位移完成虛擬采伐臂和虛擬伐木頭運動的人工控制量輸入；
[0043] 激光測量系統(tǒng)采用二維激光掃描儀，掃描距離范圍為8米，掃描角度范圍為100 度，可獲得采伐目標立木的激光點云數(shù)據(jù)，經(jīng)車載嵌入式計算機上的立木激光測量算法模 塊計算獲得立木的胸徑、方位和間距等參數(shù)，為采伐臂的軌跡規(guī)劃與控制提供基本的測量 反饋數(shù)據(jù)；
[0044] 方向盤輸出PWM信號給車載PLC控制器，主要完成虛擬林木聯(lián)合米育機車體的方 向控制輸入；
[0045] 剎車踏板輸出開關(guān)信號給車載PLC控制器，主要完成虛擬林木聯(lián)合米育機車體的 減速和制動控制輸入；
[0046] 油門踏板輸出模擬信號給車載PLC控制器，主要完成虛擬林木聯(lián)合米育機車體的 加速運動控制輸入；
[0047] 工業(yè)控制計算機和車載嵌入式計算機之間通過雙冗余的外部CAN總線連接，車載 嵌入式計算機與車載PLC控制器通過雙冗余的內(nèi)部CAN總線連接，采用雙冗余的內(nèi)部CAN 總線和外部CAN總線可保證林木聯(lián)合采育機控制系統(tǒng)數(shù)據(jù)傳輸可靠性的要求。激光測量系 統(tǒng)和車載嵌入式計算機之間通過串口連接。
[0048] 以上所述，僅為本發(fā)明較佳的【具體實施方式】，但本發(fā)明的保護范圍并不局限于此， 任何熟悉本【技術(shù)領(lǐng)域】的技術(shù)人員在本發(fā)明披露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換， 都應涵蓋在本發(fā)明的保護范圍之內(nèi)。因此，本發(fā)明的保護范圍應該以權(quán)利要求書的保護范 圍為準。
【權(quán)利要求】
1. 一種林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其特征在于，包括工業(yè) 控制計算機、車載嵌入式計算機、車載PLC控制器，所述車載嵌入式計算機連接有激光測量 系統(tǒng)，所述車載PLC控制器連接有操控手柄、方向盤、剎車踏板、油門踏板； 所述工業(yè)控制計算機用于完成林木聯(lián)合采育機的自主采伐作業(yè)任務的實時動態(tài)虛擬 顯示，包括虛擬林木聯(lián)合采育機、虛擬立木和地面、虛擬動態(tài)顯示模塊、實驗數(shù)據(jù)保存模塊、 人機交互模塊； 所述車載嵌入式計算機包括控制輸入采集模塊、輸出參數(shù)顯示模塊、立木激光測量算 法模塊和軌跡規(guī)劃與控制算法模塊； 所述車載PLC控制器用于采集處理所述操控手柄、方向盤、剎車踏板、油門踏板控制輸 入量，同時輸出實體林木聯(lián)合米育機上液壓油缸的控制信號。
2. 根據(jù)權(quán)利要求1所述的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其特 征在于，所述虛擬林木聯(lián)合采育機包括虛擬采伐臂、虛擬伐木頭和虛擬車體，所述虛擬動態(tài) 顯示模塊能將虛擬林木聯(lián)合采育機的作業(yè)運動和立木三維模型信息實時顯示到液晶顯示 器上，所述實驗數(shù)據(jù)保存模塊能實時保存虛擬林木聯(lián)合采育機的采伐臂、車體和伐木頭的 運動數(shù)據(jù)，所述人機交互模塊能完成自主作業(yè)任務的控制參數(shù)輸入。
3. 根據(jù)權(quán)利要求1所述的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其特 征在于，所述控制輸入采集模塊用于處理車載PLC控制器的輸入量，輸出參數(shù)顯示模塊完 成伐木頭的工作狀態(tài)參數(shù)和立木的胸徑、方位、間距參數(shù)的輸出顯示，所述立木激光測量算 法模塊用于采集激光掃描的數(shù)據(jù)，獲得立木的胸徑、方位和間距參數(shù)，所述軌跡規(guī)劃與控制 算法模塊能基于激光測量數(shù)據(jù)完成虛擬林木聯(lián)合采育機的作業(yè)軌跡規(guī)劃與控制算法。
4. 根據(jù)權(quán)利要求1所述的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其特 征在于，所述操控手柄用于完成虛擬采伐臂和虛擬伐木頭運動的人工控制量輸入。
5. 根據(jù)權(quán)利要求1所述的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其特 征在于，所述激光測量系統(tǒng)能獲得采伐目標立木的激光點云數(shù)據(jù)，經(jīng)車載嵌入式計算機上 的立木激光測量算法模塊計算獲得立木的胸徑、方位和間距參數(shù)，為采伐臂的軌跡規(guī)劃與 控制提供基本的測量反饋數(shù)據(jù)。
6. 根據(jù)權(quán)利要求1所述的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其特 征在于，所述方向盤用于完成虛擬林木聯(lián)合米育機車體的方向控制輸入。
7. 根據(jù)權(quán)利要求1所述的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其特 征在于，所述剎車踏板用于完成虛擬林木聯(lián)合采育機車體的減速和制動控制輸入。
8. 根據(jù)權(quán)利要求1所述的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿真系統(tǒng)，其特 征在于，所述油門踏板用于完成虛擬林木聯(lián)合采育機車體的加速運動控制輸入。
9. 根據(jù)權(quán)利要求1至8任一項所述的林木聯(lián)合采育機作業(yè)軌跡規(guī)劃與控制半實物仿 真系統(tǒng)，其特征在于，所述工業(yè)控制計算機與車載嵌入式計算機之間通過雙冗余的外部CAN 總線連接，所述車載嵌入式計算機與車載PLC控制器間通過雙冗余的內(nèi)部CAN總線連接，所 述激光測量系統(tǒng)與車載嵌入式計算機間通過串口連接。
【文檔編號】G05B19/05GK104122836SQ201410397587
【公開日】2014年10月29日 申請日期:2014年8月13日 優(yōu)先權(quán)日:2014年8月13日
【發(fā)明者】鄭一力, 劉晉浩, 王典, 孫治博, 葛桃桃, 王建利 申請人:北京林業(yè)大學