我国果园种植面积达1.2738×10⁷ hm²，居世界首位，我国还是全球最大的水果生产国、消费国和出口国之一^[1]。果园作业任务及流程多，包含除草、灌溉、施肥、施药、采摘等，其中，施药喷雾作业占果园总作业量的30%^[2]。目前，大多为果农背负喷雾器或驾驶风送喷雾机进行喷洒作业，劳动强度大，且严重影响操作人员身体健康。果园喷雾机的自主导航是实现果园喷雾作业精细化、自动化的关键，也是果园喷雾作业中人机分离的基础^[3-5]。

目前用于果园作业机械导航定位的传感器主要有里程计、惯性测量单元(Inertial measurement unit, IMU)、全球导航卫星系统(Global navigation of satellite system, GNSS)、激光雷达(Laser radar, LiDAR)和视觉传感器等^[6-9]。熊斌等^[10]在果园喷雾机上使用实时动态定位−北斗卫星导航系统(Real time kinematic-BeiDou navigation satellite system, RTK-BDS)和三轴电子罗盘，在稀疏的樱桃园进行了测试，在施药机行进速度为2 km/h时，直线跟踪最大误差小于0.13 m，平均跟踪误差小于0.03 m。Zhang等^[11]设计了BDS/IMU组合导航系统，果园喷洒机器人以0.4 m/s运动时，最大偏移误差低于10.77 cm，平均偏移误差不高于3.55 cm，横向偏移均方根误差为1.19 cm。上述导航方法高度依赖卫星定位。郁闭果园中，茂密的果树冠层易遮挡GNSS信号，导致农用机械在行间区域长时间无法有效接收信号。肖珂等^[12]在生成行间导航线的基础上，设计采用随机采样一致性(Random sample consensus, RANSAC)算法提取前排树行线；然后，计算树干边界框坐标点到前排行线的距离，筛选后排树干坐标点，采用最小乘法(Least squares method, LSM)拟合生成后排树行线，但未考虑果树缺失后光照的变化情况。Zhou等^[13]提出了一种基于计算机视觉技术的导航方法，用LSM拟合两侧生长的果树参考线，并由2条参考线确定果园机械的行驶路径，提取果园中心线的准确率为90.00%。郁闭果园因树冠高度重叠、枝叶遮挡严重，行间光照呈现强异质性(如局部光斑与深阴影并存)；同时，基于视觉传感器的方案受自然光周期性变化限制，难以实现全天候连续作业。李秋洁等^[14]基于2D LiDAR数据获取相邻树行定位信息，进而获取行间导航路径。Zhang等^[15]利用2D LiDAR，采用欧氏聚类算法和三点共线的重要几何定理提取树干中心特征点，同时基于LSM拟合导航路径，并在果园模拟环境进行了测试。二维激光雷达获取信息量少，易受周围环境影响，更多研究采用的是3D LiDAR^[16-18]。刘伟洪等^[19]基于3D LiDAR获取果园三维点云数据，采用基于果树间距的点云分割算法定位树干中心，通过RANSAC算法结合LSM完成导航路径提取，在梨园环境下，系统分别以0.68与1.35 m/s的速度跟踪树行，绝对横向偏差分别不超过21.3与22.1 cm。Liu等^[20]基于3D LiDAR使用RANSAC拟合树行，在稀疏梨园进行了测试，结果显示，其横向偏差小于22 cm，航向偏差小于4.02°。在郁闭果园环境下，GNSS/IMU组合定位导航面临严峻挑战，基于机器视觉的传感器受光照条件变化影响显著，2D LiDAR的环境感知信息量有限；尽管3D LiDAR具备较强环境适应性，但其数据处理量庞大且对算法实时处理能力要求苛刻。

现有果园环境的导航研究主要集中于密集种植场景，鲜见果树间距较大的郁闭果园导航研究。郁闭果园导航存在2个技术难点：首先，果树间距大导致激光雷达探测距离内的果树减少；其次，由于果树间距增大，激光雷达在远距离探测时获取的点云数据密度显著降低，这给基于点云的环境感知和导航控制带来了巨大挑战。这些技术瓶颈使得传统的果园导航方案难以直接应用于大间距郁闭果园环境。

本文针对郁闭果园环境提出一种基于3D LiDAR喷雾机器人的导航方法。首先基于姿态与航向参考系统(Attitude and heading reference system, AHRS)数据对3D LiDAR扫描点云进行地形补偿，其次采用改进的统计滤波方法去除噪声点云，同时保留较远处果树点云，基于自适应距离阈值和聚类体心约束的改进欧式聚类方法获得果树点云的聚类体心；在郁闭果园中，果树的纵向间距往往较大，导致点云聚类体心的位置较为稀疏。这种稀疏的分布使得直接进行导航路径拟合时，系统的抗干扰能力较差。为了提高路径拟合的鲁棒性，本文采用牛顿插值法(Newton’s interpolation, NIL)对点云聚类体心的位置进行处理，在稀疏的体心位置之间生成更平滑、更连续的路径，从而增强路径拟合的稳定性和可靠性。考虑到实际果园环境中存在部分果树偏离标准果树行的特殊情况，使用RANSAC算法进行路径拟合。该算法通过迭代随机采样和模型验证，能够有效识别并剔除偏离主路径的异常点，从而提高导航路径的拟合精度和抗干扰能力，简称为NIL-RANSAC算法；采用线性二次型调节器(Linear quadratic regulator, LQR)实现导航路径跟踪控制。LSM和RANSAC拟合算法在前述学者的研究中有大量应用^[21]，本文应用这2种算法作为对照。最后通过果园测试验证NIL-RANSAC导航方法的精确性和实时性。

1.   试验平台与系统组成

选用湖南耒阳农牧业公司生产的3WWDZ-100型植保机器人作为试验平台。机器人整机净质量110 kg，药液箱最大容量120 L；外形尺寸(长×宽×高)1300 mm×800 mm×1460 mm；整机为电机驱动；动力驱动轮6个；最大行走速度1.1 m/s；转弯半径650 mm；差速转向；满载爬坡坡度12°。搭建的自主导航试验系统包括3D LiDAR、AHRS、光电编码器、嵌入式计算平台和车载控制器等，实物如图1所示。

图  1  3WWDZ-100 型植保机器人试验平台

1：嵌入式计算平台；2：AHRS；3：驱动电机；4：编码器；5：车载控制器；6：3D LiDAR； 7：喷雾系统。

Figure  1.  Test platform of 3WWDZ-100 plant protection robot

1: Embedded computing platform; 2: AHRS; 3: Drive motor; 4: Encoder; 5: On-board controller; 6: 3D LiDAR; 7: Spray system.

下载: 全尺寸图片 幻灯片

嵌入式计算平台采用英伟达的Jetson TX2，最高8 G内存，32 G固态存储器，可以提供每秒1.33万亿次浮点运算(Tera floating point operations per second, TFLOPS)能力，装载Ubuntu 18.04 LTS操作系统；车身姿态传感器AHRS采用凌思科技的LINS16460，横滚俯仰姿态测量精度小于0.3°；光电编码器采用欧姆龙E6B2-CWZ1X，供电电压5 V，分辨率0.35°；车载控制器接收50 Hz的脉冲宽度调制(Pulse width modulation, PWM)信号对喷雾机器人两侧行走电机进行控制；选用禾赛科技生产的Pandar QT 64型短线激光雷达，其具备64个发射和接收通道，能够实现高精度的环境扫描，测量范围广泛，为0.10~60.00 m，测距精度达 ± 3 cm，确保了采集数据的可靠性。此外，该设备的水平角度分辨率为0.6°，能够提供细致的空间分辨率，其垂直视场角达到104.2°，配合最小1.45°的垂直角度分辨率，使得激光雷达在复杂的郁闭果园环境中能有效地探测。系统硬件结构如图2所示。

图  2  系统硬件框图

Figure  2.  Block diagram of the system hardware

下载: 全尺寸图片 幻灯片

系统采用3D LiDAR获取外部环境点云数据；采用AHRS获取喷雾机器人的横滚和俯仰姿态角。Jetson TX2计算平台获取激光雷达数据进行点云地形补偿、数据处理、路径拟合和路径跟踪控制。计算得到的控制量，通过控制器局域网(Controller area network, CAN)发送给喷雾机器人底层控制器。底层控制器根据获得的轮速数据和计算平台发送的期望转速进行PWM，实现喷雾机器人左、右轮速的闭环控制和路径跟踪控制。

2.   激光点云数据预处理

2.1   3D LiDAR安装偏差校正和点云地形补偿

3D LiDAR安装在喷雾机器人前端，离地高度1.33 m，AHRS安装在喷雾机器人前、后轮着地点的几何中心位置上方，AHRS与车身也存在安装偏差，校正方式参考文献[22]，激光雷达安装位置无法确保与车身坐标系重合，如图3所示，需要校正3D LiDAR安装偏差。喷雾机器人在果树行间行走，俯仰侧倾明显。为提高喷雾机器人相对果树行间位置的检测精度，利用AHRS输出的横滚和俯仰姿态角，对激光雷达扫描点云数据进行地形补偿。

图  3  3D LiDAR和AHRS安装示意图

Figure  3.  Schematic diagram of 3D LiDAR and AHRS installation

下载: 全尺寸图片 幻灯片

定义车身坐标系为{C}，激光雷达坐标系为{L}，导航坐标系为{I}，其与车身坐标系具有共同坐标原点，其X-Y平面与大地水准面平行，各坐标系坐标轴指向如图3 所示。

首先对3D LiDAR进行偏差校正。选取2个垂直对立墙面，人工测取得到两侧墙面中线。驾驶车辆沿中线行驶3次，去除地面和上方点云，将获得两侧对立墙面点云投影到{L}系X-Y平面，使用RANSAC算法分别拟合左右墙面点云，获取中间导航线，计算横向偏差$\Delta Y$和航向偏差${y_{{\mathrm{e}}}}$。采集得到校正数据(图4)。

图  4  校正数据采集

Figure  4.  Calibration data acquisition

下载: 全尺寸图片 幻灯片

对采集得到的校正数据取平均值。根据式(1)完成激光雷达安装偏差校正。3D LiDAR和AHRS安装在同一平面，其横滚和俯仰通过AHRS地形补偿完成校正。

${\left( {\begin{array}{*{20}{c}} {{X_{\mathrm{L}}^*}}&{{Y_{\mathrm{L}}^*}}&{{Z_{\mathrm{L}}^*}} \end{array}} \right)^{\mathrm{T}}} $和$ {\left( {\begin{array}{*{20}{c}} {{X_{\mathrm{L}}}}&{{Y_{\mathrm{L}}}}&{{Z_{\mathrm{L}}}} \end{array}} \right)^{\mathrm{T}}} $分别表示校正完成前、后激光雷达点云数据。

在{L}下的坐标转换到{I}，如式(2)所示。

式中，$ \partial 、\beta 、\rho $分别为绕AHRS的Z轴、Y轴和X轴旋转角度。${\left( {\begin{array}{*{20}{c}} {\Delta X_{\mathrm{L}}^{\mathrm{I}}}&{\Delta Y_{\mathrm{L}}^{\mathrm{I}}}&{\Delta Z_{\mathrm{L}}^{\mathrm{I}}} \end{array}} \right)^{\mathrm{T}}}$为激光雷达坐标系到车身坐标系的平移量，$ {(\begin{array}{*{20}{c}} {{X_{\mathrm{I}}}}&{{Y_{\mathrm{I}}}}&{{Z_{\mathrm{I}}}} \end{array})^{\mathrm{T}}} $为转换到车身坐标系下的点云。由于3D LiDAR和AHRS均固定在车体本身，因此$\partial 、\beta 、\rho $和${\left( {\begin{array}{*{20}{c}} {\Delta X_{\mathrm{L}}^{\mathrm{I}}}&{\Delta Y_{\mathrm{L}}^{\mathrm{I}}}&{\Delta Z_{\mathrm{L}}^{\mathrm{I}}} \end{array}} \right)^{\mathrm{T}}}$均已知。

式中，$ \phi 、{{\psi }} $为AHRS输出的横滚和俯仰姿态角。由于AHRS输出航向数据存在累积误差，实际测试发现喷雾机器人在果树行间行驶航向偏差大多为 ± 4°，因此近似认为绕Z轴旋转矩阵为单位矩阵。

完成点云地形补偿后，采用开源的三维点云处理库(Point cloud library, PCL)的直通滤波方法提取扫描点云感兴趣区域(Region of interest, ROI)。

2.2   点云地面点去除

传统方法大都基于点云在高程等方面的变化来区分地面和非地面点云，地面杂草和地面凹凸不平将会对其产生较大影响。2016年，Zhang等^[23]提出了模拟布料下落物理过程的“布料”滤波算法(Cloth simulation filter, CSF)。该方法首先将点云180°翻转；然后假设存在一块黏性的布料，布料受到重力后从上方落下，最终布料上吸附的点云即为地面点云。CSF点云滤波示意图如图5所示。

图  5  CSF点云滤波示意图

Figure  5.  Schematic diagram of CSF point cloud filtering

下载: 全尺寸图片 幻灯片

CSF方法中的动力学基本公式为：

式中，N是模拟“布料”中的粒子在t时刻的位置信息，$ {F_{{\mathrm{out}}}}(N,t) $代表重力、碰撞等其他外部驱动因素，$ {F_{{\mathrm{in}}}}(N,t) $代表粒子间的内部驱动因素。对每个粒子反复施加外力和内力以驱动粒子运动，如果粒子最大高度变化小于设定阈值或者达到最大迭代次数时停止。计算点云与粒子之间的距离，小于设定距离阈值的则被认定为地面点加以去除。

采集果园环境点云数据，使用CSF进行地面点云去除，使用机器人操作系统(Robot operating system, ROS)框架的可视化工具RViz查看地面点云去除效果，对CSF的6个基本参数进行调整。首先，根据果园环境情况将“陡峭斜坡”设置为false，提高模拟的准确性和效率；其次，将布料网格分辨率设为0.5，增强布料细节的表现力，使模拟效果更加逼真；接着，将布料刚性设为2.0，模拟更接近真实布料的物理特性；此外，将时间步长设为1.0，保证模拟过程的稳定性，同时提高计算效率；将距离阈值设为0.4，控制布料模拟中粒子之间的相互作用，避免不必要的计算开销；最后，将迭代次数设为500，确保模拟过程的收敛性和结果的精确性。点云地面点去除效果如图6所示，如图6a所示，地面点云呈现典型的圆弧扫描形态，其分布特征主要受激光雷达扫描几何特性影响。通过高程划分方法处理后的点云如图6b所示，可以观察到地面点云的去除效果存在空间异质性：部分区域点云被有效滤除，而部分区域仍有残留。采集果园数据时发现果园横滚角可达10°，地面平整度较差，基于高程划分的地面点云去除方法难以适应。CSF对地面点云的去除效果如图6c所示，它不但去除了地面杂草点云，也去除了地面点云。通过对采集的500帧果园点云数据进行处理，每帧点云数据处理时间平均为0.03 s，满足实时性去除地面点云的需求。

图  6  地面点云去除效果对比

图b中红框位置为地面及杂草点云；图c中蓝框位置为噪声点云。

Figure  6.  Comparison of ground point cloud removal effects

The red box in the figure b shows the ground and weed point clouds; The blue box in the figure c shows the noise point cloud.

下载: 全尺寸图片 幻灯片

2.3   改进统计滤波方法去除噪声点云

激光雷达在扫描点云过程中会出现噪声点云，主要包含系统噪声、反射后噪声和环境噪声。需要去除噪声，避免在点云聚类和拟合导航路径时产生干扰。其中，环境噪声如树叶随风晃动、树叶掉落、飞虫等是点云干扰的主要来源。统计滤波算法是一种常用的去除噪声点云的方法，基于噪声点云距离目标较远这一特征。通过设定一个固定的邻域范围，计算邻域内每个点与其他点的距离，统计平均值。设定一个距离阈值，将超过阈值的点视为噪声点，从点云中剔除。采用传统的统计滤波公式进行计算。

式中，S_i为邻域内某点到其他点的欧式距离，$ \mu $和$ \sigma $分别为点到邻域点的欧式距离平均值和标准差，$ S $为距离阈值，$ a $为比例系数，$ n $为邻域点云数量。

激光雷达的扫描线从其发光点向外散射，因此点云靠近时激光雷达密集，远离时激光雷达稀疏。本文在点云噪声去除时考虑点云距离因素，提出距离阈值的改进，对式(7)进行改进后得到式(8)。

利用PCL点云处理库中的kdtree数据结构，可以加速搜索点云中最近的K个邻近点。处理采集的果园数据时发现$a = 5$时可以达到较好的滤波效果，如图7所示。由图7可知，改进统计滤波算法保留了前方较远目标果树点云，对比图6c，噪声点云得以较好去除。

图  7  统计滤波方法效果对比

图b中的红框显示的是果树点云。

Figure  7.  Effect comparison of statistical filtering methods

The red box in figure b shows the fruit tree point cloud.

下载: 全尺寸图片 幻灯片

3.   果树行间导航线提取

3.1   改进欧式聚类方法

欧式聚类算法基于欧式距离，将三维空间中的点云数据划分为具有相似特征的簇。其核心思想是利用点与点之间的欧氏距离作为相似性度量，将距离较近的点归为同一簇^[24]。因此适用于郁闭果树的聚类条件，同一棵果树点距离较近，不同果树点距离较远。为实现单棵果树的聚类检测，需要设置距离阈值。点云欧式距离$d({n_1},{n_2}) $计算如式(9)所示。

式中，${n_1}$、${n_2}$为扫描得到的点，$({x_1},{y_1},{z_1})$为${n_1}$点坐标，$({x_2},{y_2},{z_2})$为${n_2}$点坐标。

式中，$\partial 、\beta$分别为激光雷达水平角度和垂直角度分辨率，L为目标探测点到激光雷达的距离，h为相邻激光扫描线距离，d为同一扫描线的相邻点距离，相邻激光扫描线距离和目标物与激光雷达之间的距离有关。激光雷达扫描线的平均垂直角度分辨率为1.65°，水平角度分辨率为0.60°。果园环境中果树间隔5 m左右激光雷达探测目标物的扫描线如图8所示。

图  8  激光雷达扫描线示意图

h为相邻激光扫描线距离，d为同一扫描线的相邻点距离。

Figure  8.  Schematic diagram of the LiDAR scan line

h represents the distance between adjacent laser scan lines, and d represents the distance between adjacent points on the same scan line.

下载: 全尺寸图片 幻灯片

点云欧式聚类的效果易受距离阈值影响。距离阈值设置过小时，会导致同一果树点云被聚类为多个类或者距离较远的目标果树点云无法聚为1类；距离阈值设置过大时，会导致不同目标果树的点云聚类为1类，如图9所示。本文基于3D LiDAR点云扫描特点和三角形不等式条件，设计了一种带有聚类体心约束的自适应距离阈值计算方法，以提高点云欧式聚类效果。

图  9  传统欧式聚类的效果

左侧红框表示2棵果树聚为一类，右侧红框表示较远距离果树点云无法聚类。

Figure  9.  Effect of traditional Euclidean clustering

The left red box indicates that the two fruit trees are clustered into one cluster, while the right red box indicates that the fruit tree point cloud at the farther distance cannot be clustered.

下载: 全尺寸图片 幻灯片

首先对果树目标点云按照5 m欧式距离(芒果树间距为5 m)进行等距分割，然后获得此分段点云的最邻近点距离平均值($\Delta {d_i}$)和在Z轴分布的距离平均值($\Delta {h_i}$)。

式中，${r_i}$为不同分段距离阈值，$ {\lambda }_{i}、{\mu }_{i} $分别为不同分段的最邻近点系数和Z轴分布系数。

计算$\Delta {h_i}$时，为避免果树形状不规则导致垂直方向的点云分布出现较大问题，将点云按照Z轴坐标从大到小排序，合并同一Z轴坐标点；然后将前、后两点Z轴坐标距离大于2h的理论距离点移除；由于车辆运动过程会产生点云畸变，合并小于理论距离点，计算Z轴距离差值；最后求取$\Delta {h_i}$。

由于激光雷达水平角度分辨率小于垂直角度分辨率，因此最邻近点主要为水平扫描获得点云。同时果树点云在空间中3个邻近点满足三角形不等式：任意两边之和大于第三边。因此果树点云在非平面分布条件下可以基于此法则结合欧式聚类距离阈值条件，实现欧式聚类距离阈值的自动获取，如图10所示，可以观察到由于果树树叶的遮挡，部分果树的点云数据呈现出间距较远的情况，导致同一棵果树的点云被聚类为多个独立的聚类集合体。为了解决这个问题，本文提出了一种聚类体心约束方法。

图  10  自适应距离阈值欧式聚类的效果

黄色框表示同一棵果树点云被聚类为2类。

Figure  10.  Effect of Euclidean clustering with adaptive distance threshold

The yellow box indicates that the point cloud of the same fruit tree is clustered into two categories.

下载: 全尺寸图片 幻灯片

该方法首先确定每个点云聚类的体心位置，随后计算这些体心点与最近邻体心之间的距离。若2个体心之间的距离小于预设的距离阈值(在本例中为2.0 m)，则将这2个体心所对应的聚类集合体进行合并。这一合并过程不断重复，直至所有体心之间的距离均大于设定的距离阈值为止。最终，通过这种方式重新获得经过聚类体心约束处理后的点云聚类体心位置坐标。

对果园采集数据选取连续500帧点云合并为bag包文件，运行程序及包文件获取各分段区域$ \Delta {d_i} $以及点云最邻近距离$({d_n})$，将满足95%点云距离设置为目标距离$({d_{si}})$，最邻近点系数$({\lambda _i})$计算公式为：

${\mu _i}$采用与${\lambda _i}$相似的计算方法得到。根据果园环境和果树间距情况，距离设置为5~30 m，间隔为5 m，计算得到的${\lambda _i}$依次为3.33、3.55、2.28、1.51和1.27；${\mu _i}$相应的数值分别为1.24、1.45、1.80、1.27和1.33。

采集果园点云按照时间排序分段选取100帧点云合并为1个包文件，将人工筛选与自适应欧式聚类方法进行对比。其聚类成功率如表1所示。无法聚类的果树点云≥10个，将其计入失败聚类，如图11在24.6 m处果树点云为12个。为避免在5 m分段位置出现2个聚类集合体，将前一次聚类集合体与当前聚类点云集合体融合后输出聚类结果。

表  1  自适应欧式聚类方法果树聚类成功率

Table  1.  Successful rate of fruit tree clustering by adaptive Euclidean clustering method

距离/m Distance	果树数量 Number of fruit trees	聚类成功数量 Number of successful clusters	目标区域成功率/% Target area success rate
5	200	200	100.00
10	222	220	99.10
15	220	208	95.55
20	188	171	90.96
30	175	141	80.57

下载: 导出CSV 

| 显示表格

图  11  改进欧式聚类的效果

红色点为改进欧式聚类方法后的果树点云聚类体心。

Figure  11.  Effect of improved Euclidean clustering

The red points represent the centroids of the fruit tree point cloud clusters after applying the improved Euclidean clustering method.

下载: 全尺寸图片 幻灯片

由表1可知，在15 m以内果树聚类成功率可以达到95%以上。距离激光雷达较近处的点云密集，聚类成功率较高。由于果树被前方果树冠层遮挡使得一侧果树点云稀疏，导致果树无法完成聚类。通过上述测试数据证明了本文提出的自适应欧式距离阈值方法的有效性。

聚类完成后得到${Q_n}$个点云集合体，获得每个集合体的最值横坐标点分别为$\{ {X_n}_{\min },{Y_n}_{\min },{Z_{n\min }}\} $和$\{ {X_{n\max }},{Y_{n\max }},{Z_{n\max }}\} $。根据最值坐标点获得${Q_n}$的体心集合体(${E_n}$)。${E_n}$的体心坐标(X_cn, Y_cn)计算公式如下。果园行间导航主要关注果树树干位置的平面坐标，体心坐标的Z坐标可投影到导航坐标系X-Y平面。

如图11所示，应用了聚类体心约束方法后，原本因树叶遮挡而被分割的果树点云聚类得到了有效的合并，从而提高了点云聚类体心位置的准确性和完整性。

经过上述计算可获得目标果树点云的体心坐标，果树聚类集合体心可近似表示为果树点云树干位置^[18]。欧式聚类方法改进效果如图11所示。同一帧点云下，聚类距离设置过大导致2棵果树被聚类为1个聚类体心，聚类距离设置过小容易导致远处果树点云无法完成聚类。本研究聚类体心点最远达到24.6 m，而传统欧式聚类方法仅可获得11.3 m范围内的聚类体心位置。将获得的果树树干位置信息进行导航路径拟合，较多的果树树干位置信息有助于提升导航路径拟合精度^[25]。

3.2   导航路径拟合

以导航坐标系X轴为分界线，将果树体心坐标分割为左、右2个集合体$ {E_{{\mathrm{l}}n}} $和$ {E_{{\mathrm{r}}n}} $。果树体心位置间距在5 m以上，直接进行直线拟合存在果树体心较少的问题，极易因为噪声干扰导致出现直线拟合偏差过大的问题。使用牛顿插值法对体心坐标插值构建连续可导的导航基准线。以左边部分体心为例进行插值，如下所示。

式中，$ {X_{{\mathrm{l}}0}} $和$ {Y_{{\mathrm{l}}0}} $为左边最近的体心点坐标，$ {X_{{\mathrm{l}}i{\mathrm{max}} }} $为左边最远体心点横坐标，$ {X_{{\mathrm{l}}i}} $和$ {Y_{{\mathrm{l}}i}} $为插值点坐标，$ \tau $为插值步长，设置为0.1 m，果树间距为5 m，插值完成后曲率变化率如式(18)所示。代入参数后得到曲率变化率(C_CR)小于0.01 m⁻¹，满足高精度导航需求。

式中，${k_i}$为第$i$点处曲率，$m$为插值点总数。

插值完成后使用具有较强抗干扰能力的RANSAC算法，对插值完成后的左、右两侧点进行树行拟合。取左、右两侧的中间线进行导航路径拟合。设导航线方程为

式中，${k_{\mathrm{l}}}$和${k_{\mathrm{r}}}$分别为左、右直线的斜率。由于在行间导航喷雾机器人航向偏差很少可以达到 ± 20°，基于此对拟合直线斜率约束在 ± 20°。

插值完成后，导航线拟合效果如图12所示。左侧树行因前方果树冠层遮挡导致点云稀疏，聚类困难，实现了理想的导航线拟合效果。

图  12  果园环境导航路径拟合效果

红点表示聚类体心，绿点表示以0.1 m间隔的插值点。

Figure  12.  Effect of navigation path fitting in orchard environment

The red points mark the centroids of the clusters, the green points represent interpolated points with 0.1 m interval.

下载: 全尺寸图片 幻灯片

4.   路径跟踪控制方法

喷雾机器人为双电机驱动底盘，且满足差速运动学模型，给喷雾机器人发送两侧轮速实现喷雾机器人转向。同时喷雾机器人配备了编码器可实现轮速闭环控制。

LQR是一种差速底盘常用的路径跟踪控制方法^[26]。在差速底盘喷雾机器人的运动学模型中，车头方向与车身坐标系X轴重合，经过时间t后车身位置航向为$ [X\left( t \right),Y\left( t \right),\theta \left( t \right)] $，$\theta $表示喷雾机器人的航向角。

式中，${V_{\mathrm{l}}}$和${V_{\mathrm{r}}}$分别为左、右轮速度，D为喷雾机器人轮距。

差速底盘喷雾机器人在平面运动状态的方程为：

式中，v、$\omega $分别表示机器人行驶线速度和角速度。对状态方程进行离散化处理后得到式(24)：

其中，$ {{\boldsymbol{A}}_{{\boldsymbol{k}}}} $和$ {{\boldsymbol{B}}_{{\boldsymbol{k}}}} $计算如式(25)、(26)所示：

式中，t为采样时间，A_k、B_k分别表示k时刻状态转移矩阵和控制输入矩阵，v_k、$\theta _k $分别表示k时刻机器人前进速度和航向角。

LQR的目标函数求解，此处不再赘述。最大迭代次数设置为500，对LQR的参数进行整定后得到误差权重矩阵Q为85，控制权重矩阵R为2。通过反馈矩阵获得2轮期望速度${V_{\mathrm{r}}}$和${V_{\mathrm{l}}}$实现喷雾机器人路径跟踪控制。

5.   果园试验

试验果园为华南农业大学树木园内(23.16°N，113.35°E)芒果园。芒果树树龄均在20年以上，果树行间距约4.5 m，果树株距5.2~5.7 m。果树树冠相连成片，环境郁闭。随机测量10棵果树，树高3.5~4.5 m，其中树干部分高度为2.1~2.5 m，平均高度约2.3 m。树行普遍呈直线分布，地面被杂草覆盖、坑洼不平。根据果园测量结果，ROI区域X向设置为0~30 m，Y向设置为−3.5~3.5 m，Z向设置为−1.5~1.0 m。

芒果树是直线等距排列种植的，采用人工标定果树行间导航线与颜料标记实际行驶轨迹检验喷雾机器人3D LiDAR导航方法的有效性。选择1条典型芒果树行间，采用卷尺测取其果树行间中心线作为人工标定导航线，标定导航线长度30 m。在喷雾机器人后轮轴中点悬挂颜料，行走过程中自动滴下，实现喷雾机器人的实际行驶轨迹标记，如图13所示。以传统RANSAC和LSM导航线拟合算法为参照，对比测试本文所提NIL-RANSAC导航线拟合算法的有效性，同时验证完整导航方法的有效性。3种导航路径拟合方式各测试3次，共9组测试。每次测试时，喷雾机器人均从标定导航线的同一起点沿相同方向行驶。在30 m标定导航线上，间隔1 m获得测量数据。人工测量颜料位置标记点到标定导航线的距离作为导航跟踪横向误差。沿喷雾机器人行驶方向观察，颜料位置标记点位于标定导航线右侧时，横向误差为正，反之为负。观察寻找30 m标定导航线上的最大跟踪横向误差标记点，测取记录加以评估。间隔1 m采样数据如图14所示。在试验过程中，车辆负载及外部果园环境均未发生变化，说明对比试验有效。

图  13  喷雾机器人标定导航线(a)和悬挂颜料位置(b)

Figure  13.  Calibration of navigation lines (a) and suspension position of paint (b) in vehicle

下载: 全尺寸图片 幻灯片

图  14  不同算法的间隔1 m测量数据

Figure  14.  Measurement data with 1 m interval of different algorithms

下载: 全尺寸图片 幻灯片

统计结果(表2)表明，使用RANSAC算法得到3组数据的导航横向偏差的最大绝对值为0.32 m，LSM算法为0.42 m，NIL-RANSAC为0.26 m，说明横向偏差方面，NIL-RANSAC 优于RANSAC，同时RANSAC优于LSM。

表  2  测量数据统计

Table  2.  Measurement data statistics

导航线拟合方式 Fitting method of navigation line	编号 No.	最大横向偏差绝对值/m Absolute value of maximum lateral deviation	横向偏差均值/m Average lateral deviation	标准差/m Standard deviation	算法平均耗时/ms Average time taken by algorithm
RANSAC	1	0.28	−0.04	0.13	90
	2	0.30	−0.06	0.13	95
	3	0.32	−0.05	0.12	93
LSM	1	0.34	0.09	0.16	110
	2	0.42	0.07	0.17	108
	3	0.38	0.11	0.16	115
NIL-RANSAC	1	0.25	−0.03	0.08	95
	2	0.23	−0.05	0.09	96
	3	0.26	−0.04	0.09	94

下载: 导出CSV 

| 显示表格

间隔1 m测量数据中，LSM算法的横向偏差均值最大，其他2种算法横向偏差均值则相对较小。RANSAC算法的横向偏差标准差为0.13 m，LSM算法则为0.16 m，相比之下，NIL-RANSAC算法的标准差仅为0.09 m，较小的标准差意味着横向偏差数据点相对集中，波动范围较小，反映了车辆行驶的稳定性。不论是最大横向偏差还是标准差，NIL-RANSAC算法的均比RANSAC和LSM算法小，这证明了NIL-RANSAC算法的精确性和适用性。RANSAC算法较LSM横向偏差数据小，说明RANSAC算法的拟合导航线方式比LSM更好。在果园环境中，树枝随风飘动、树冠层遮挡以及部分果树偏离果树行等外部干扰因素不可避免，导致采集数据出现异常噪声，从而影响导航路径的拟合精度。NIL-RANSAC算法通过获取果树聚类体心位置，能够相对固定地剔除聚类集合体外部的异常噪声，提高导航路径的稳定性和可靠性。RANSAC算法则通过迭代随机采样和模型验证，有效识别并剔除偏离主路径的异常点，在处理异常值方面具有显著优势，能够有效提高导航路径的拟合精度。相比之下，LSM算法在处理异常值时较为敏感，当存在由风引起的噪声或遮挡导致的异常点时，LSM算法的拟合结果可能会受到较大影响，从而降低导航路径的精度。

在算法平均耗时方面，ROS可以获得的系统时间，其精度级达到μs。在获得点云数据进行处理时记录当前时间，发送完成底盘控制量后再次记录当前时间，将2次时间相减后得到整个算法处理时间。对文件数据处理后发现，NIL-RANSAC和RANSAC算法完整导航的平均耗时不超过100 ms，满足导航系统的实时性要求。

3次试验偏差不一致，导航路径跟踪误差存在波动，一方面是因为真实果园路面长有杂草且不平整，喷雾机器人体型较小、质量较轻，差速转向容易产生滑移；另一方面，喷雾机器人在行进过程中产生的滑移具有时变性，偏离果树行中心线，左、右侧果树目标点云获取不均衡，果树行拟合中心线有波动，也容易产生导航路径跟踪误差。

6.   结论

本文针对郁闭的芒果园环境，以轮式喷雾机器人为试验平台，设计了基于3D LiDAR的导航方法，主要结论如下：

1)使用CSF方法在郁闭果园环境中可以有效去除杂草和凹凸不平的地面点云，避免地面点云影响目标聚类和路径拟合，并且其处理平均时间仅为0.03 s，满足果园导航实时性去除地面点云的需求。基于点云欧式距离设计的改进的统计滤波方法能够避免较远果树点云被当作噪声点去除。

2)基于激光雷达扫描特性和空间点云距离的三角不等式，设计了自适应距离阈值确定方法，该方法在15 m范围内果树点云聚类成功率超过95%，增强了距离阈值的自适应能力。同时，采用聚类体心约束策略，确保了每棵果树点云聚类体心的唯一性。相较于传统欧式聚类方法，改进后的方法展现出对果树聚类的较强适应性，目标聚类范围扩大了13.3 m。

3)基于点云聚类体心的NIL-RANSAC算法拟合导航线，比传统的RANSAC和LSM算法具有更好的导航效果。NIL-RANSAC算法导航横向偏差最大绝对值为0.26 m，标准差为0.09 m，验证了NIL-RANSAC算法拟合导航线的适用性。本文所提出的3D LiDAR导航方法平均耗时不超过100 ms，可以满足郁闭果园环境对喷雾机器人导航的实时性要求。

针对试验研究过程中遇到的问题和论文成果的局限性，特提出如下改进方向：

1)喷雾机器人作业过程中，药液载荷的时变递减特性将引发质量参数非线性衰减、质心位置动态偏移及轮胎−地面耦合关系改变，显著降低路径跟踪控制精度。针对药液时变载荷对运动控制的影响，可考虑构建多传感器信息融合的载荷衰减观测器，实现数据驱动的载荷衰减轻量化补偿；同时，设计基于地形适应度的前馈−反馈双模控制器，根据激光雷达点云熵值动态调节控制器参数，提升系统对药液时变载荷的控制适应性，确保施药作业的稳定性和均匀性。

2)针对果园喷雾机器人除草施药作业的特定需求，开发基于作业状态机的点云处理引擎，通过时间−空间编码实现导航点云与原始点云的按需切换；集成多光谱成像模块，构建杂草特征波长数据库，结合深度学习模型实现除草靶标识别；优化CSF滤波参数动态调节策略，在导航点云中稀疏保留地表特征点，为施药决策提供空间拓扑参照。该技术改进有助于实现果园喷雾机器人导航安全性与作业精准度的协同优化。