基于激光雷达的农业机器人果园树干检测算法

牛润新 张向阳 王 杰 祝 辉 黄 健 陈正伟

(1.中国科学院合肥物质科学研究院， 合肥 230088； 2.中国科学技术大学， 合肥 230026)

0 引言

随着我国农业机械自动化水平的不断提高，农业机器人相关领域的研究也越来越受到重视[1-2]。果园生产管理过程中的耕作[3]、修剪[4]、施药[5-6]、除草[7]、采摘[8-9]等活动都离不开农业机器人对果园环境的感知[10]。机器人在果园这种非结构化环境中作业，首先要知道果树的准确位置[11-12]。激光雷达作为一种新兴的导航定位传感器，具有精度高、实时性好、全天候等特点[13-15]，在农业机器人环境感知研究中得到了广泛应用。

文献[16]将二维激光雷达垂直安装，通过设定高度阈值排除树冠点及地面点，并采用隐半马尔可夫模型进行结果优化，最终树干信息提取准确率达89%，由于激光雷达一次只能提取一棵树干信息，故检测范围较小，效率较低。文献[17]将二维激光雷达水平安装，利用果树行间距固定的特点，通过计算激光雷达数据中的内凹点，将成等差数列的数据点视为树干点，由于依赖行间距，故对株距不固定或果树缺失情况下果树检测的适应性不佳。文献[18]利用二维激光雷达，采用K-均值聚类算法，通过斜率变化的方法确定聚类数，从而实现对立木的识别，在3种不同树干分布下准确率达95.5%，但试验在室内进行，未考虑杂草及其他障碍物等干扰因素。文献[19]通过改进的基于密度聚类(Density-based spatial clustering of application with noise, DBSCAN)算法对果园树干进行检测，并通过航位推算模型排除地面干扰，但算法复杂、计算量较大，且同样未考虑杂草对果树检测的影响。

针对现有果树检测方法存在的计算量大、未考虑杂草等影响、适应性较差等问题，本文提出一种基于自适应密度聚类的树干检测算法。首先，将激光雷达获取的数据点进行数据预处理，排除噪声点及不可利用的数据点，并根据数据点到激光雷达的距离自适应设定密度阈值，进行初步聚类；然后，利用地面类中的数据点过多以及数据点之间趋于直线的特点，通过最小二乘法进行圆的拟合，并设定半径阈值，将地面类剔除；最后，利用杂草枝叶类中的数据点之间距离不连续的特征，将杂草枝叶类剔除，完成对果园中果树树干的检测。

1 研究平台与方法

1.1 研究平台介绍

本研究以中国科学院合肥物质科学研究院应用技术研究所自主研制的履带式农业机器人为平台，所用传感器为德国SICK公司生产的LMS111型单线激光雷达，其扫描分辨率为0.5°/0.25°，扫描频率为25/50 Hz，扫描角最大为270°，最大检测距离为20 m，当超出最大检测距离时，返回距离为0。激光雷达在机器人的正前方水平安装，扫描平面距离地面约350 mm。激光雷达传感器经Ethernet网络接口将采集的数据传输至上位机，由上位机进行数据处理，最终结果通过RS-232接口发送至下位机，下位机也可通过RS-232接口接收遥控器信号，输出脉宽调制(Pulse width modulation, PWM)信号到电动机驱动器驱动电动机，实现机器人行走，如图1所示。

图1 研究平台系统结构图Fig.1 System structure diagram

定义机器人的局部坐标系原点与激光雷达位置重合，y轴正方向指向机器人前进的正前方，x轴正方向与机器人前进方向垂直向右。二维激光雷达采集的数据点qi以极坐标(di,θi)的形式表示，为了方便计算，将极坐标转换为笛卡尔坐标形式(xi,yi,θi)，计算公式为

(1)

式中di——数据点qi到激光雷达的距离

θi——数据点qi和激光雷达的连线与x轴正方向的夹角

在激光雷达返回的数据点中，任意2个数据点qi和qj之间的距离di,j为

(2)

1.2 数据预处理

激光雷达在采集数据过程中，发射器的光束通常会经过多次反射和折射，为了降低干扰，激光雷达拒收超过一个波长时间段的反射光，但在结果中仍然会包含一些孤立噪声点[20-21]，这些噪声点并不是真实测得的数据信息，它们不仅会对激光雷达数据处理造成一定程度的干扰，而且会增加数据处理的时间，因此在数据处理之前应将其滤除。另外随着测量距离的增加，激光雷达数据点变得稀疏，所能提供可利用的有效信息变少，参考意义不大，在数据处理之前也应将其滤除。

针对以上问题，在获取激光雷达数据后，选取机器人正前方180°的数据点并进行遍历，将距离大于10 m的数据点滤除，然后在保留的数据中分别计算当前数据点与相邻数据点之间的距离，并将与左右相邻数据点距离都较远的数据视为无效噪声点，也将其从原始数据中滤除。如图2所示，矩形框中的数据点距离激光雷达较远，无法提供可利用的信息，视为无效点；圆形框标记的数据点与相邻的数据点均较远，视为噪声点。

图2 数据滤波示意图Fig.2 Data filtering diagram

1.3 基于距离的自适应密度聚类

常规密度聚类算法DBSCAN可以有效地寻找到被低密度区域分割的高密度区域，在具有噪声的数据中发现任意形状的簇，但需提前确定聚类半径和邻域内点的数量2个全局参数，且聚类效果对其设定的全局参数值较为敏感[22-23]。由二维激光雷达的基本原理[24-25]可知，激光雷达数据点的密度随数据点到激光雷达的距离增加而降低。考虑到果园中杂草和枝叶干扰的情况，若直接使用DBSCAN 聚类算法，当检测距离较近时，数据点密度较高，聚类半径相同的情况下，会存在距离较近且较细的杂草枝叶与距离较远且直径较大的树干相比接收到的数据点数量接近甚至更多的现象，从而出现大量误判与漏检。因此，为得到更好的聚类效果，提出一种基于自适应密度聚类算法。

为提取出树干信息，以树干为主要目标设定聚类半径，设果园中最细的果树树干直径为Dmin，则聚类半径为Dmin/2。激光雷达相邻数据点之间的理论距离h与激光雷达的角度分辨率φ及数据点到激光雷达的距离d之间的关系为

h=dtanφ

(3)

则理论上，以Dmin/2为聚类半径的圆内所接收到数据点的个数n为：n=Dmin/h=Dmin/(dtanφ),考虑到树干的横截面为圆形，则聚类的数据点数目阈值设为

(4)

当以数据点qi为中心，以Dmin/2为半径的圆内数据点的数目ni≥Nmin时，则该点为核心点，其周围的点为边界点。从激光雷达第1个数据点开始依次遍历所有数据点，找出所有的核心点。由于激光雷达数据点在一个平面内是按逆时针顺序展开，为了加快搜索速度，提高计算效率，寻找核心点时只需要计算当前数据点与其一定范围内的数据点的距离即可。搜索半径为Dmin/2，则计算与该点左右相邻的点的个数为：mi=Dmin/(2h)=Dmin/(2ditanφ)，计算出所有核心点后，随机选取一个核心点初始化队列Q，并取出队列中的数据点q，如果数据点q为核心点，则将以该点为核心的边界点加入到队列Q中，依次取出队列Q中的数据点直到队列Q为空，此时，所有访问过的数据点生成一个新的聚类簇。然后随机取出另一未访问过的核心点初始化队列Q继续计算并生成新的类簇，直到所有的核心点均被访问,完成数据的初步聚类。

1.4 地面类的排除

图3 地面聚类示意图Fig.3 Ground clustering diagram

由于丘陵果园中存在斜坡，或者由于路面颠簸，激光雷达可能扫描到地面，从而影响果树检测的准确率，因此必须排除地面类的干扰。如图3所示，当激光雷达扫描地面时，有可能将地面聚为一类，但地面类数据点具有连续、量大且大致呈一条直线的特点，利用此特点，可以将地面类排除。

根据类中第1个数据点qf和最后一个数据点qe的中心计算该类的中心点坐标

(5)

式中Gx——类中心点横坐标

Gy——类中心点纵坐标

df——数据点qf到激光雷达的距离

de——数据点qe到激光雷达的距离

θf——数据点qf与x方向夹角

θe——数据点qe与x方向夹角

果园中果树树干最大直径为Dmax，则根据类中心到激光雷达的距离dg计算类中数据点的数目最大值为nm=Dmax/h=Dmax/(dgtanφ)。考虑到果园中地面不平整，激光雷达扫描平面与水平面可能存在夹角，此时树干接收到的数据点变多，因此设定类内数据点数目的阈值Nm=1.5nm。如果类内数据点数量Nc≥Nm，则认为该类为地面类。如果类内数据点数量Nc

R2=(x-A)2+(y-B)2=
x2-2Ax+A2+y2-2By+B2

(6)

式中R——拟合圆半径

A——拟合圆圆心横坐标

B——拟合圆圆心纵坐标

令a=-2A，b=-2B，c=A2+B2-R2，得到圆曲线方程的另一种表达形式为

x2+y2+ax+by+c=0

(7)

根据最小二乘法原理，求目标函数F(a,b,c)的最小值

(8)

将F(a,b,c)对a、b、c求偏导，求得最小二乘解为

(9)

由于地面类数据点更趋向于直线，如果2R≥1.5Dmax，则认为该类为地面类，并将此类从数据中剔除。

1.5 排除杂草及枝叶的干扰

理想的果园环境中，排除地面背景后的剩余类即为果树类，但丘陵果园环境比较复杂，常伴有少量较高的杂草和较低的果树枝叶。尽管选择合适的激光雷达安装高度可减少杂草或枝叶的干扰，但却不能完全避免，因此须考虑杂草和枝叶干扰的情况。

由于单个枝叶宽度远小于树干直径，相同的聚类半径内其返回数据点的个数必定小于聚类的阈值，因此在初次聚类的过程中已将其排除。而连续的几个枝叶在同一平面的投影长度可能大于树干的直径，因此在初次聚类的过程中有可能将其聚为一类。为准确提取出树干的位置信息，必须将杂草及枝叶类排除。观察实际杂草及枝叶的形态可知，尽管其在同一平面的投影连续，但其纵向枝叶间存在一定间距，反映在数据中则会出现相邻数据点之间的距离产生突变、不连续的现象(图4)，因此可利用此特点排除杂草及枝叶类的干扰。

图4 杂草枝叶聚类示意图Fig.4 Schematic of weeds and foliage clustering

在初次聚类结果中依次计算相邻数据点的实际距离，当存在相邻2个数据点的距离大于给定的阈值时可判定此类为杂草或枝叶类。由于树干为柱形且表面凹凸不平，设定类中相邻数据点的距离阈值为

Ti,i+1=2h=2dmtanφ

(10)

式中dm——数据点qi和qi+1到激光雷达距离的平均值

如果类中存在2个相邻数据点之间的距离di,i+1≥Ti,i+1，则认为该类为杂草或枝叶类。另外，对于存在个别枝叶间的纵向距均小于阈值Ti,i+1的情况，由于树干的横截面呈类圆形，在其表面分布的激光雷达相邻数据点之间的斜率逆时针方向存在一次由正向负的变化，而由多个杂草或枝叶形成的类中由于枝叶间的交错，必然存在多次相邻数据点之间斜率正负的变化。同一类簇中相邻数据点之间的斜率为

(11)

如果类中相邻数据点之间的斜率正负变化次数大于等于3次，那么该类同样被认为是杂草或枝叶类，并将此类从数据中剔除。

2 试验与结果分析

2.1 试验设计

为检验自适应密度聚类算法对于树干检测的效果以及利用特征点对地面和杂草等干扰排除的效果，针对四川省阆中杨氏果业生产应用示范基地中的柠檬(Citruslimon(L.)Burm.f)园中存在斜坡且树干较为明显的特点，以安徽省合肥市大蜀山森林公园及周边的香樟树园为试验场地(由于季节和实验条件原因，以此试验地模拟)，树木为香樟树(Cinnamomumcamphora(L.) Presl)，试验中最大坡度为20°，选择的树高3～5 m，枝下高1.2～2 m，株距约3 m，环境中主要的干扰为少量较高的杂草以及斜坡，杂草主要为野生的草本植物以及少量的野生小灌木。为检验该方法对树干检测的准确性以及在存在地面及杂草干扰情况下的适应性，试验场景主要分为4种类型，分别为只存在树干且无干扰的平整地形场景、存在树干和斜坡的场景、存在树干和杂草干扰的场景、存在树干及斜坡和杂草干扰的场景。

试验中，设置激光雷达角度分辨率为0.5°，扫描频率为25 Hz，履带机器人行驶速度0.2 m/s，试验平台及场景如图5所示。

图5 试验平台及场景Fig.5 Experiment platform and scene1.上位机 2.履带式农业机器人 3.激光雷达

2.2 聚类及干扰排除试验

运用基于距离的自适应密度聚类方法，在只存在树干且无干扰的平整地形环境下进行试验，用以检验在无干扰情况下算法对树干检测的准确率。试验中每隔0.5 m取出一次完整数据帧，共选取50帧数据进行聚类分析。

首先进行数据预处理，将噪声点以及不可利用的无效数据点去除，然后利用基于自适应密度聚类方法进行树干检测。聚类结果如图6所示，其中圆形标记的类为真实场景中存在的树干(下同)，统计聚类后结果中的树干数目nc以及真实场景中树干的数目nc，并计算对果树的漏检率与误检率。在选取的50帧数据中共存在真实的果树263棵，出现误检的有2棵，误检率为0.76%，出现漏检的有5棵，漏检率为1.90%，无其他干扰情况下的树干检测平均正确率为97.3%。当数据点在较粗的树干上出现噪声，在数据预处理时噪声点被滤除，树干上的数据点被滤除的噪声分隔开，因此树干被聚为两类，从而出现误检的现象。当激光雷达位置靠近树行的一侧且激光雷达正前方与树行平行时，由于角度原因，接近激光雷达的树干对远离激光雷达的树干进行了部分遮挡，导致被部分遮挡的树干出现了漏检，随着履带小车的移动，激光雷达与树干之间的角度不断变化，漏检的现象消失。

图6 无干扰时的聚类结果Fig.6 Clustering results without interference

2.3 地面干扰排除试验

在存在树干和斜坡的场景中进行试验，选取20帧同时具有树干和地面点的数据帧进行聚类及地面干扰的排除。图7为聚类后地面干扰排除前的结果，其中椭圆标记的类为真实场景中的地面(下同)，图8为地面干扰排除后的结果。在选取的20帧数据中共存在真实的果树76棵，其中出现了3次将地面误判为果树的情况，在存在地面干扰情况下的正确率为96.1%，误判是由于地面中的石块等突然凸起的物体导致的误检。

图7 只存在地面干扰时的聚类结果Fig.7 Clustering results with only ground interference

图8 地面干扰排除后的结果Fig.8 Results after ground interference removal

2.4 杂草干扰排除试验

在存在树干和杂草干扰的场景中进行试验，选取20帧同时具有树干和杂草干扰的数据帧进行聚类及杂草干扰的排除。图9为聚类后杂草排除前的结果，其中矩形标记的类为真实场景中的杂草(下同)，图10为杂草排除后的结果(存在一处杂草未被排除的情况)。在选取的20帧数据中共存在真实果树93棵，其中出现了8次将杂草误判为果树的情况，在存在杂草干扰情况下的正确率为91.4%，误判是由于数据点恰好全部落在最靠近激光雷达的杂草表面以及杂草过密所造成的。

图9 只存在杂草干扰时的聚类结果Fig.9 Clustering results with only weed interference

图10 杂草干扰排除后的结果Fig.10 Results after weed interference removal

2.5 综合试验

为检验在同时存在地面和杂草干扰情况下树干检测的准确率，选取10帧同时具有地面和杂草干扰的数据帧进行干扰的排除。图11为干扰排除前的聚类结果，图12为干扰排除后的结果。在选取的10帧数据中共存在真实果树37棵，其中出现了3次将地面或杂草误判为果树的情况，在同时存在地面和杂草干扰情况下的正确率为91.9%。

在选取的全部100帧数据中，共存在真实果树469棵，误判的次数共21次，综合平均正确率为95.5%，该算法可用于地面平整度低且存在杂草干扰的丘陵果园中的树干检测。

图11 存在杂草和地面干扰时的聚类结果Fig.11 Clustering results in presence of weeds and ground interference

图12 地面和杂草干扰排除后的结果Fig.12 Results after ground and weed interference removal

3 结论

(1)主要分析了在存在斜坡及杂草干扰的环境中，利用二维激光雷达获取环境信息、并基于自适应密度聚类的树干检测算法。数据预处理中，在排除噪声点及无法利用的数据点后进行聚类，然后利用地面及杂草数据点的特点排除对树干检测的干扰。

(2)试验表明，在无干扰情况下，对树干的误检率为0.76%、漏检率为1.90%、平均正确率为97.3%；在只存在地面干扰的情况下，树干检测平均正确率为96.1%；在只存在杂草干扰和同时存在地面和杂草干扰的情况下，树干检测平均正确率分别为91.4%和91.9%；综合以上各种情况的树干检测平均正确率为95.5%，说明在具有干扰情况下树干检测正确率较高，对存在斜坡及杂草干扰的乔化果园环境适应性较强，可用于丘陵地区树干较明显的乔化果园中的树干检测。由于单线激光雷达传感器单一、信息量较小，对于误判情况及更细果树的检测，后续研究中可融合视觉进行检测。