高枝修剪機姿態(tài)協(xié)調(diào)控制與精準鋸切定位

李 揚，杜 帥，陳寶林，付光華，苑 進※

（1. 山東農(nóng)業(yè)大學機械與電子工程學院，泰安 271018；2. 山東省園藝機械與裝備重點實驗室，泰安 271018）

高枝修剪機姿態(tài)協(xié)調(diào)控制與精準鋸切定位

李 揚1,2，杜 帥1，陳寶林1，付光華1，苑 進1,2※

（1. 山東農(nóng)業(yè)大學機械與電子工程學院，泰安 271018；2. 山東省園藝機械與裝備重點實驗室，泰安 271018）

修枝是林業(yè)撫育管理的重要措施之一，但適用于高大樹木的修枝機械尚處起步階段。針對目前國內(nèi)外修枝機械普遍存在作業(yè)高度不足、靈活性不高，操作人員勞動強度大、作業(yè)危險性高等問題，設計研發(fā)了一種高枝修剪機及其控制系統(tǒng)，實現(xiàn)15 m以內(nèi)高大樹木的修枝作業(yè)。本文詳細闡述了高枝修剪機的工作原理，基于多自由度修剪機械臂正、逆運動學進行鋸切定位分析，提出了修剪機的機械臂姿態(tài)協(xié)調(diào)控制與末端修枝鋸的鋸切定位方法，輔以末端攝像頭觀察待修樹枝的相對位置，通過人機交互界面實現(xiàn)修枝臂協(xié)調(diào)控制和鋸切定位調(diào)整，自動化實現(xiàn)升降臺、回轉(zhuǎn)臺和修剪機械臂姿態(tài)調(diào)整。此外，通過在開放場地進行樣機修枝試驗及誤差測量與補償，試驗結(jié)果表明：在回轉(zhuǎn)臺轉(zhuǎn)角固定情況下，本文設計的控制系統(tǒng)和控制方法可以實現(xiàn)末端修枝鋸在空間2個坐標軸上的定位標準差均小于5 mm，在作業(yè)高度15 m、作業(yè)半徑6 m的范圍內(nèi)，與同類機械相比具有更高的相對定位精度，能夠?qū)崿F(xiàn)精確定位和鋸切作業(yè)。

林業(yè)；位置控制；運動學；高枝修剪機；鋸切定位；協(xié)調(diào)控制；控制系統(tǒng)實現(xiàn)

李 揚，杜 帥，陳寶林，付光華，苑 進. 高枝修剪機姿態(tài)協(xié)調(diào)控制與精準鋸切定位[J]. 農(nóng)業(yè)工程學報，2017，33(10)：40－48. doi：10.11975/j.issn.1002-6819.2017.10.006 http://www.tcsae.org

Li Yang, Du Shuai, Chen Baolin, Fu Guanghua, Yuan Jin. Attitude coordination control and accurately sawing-positioning of high-branch pruning machine[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2017, 33(10): 40－48. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2017.10.006 http://www.tcsae.org

0 引 言

樹木修枝是森林撫育的主要措施，對樹木生長、成材有非常積極的意義[1-3]。國外機械化修枝技術(shù)起步較早，配套齊全[4-5]，8m以下的側(cè)枝一般采用修枝剪搭配伸縮臂實現(xiàn)，但8 m以上側(cè)枝修剪缺乏適用機械[6]。近年來，國內(nèi)修枝機械研究取得了一些成果，但仍以人工作業(yè)為主。辛繼紅等[7-8]分別設計了以汽油機和電動機為動力的背負式修枝機，采用齒輪傳動方式，操作者背負修枝鋸手持機桿進行作業(yè)。華南熱帶作物機械研究所[9]設計制造了3SG-8型升降修枝機，工作臺最大起升高度為 8.5m，操作員通過安裝在工作臺上的操縱手柄來控制工作臺的位置實現(xiàn)修枝作業(yè)。上述機械均通過操作員手持設備實現(xiàn)作業(yè)，鋸切范圍窄、自動化程度低、勞動強度大。焦恩璋等[10]設計了一種車載式高枝修剪機，以高空作業(yè)車為基礎，在機械臂末端裝上修枝鋸、擺動機構(gòu)和輔助構(gòu)件實現(xiàn)高空修枝作業(yè)，通過機械臂輔助定位，增加了作業(yè)空間，減輕了作業(yè)負擔，但機械配有專用車輛，液壓系統(tǒng)復雜，整機成本很高。

針對國內(nèi)外普遍缺乏適用高空修枝機械的現(xiàn)狀，為拓展修枝范圍，提高自動化作業(yè)程度，進而提高工作效率和作業(yè)質(zhì)量，本文創(chuàng)新設計并實現(xiàn)了一種針對15 m以下高大樹木的修剪機及其控制方法[11-12]。在闡述修剪機機械結(jié)構(gòu)和工作原理基礎上，對修剪機械臂進行正、逆運動學和結(jié)構(gòu)分析，提出末端修枝鋸鋸切定位和修枝作業(yè)的控制方法，設計實現(xiàn)其控制系統(tǒng)并集成控制方法。樣機試制后，為了檢驗修剪機在作業(yè)過程中的鋸切定位精度，在山東農(nóng)業(yè)大學開放場地開展樣機修枝試驗，測量并分析鋸切定位誤差，并通過誤差補償?shù)姆绞竭M一步提高鋸切定位的精度，使其更加方便、快速、準確的將待修樹枝對入鋸口，實現(xiàn)對高大樹木的修枝作業(yè)。

1 工作原理

設計高枝修剪機機械結(jié)構(gòu)如圖1a所示，操作人員通過觀察鋸頭與樹枝的相對位置，利用機載人機界面靈活控制 3個節(jié)臂協(xié)調(diào)動作進行鋸切定位。為了進一步提高機械易用性，在鋸頭前端設計了無線攝像頭，輔助地面操作員觀察高空中鋸頭和樹枝的距離。由 3節(jié)臂組成的臂架系統(tǒng)被固定在1個升降平臺的回轉(zhuǎn)臺上，每節(jié)臂由1個關(guān)節(jié)驅(qū)動繞軸心旋轉(zhuǎn)，修枝鋸固定于第3節(jié)臂的末端。高枝修剪機各節(jié)臂參數(shù)如表 1所示，整機修枝高度能夠達到15 m，作業(yè)半徑可達6 m，最大修枝直徑為10 cm，能夠滿足高空修枝作業(yè)的要求。

圖1 高枝修剪機Fig.1 High-branch pruning machine

表1 高枝修剪機各節(jié)臂參數(shù)Table 1 Arm parameters of flexible high-branch pruning machine

修枝臂關(guān)節(jié)旋轉(zhuǎn)是由電動缸伸縮配合連桿動作驅(qū)動，定義回轉(zhuǎn)臺的轉(zhuǎn)角以及修枝臂的 3個關(guān)節(jié)角分別為q0、q1、q2、q3，關(guān)節(jié)角的大小由電動缸長度唯一決定。樣機中采用535、350、390 mm伸長范圍的電動缸（GL20-05型，上海光劍自動化設備有限公司），由臺達ASDA-A2伺服器配伺服電機驅(qū)動，單向220 V AC電源供電，最終實測形成的轉(zhuǎn)角范圍分別為

根據(jù)各節(jié)臂長度和轉(zhuǎn)角參數(shù)可計算出修枝修枝臂的位形空間，該空間描述了末端執(zhí)行器的活動范圍，是機械臂運動靈活性的重要指標[13-16]。理論上講，此活動范圍可以覆蓋修枝臂所在位置前后的一個球狀區(qū)域。但在關(guān)節(jié)結(jié)構(gòu)和機構(gòu)運動奇異性等因素的約束下，其運動空間受到了限制，對高枝修剪機末端實際可達位置進行計算仿真可得位形空間為圖1b所示。因此，林業(yè)高枝修剪機械的工作即為利用設計的控制器和控制算法，在修枝鋸可達空間范圍進行鋸切定位、對準、修枝等作業(yè)。

2 鋸切定位分析

位形空間中的某點可以對應不同的升降臺高度和修枝臂姿態(tài)，實際操作時從安全的角度考慮，總是先調(diào)整修枝臂姿態(tài)，如果鋸切位置超出修枝臂可達范圍，再進行升降臺升降動作。

采用 D-H方法對修剪機修枝臂進行運動學建模分析[17-22]。為了便于分析和計算，簡化修枝臂結(jié)構(gòu)如圖 2所示，臂架系統(tǒng)共包含4個自由度，分別由4個旋轉(zhuǎn)軸組成，帶動修剪機末端修枝鋸靈活地運動并進行修枝作業(yè)。依照D-H方法得到修枝臂的連桿參數(shù)如表2所示。

圖2 修剪機械臂坐標空間示意圖Fig.2 Pruning machine arm coordinate space diagram

表2 修枝臂D-H參數(shù)Table 2 Manipulator D-H parameters

得到修剪機機械臂的齊次變換矩陣為

2.1 正運動學分析

修枝臂的正運動學是在已知各個關(guān)節(jié)角的情況下，求末端的位置和姿態(tài)，即求出齊次變換矩陣為

由式（1）可知，代入對應關(guān)節(jié)角θ后，即可求出矩陣即得到修枝臂姿態(tài)和末端位置。

2.2 逆運動學分析

修枝臂的逆運動學分析是在已知末端的位置和姿態(tài)的情況下（即已知），求各個關(guān)節(jié)角θi。

如前所述，已知末端執(zhí)行機構(gòu)的齊次變換矩陣與基底坐標系之間的轉(zhuǎn)換中有如下關(guān)系

將式中含有θi的部分移到方程的左邊，可以依此得到關(guān)節(jié)角的值。

1）轉(zhuǎn)角θ0的計算

由式（2）得到方程

對比等式兩邊矩陣中第3行第4列的對應元素相等，可以得到

2）關(guān)節(jié)角θ1的計算

進一步由式（2）得到方程對比等式兩邊第1行中第2列與第4列，第2行第2列與第4列，化簡計算得到

3）關(guān)節(jié)角θ2的計算

考察式（3）兩邊第1行第1、2、4列，第2行第4列分別對應相等，通過計算化簡，得到式（8、9）

將式（8）代入式（9）中，整理可得

將上式化簡可得

求出關(guān)節(jié)角θ1和關(guān)節(jié)角θ2的和之后，再減去關(guān)節(jié)角θ1的值，得到關(guān)節(jié)角θ2為

4）關(guān)節(jié)角θ3的計算

由式（2）可知

根據(jù)式（12）兩邊第1行第4列和第2行第4列對應相等，通過計算化簡，得到下式

根據(jù)前述推導過程，修枝臂逆運動學所需的 1個旋轉(zhuǎn)角和 3 個關(guān)節(jié)角分別由式（4）、（7）、（11）、（13）求出，完成修枝臂逆運動學求解分析。

2.3 電動缸伸縮量計算

機械結(jié)構(gòu)決定了修枝臂關(guān)節(jié)角θi（i=1,2,3）與電動缸長度Li存在的一一對應關(guān)系，前面通過修枝臂逆運動學分析求解了關(guān)節(jié)角度θi，但實際控制時要通過這種對應關(guān)系將θi轉(zhuǎn)換成電動缸長度Li，才能確定每個電動缸的伸縮量，從而實現(xiàn)對修枝臂的控制。

對 3個節(jié)臂結(jié)構(gòu)進行分析，得到其幾何關(guān)系圖（圖3），經(jīng)過相關(guān)推導，可以得到3節(jié)機械臂的電動缸長度Li與3個關(guān)節(jié)轉(zhuǎn)角之間的對應關(guān)系分別為

圖3 機械臂幾何結(jié)構(gòu)示意圖Fig.3 Mechanical arm geometry diagram

3 鋸切定位控制的實現(xiàn)

3.1 修枝臂的控制流程

前面分析了修枝臂逆運動學及結(jié)構(gòu)特征，在給定末端目標位置(xf,yf)前提下，理論上可以通過前述結(jié)構(gòu)關(guān)系得到電動缸伸長量，計算出伺服器參數(shù)進行控制。但由于修枝位置是不確定的，實際環(huán)境中沒有對應的坐標空間，因此無法直接獲取目標坐標。本文實現(xiàn)定位控制的方法是首先將修枝臂初始化為某一姿態(tài)，計算相對位移，通過 3個節(jié)臂的協(xié)調(diào)控制方式將修枝鋸移向被修樹枝，最后啟動修枝鋸完成修枝作業(yè)。

如圖1b所示，臂架系統(tǒng)安裝于升降臺上，最大起升高度為6 m，修枝臂在初始化姿態(tài)下，操作員通過攝像頭輔助觀察鋸頭和樹枝之間的距離，分別進行 3個軸向上的調(diào)整。通過控制總線向升降臺系統(tǒng)硬件、回轉(zhuǎn)臺系統(tǒng)硬件和修枝臂系統(tǒng)硬件發(fā)送指令，進而控制升降臺、回轉(zhuǎn)轉(zhuǎn)盤和修枝臂，實現(xiàn)修枝鋸鋸切定位。操作人員通過工業(yè)平板電腦控制擺動推桿帶動修枝鋸運動到樹枝底部，徑向推桿和修枝鋸相互配合進行修枝作業(yè)；完成作業(yè)后，修剪機由工作位置運動到原始位置，關(guān)閉修剪機。整個工作流程如圖4所示。

圖4 修枝臂姿態(tài)調(diào)整流程Fig.4 Manipulator attitude adjustment process

3.2 電動缸伸縮量控制

伺服器將電動機旋轉(zhuǎn)編碼器得到的脈沖轉(zhuǎn)換為用戶單位（PUU，pulse of user unit）[23]，具體換算關(guān)系為編碼器實際返回值是1 280 000 pls/rev，通過設置電子齒輪比，比例分子為 128，分母為 10，因此得到用戶單位返回值為實際采用的電動缸額定伸縮速度為50 mm/s，其內(nèi)部電機額定轉(zhuǎn)速為3 000 rev/min (50 rev/s)，因此每1 rev對應電動缸伸長量1 mm。臺達ASDA-A2伺服器提供了非常便捷的位置控制模式（Pr模式），可以在伺服器中保留64組命令寄存器供用戶設置不同位置的 PUU值，每組有唯一的 Pr序號。以控制電動缸1伸縮Lxmm為例，具體控制流程如下：

1）設定伺服驅(qū)動器工作在內(nèi)部驅(qū)動的 Pr位置控制模式；

2）上電讀取伺服驅(qū)動器初始化位置的 PUU值并記錄該值①安全起見，在停機、過載、報警等異常情況下應該啟動電動缸復位相對應的Pr序號，實現(xiàn)機械臂復位操作。；

3）定義某1個Pr序號（如Pr1）為復位動作，將初始值寫入相應寄存器，并進行動作參數(shù)設定（加速度值、最大速度、加減速延時時間等）；

4）定義第2個Pr序號（如Pr2）為伸長動作，對應寄存器寫入數(shù)值Lx′ 100 000，并進行動作參數(shù)設定；

5）為簡化電動缸控制，定義第3個Pr序號（如Pr3）為收縮動作，對應寄存器寫入數(shù)值 -Lx′ 100 000，并進行動作參數(shù)設定；

6）啟用伺服器內(nèi)部輸入模式；

7）由內(nèi)部寄存器啟動伺服器；

8）啟動相應的Pr序號，完成1次電動缸復位或點動伸長（或收縮）Lxmm的操作。

3.3 三節(jié)臂協(xié)調(diào)控制實現(xiàn)方法

由于修枝作業(yè)環(huán)境的非結(jié)構(gòu)化特征（目標物體遮擋、對齊效率等）[24]，各節(jié)臂獨立調(diào)整只能滿足修枝臂大幅度姿態(tài)調(diào)整的需求，從而實現(xiàn)修枝鋸的粗定位。當修枝鋸調(diào)整到距樹枝較近的范圍，則需要對修枝鋸進行上下左右（在一個縱剖面內(nèi)）的微調(diào)，才能將樹枝卡入修枝鋸槽口，微調(diào)操作需要通過3個節(jié)臂的協(xié)調(diào)控制實現(xiàn)。

協(xié)調(diào)控制是指在已知定位目標位置前提下，通過修枝臂運動學逆解和結(jié)構(gòu)關(guān)系求解各電動缸長度，再根據(jù)計算得到的伸縮量控制伺服電機。由于現(xiàn)實環(huán)境中沒有坐標空間，控制時可以由當前電動缸長度先通過運動學正解計算出末端修枝鋸在虛擬坐標空間內(nèi)的位置坐標，并累加 1次位移產(chǎn)生的偏移量得到新坐標，最后根據(jù)運動學逆解和結(jié)構(gòu)關(guān)系求解電動缸伺服電機的控制參數(shù)，進而實現(xiàn)3個節(jié)臂的協(xié)調(diào)控制。以1次位移的步長Lsmm為例，具體控制流程如下②與獨立控制相似，協(xié)調(diào)控制同樣需要保留實現(xiàn)復位相關(guān)的Pr操作，以確保在停機、伺服器異常等行為下進行必要的修枝臂復位操作。：

1）設定伺服驅(qū)動器工作在內(nèi)部驅(qū)動的Pr位置控制模式；

2）通過伺服器中存儲的由回授脈沖轉(zhuǎn)換的 PUU值得到當前電動缸伸長量，通過修枝臂運動學正解（式1），并根據(jù) 3個電動缸當前長度，計算修枝鋸在虛擬坐標空間中的位置坐標，記為初始位置 (x0,y0)；

3）根據(jù)縱剖面內(nèi)的運動方向和運動步長，得到目標位置坐標 (x0±Ls,y0)（左右方向）或 (x0,y0±Ls)（上下方向），記為(xf,yf)；

4）根據(jù)幾何關(guān)系和由式（4）、（7）、（11）、（13）確定的運動學逆解，將(xf,yf)轉(zhuǎn)換為每個電動缸對應的伸縮量，分別記為Ls1、Ls2、Ls3；

5）對3個伺服器分別定義一個Pr序號（如Pr4）為伸縮動作，對應寄存器寫入數(shù)值Lsi′ 100 000，i=1,2,3，并進行動作參數(shù)設定；

6）啟用伺服器內(nèi)部輸入模式；

7）由內(nèi)部寄存器啟動伺服器；

8）對3個伺服器同時啟動相應的Pr序號，完成修枝臂沿某方向上一次步長為Lsmm的協(xié)調(diào)控制。

3.4 升降臺和回轉(zhuǎn)臺控制

當超出臂架作業(yè)空間之后（主要考慮垂直方向距離），應當啟動升降臺進行定位調(diào)整，而當鋸切位置不在修枝臂確定的剖面內(nèi)時需要進行回轉(zhuǎn)調(diào)整。

根據(jù)修剪機正運動學，得到修剪機末端位置坐標后改變末端上（下）方向坐標，利用修剪機逆運動學，得到各個關(guān)節(jié)角度，轉(zhuǎn)換成修枝臂電動缸的伸長量；當修枝高度超出當前工作范圍，修剪機末端上（下）運動過大時，啟動升降臺，升降臺的上升和下降配合電動缸的伸長和收縮，實現(xiàn)修剪機末端修枝鋸上（下）微動，具體控制流程如下：

1）對修枝臂進行正運動學計算，得到修枝鋸在虛擬坐標系中的坐標位置 (x0,y0)；

2）根據(jù)鋸切位置調(diào)整改變縱向位置坐標 (x0,y0+Dy)，進行修枝臂逆運動學分析，得到對應電動缸目標伸長量，并依此判斷是否超出當前鋸切最大高度限制；

3）如沒有超出最大高度限制，則實現(xiàn)電動缸伸縮控制；否則啟動升降臺升降控制，通過控制控制繼電器的閉合，實現(xiàn)升降臺的上升、下降。

回轉(zhuǎn)臺控制主要控制回轉(zhuǎn)步進電機，實現(xiàn)旋轉(zhuǎn)臺順時針旋轉(zhuǎn)、逆時針旋轉(zhuǎn)和停止動作?？刂苹剞D(zhuǎn)步進電機的脈寬調(diào)制（pulse width modulation，PWM）信號發(fā)生器采用固定周期（100μs）信號輸出，通過改變PWM信號的占空比實現(xiàn)旋轉(zhuǎn)臺的啟停。設定當PWM信號發(fā)生器輸出信號占空比為0時，旋轉(zhuǎn)臺停止旋轉(zhuǎn)，占空比為50%時旋轉(zhuǎn)臺開始旋轉(zhuǎn)。

回轉(zhuǎn)臺方向控制由DC 5 V電源模塊通過繼電器與步進電機編碼器方向信號端口連接，通過改變繼電器開關(guān)狀態(tài)對轉(zhuǎn)向進行控制。當步進電機編碼器方向信號繼電器關(guān)閉時，控制步進電機帶動旋轉(zhuǎn)臺順時針旋轉(zhuǎn)；當步進電機編碼器方向信號繼電器打開時，控制步進電機帶動旋轉(zhuǎn)臺順時針旋轉(zhuǎn)。

4 樣機鋸切定位試驗

4.1 樣機試驗系統(tǒng)

修剪機控制系統(tǒng)以工業(yè)平板電腦為控制核心。在平板上開發(fā)基于Windows MFC的人機交互程序[25-26]，如圖5a所示，作業(yè)人員通過觸摸平板上的虛擬按鍵實現(xiàn)修剪機操控，完成修枝作業(yè)。為了便于修枝鋸對準待修樹枝，在修枝鋸旁配置了無線攝像頭，通過WiFi無線通信的方式將采集圖像信號傳輸至平板電腦上的接收端。圖 5b、5c所示為無線攝像頭在修枝鋸關(guān)鍵姿態(tài)下采集到的影像，可以看出通過視覺輔助的方式更容易判斷修枝鋸與待修樹枝間的相對位置。

修剪機控制系統(tǒng)整體結(jié)構(gòu)如圖6所示，其中對3個伺服電機的控制是系統(tǒng)主要功能之一。為了盡可能避免總線競爭對伺服控制產(chǎn)生影響，設計了1條RS485總線專用于伺服器通訊。此外，旋轉(zhuǎn)臺步進電機、控制修枝鋸動作和升降臺的繼電器均采用帶RS485通訊的控制單元，并通過第2條RS485總線實現(xiàn)。

圖5 應用程序及攝像頭輔助定位Fig.5 Application and camera assisted positioning

圖6 控制系統(tǒng)整體結(jié)構(gòu)框圖Fig.6 Control system structure block diagram

作業(yè)時，操作人員通過工業(yè)平板的觸摸屏觀察樹枝位置，并通過工業(yè)平板電腦發(fā)送主動控制指令?？刂栖浖闪藱C械臂正逆運動學解算以及結(jié)構(gòu)關(guān)系，能夠?qū)崿F(xiàn) 3個節(jié)臂的獨立控制和協(xié)調(diào)控制。此外，控制程序還可實現(xiàn)回轉(zhuǎn)臺旋轉(zhuǎn)、升降臺升降、修枝鋸擺動和修枝等功能。

樣機修枝試驗在山東農(nóng)業(yè)大學校內(nèi)開放區(qū)域內(nèi)進行，主要修除具有多年長齡樹木的死枝和影響生長的競爭枝。如圖7所示，圖7a為修剪機作業(yè)現(xiàn)場；圖7b為修枝作業(yè)試驗中樹枝與修枝鋸槽口對準操作過程；圖7c為修枝鋸剪枝后的樹枝切口。

圖7 樣機試驗Fig.7 Prototype test

4.2 鋸切定位誤差測量及補償

為了檢驗并提高修剪機在作業(yè)時的鋸切定位精度，修枝試驗過程中采用逐點測量鋸切定位誤差的方法，在給定末端位置的基礎上，通過應用程序控制末端修枝鋸進行定位，移動后測量并記錄修枝鋸到達的實際位置，并將實際位置與理論位置進行對比分析，如誤差過大則當進行誤差補償處理。

修枝作業(yè)時首先調(diào)整回轉(zhuǎn)臺，對準待剪樹枝方向后再調(diào)節(jié)機械臂，通過3節(jié)臂聯(lián)動將樹枝對入鋸口。因此，協(xié)調(diào)控制是提高鋸切定位精度的關(guān)鍵[27-30]。將控制系統(tǒng)應用于樣機作業(yè)，為評估定位控制的精確度，將回轉(zhuǎn)角0q固定，在一個縱剖面內(nèi)定位末端修枝鋸，通過測距設備測量修枝鋸在虛擬坐標空間中的橫向、縱向距離，計算由控制系統(tǒng)產(chǎn)生的定位誤差。這里僅分析一般情況，即以修枝臂初始化姿態(tài)為初始點的相對運動。采用逐點測量的方法，機械臂初始化后同時在空間 2個坐標軸向上（橫向、縱向）以 10 cm為單位移動，測量移動后實測位置如表3中所示。

表3 定位試驗結(jié)果（數(shù)據(jù)格式：(y, x)；m）Table 3 Positioning test result (format (y, x); m)

從表 3中可以看到，由于關(guān)節(jié)間隙、桿件精度、裝配工藝等多方面因素的制約，未經(jīng)補償?shù)臋C械臂定位誤差較大。此外，由于修剪機械臂的臂展長，負載自重比大，存在一定的柔性，是導致定位誤差的另一重要因素。

為了進一步提高系統(tǒng)的定位精度，采用二次回歸的方法對試驗誤差數(shù)據(jù)進行誤差擬合[31]，在定位時將擬合誤差累加到定位坐標上，再通過運動學逆解求的各電動缸的長度。由于表 3所示橫向定位誤差比縱向定位誤差大的多，因此實際系統(tǒng)中主要是對橫向定位誤差進行補償[31]。根據(jù)表3中補償前位置誤差數(shù)據(jù)，得到擬合方程

式中d為補償誤差，m；Dx為相對初始點的移動距離，m；將補償公式集成于控制程序，重復試驗得到新的試驗結(jié)果如表 3中補償后位置數(shù)據(jù)所示。將表中數(shù)據(jù)繪制在同一坐標系下（圖 8），可以看到，經(jīng)過誤差補償后橫向定位誤差可以修正到縱向誤差水平，基本上可以滿足定位精度的需求。經(jīng)簡單計算，補償后在 2個軸向上的定位標準差均小于5 mm，與類似農(nóng)業(yè)工程機械手相比（如采摘機械手）[32-33]，定位精度能夠達到同類機械手水平，且考慮到高枝修剪機定位范圍更廣（作業(yè)高度 15 m、作業(yè)半徑6 m），表明其相對定位精度更高，能夠達到設計要求，實現(xiàn)精確定位和鋸切作業(yè)。

圖8 補償前后定位位置Fig.8 Positioning error before and after compensation

5 結(jié) 論

1）研制了一類高枝修剪機械，能夠?qū)崿F(xiàn) 15 m以內(nèi)高大樹木的修枝工作，對其臂架系統(tǒng)進行了正逆運動學和結(jié)構(gòu)特征分析，在此基礎上提出了機械臂鋸切定位控制方法。

2）設計實現(xiàn)了基于Windows工業(yè)平板的高枝修剪機的控制系統(tǒng)和應用軟件，并集成提出的鋸切定位控制方法，實現(xiàn)了 3節(jié)修枝臂的獨立控制和協(xié)調(diào)控制，以及對升降平臺、回轉(zhuǎn)臺的控制，一定程度上緩解了修枝技術(shù)落后，修枝裝置機械化、自動化程度不高的現(xiàn)狀，有效提高高枝修剪的工作效率。

3）對研制的高枝修剪機進行了鋸切定位和修枝試驗，并對鋸切定位誤差進行了測量及補償，補償后在 2個軸向上的定位標準差均小于5 mm，滿足高空剪枝作業(yè)要求，為進一步發(fā)展高枝修剪機械積累了寶貴經(jīng)驗。

[1]張秀秀，曹幫華，張曉文，等. 楊樹修枝理論和技術(shù)研究進展[J]. 河北林果研究，2010，25(3)：235－239.Zhang Xiuxiu, Cao Banghua, Zhang Xiaowen, et al. Progress in research on theory and technology of poplar pruning[J].Hebei Journal of Forestry and Orchard Research, 2010, 25(3):235－239. (in Chinese with English abstract)

[2]王春勝，趙志剛，吳龍敦，等.修枝高度對西南樺擬木蠹蛾為害的影響[J]. 西北林學院學報，2012，27(6)：120－123.Wang Chunsheng, Zhao Zhigang, Wu Longdun, et al. Effects of pruning height on trunk borer (arbela spp.) damage to betula alnoides[J]. Journal of Northwest Forestry University,2012, 27(6): 120－123. (in Chinese with English abstract)

[3]丁思一，佘濟云，楊慶朋，等. 間伐和修枝對杉木人工林土壤微生物量碳和酶活性的影響[J]. 中南林業(yè)科技大學學報，2015，35(6)：75－79.Ding Siyi, She Jiyun, Yang Qingpeng, et al. Effects of thinning and pruning on soil microbial biomass carbon and soil enzyme activities in chinese fir plantation[J]. Journal of Central South University of Forestry & Technology, 2015,35(6): 75－79. (in Chinese with English abstract)

[4]顧正平，沈瑞珍. 國內(nèi)外園林綠化機械現(xiàn)狀與發(fā)展趨勢[J].林業(yè)機械與木工設備，2004，32(2)：4－7.Gu Zhengping, Shen Ruizhen. Present status and prospect of gardening machines at home and abroad[J]. Forestry Machinery & Woodworking Equipment, 2004, 32(2): 4－7.(in Chinese with English abstract)

[5]賈啟明，馬秀娟，張大海. 關(guān)于城市高大樹木修剪機械化的探討[J]. 遼寧林業(yè)科技，2001，5(3)：43－44 Jia Qiming, Ma Xiujuan, Zhang Dahai. Discussion on the mechanization of pruning tall trees in cities[J]. Journal of Liaoning Forestry Science & Technology, 2001, 5(3): 43－44.(in Chinese with English abstract)

[6]李世葳，王述洋，王慧，等.樹木整枝修剪機械現(xiàn)狀及發(fā)展趨勢[J]. 林業(yè)機械與木工設備，2008，36(1)：15－16.Li Shiwei, Wang Shuyang, Wang Hui, et al. Present status and prospect of tree pruning machinery[J]. Forestry Machinery& Woodworking Equipment, 2008, 36(1): 15－16. (in Chinese with English abstract)

[7]辛繼紅，湯興初，全臘珍，等.背負式可調(diào)高枝修剪機的研制[J]. 湖南農(nóng)業(yè)大學學報：自然科學版，2003，29(1)：63－64.Xin Jihong, Tang Xingchu, Quan Lazhen, et al. On compact adjustable tree pruner[J]. Journal of Hunan Agricultural University: Natural Sciences, 2003, 29(1): 63－64. (in Chinese with English abstract)

[8]楊乾華，向北平.電動綠籬修枝機的結(jié)構(gòu)設計與研究[J]. 林業(yè)科技，2010，35(1)：48－50.Yang Qianhua, Xiang Beiping. Structural design and research on electricaly operated hedgerow pruner[J]. Forestry Science& Technology, 2010, 35(1): 48－50. (in Chinese with English abstract)

[9]3SG-8型全液壓修枝整形機[J]. 農(nóng)業(yè)機械，1981(2)：10－11.3SG-8 full hydraulic pruning machine[J]. Agricultural Machinery, 1981(2): 10－11.

[10]焦恩璋，張建紅.車載式高枝修剪機的研制[J]. 南京林業(yè)大學學報，1990，14(1)：63－67.Jiao Enzhang, Zhang Jianhong. A study of lorry-mounted highlift pruner[J]. Journal of Nanjing Forestry University,1990, 14(1): 63－67. (in Chinese with English abstract)

[11]陳延甫. 高大樹木修枝機械的設計與優(yōu)化分析[D]. 泰安：山東農(nóng)業(yè)大學，2016.Chen Yanfu. Design and Optimization Analysis of Tall Trees Pruning Machine[D]. Taian: Shandong Agricultural University,2016. (in Chinese with English abstract)

[12]苑進，武軍，曹邦華，等. 一種楊樹自動修枝裝置：2013 103070033B[P]. 2013-12-11.

[13]稅海濤，李迅，馬宏緒. 空間機械臂位形與基座姿態(tài)協(xié)同控制研究[J]. 宇航學報，2011，32(8)：1709－1714.Shui Haitao, Li Xun, Ma Hongxu. Coordinated control for space manipulator configuration and base orientation[J].Journal of Astronautics, 2011, 32(8): 1709－1714. (in Chinese with English abstract)

[14]劉亞軍，黃田. 6R操作臂逆運動學分析與軌跡規(guī)劃[J]. 機械工程學報，2012，48(3)：9－15.Liu Yajun, Huang Tian. Inverse kinematics and trajectory planning of 6R serial manipulators [J]. Journal of Mechanical Engineering, 2012, 48(3): 9－15. (in Chinese with English abstract)

[15]梁喜鳳，苗香雯，崔紹榮，等. 番茄收獲機械手運動學優(yōu)化與仿真試驗[J]. 農(nóng)業(yè)機械學報，2005，36(7)：96－100.Liang Xifeng, Miao Xiangwen, Cui Shaorong, et al.Experiments of optimization and simulation on kinematics of a tomato harvesting manipulator[J]. Transactions of the Chinese Society of Agricultural Machinery, 2005, 36(7): 96－100. (in Chinese with English abstract)

[16]竇永磊，汪滿新，王攀峰，等. 一種6自由度混聯(lián)機器人靜剛度分析[J]. 機械工程學報，2015，51(7)：38－44.Dou Yonglei, Wang Manxin, Wang Panfeng, et al. Stiffness analysis of a 6-DOF hybrid robot[J]. Journal of Mechanical Engineering, 2015, 51(7): 38－44. (in Chinese with English abstract)

[17]羅天洪，馬翔宇，陳才，等. 基于操縱子學模型的5自由度宏-微精密機械手運動的映射關(guān)系[J]. 機械工程學報，2015，52(13)：48－53.Luo Tianhong, Ma Xiangyu, Chen Cai, et al. Control system of five degrees of freedom of macro-micro precision manipulator based on operon model mapping [J]. Journal of Mechanical Engineering, 2015, 52(13): 48－53. (in Chinese with English abstract)

[18]孫良，趙勻，俞高紅，等. 基于D-H變換矩陣的寬窄行分插機構(gòu)運動特性分析與設計[J]. 農(nóng)業(yè)工程學報，2012，28(5)：13－18.Sun Liang, Zhao Yun, Yu Gaohong, et al. Design and kinematics analysis of wide-narrow distance transplanting mechanism based on D-H transformation matrix[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2012, 28(5): 13－18. (in Chinese with English abstract)

[19]張旭，鄭澤龍，齊勇. 6自由度串聯(lián)機器人D-H模型參數(shù)辨識及標定[J]. 機器人，2016，38(3)：260－370.Zhang Xu, Zheng Zelong, Qi Yong. Parameter identification and calibration of D-H model for 6-DOF serial robots[J].Robot, 2016, 38(3): 260－370. (in Chinese with English abstract)

[20]張鵬程，張鐵. 基于包絡法六自由度工業(yè)機器人工作空間的分析[J]. 機械設計與制造，2010(10)：164－166.Zhang Pengcheng, Zhang Tie. Study on workspace analysis of 6r robot based on envelope method[J]. Machinery Design& Manifacture, 2010(10): 164—166. (in Chinese with English abstract)

[21]甘屹，王均壘，孫福佳.基于給定工作空間的6R型機器人[J]. 中國機械工程，2014，25(22)：3003—3011.Gan Yi, Wang Junlei, Sun Fujia. Optimal design of D-H parameters of a 6r robot for prescribed workspace[J]. China Mechanical Engineering, 2014, 25(22): 3003—3011. (in Chinese with English abstract)

[22]俞高紅，張瑋煒，孫良，等. 偏心齒輪-非圓齒輪行星輪系在后插旋轉(zhuǎn)式分插機構(gòu)中的應用[J]. 農(nóng)業(yè)工程學報，2011，27(4)：100－105.Yu Gaohong, Zhang Weiwei, Sun Liang, et al. Application and analysis of planetary gear train with eccentric gears and non-circular gear in backward rotary transplanting mechanism[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2011, 27(4): 100－105. (in Chinese with English abstract)

[23]謝存禧，李琳. 空間機構(gòu)設計與應用創(chuàng)新[M]. 北京：機械工業(yè)出版社，2007.

[24]臺達伺服ASDA-A2使用手冊[M]. 臺灣：中達電通股份有限公司, 2011.

[25]馬永春. 楊樹人工林修枝機理及修枝技術(shù)體系的研究[D].南京：南京林業(yè)大學，2012.Ma Yongchun. Study on Pruning Mechanism and Pruning Technology System of Poplar Plantation[D]. Nanjing:Nanjing Forestry University, 2012. (in Chinese with English abstract)

[26]奚陶. 工業(yè)機器人運動學標定與誤差補償研究[D]. 武漢：華中科技大學，2012.Xi Tao. Research on Kinematic Calibration and Error Compensation of Industrial Robot[D]. Wuhan: Huazhong University of Science & Technology, 2012. (in Chinese with English abstract)

[27]杰瑞夫，克里斯托夫. Windows核心編程(第5版)[M]. 北京：清華大學出版社，2008.

[28]侯俊杰. 深入淺出MFC(第2版)[M]. 武漢：華中科技大學出版社，2001.

[29]鄧永剛. 工業(yè)機器人重復定位精度與不確定度研究[D].天津：天津大學，2014.Deng Yonggang. Research on the Repeatability and Uncertainty of Industrial Robots[D]. Tianjin: Tianjin University, 2014. (in Chinese with English abstract)

[30]陳志煌，陳力. 閉鏈雙臂空間機器人抓持載荷基于徑向基函數(shù)神經(jīng)網(wǎng)絡的補償控制[J]. 機械工程學報，2011，47(7)：38－44.Chen Zhihuang, Chen Li. Compensation control for grasped object of dual-arm space robot with close-chain based on radial basis function neural network[J]. Journal of mechanical Engineering, 2011, 47(7): 38－44. (in Chinese with English abstract)

[31]龔星如，沈建新，田威，等. 工業(yè)機器人的絕對定位誤差

模型及其補償算法[J]. 南京航空航天大學學報，2012，44(增刊 1)：60－64.Gong Xingru, Shen Jianxin, Tian Wei, et al. Absolute positioning error model of industrial robot and compensation algorithm[J]. Journal of Nanjing University of Aeronautics &Astronautics, 2012, 44(Supp.1): 60－64. (in Chinese with English abstract)

[32]王麗麗，范晉偉，趙博，等. 果蔬采摘機械臂結(jié)構(gòu)設計與性能測試[J]. 農(nóng)業(yè)工程, 2017, 7(2):107－113.Wang Lili, Fan Jinwei, Zhao Bo, et al. structural design and performance test of fruit and vegetable picking manipulator[J].Agricultural Engineering, 2017, 7(2):107－113. (in Chinese with English abstract)

[33]李楨. 獼猴桃采摘機器人機械臂運動學仿真與設計[D].楊凌：西北農(nóng)林科技大學，2015.Li Zhen. The Kinematis Simulation and Design of Kiwi Fruit Picking Manipulator[D].Yangling: Northwest A&F University,2015. (in Chinese with English abstract)

Attitude coordination control and accurately sawing-positioning of high-branch pruning machine

Li Yang1,2, Du Shuai1, Chen Baolin1, Fu Guanghua1, Yuan Jin1,2※
(1.College of Mechanical & Electronic Engineering, Shandong Agricultural University, Tai'an271018,China;2. Shandong Provincial Key Laboratory of Horticultural Machinery and Equipment, Tai’an271018,China)

bstract: Tree pruning can promote the accumulation of photosynthetic products, reduce the forest stand density,improve the utilization performance of air and light for stands, and promote the growth of trees, which is an important task for the forest maintenance. In recent years, the domestic pruning machinery research has made some achievements. However, the domestic and international high-altitude pruning machinery is still in its infancy at present. The operating height is generally less than 8 m, and the machine is difficult to improve the work efficiency of pruning high branches. Against to the current prevalence of pruning machinery at home and abroad, i.e. the high-degree lack of mechanical flexibility, which results in high labor intensity for operator and high operational risk, the mechanical structure of pruning high branches machine was designed(in theory tall tree pruning work within 15 m can be realized). By observing the relative position of the saw head and the branches, the operator can flexibly control the 3-arm coordinated movement to perform sawing and positioning using the human machine interface on the industrial plate pc (personal computer). In order to improve the ease use of the machine further, the wireless camera was designed to fix in front of the saw head to assist the ground operator to observe the high-altitude saw head and branch distance. The arm system is composed of 3 arms, which is fixed on the turntable of a lifting platform. Each arm is driven by one joint to rotate around the axis, and the pruning saw is fixed to the end of the third arm. The whole pruning height can reach 15 m, the pruning operating radius is up to 6 m, and the maximum pruning diameter can reach 10 cm, which meet the requirements of high altitude pruning operations. In this paper, the working principle of high-altitude pruning machine was described, and the pruning positioning was implemented by forward and inverse kinematics analysis of the manipulators. According to the mechanical structure of the pruning machine, the structural feature of the manipulator system was analyzed, and the mapping relation between the joint angle and the telescopic amount of the electric cylinder was obtained. The high-altitude pruning machine control system and integrated cutting positioning method were researched and developed. The control system of high-branch pruning machine based on touch-screen industrial flat plate was designed, and the control application based on Windows and MFC (Microsoft Foundation Classes) was adopted to realize the independent action or coordinated control of the 3 manipulators, the platform control, and the turntable control, which realized the positioning method of pruning machine. The open-area prototype pruning test was implemented, and the controller and control method were applied to the prototype test. In addition, through the prototype test, the error measurement and the error compensation in the open area were performed. The test results showed that the control system and the control method designed in this paper could realize the precise positioning of end-point pruning saw in the 2 coordinate axes of the space, of which the standard deviation was less than 5 mm. With the 15 m operating height, and the 6 m operating radius range of the pruning machine, it obtained a higher accuracy. So, it can meet the design requirements, and achieve precise positioning and pruning operations.

forestry; position control; kinematics; high-branch pruning machine; pruning positioning; coordinated control;control system implement

10.11975/j.issn.1002-6819.2017.10.006

S776.27+4

A

1002-6819(2017)-10-0040-09

2016-10-26

2017-04-16

國家自然科學基金項目( 51675317、51475278)；“十二五”國家科技支撐計劃資助項目（2014BAD08B01-2）；山東科技發(fā)展計劃項目（2013GNC11203、2014GNC112010）；山東省農(nóng)機裝備研發(fā)創(chuàng)新計劃項目（2015YB201）

李 揚，男，山東泰安人，博士，講師，主要從事嵌入式系統(tǒng)、智能農(nóng)機裝備方向研究。泰安 山東農(nóng)業(yè)大學機械與電子工程學院，271018。

Email：mtlyab@sdau.edu.cn

※通信作者：苑 進，男，山東泰安人，博士，教授，主要從事智能機電控制技術(shù)研究。泰安 山東農(nóng)業(yè)大學機械與電子工程學院，271018。

Email：jyuan@sdau.edu.cn