果园喷雾机器人靶标探测与追踪系统

江世界，马恒涛，杨圣慧，张 超， 苏道毕力格，郑永军,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）针对单一果树冠层靶点位置变化幅度较小的实际情况，对果树冠层进行等分处理，在保证靶标跟随效果的同时，有效减少计算量以及仰角调节机构的响应时间要求。该系统为喷雾作业时减少雾滴脱靶，提高农药利用率提供技术参考。