江世界，馬恒濤，楊圣慧，張 超， 蘇道畢力格，鄭永軍,2,3※，康 峰

（1. 中國農(nóng)業(yè)大學(xué)工學(xué)院，北京100083；2. 現(xiàn)代農(nóng)業(yè)裝備與設(shè)施教育部工程研究中心，北京100083； 3. 中國農(nóng)業(yè)大學(xué)煙臺研究院，煙臺264670；4. 北京林業(yè)大學(xué)工學(xué)院，北京100083）

0 引言

病蟲害防治工作是整個果樹管理作業(yè)中重要的環(huán)節(jié)，隨著季節(jié)變更、氣候變化，每年噴藥8～15次，其工作量約占整個果樹管理工作量的30%，果園植保技術(shù)主要包括地面植保技術(shù)和航空植保技術(shù)，地面植保技術(shù)可分為人工施藥技術(shù)、傳統(tǒng)風(fēng)送施藥技術(shù)和精準施藥技術(shù)等[1-2]。

風(fēng)送施藥技術(shù)能夠改善霧滴霧化性能，有效提高作業(yè)效率，得到了廣泛研究和應(yīng)用。但是傳統(tǒng)風(fēng)送施藥機的噴頭布置多采用固定安裝方式，常見有固定圓環(huán)式[3-5]、噴桿式[6-7]、門式[8]、組合圓盤式[9]等，噴頭位置不能隨意調(diào)整，容易造成無效噴施和霧滴脫靶飄移，農(nóng)藥浪費嚴重。隨著技術(shù)的發(fā)展，對靶[10-12]、仿形、變量[13-14]等精準施藥技術(shù)快速發(fā)展，將傳統(tǒng)連續(xù)噴霧模式變?yōu)殚g歇性、變量噴霧模式，為提高農(nóng)藥利用率，減少環(huán)境污染提供了一種途徑[15-17]。馬馳等[18]研制一種丘陵山地柑橘果園多方位自動噴藥裝置，能夠根據(jù)檢測的果樹冠層輪廓自動切換噴藥模式，設(shè)計豎直噴藥、45°傾斜噴藥和對地噴藥等不同模式，提高了對不同果樹適應(yīng)性；Osterman等[19]提出一種基于激光雷達的多關(guān)節(jié)風(fēng)送噴霧機，并設(shè)計噴霧機構(gòu)定位算法，能夠根據(jù)果樹冠層特征自動調(diào)整噴頭方向和位置，實現(xiàn)定向仿形噴霧；張曉輝等[20]設(shè)計了履帶自走式果園定向風(fēng)送噴霧機，可根據(jù)果樹長勢調(diào)整噴頭噴霧角度，實現(xiàn)定向仿形噴霧，試驗表明定向噴霧變異系數(shù)降低55.3%；李龍龍等[21]開發(fā)一種基于變風(fēng)量與變噴霧量的果園自動仿形噴霧機，搭載多個風(fēng)機和噴頭，根據(jù)激光傳感器掃描的冠層特征調(diào)節(jié)風(fēng)量和噴霧量實現(xiàn)仿形變量噴霧；Li等[22]設(shè)計基于激光傳感器的果園變量噴霧機，利用高精度激光傳感器獲取果樹冠層體積，并實時控制噴嘴的流量和噴嘴位置的風(fēng)扇轉(zhuǎn)速，能夠有效節(jié)省藥量，減少地面和空氣中霧滴漂移量。此外，研究者基于激光雷達[23-24]、機器視覺[25]、超聲傳感器[26]等不同傳感器類型開展了一系列對精準施藥技術(shù)的探索，但是受傳感器性能、果園種植模式等因素限制，鮮見有規(guī)模化應(yīng)用。

綜上，本研究設(shè)計一種果園噴霧機器人靶標檢測與追蹤系統(tǒng)，通過對激光雷達掃描獲取的冠層點云信息的處理，實時獲取靶標位置以及噴霧仰角，并設(shè)計基于增量式比例積分微分（Proportional Integral Derivative，PID）的噴霧仰角控制算法，從而實現(xiàn)靶標跟隨噴霧，為果園施藥技術(shù)與裝備的發(fā)展提供參考。

1 靶標探測與追蹤系統(tǒng)設(shè)計

1.1 系統(tǒng)載體

本研究以中國農(nóng)業(yè)大學(xué)工學(xué)院研制的靶標跟隨式小型噴霧機器人為系統(tǒng)載體（圖1），主要包括電動履帶底盤、靶標跟隨式噴霧機構(gòu)、離心式風(fēng)機、噴霧組件和激光雷達等。其中，激光雷達安裝在電動履帶底盤的最前端，用于靶標的探測；靶標跟隨式噴霧機構(gòu)安裝在電動履帶底盤的后端，主要包括升降機構(gòu)、噴霧支架、電動推桿、噴頭等，采用多噴頭對稱式設(shè)計，噴霧角度可根據(jù)靶標位置進行調(diào)節(jié)，雙側(cè)與單側(cè)作業(yè)模式自由切換；離心式風(fēng)機產(chǎn)生的氣流被氣流分配器分發(fā)輸送到噴頭位置，輔助提高霧滴的霧化程度。主要技術(shù)參數(shù)如表1所示。

表1 靶標跟隨式小型噴霧機器人的主要參數(shù) Table 1 Main parameters of target following small spraying robot

1.2 靶標跟隨式小型噴霧機器人作業(yè)原理

靶標跟隨式小型噴霧機器人主要針對果樹冠層中下部施藥，可與植保無人機配合形成地空協(xié)同立體植保模式。地空協(xié)同立體植保模式示意圖如圖2所示，其中植保無人機對果樹冠層上部施藥，靶標跟隨式小型噴霧機器人對冠層中下部施藥，實現(xiàn)對果樹冠層全覆蓋施藥。

1.3 靶標探測與追蹤系統(tǒng)方案設(shè)計

1.3.1 靶標探測與追蹤系統(tǒng)硬件框圖

靶標探測與追蹤系統(tǒng)的硬件框圖如圖3所示，主控單元采用STM32F429單片機，接收和讀取微型工控機、測距傳感器的信號，通過脈沖寬度調(diào)制方式（Pulse width modulation，PWM）控制電動推桿的伸縮長度和升降機構(gòu)的升降高度，并能根據(jù)推桿編碼器的脈沖反饋信號實時調(diào)整。其中微型工控機對激光雷達掃描的點云信號進行處理，得到靶標位置、目標仰角等參數(shù)并發(fā)送給主控單元。

1.3.2 靶標跟隨式小型噴霧機作業(yè)邏輯框圖

靶標跟隨式小型噴霧機作業(yè)流程如圖4所示，作業(yè)開始前，作業(yè)人員首先根據(jù)果樹冠層的整體高度對噴霧機構(gòu)的高度進行預(yù)調(diào)節(jié)，使噴頭組噴霧范圍能夠覆蓋冠層中下部；作業(yè)時，噴霧機器人能夠自主在果樹行間行駛，激光雷達的掃描平面垂直行駛方向，點云數(shù)據(jù)可表征樹冠間隙、樹冠方位和密度。噴霧機構(gòu)能夠根據(jù)激光雷達的點云數(shù)據(jù)實時探測靶標位置，并根據(jù)靶標位置實時調(diào)節(jié)噴霧機構(gòu)角度，減少霧滴脫靶。

2 靶標探測方法

2.1 靶標探測區(qū)域確定

靶點探測傳感器采用激光雷達（RPLIDAR S1，上海思嵐科技有限公司，中國），其掃描角度為360°，分辨率為0.391°，掃描頻率為8～20 Hz，掃描距離為0.1～40.0 m，可滿足果樹冠層探測需求。

為避免雜草和樹干的干擾，減少數(shù)據(jù)運算量，需先確定靶點探測區(qū)域，取梯形ABOD區(qū)域作為單側(cè)靶點探測區(qū)域，O點為激光雷達的探測中心點，以過O點的水平線作為基準線，令基準線以下掃描角度為負值，基準線以上為正值（圖5）。

對廣西南寧、北京平谷、山西運城等地不同種類果園調(diào)研，發(fā)現(xiàn)果樹行間距一般為3～5 m，株高為3.5～4.0 m，主干高為0.7～0.9 m，因此本研究取O點距地面的距離h為850 mm，OB長為1 000 mm，AB長為2 000 mm，OD與水平線的夾角為20°，由幾何運算得，OA與水平線的夾角為26.57°。則探測區(qū)域（S(φ)，mm）如式（1）所示：

式中φ為探測區(qū)域內(nèi)任意一條掃描線與水平線的夾角，(°)；S(φ)為掃描角度為φ處的探測區(qū)域，mm。

2.2 靶點選取規(guī)則

以單側(cè)噴頭連線中垂線對準的目標點為靶點M，以噴頭組的中垂線與激光雷達掃描的0°線夾角（α，(°)）為噴頭組的目標仰角（圖5）。靶點選取方法步驟如下：

1）確定靶點極徑大小

取一幀點云數(shù)據(jù)，以激光雷達的探測中心為極坐標系的原點O，創(chuàng)建冠層中下部截面點云極坐標系如圖6所示，其中，P1(rP1,θP1)為極角最大點，P0(rP0,θP0)為極角最小點。

在靶點選取時，若極徑選擇過小，會導(dǎo)致噴霧仰角加大，上噴頭的噴幅可能會越過冠層噴霧區(qū)域上邊界；若極徑選擇過大，下噴頭的噴幅可能會越過冠層噴霧區(qū)域下邊界，過小和過大都會增加霧滴脫靶的可能性，因此以單幀點云數(shù)據(jù)的點平均極徑作為靶點的極徑（，mm），其計算如式（2）所示：

式中ri為極坐標中第i個點的極徑，mm；n為靶標點云的總點數(shù)。

2）確定靶點極角大小

為確定靶點的位置，還需進一步確定靶點M的極角。由電動推桿尺寸和行程參數(shù)可知，目標仰角存在最小仰角0°和最大仰角為（αmax，(°)），故靶點極角也應(yīng)存在邊界值。在確定靶點極角的邊界值時，需同時考慮噴頭組的仰角極限值以及噴頭組旋轉(zhuǎn)中心與激光雷達的相對位置。

為便于分析，將噴頭組的運動平面映射到激光雷達的掃描平面中，并以激光雷達探測中心點為原點O，以激光雷達的0°掃描線為x軸，以90°掃描線為y軸建立笛卡爾直角坐標系，當噴霧仰角為0°時，靶點極角存在下邊界（δmin，(°)），當噴霧仰角為αmax時，靶點極角存在上邊界（δmax，(°)）如圖7所示。

由幾何關(guān)系求解得到靶點M的最小極角（δmin，(°)）和最大極角（δmax，(°)）的數(shù)學(xué)表達式如式（3）和式（4）所示：

式中H為噴頭組旋轉(zhuǎn)中心與電動履帶底盤上表面的垂直距離，mm。

由式（3）和式（4）可知，當和αmax一定時，δmin和δmax隨H的增大而增大，隨H的減小而減小，當冠層高度相對較低時，可通過控制升降機構(gòu)將噴頭組高度適當降低，以保證噴頭組噴幅的下邊界能覆蓋冠層最下部，同理，當株高相對較高時，可適當升高噴頭組的高度以保證噴頭組噴幅上邊界能覆蓋冠層中部；當H和αmax一定時，δmin隨著的增大而減小，而δmax隨著的增大而增大。由此可知，不同幀的點云中，靶點M的極角邊界值與該幀點云中的點平均極徑有關(guān)。

為確保2個噴頭的噴施區(qū)域相對均衡，取單幀靶標點云中的極角最大點P1的極角θP1與δmin的平均值作為靶點M的極角（δM，(°)），其計算如式（5）所示：

式中θP1為極角最大點P1的極角，（°）。

當通過式（5）計算得到的δM超出邊界值時，則用邊界值代替，得出δM關(guān)于θP1的分段函數(shù)表達式如式（6）所示：

通過上述方式可獲一幀數(shù)據(jù)的靶點為M()，但在實際作業(yè)中單幀靶點跳動較大，較難實現(xiàn)噴頭對靶運動控制，需要連續(xù)取多幀數(shù)據(jù)，并求每幀點云數(shù)據(jù)的靶點，再按時序取多組靶點進行均值濾波處理，可得到該時域?qū)?yīng)的三維點云空間內(nèi)的平均靶點Ma()作為對靶噴霧作業(yè)的實際靶點。

根據(jù)靶點選取規(guī)則，可繪制作業(yè)過程中平均靶點Ma的變化軌跡示意圖（圖8）。在樹冠間隙處，探測區(qū)域內(nèi)無靶點，只有當激光雷達檢測到靶標存在時，探測區(qū)域才產(chǎn)生靶點。

2.3 噴霧機構(gòu)目標仰角獲取

目標仰角（α，(°)）是指在噴頭組中垂線對準平均靶點Ma時的噴霧仰角。根據(jù)平均靶點Ma計算噴頭組的目標仰角，并通過串口通訊將計算結(jié)果傳輸至STM32F429單片機，控制噴頭組的仰角。為求取目標仰角，將噴頭組的運動平面映射到激光雷達的掃描平面中，構(gòu)成目標仰角求解直角坐標系如圖9所示。

經(jīng)幾何求解，可得到噴頭組的目標仰角α與、Ma和H等參數(shù)的關(guān)系表達式如式（7）所示：

式中為平均靶點Ma的極徑，mm；δMa為平均靶點Ma的極角，（°）。

3 仰角追蹤方法

3.1 仰角測量

噴霧仰角調(diào)節(jié)的執(zhí)行機構(gòu)采用電動推桿（型號為TGA-Y），該電動推桿的最小安裝距離為205 mm，最大行程為100 mm，最大伸縮速度為60 mm/s，推力為100 N，滿足噴霧機構(gòu)仰角調(diào)節(jié)的需求。電動推桿自帶霍爾編碼器，推桿電機旋轉(zhuǎn)10圈，絲杠旋轉(zhuǎn)1圈，編碼器可輸出6個脈沖，而絲杠旋轉(zhuǎn)一圈推桿行程變化9 mm?？衫描b相原理通過單片機對編碼器反饋的脈沖信號進行“加”計數(shù)或“減”計數(shù)，可獲得反饋脈沖總數(shù)NP，且進而得到推桿行程（l，mm）的表達式如式（8）所示：

式中l(wèi)為推桿行程，mm；NP為編碼器反饋的脈沖總數(shù)。

推桿行程的變化可帶動噴霧支架轉(zhuǎn)動，從而改變噴頭組的仰角，電動推桿行程l=0時，噴霧支架處于豎直狀態(tài)，噴頭組處于水平噴霧姿態(tài)，噴霧仰角為0°（圖10a）；當電動推桿行程l>0時，噴霧支架處于向內(nèi)傾斜狀態(tài)，噴頭組處于斜上噴霧姿態(tài)，目標仰角為α（圖10b）。其中，O1為噴霧支架的旋轉(zhuǎn)中心，O2為電動推桿固定端安裝位置，O3和O3'分別為噴霧仰角為0°和α?xí)r，電動推桿活動端安裝位置，K1為O2在O1所在水平面的投影，K2和K2'分別為噴霧仰角為0°和α?xí)r，O3在O1沿噴霧支架方向上的投影。

根據(jù)推桿行程與目標仰角變化示意圖提取噴霧仰角調(diào)節(jié)原理圖，如圖11所示，ΔO1K1O2、ΔO1K2O3是剛體三角形，O2O3是電動推桿最短安裝尺寸，即O2O3= 205 mm，γ為噴霧支架與水平線的夾角（°），且與α互為余角。當噴頭水平噴霧時，噴霧支架處于初始位置，此時有K2O1⊥K1O1。當噴霧支架繞O1點轉(zhuǎn)動α角度時，此時ΔO1K2O3繞頂點O1轉(zhuǎn)動至ΔO1K2'O3'處。此時推桿長度由O2O3變化為O2O3'，噴霧仰角由0°變?yōu)棣痢?/p>

利用余弦定理和剛體運動規(guī)律，推導(dǎo)得到噴霧仰角與推桿行程關(guān)系如式（9）所示：

綜合式（8）和式（9）可得到反饋脈沖數(shù)NP與噴角α之間的關(guān)系如式（10）所示：

因此，根據(jù)上述推導(dǎo)過程可知，通過測量電動推桿的行程變化，可求得噴頭組的仰角變化，進而實現(xiàn)噴霧仰角測量。

3.2 基于增量式比例積分微分（PID）的噴霧仰角控制算法

為提高噴霧仰角的控制精度，設(shè)計了基于增量式PID的噴霧仰角控制算法，控制模型如圖12所示。在進行噴霧仰角控制時，控制系統(tǒng)首先采集推桿編碼器反饋的脈沖信號，并同時獲取靶標探測系統(tǒng)發(fā)送的目標仰角。然后控制器對編碼器脈沖信號進行鑒相和計數(shù)，并通過式（10）得到實時噴霧仰角。最后，PID控制器將噴霧仰角實際值與噴霧仰角目標值作比較，求出二者誤差ek，并由PID控制器根據(jù)誤差進行系統(tǒng)誤差修正，向推桿電機輸入相應(yīng)的控制量。

基于增量式PID的噴頭仰角控制算法原理是使用離散的差分方程代替連續(xù)的微分方程，其離散型PID控制方程如式（11）所示：

式中u(m)為第m次控制輸出量；ek為第k次目標值與實際值之間誤差；ek-1為第k-1次目標值與實際值之間誤差；Kp為比例控制環(huán)節(jié)系數(shù)；Ki為積分控制環(huán)節(jié)系數(shù)；Kd為微分控制環(huán)節(jié)系數(shù)。

通過對各參數(shù)進行整定得到最優(yōu)PID參數(shù)組合，由式（11）計算得到的第m次與第m-1次控制算法輸出量，并進行兩次輸出量差值，得到控制增量輸出。

4 試驗與結(jié)果分析

4.1 試驗條件與試驗設(shè)計

為驗證該系統(tǒng)的靶標探測和目標仰角定位的準確性，2020年10月－11月在中國農(nóng)業(yè)大學(xué)東校區(qū)工學(xué)院西側(cè)海棠樹林開展試驗，試驗場地長×寬規(guī)格為40 m× 12 m，主要種植參數(shù)為行距約3.0 m，株距約3.0 m，株高約4.0 m，樹干高度約0.8 m，試驗時環(huán)境溫度約5 ℃，自然風(fēng)速小于0.5 m/s，主要設(shè)備為靶標跟隨式小型噴霧機、激光雷達等（圖13a）。

試驗方案示意圖如圖13b所示，試驗時噴頭組旋轉(zhuǎn)中心距電動履帶底盤上表面的高度H為565 mm，激光雷達安裝在車體最前段，安裝位置距電動履帶底盤上表面高度為470 mm。試驗過程中靶標跟隨式小型噴霧機從起始位置以0.5 m/s速度勻速在行間行駛，到達地頭后調(diào)轉(zhuǎn)方向從相鄰行間返回，激光雷達掃描海棠樹冠層點云數(shù)據(jù)并存儲到機器人操作系統(tǒng)（Robot Operating System，ROS）的rosbag文件中，沿行進路線隨機選取3棵樹并標記為樹1、樹2、樹3。利用設(shè)計的靶標探測與追蹤系統(tǒng)對選取的3棵樹的冠層進行靶點定位與目標仰角處理。

4.2 結(jié)果與分析

進行靶標探測與仰角追蹤處理前，首先對冠層點云數(shù)據(jù)的靶標探測區(qū)域進行識別，垂直于車體行駛方向?qū)呙璧狞c云數(shù)據(jù)進行探測區(qū)域劃分，探測區(qū)域劃分示意圖如圖14所示，其中O點為激光雷達探測中心點，實線梯形框ABOD和虛線梯形框EBOF分別表示前進方向右探測區(qū)域和左探測區(qū)域。

確定靶標探測區(qū)域后，利用式（2）～式（10）對點云數(shù)據(jù)進行分割和計算，分別得到隨機選取的3棵海棠樹冠層探測區(qū)域內(nèi)的靶點位置和對應(yīng)噴霧仰角，試驗結(jié)果如圖15和圖16所示。所提出的靶標探測與追蹤系統(tǒng)能夠適應(yīng)不同規(guī)格果樹冠層，可以依據(jù)冠層特征計算靶點位置；并進一步得到靶點對應(yīng)的噴霧仰角，調(diào)節(jié)噴霧機構(gòu)；3棵果樹的目標靶點主要集中在距地2.0～3.5 m范圍內(nèi)，相對于5.0 m左右的樹高，滿足果樹冠層中下部噴霧作業(yè)需求；最小噴霧仰角為47.8°，最大噴霧仰角為51.4°，連續(xù)目標靶點之間噴霧仰角最大調(diào)節(jié)時間為0.06 s，可滿足對靶的時效需求。分別將試驗3棵樹的機器測量靶點位置坐標表征到實際果樹冠層上，通過該方法獲取的靶點離地高度與實際靶點離地高度誤差小于10%，符合果樹冠層覆蓋噴霧需求。

此外，單一果樹冠層靶點位置和噴霧仰角變化幅度很小，因此可根據(jù)噴霧機的行駛速度對冠層進行等分處理。以果樹1的三等分為例，對于0.5 m/s的行進速度，需要約50幀點云數(shù)據(jù)表征果樹三維模型，可將其中約15幀點云數(shù)據(jù)的靶點坐標取平均值得到平均靶標位置的坐標，以及平均靶點對應(yīng)的噴霧仰角，優(yōu)化后目標靶點如圖17所示。等分處理可有效減少計算量以及仰角調(diào)節(jié)機構(gòu)的響應(yīng)時間要求，提高系統(tǒng)的適用性。

5 結(jié) 論

本研究針對傳統(tǒng)風(fēng)送式噴霧機作業(yè)過程中霧滴脫靶率高的問題，設(shè)計了一種基于果園噴霧機器人的靶標探測與追蹤系統(tǒng)，主要結(jié)論如下：

1）采用激光雷達獲取果樹冠層點云數(shù)據(jù)，通過點云分割計算等處理，獲取目標靶點的極徑、極角等信息，進而得到對應(yīng)噴霧仰角，為后續(xù)靶標跟隨噴霧的效果研究提供理論基礎(chǔ)；

2）開展了實際果樹冠層靶標探測和仰角測量試驗，試驗結(jié)果表明，隨機選取的3棵果樹利用該系統(tǒng)計算得到的目標靶點主要集中在2.0～3.5 m范圍內(nèi)，相對于5.0 m左右的樹高，滿足中下部噴霧作業(yè)需求，最小噴霧仰角為47.8°，最大噴霧仰角為51.4°，連續(xù)目標靶點之間噴霧仰角最大調(diào)節(jié)時間為0.06 s，可滿足對靶的時效需求，能夠適應(yīng)不同規(guī)格果樹冠層，為地面機械與無人機立體協(xié)同植保提供了技術(shù)手段。

3）針對單一果樹冠層靶點位置變化幅度較小的實際情況，對果樹冠層進行等分處理，在保證靶標跟隨效果的同時，有效減少計算量以及仰角調(diào)節(jié)機構(gòu)的響應(yīng)時間要求。該系統(tǒng)為噴霧作業(yè)時減少霧滴脫靶，提高農(nóng)藥利用率提供技術(shù)參考。