果園噴霧機器人靶標探測與追蹤系統

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

（1. 中國農業大學工學院，北京100083；2. 現代農業裝備與設施教育部工程研究中心，北京100083； 3. 中國農業大學煙臺研究院，煙臺264670；4. 北京林業大學工學院，北京100083）

0 引言

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

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

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

1 靶標探測與追蹤系統設計

1.1 系統載體

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

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

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

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

1.3 靶標探測與追蹤系統方案設計

1.3.1 靶標探測與追蹤系統硬件框圖

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

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

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

2 靶標探測方法

2.1 靶標探測區域確定

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

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

對廣西南寧、北京平谷、山西運城等地不同種類果園調研，發現果樹行間距一般為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°。則探測區域（S(φ)，mm）如式（1）所示：

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

2.2 靶點選取規則

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

1）確定靶點極徑大小

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

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

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

2）確定靶點極角大小

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

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

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

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

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

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

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

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

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

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

2.3 噴霧機構目標仰角獲取

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

經幾何求解，可得到噴頭組的目標仰角α與、Ma和H等參數的關系表達式如式（7）所示：

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

3 仰角追蹤方法

3.1 仰角測量

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

式中l為推桿行程，mm；NP為編碼器反饋的脈沖總數。

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

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

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

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

因此，根據上述推導過程可知，通過測量電動推桿的行程變化，可求得噴頭組的仰角變化，進而實現噴霧仰角測量。

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

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

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

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

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

4 試驗與結果分析

4.1 試驗條件與試驗設計

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

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

4.2 結果與分析

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

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

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

5 結 論

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

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

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

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