基于欧氏聚类的三维激光点云田间障碍物检测方法

尚业华 张光强 孟志军 王 昊 苏春华 宋正河

(1.中国农业大学工学院， 北京 100083； 2.北京市农林科学院智能装备技术研究中心， 北京 100097；3.农业农村部农业机械化技术开发推广总站， 北京 100122)

0 引言

近几年国内对农机自动驾驶技术的需求不断增长，而已应用的农机自动驾驶技术主要是辅助驾驶和特定地块基于路径规划的自动驾驶。要实现全场景的无人驾驶，田间障碍物检测是首要的环境感知技术[1]。目前主要的障碍物检测有基于视觉的环境感知方案、基于超声波与毫米波雷达的感知方案和基于激光雷达等的感知方案[2-3]。其中基于视觉的环境感知方案由于受环境光影响较大且在夜晚不能保证感知精度，单独用于自动驾驶中的田间识别与避障仍有一定的局限性,需融合其他传感器[4]。基于三维激光雷达对农田环境的障碍物检测研究较少，目前农机领域研究较多的为二维激光雷达，主要应用在障碍物的距离判别，但因输出信息为二维点，不能实时检测障碍物的尺寸、形状等信息[5-8]。因此探索采用三维激光雷达对农田环境障碍物进行检测具有重要的科研与应用意义。

在对农田障碍物检测方面，国内外已有相关研究报道[9-20]。目前对农田环境障碍物检测的方法主要有基于单目、双目和多视角几何进行图像特征与深度学习的方法。其中基于单目视觉的障碍物检测主要有基于特征和基于学习的方法，基于双目和多视角几何的检测对环境进行三维重建以获取深度信息；另有二维激光雷达与视觉结合或采用Kinect获取图像和深度信息进行障碍物的检测方法。但此类以视觉为主导的障碍物检测方案受光照情况影响较大，在夜间作业时检测效果大幅下降。

三维激光雷达因为探测距离较远且精度高、受环境光变化的影响小等而成为道路车辆自动驾驶方案的重要传感器之一，其在道路车辆自动驾驶中主要负责检测道路车辆、行人等障碍物[21-25]，但在农业领域的研究主要在作物表型方面[26]，在农机自动驾驶领域应用较少[27]。近两年随着农机自动驾驶功能需求的增加和三维激光雷达成本的下降，基于三维激光雷达在农机自动驾驶方面的研究逐渐增多。文献[28]以拖拉机为载体设计了基于激光雷达的农田环境点云采集系统。文献[29]通过安装2个二维激光雷达在不同的位置获得不同的扫描信息并进行融合来进行障碍物的检测。文献[30]采用基于栅格的方法识别三维点云中的障碍物信息，但对尺寸不一、形态各异的田间障碍物并不具有适应性。目前三维激光雷达在农田环境障碍物检测方面的研究还处于初始阶段，但由于其不受光照条件影响，且三维激光雷达相较于图像方法或二维激光雷达有更丰富的深度特征信息，为农田环境障碍物检测的准确性和可靠性提供了更多的信息支持。

针对目前农田环境障碍物检测的问题，本文提出一种基于三维激光点云聚类的田间障碍物检测方法。本文拟对三维激光雷达扫描的包含农田障碍物的点云进行滤波、地面分割和聚类，针对农田地面特性对聚类后的障碍物点云进行滤波，对聚类得到的障碍物点云添加长方体框来标识，通过对多帧田间障碍物点云进行检测来验证方法的有效性。

1 材料与方法

本研究使用的雷达为速腾聚创公司的RS-LiDAR-32型三维激光雷达。其激光发射线束为32线，垂直视场角-25°～15°，垂直角分辨率非线性分布，水平视场角360°，水平角分辨率0.1°，测距范围0.4～120 m(目标反射率20%)，最大测距200 m，测距精度±3 cm，输出点云约每秒60万点。

在进行点云处理时使用Ubuntu 16.04系统平台，配合雷达本身的ROS驱动，采用点云库PCL 1.8.0的部分库函数，用C++语言进行处理。在对激光点云进行田间障碍物检测时主要有滤波、地面分割和聚类几个环节。其分别使用体素栅格滤波下采样、感兴趣区域分割、基于RANSAC算法的地面点云检测与分割、基于K维树(K-d tree)最近邻搜索的欧氏聚类等方法检测出障碍物点云，并采用PCL点云库函数进行点云的渲染，用长方体框标识出。具体的点云处理流程如图1所示。

图1 点云障碍物检测流程图Fig.1 Point cloud obstacle detection flowchart

1.1 体素栅格下采样

由于三维激光雷达的点云数据量较大，在对点云数据进行处理时需先进行滤波，对点云进行体素栅格下采样，即在不损失点云重要特征信息的情况下对点云进行减量，从而加速后续处理计算。

体素(Voxel)概念上类似于二维图像像素，表示在三维空间中一个微小的立方体。体素栅格为微小的空间三维立方体的集合，其将三维点云进行空间栅格划分，即一个点一定属于某一个体素，一个体素可能包含零至多个点，如图2所示。

图2 体素栅格示意图Fig.2 Voxel grid diagram

本文采用的体素栅格下采样方法首先为输入的点云数据创建一个三维体素栅格，然后在每个体素(即三维立方体)内，用体素内所有点的质心来近似显示体素中其他点。这样经过体素下采样滤波后，减少了点云数量，提高了后续算法处理速度，同时由于采用体素内所有点的质心点表示，相比于用体素中心来逼近的方法能更好地保留点云的特征。

1.2 感兴趣区域点云分割

由于三维激光雷达扫描的范围较大，而农机行驶过程中一般只对前方行驶区域的障碍物感兴趣。所以对下采样滤波后的点云框选出一个感兴趣区域，删掉区域外的点云，对区域内的点云进行后续处理，减少处理时间，增加精度。

选定的感兴趣区域是在三维空间中框选出一个三维立方体，此立方体为包围雷达发射中心的一个三维区域。在选取感兴趣区域时立方体的长边为将要探及障碍物的距离范围，综合考虑农田常用动力机械的车速、刹车距离以及车身四周的预留空间，长边的长度为农机前方2倍的刹车距离和农机后方2个车身的长度之和。感兴趣区域的宽度为以农机为中心，左右两侧共计2个农机作业幅宽的宽度。感兴趣区域的高度为探及障碍物的高度范围，以激光雷达所在高度为零点，包括上下2部分范围。同时由于雷达在水平视场角360°扫描，感兴趣区域内包含农机本身的点云，其在后续聚类过程中会造成误聚类，故在感兴趣区域内将农机本身的点云去除，为方便划分，该区域选取为农机外接矩形的尺寸，如图3所示。

图3 感兴趣区域划分示意图Fig.3 Region of interest segment diagram

1.3 基于RANSAC算法的地面分割

经过滤波处理的点云在进行障碍物检测前需先将地面的点云去除，因此要基于点云进行地面的检测。由于农田地面并不像机动车道路一样平坦，其土质及颗粒尺寸变异较大，在农田地面点云附近会有一些噪点。根据高度信息采用一般的直通滤波不能很好地将地面去除，最小二乘拟合是从整体误差最小的角度考虑，容易因离群点使得拟合面与真实面的误差较大，因此本研究应用随机采样一致性(Random sample consensus, RANSAC)算法进行地面检测。

RANSAC算法是一种通过对观测数据进行随机采样来估计模型参数的学习算法。其假设给定数据集同时包含模型内点和模型外点，在初始时随机选取一个原始数据的子集，并假定其为待拟合模型，然后针对该拟合模型测试所有其他数据，当数据满足误差阈值时即认为其为该模型共识集的一部分，随后将随机选取点假定为待拟合模型得到共识集，进行一定次数的迭代后判断共识集内点数最多的模型即为待拟合模型结果。

将RANSAC算法应用在三维点云的地面检测中，将检测出的地面与待聚类的障碍物点云区分，防止后续聚类过程中误将地面点云检测为障碍物。在算法设计时首先设定算法的迭代次数为I，距离误差阈值为ΔT1，点云的总点数为N，在初始时随机选取3个点构成待拟合地面，设3个点的三维坐标为(X1，Y1，Z1)、(X2，Y2，Z2)、(X3，Y3，Z3)，则拟合的平面模型为

Ax+By+Cz+D=0

(1)

其中

A=(Y2-Y1)(Z3-Z1)-(Z2-Z1)(Y3-Y1)
B=(Z2-Z1)(X3-X1)-(X2-X1)(Z3-Z1)
C=(X2-X1)(Y3-Y1)-(Y2-Y1)(X3-X1)
D=-(AX1+BY1+CZ1)

则空间任一点(X0，Y0，Z0)到该平面的距离L为

(2)

当某点与此次假设平面的距离L≤ΔT1时，则此点为模型内点。依次遍历除初始采样的3点之外的其他N-3个点，记录此模型的内点数目。

再随机采样3个点构造一个平面模型，按照同样的方法得到该模型的内点数目。按照此随机采样的方法迭代I次。随着迭代次数的增加产生合理结果的概率亦随之增加。最终基于每个模型的内点数目表决，选取内点数目最多的地面模型即为最佳拟合结果。

在应用该算法进行地面拟合的设计阶段，为验证其有效性，在Matlab中进行了测试，对随机生成的高斯分布的三维点云进行平面拟合，如图4所示。经试验该算法可以很好地对有离群值的三维点云进行地面拟合。

图4 三维点云RANSAC算法平面拟合示意图Fig.4 3D point cloud RANSAC test diagram

1.4 基于K-d tree最近邻搜索的欧氏聚类

经过上述滤波和田间地面点云分割后，其余点云即为田间障碍物点云，将此点云进行基于K-d tree最近邻搜索的欧氏聚类处理检测出障碍物。

1.4.1K-d tree的三维应用

K-d tree是K维空间中划分点的一种数据结构[31]，其常用于多维空间中关键数据的搜索。该数据结构将空间划分以便提高搜索效率，其每个节点所在超平面将K维空间划分为2部分。图5为K-d tree在三维空间中的划分示意图。图中的红框垂直平面为第1次分割，将空间切割为2个子单元格，然后将每个子空间由绿色水平平面分割为2个子单元格。最后，将4个单元格分别由4个蓝色垂直平面分成2个子单元格。

图5 K-d tree 三维示例Fig.5 K-d tree three-dimensional diagram

K-d tree形似二叉搜索树，本研究将其应用在三维点云处理中，故K-d tree中的K=3，在确定每个节点的关键值时依照下面的原则：在树的第n层(n对3取余的数值序号是该层中节点关键值的序号)，在每个节点以该关键值建立一个超平面，且超平面垂直于该关键值的轴，该超平面把空间分为2部分，左侧的点由该节点的左子树表示，右侧的点由右侧的子树表示。在对每帧点云数据建树过程中根节点不同，根节点为每帧数据的第1个点，当给定根节点后其余点即可按此规则挂载建立K-d tree。为便于理解该方法，对三维K-d tree举例说明，如图6所示。

华北多特高压交直流强耦合大受端电网系统保护方案设计//罗亚洲,陈得治,李轶群,王青,张剑云,訾鹏,等//(22):11

图6 K-d tree 示例Fig.6 A K-d tree example

图6中红色数字为该层节点的关键值，蓝色数字为与上一层关键值比较的数值。由图6可见，在根节点(3，5，6)取第1维3为该层关键值，(2，1，8)中的2小于3，故在其左子叶，(4，5，6)中的4大于3，故在其右子叶；在第2层取第2维为该层的关键值，下一层的(2，0，5)中0小于(2，1，8)中的1，故在下一层的左子叶，(2，3，7)中的3大于(2，1，8)中的1，故在下一层的右子叶。后续依此类推，首先确定该层节点的关键值，若数据点同一维度的数值小于该节点的关键值则在该节点的左子树，反之则在右子树。其在空间的划分如图7所示。

图7 K-d tree示例在三维空间的划分Fig.7 Schematic of K-d tree example in threedimensional space

1.4.2K-d tree 最近邻搜索的欧氏聚类

最近邻搜索算法旨在找到最接近给定输入点的点。在应用K-d tree进行三维空间点的搜索时，从根节点开始，算法以递归的方式向下沿着树进行搜索，一旦到达叶子节点，检查该节点到给定点的欧氏距离，如果小于设定的最近邻距离阈值则认为其为最近邻点[32-33]。该算法在搜索时检查在分割平面的另一侧是否有任何点比当前最佳点更靠近搜索点，将分割超平面与以当前最近距离为半径的搜索点构成的超球面相比较，如果超球面越过该平面，则该平面的另一侧可能会有更近的点，则从当前节点向下移动树的另一分支以寻找更近的点；如果超球面不与分割平面相交，则算法将继续沿树搜索，并消除该节点另一侧的整个分支[34-35]。由于K-d tree在树的每一层上将域的范围划分为一半，因此若对有m个节点的K-d tree应用最近邻搜索，时间复杂度由原来的2m降到lbm。

通过使用树的属性可以快速消除搜索空间的大部分，更快完成此搜索过程。本研究应用基于K-d tree的最近邻搜索，对输入的三维点云建立K-d tree，在该点云树上搜索给定目标点的最近邻点。

从根节点开始按照树的结构依次遍历树的每个节点，设定两点间距离阈值为ΔT，Δ为树的该层节点关键值与待检测点值的差，考虑到点云坐标有负值的情况，且为了避免漏搜索最近邻点，当Δ>-ΔT时则进行下一层左子树的搜索，当树的该层节点关键值Δ<ΔT时则进行下一层右子树的搜索。假设待搜索的节点坐标为(Xnode，Ynode，Znode)，目标点的坐标为(Xtarget，Ytarget，Ztarget)，令

Δx=Xnode-Xtarget

(3)

Δy=Ynode-Ytarget

(4)

Δz=Znode-Ztarget

(5)

若|Δx|≤ΔT、|Δy|≤ΔT、|Δz|≤ΔT3个条件同时满足，则计算待检测点与目标点的欧氏距离D，计算公式为

(6)

若D≤ΔT，则认为此节点为目标点的最近邻点。按照此方法对所有点进行搜索，直到满足距离误差阈值的所有最近邻点搜索完毕。将搜索到的该目标点的最近邻点均记录在一个集合中，当所得最近邻点集合的元素数量满足设定的障碍物类点簇的数量范围时，则识别此点云集合为一个障碍物点簇。因在聚类进行最近邻搜索时计算的距离为欧几里得距离，故称此过程为欧氏聚类。

依次对所有预处理后的点云进行聚类，如果待处理点已属于某一类，则进行剩余其他点的处理，防止重复聚类。当聚类完毕时得到多个点云集合，即为多个障碍物点簇。

1.5 聚类后类簇滤波

由于农田环境的复杂性和目前三维激光雷达的不稳定性，容易有一些小土块和不平的地面被误检测为障碍物，因此对聚类后的点云类簇在最后一步再进行滤波。本研究中共进行2种滤波，即针对类簇点数量的滤波和外接长方体尺寸的滤波。

针对类簇点数量的滤波即判断聚类后每个类簇内点的数量，设定一个待检测障碍物的最小数量和最大数量，将不在此范围内的聚类认为是误检测的类，只保留在此范围内的聚类。外接长方体尺寸的滤波，以下简称体滤波，即判断聚类后每个类簇的最小外接长方体尺寸并按尺寸将一部分非障碍物过滤掉。给聚类后的每个类添加一个最小外接长方体，计算每个聚类的外接长方体的体积，将体积过小或体积过大的类删掉，剩下的即认为是聚类的障碍物。体滤波阈值的选取可依据待检测障碍物的外接长方体体积确定。

为测试体滤波对障碍物检测的效果，进行了对照试验。试验时拖拉机与横穿农田的行人距离不变，考虑到农机的刹车距离特性，此处选为10 m。按照上述步骤完成聚类后，对障碍物点云对照进行处理，一组经过体滤波，另外一组不经过体滤波。结果如图8所示。

图8 体滤波对检测的影响Fig.8 Influence of volume filtering on detection

图8为体滤波处理后的点云鸟瞰图，其中图8a中间处的红色长方体框为检测到的行人，左上角处的红色长方体框为误检测的障碍物地面小突起；图8b中间处的红色长方体框为检测到的行人，左上角处的绿色长方体框为经过体滤波过滤掉的点云聚类。由图8可以看出体滤波能够对农田地面的误检测起到很好的过滤作用。

2 试验与结果分析

为验证算法的有效性、评估算法的检测误差，本研究使用RS-LiDAR-32型三维激光雷达于2020年7月在北京市小汤山国家精准农业示范基地开展田间试验。试验时将激光雷达安装在迪尔904型拖拉机上，安装位置为拖拉机前配重块上方，安装高度为95 cm，安装方位角为雷达自身坐标系的x轴正方向对准拖拉机的正前方。安装方法见图9。

图9 试验安装图Fig.9 Test installation

试验时将拖拉机前部激光雷达对着一片玉米地，拖拉机前部安装的激光雷达距离玉米地30 m，玉米株高约0.5 m，试验场景如图10所示。

图10 试验场景Fig.10 Test environment

试验采集的三维点云在ROS下记录为bag包数据，在处理时将bag包分帧转换为pcd格式的点云，将pcd格式的点云按照上述算法进行处理，最后将检出的障碍物点云用以最小点和最大点为对角点的长方体框标注出来。

试验时针对农田环境的特性和雷达的点云密度特性设定了算法的参数。在体素栅格下采样时设定栅格边长为0.15 m。划定感兴趣区域时，为尽可能将车身安全距离范围内障碍物均检测到，设定长度为60 m，其中雷达前方50 m，后方10 m；宽度为20 m，以雷达为中心左右两侧各10 m；高度为7.5 m，由于雷达本身安装在拖拉机上且考虑到对地面的检测，故感兴趣区域中包括雷达下方1.5 m，雷达上方6 m。在划定立方体感兴趣区域时以雷达坐标为原点，在三维空间中以立方体的对角点确定立方体，其中最小点坐标为(-10，-10，-1.5) m、最大点坐标为(50，10，6) m；为避免装载雷达的拖拉机自身反射点云干扰聚类，将车身的外接立方体从感兴趣区域中去除，用同样的方法设定立方体的对角点最小点坐标为(-5，-1.5，-1.5) m、最大点坐标为(0.4，1.5，1.5) m，滤波时将此部分区域的点云滤除。在进行基于RANSAC算法的地面检测时设定迭代次数为60，距离误差阈值为0.3 m；在进行基于K-d tree的欧氏聚类时设定距离误差阈值为0.6 m。对聚类后类簇的滤波时选用单个障碍物类簇的最小点云数为10，最大点云数为240，在进行体滤波时为平衡障碍物漏检率和地面误检率，在滤波阈值的选择上取的相对折中，其中最小阈值为0.06，最大阈值为1.2。

主要设计了2个试验：田间常见障碍物的检测试验和障碍物距离对检测正确率的影响试验。

2.1 田间常见障碍物检测试验

为验证该算法的田间障碍物检测效果，对田间常见障碍物应用该算法进行了试验。障碍物包括：自然放置在田间农机具、田间草堆和田埂等低矮障碍物；地头矮房等多障碍物；田间道路两侧树木等有重叠的障碍物。检测结果如图11所示。在处理后的点云图中将检测出的障碍物用外接长方体框标注出来。由图11可以看出该算法可以有效检测田间农机具、草堆和田埂等低矮障碍物；同时可以检测地头矮房等多个障碍物；对道路两侧树木等多个重叠障碍物满足算法设定的参数条件也可以同时检测并用长方体框标注出来。试验结果表明该算法均可以检测田间低矮障碍物、多个障碍物和障碍物间有重叠的场景。

图11 田间常见障碍物的检测结果Fig.11 Detection result of common obstacles in field

2.2 距离对障碍物检测的影响试验

为测试距离对障碍物检测的影响，对不同距离的障碍物进行了试验。考虑到田间运动的行人是影响农机行驶安全的重要因素之一，试验时以行人作为待检测障碍物横穿拖拉机前方，待检测的人身高为1.75 m，分别与雷达距离5、10、15、20、25、30 m进行点云的扫描。采用上述算法和参数对采集的不同距离的点云进行处理。处理前后点云如图12所示。

图12 对田间不同距离行人的检测Fig.12 Detection of pedestrians at different distances in field

图12中每图的左侧为采集的原始点云，右侧为处理后的点云，对检出的行人和玉米地用红色长方体框标出。由图12可以看出，原始点云的密度较大， 且在雷达周边有拖拉机反射的点云，经过滤波和感兴趣区域分割后点云数量减少，但扫描的障碍物特征并未损失且加速了计算过程。每帧点云的体素栅格滤波、感兴趣区域切分和基于RANSAC的地面分割总平均用时500 ms，基于K-d tree的聚类平均用时1 ms。

试验共采集有效点云1 937帧，平均单帧点云约115 200个点，对扫描的单帧点云按照前述方法进行处理并统计，结果如表1所示。

表1列出了不同距离的采集点云帧数、地面误检率和行人检出率。其中地面误检率是指由于农田土地突起不平整的干扰在聚类过程中因满足聚类条件而被检测出的概率。行人检出率是指经过算法处理能够检测出田间行人的概率。经试验，该算法参数的平均地面误检率为4.76%，对田间平均行人检出率为96.11%。

表1 不同距离下田间行人的检测准确率Tab.1 Detection accuracy of pedestrians in the field at different distances

经分析可发现在距离5～20 m内，行人检出率较高，均在96%以上，剩余未检出的原因为在最后进行体滤波处理时由于激光雷达本身不稳定造成的个别帧的点数较少，外接矩形较小而被过滤掉从而造成漏检。在距离25～30 m时，由于距离相对较远，根据三维激光雷达向外发散的特性，远处的点较近处的点稀疏，在滤波作用下检出率稍有下降；且因靠近田埂，故地面误检率稍有上升。

在农田环境中，由于农机驾驶速度一般较低，其刹车距离也较小。假设车速为30 km/h，摩擦因数取0.3，则刹车距离为11.76 m。故在近距离内的障碍物检出率可以很大程度上为农机行驶提供可靠的数据。后续可以在算法上进行更深入研究，融合多种传感器感知，并在控制策略上进行设计来最大程度地保证安全。

3 结论

(1)以三维激光雷达为传感器，提出一种基于欧氏聚类的三维激光点云田间障碍物检测方法。该方法首先划定点云中的三维感兴趣区域并应用体素下采样方法对点云进行滤波以加快点云处理的速度，然后采用RANSAC算法对点云中的农田地面和地上物体进行区分，最后对地面上点云采用基于K-d tree的欧氏聚类，对聚类后的障碍物点云添加长方体框来标识。

(2)实现了以Ubuntu 16.04为系统平台，以C++为编程语言的三维点云欧氏聚类算法。实现了对点云的滤波、地面分割和障碍物聚类的检测流程，且对算法中最后一步滤波的影响进行了对比试验，试验表明体滤波可以显著降低地面的误检率。

(3)对该算法进行了田间试验，实现了农田中农机具、草堆、田埂、矮房和田间道路两侧树木的检测。

(4)对在农田中横穿的行人进行了多种距离的采样和检测。试验结果表明应用该算法对30 m内田间行人平均检出率为96.11%。