CN111539473A - 基于3D Lidar的果园行间导航线提取方法 - Google Patents

Abstract

本发明属于三维点云处理技术领域，尤其涉及一种适用于作业机具自动行走的基于3D Lidar的果园行间导航线提取方法。该方法包括步骤：S1.采集目标果园行间的三维原始点云数据；S2.三维原始点云数据预处理；S3.欧式聚类；S4.树行拟合、导航线生成。本发明方法使用树干的三维投影的中点代替树干的位置，实现果园行间的树干定位，进而提取导航线，具有运算量小、实时性高、受环境干扰小、精度高、鲁棒性高等优点。

Description

基于3D Lidar的果园行间导航线提取方法

技术领域

本发明属于三维点云处理技术领域，尤其涉及一种适用于作业机具自动行走的基于3D Lidar的果园行间导航线提取方法。

背景技术

果园自动导航技术是实现人机分离、无人驾驶精细化果园管理的核心，也是未来智慧农业发展的关键技术。目前，果园自动导航技术按照环境感知分类主要有GNSS导航、机器视觉导航、声源导航、多传感器融合导航和激光导航等。对于GNSS导航，地面行走卫星信号接收机容易受果树生产期内不断生长变化的树冠枝叶遮挡而丢失定位信号；对于机器视觉导航，其导航精度较高，但摄像头受环境光线干扰较大，大多需要增加补充光源来获取清晰可用的图像，且图像数据量较大，算法实时性较差；对于声源导航，需要重点解决声源频率、噪声干扰、障碍物阻挡及接收器误差等因素对定位误差的影响；对于多传感器融合导航，其具有显著的优点，融合多个廉价的传感器来实现高精度的定位，前景向好，但系统的结构、计算方法与控制软硬件复杂、综合成本高；对于激光导航，尤其是三维激光雷达传感器具有测距精度高、信息量丰富、受环境干扰小等优点，同时随着激光雷达在我国研发制造生产技术的拓展简化与规模化应用，激光雷达的成本得到了很大的降低，因此，使用该传感器开发的三维激光导航系统能够为果园作业机具的行走导航技术提供一种高性能低成本的解决方案。

目前，对于农业领域激光雷达导航的研究，主要是基于二维激光雷达进行的，但由于二维激光雷达只能获取一个平面的信息，信息量过少，对于真实的果园行间环境，很容易因树干的几何尺寸不一而受到干扰，使得对特征物的识别不准，导致导航精度降低。因此，使用三维激光雷达可以很好地解决二维激光雷达数据量少的缺点，同时对采集到的三维点云数据进行相应滤波处理，降低冗余的数据，调整传感器获得适量的点云数据，提高点云数据处理的实时性，从而充分发挥激光导航的优点。

果园定植后，行间路径一般较窄且规整固定，通常为规则固定的果树株距与相对的直线树行行距，对于作业的农业机器人，一般需要沿着树行的中心直线行走，因此，对于果园行间导航线的提取实际上是对树行的中心线的提取。

目前，国内外关于对果园行间导航线提取的相关研究主要是基于图像处理、二维激光雷达等实现的，图像处理数据量较大，且受环境干扰较大，算法实时性较低，而二维激光雷达数据量过少，只能反映环境中的一个平面信息，容易出错，所以二维激光雷达方案的鲁棒性较低。因而，运算量小、实时性高、受环境干扰小、精度高、鲁棒性高的基于3D Lidar的果园行间导航线提取方法易行可靠。

发明内容

针对上述技术问题，本发明的目的是提供一种运算量小、实时性高、受环境干扰小、精度高、鲁棒性高等特点的基于3D Lidar的果园行间导航线提取方法。

为了实现上述目的，本发明提供了如下技术方案：

一种基于3D Lidar的果园行间导航线提取方法，包括如下步骤：

S1.采集目标果园行间的三维原始点云数据；

通过3D Lidar采集目标果园行间的三维原始点云数据，并构建三维坐标系OXYZ；

S2.三维原始点云数据预处理；

S2.1对三维原始点云数据进行三维裁剪；

对三维原始点云数据的X轴、Y轴、Z轴进行数据裁剪，获得裁剪后的三维数据，使得移动机器人在移动过程中，单侧视野中至少包含两棵树，左右视野至少覆盖两列树，纵向视野覆盖且不超过树干平均净长；

S2.2对裁剪后的数据进行降维；

将裁剪后的三维数据投影到一个给定三维空间平面，获得降维后的点云数据，实现数据由三维到二维的降维；

S3.欧式聚类；

S3.1创建数据搜索方式kd-tree；

创建一个kd-tree的搜索算法指针tree，将步骤S2.2获得的降维后的点云数据的指针作为搜索算法指针tree的成员函数SetInputCloud()的输入参数，由此完成点云数据搜索方式kd-tree的创建；

S3.2使用欧式聚类算法对搜索后的有效点进行聚类；

欧式聚类算法有5个输入参数，即需聚类的目标点云数据、聚类点与点之间的距离阈值、最少聚类点数目、最多聚类点数目和搜索方式；将需聚类的目标点云数据设为步骤S2.2中降维后的点云数据，根据需要分别设置聚类点与点之间的距离阈值、最少聚类点数目和最多聚类点数目，将搜索方式设为步骤S3.1中创建的kd-tree方式；经过该算法将产生j个数组，即j个类别(j＝0,1,2,…)，每个数组包含m_j个点；

S3.3求取步骤S3.2中第j个数组中m_j个点的中点用以替代相应的第j个类别，即该点为对应类别的等效点；

不同等效点的坐标表示为：

(j＝0,1,2,…)；式中，xc_j、yc_j分别为每个类别数据的横、纵坐标的中点，亦即对应的第j个类别的等效点，m_j为第j个类别对应的数据点总数，下标j表示第j个类别，下标i表示第j个类别中的第i个点，(i＝0,1,…,m_j)，x_i、y_i分别为第j个类别中第i个数据点对应的横、纵坐标；

S4.树行拟合、导航线生成；

S4.1对步骤S3.3获得的聚类后的数据的等效点(xc_j、yc_j)按照点与坐标系OXYZ的几何位置特征进行分类，分离出左树行等效点、障碍物等效点和右树行等效点；

S4.2使用最小二乘法分别拟合左树行直线、右树行直线，获得左树行直线方程和右树行直线方程；

S4.3求取左右树行的中线得到所述导航线，使用步骤S4.2中得到的左树行方程和右树行方程，等距截取一定数量点对，求取点对的中点作为导航线上的点，再次使用最小二乘法拟合即得到左右树行的中线亦即所述导航线。

所述步骤S1中，移动机器人的几何中心和3D Lidar的几何中心位于垂直于水平面的Z轴上，以3D Lidar的几何中心为三维直角坐标系OXYZ的原点O，移动机器人的运动方向为X轴，在水平面内垂直于X轴为Y轴。

所述步骤S2.1中，对三维原始点云数据的X轴、Y轴、Z轴进行数据裁剪：根据果树单行株距、果树行距、树干平均净长分别设定X轴、Y轴、Z轴的裁剪阈值，其中，X轴的裁剪阈值至少为果树单行株距的3倍，Y轴的裁剪阈值至少为果树行距的1.5倍，Z轴的裁剪阈值不超过树干平均净长，然后根据各裁剪阈值对三维原始点云数据进行裁剪，获得裁剪后的三维数据。

所述步骤S4.1的具体过程为：以X轴为中心线，按照各等效点(xc_j、yc_j)相对于中心线的位置分别提取左树行点、障碍物点、右树行点，通过两次矩形选择算法实现；矩形选择算法有4个输入参数，包括X轴方向的两个边界值(x_n,x_p)和y轴方向的两个边界值(y_n,y_p)，其中，下标n表示负方向，下标p表示正方向；

第一次矩形选择算法以步骤S3.3获得的所有等效点(xc_j、yc_j)(j＝0,1,2,…)为目标点，X轴方向的两个边界值(x_n,x_p)的取值范围至少为果树单行株距的3倍，y_n和y_p的绝对值小于果树行距的二分之一，构建第一次矩形选择框，框内点为障碍物点，框外点为树行点；

第二次矩形选择算法以第一次分离得到的树行点为目标点，X轴方向的两个边界值(x_n,x_p)的取值范围至少为果树单行株距的3倍，y_n的绝对值大于果树行距的二分之一，y_p取值0，构建第二次矩形选择框，框内点为右树行点，框外点为左树行点。

所述步骤S4.2的具体拟合过程为：

最小二乘法算法包含4个输入参数：待拟合的数据点指针*data、直线方程参数a、直线方程参数b和直线方程参数c。对于左树行直线的拟合，将步骤S4.1中聚类后的左树行等效点作为最小二乘法的第一个输入参数*data，分别定义a₀、b₀、c₀作为后面3个参数，该算法将拟合的直线方程3个参数分别写入后3个输入参数a₀、b₀、c₀中，由此得到左树行的直线方程为a₀x+b₀y+c₀＝0；对于右树行的拟合，将步骤S4.1中聚类后的右树行等效点作为最小二乘法的第一个输入参数*data，分别定义a₁、b₁、c₁作为后面3个参数，该算法将拟合的直线方程3个参数分别写入后3个输入参数a₁、b₁、c₁中，由此得到右树行直线方程为a₁x+b₁y+c₁＝0。

所述步骤S4.3的具体过程为：

对于直线方程的点对截取，该算法有6个输入参数：直线方程参数a、直线方程参数b、直线方程参数c、点对个数num、坐标平移步长d和起始点横坐标x_start，对于左树行直线方程的点对截取，将步骤S4.2中获得的左树行直线方程a₀x+b₀y+c₀＝0的3个参数a₀、b₀、c₀作为点对截取算法的前三个输入参数，将点对个数num设为2，将坐标平移步长d设为0.5，将起始点横坐标x_start设为0，该算法将返回2个点对的横纵坐标(x_l0,y_l0)、(x_l1,y_l1)，将前3个输入参数换成步骤S4.2中获得的右树行直线方程a₁x+b₁y+c₁＝0的3个参数a₁、b₁、c₁，其他3个参数不变，用相同的方法得到右树行直线方程的2个点对(x_r0,y_r0)、(x_r1,y_r1)；

对于导航线上的点对计算，将获得的4个点对进行如下求均值运算，

由此得到导航线上的两个点对(x₀,y₀)、(x₁,y₁)，然后将这两个点对存放于一个数组作为最小二乘法拟合算法的第一个参数，定义导航线直线方程参数A、B、C并代入最小二乘法拟合算法，该算法将拟合后的直线方程3个参数分别存入A、B、C，由此得到导航线的直线方程Ax+By+C＝0。

与现有技术相比，本发明的有益效果在于：

本发明方法使用树干的三维投影的中点代替树干的位置，实现果园行间的树干定位，进而提取导航线，具有运算量小、实时性高、受环境干扰小、精度高、鲁棒性高等优点。

附图说明

图1为本发明的基于3D Lidar的果园行间导航线提取方法的流程图；

图2为本发明的果园行间的原始三维点云图；

图3为本发明的三维裁剪及降维后的三维点云图；

图4为本发明点云数据聚类结果及等效点分类图；

图5为本发明的左右树行拟合及导航线提取图。

具体实施方式

下面结合附图和实施例对本发明进行进一步说明。

如图1所示，本发明的一种基于3D Lidar的果园行间导航线提取方法，包括如下步骤：

S1.采集目标果园行间的三维原始点云数据；

通过3D Lidar采集目标果园行间的三维原始点云数据，并构建三维坐标系OXYZ。

如图2所示，图中方块为移动机器人，移动机器人的几何中心和3D Lidar的几何中心位于垂直于水平面的Z轴上，以3D Lidar的几何中心为三维直角坐标系OXYZ的原点O，移动机器人的运动方向为X轴，在水平面内垂直于X轴为Y轴，且X轴、Y轴、Z轴的正方向分别为向前、向左、向上。图中白色点云为三维场景中的物体与坐标原点的相对位置，如移动机器人左右两边的点云为果树，前面的点云为地面反射，后面的点云为移动机器人操作员。该数据场景信息丰富、精准、覆盖范围广，水平方向360度全覆盖，垂直方向从地面到果树以上均覆盖。本实例所用3D Lidar为16线激光雷达，其水平视场角为360°，垂直视场角为30°(±15°)，最大探测距离为200m。

S2.三维原始点云数据预处理；

S2.1对三维原始点云数据进行三维裁剪；

对三维原始点云数据的X轴、Y轴、Z轴进行数据裁剪：根据果树单行株距、果树行距、树干平均净长分别设定X轴、Y轴、Z轴的裁剪阈值，其中，X轴的裁剪阈值至少为果树单行株距的3倍，Y轴的裁剪阈值至少为果树行距的1.5倍，Z轴的裁剪阈值不超过树干平均净长，然后根据各裁剪阈值对三维原始点云数据进行裁剪，获得裁剪后的三维数据，其目的是选取一定可视范围的数据、减少数据量、提升处理速度。

其中，对三维原始点云数据的X轴进行裁剪，是为了选择移动机器人的前后探测范围，X轴的裁剪阈值的选择需参考果树单行株距，通常至少应覆盖果树单行株距的3倍。选取该裁剪阈值是为了保证移动机器人移动过程中，单侧视野中至少包含2棵树。本实施例中X轴的裁剪阈值选取范围为(-8m,8m)，其中果树单行株距均值约为5m。

对原始三维点云数据的Y轴进行裁剪，是为了选择移动机器人的左右探测范围，Y轴的裁剪阈值的选择需要参考果树行距，通常至少应包括果树行距的1.5倍。选取该裁剪阈值是为了保证移动机器人移动过程中，左右视野至少覆盖两列树。本实施例中Y轴的裁剪阈值选取范围为(-4m,4m)，其中果树行距均值约为4m。

对原始三维点云数据的Z轴进行裁剪，是为了选择移动机器人的纵向探测范围，Z轴的裁剪阈值的选择需要参考树干平均净长，通常不超过树干平均净长。选取该裁剪阈值是为了保证移动机器人移动过程中，纵向视野覆盖而又不超过树干平均净长。超过后，会带来干扰与数据冗余，不足又会使得纵向信息量过少，影响对树干中心的识别精度。本实施例中轴的裁剪阈值选取范围为(-0.45m，-0.35m)，其中树干平均净长约为0.5m。

S2.2对裁剪后的数据进行降维；

将裁剪后的三维数据投影到一个给定三维空间平面。本实施例中，给定的三维空间平面为：x＝0,y＝0,z＝1。获得降维后的点云数据，实现数据由三维到二维的降维，进而实现几何问题的简化。如图3所示，图中左右两边的点均为树干在给定的三维空间平面(x＝0,y＝0,z＝1)的投影，中间的点为操作员在给定的三维空间平面(x＝0,y＝0,z＝1)的投影。

S3.欧式聚类；

S3.1创建数据搜索方式kd-tree；

创建一个kd-tree的搜索算法指针tree，将步骤S2.2获得的降维后的点云数据的指针作为搜索算法指针tree的成员函数SetInputCloud()的输入参数，由此完成点云数据搜索方式kd-tree的创建。

S3.2使用欧式聚类算法对搜索后的有效点进行聚类；

欧式聚类算法有5个输入参数，即需聚类的目标点云数据、聚类点与点之间的距离阈值、最少聚类点数目、最多聚类点数目和搜索方式。分别将需聚类的目标点云数据设为步骤S2.2中降维后的点云数据，将聚类点与点之间的距离阈值设为0.5，将最少聚类点数目设为10，将最多聚类点数目设为500，将搜索方式设为步骤S3.1中创建的kd-tree方式。经过该算法将产生j个数组，即j个类别(j＝0,1,2,…)，每个数组(类别)包含m_j个点。

S3.3求取步骤S3.2中第j个数组中m_j个点的中点用以替代相应的第j个类别，即该点为对应类别的等效点；

不同等效点的坐标表示为：

(j＝0,1,2,…)。式中，xc_j、yc_j分别为每个类别数据的横、纵坐标的中点，亦即对应的第j个类别的等效点，m_j为第j个类别对应的数据点总数，下标j表示第j个类别，下标i表示第j个类别中的第i个点，(i＝0,1,…,m_j)，x_i、y_i分别为第j个类别中第i个数据点对应的横、纵坐标。

S4.树行拟合、导航线生成；

S4.1对步骤S3.3获得的聚类后的数据的等效点(xc_j、yc_j)按照点与坐标系OXYZ的几何位置特征进行分类，分离出左树行等效点、障碍物等效点和右树行等效点。具体过程为：

以X轴为中心线，按照各等效点(xc_j、yc_j)相对于中心线的位置分别提取左树行点、障碍物点、右树行点。所述各等效点(xc_j、yc_j)相对于中心线的位置为各等效点(xc_j、yc_j)相对于中心线居左、居中、居右。此处的分类主要通过两次矩形选择算法实现。矩形选择算法有4个输入参数，包括X轴方向的两个边界值(x_n,x_p)和y轴方向的两个边界值(y_n,y_p)，其中，下标n表示负方向，下标p表示正方向。该算法将所有数据点进行分类，最终分别返回矩形框内的数据、矩形框外的数据，共两个类别。如图4所示，第一次以步骤S3.3获得的所有等效点(xc_j、yc_j)(j＝0,1,2,…)为目标点，使用矩形选择算法分离出树行点、障碍物点，其中框内点为障碍物点，框外点为树行点，四个输入参数(x_n,x_p,y_n,y_p)为(-5,10,-1.5,1.5)；第二次以第一次分离得到的树行点为目标点，使用矩形选择算法分离出左树行点、右树行点，其中框内点为右树行点，框外点为左树行点，四个输入参数(x_n,x_p,y_n,y_p)为(-∞,+∞,-∞,0)。

S4.2使用最小二乘法分别拟合左树行直线、右树行直线，获得左树行直线方程和右树行直线方程，具体拟合过程为：

最小二乘法算法包含4个输入参数：待拟合的数据点指针*data、直线方程参数a、直线方程参数b和直线方程参数c。对于左树行直线的拟合，将步骤S4.1中聚类后的左树行等效点作为最小二乘法的第一个输入参数*data，分别定义a₀、b₀、c₀作为后面3个参数，该算法将拟合的直线方程3个参数分别写入后3个输入参数a₀、b₀、c₀中，由此得到左树行的直线方程为a₀x+b₀y+c₀＝0；对于右树行的拟合，将步骤S4.1中聚类后的右树行等效点作为最小二乘法的第一个输入参数*data，分别定义a₁、b₁、c₁作为后面3个参数，该算法将拟合的直线方程3个参数分别写入后3个输入参数a₁、b₁、c₁中，由此得到右树行直线方程为a₁x+b₁y+c₁＝0。如图5所示，图中移动机器人左边的树行所连成的线为拟合的左树行直线，移动机器人右边的树行所连成的线为拟合的右树行直线。

S4.3求取左右树行的中线得到所述导航线，使用步骤S4.2中得到的左树行方程和右树行方程，等距截取一定数量点对，求取点对的中点作为导航线上的点，再次使用最小二乘法拟合即得到左右树行的中线亦即所述导航线，具体过程为：

对于直线方程的点对截取，该算法有6个输入参数：直线方程参数a、直线方程参数b、直线方程参数c、点对个数num、坐标平移步长d和起始点横坐标x_start，对于左树行直线方程的点对截取，将步骤S4.2中获得的左树行直线方程a₀x+b₀y+c₀＝0的3个参数a₀、b₀、c₀作为点对截取算法的前三个输入参数，将点对个数num设为2(因为所求导航线被近似为直线，固而2个点即可确定该直线)，将坐标平移步长d设为0.5，将起始点横坐标x_start设为0，该算法将返回2个点对的横纵坐标(x_l0,y_l0)、(x_l1,y_l1)，将前3个输入参数换成步骤S4.2中获得的右树行直线方程a₁x+b₁y+c₁＝0的3个参数a₁、b₁、c₁，其他3个参数不变，用相同的方法得到右树行直线方程的2个点对(x_r0,y_r0)、(x_r1,y_r1)。

对于导航线上的点对计算，将本步骤上面获得的4个点对进行如下求均值运算，

由此得到导航线上的两个点对(x₀,y₀)、(x₁,y₁)，然后将这两个点对存放于一个数组作为最小二乘法拟合算法的第一个参数，定义导航线直线方程参数A、B、C并代入最小二乘法拟合算法，该算法将拟合后的直线方程3个参数分别存入A、B、C，由此得到导航线的直线方程Ax+By+C＝0。如图5所示，图中穿过移动机器人的直线为拟合的导航线。

以上实施方式仅用于说明本发明，而并非对本发明的限制。尽管参照实例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行各种组合、修改或者等同替换，都无法脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.一种基于3D Lidar的果园行间导航线提取方法，其特征在于：包括如下步骤：

S1.采集目标果园行间的三维原始点云数据；

通过3D Lidar采集目标果园行间的三维原始点云数据，并构建三维坐标系OXYZ；

S2.三维原始点云数据预处理；

S2.1对三维原始点云数据进行三维裁剪；

对三维原始点云数据的X轴、Y轴、Z轴进行数据裁剪，获得裁剪后的三维数据，使得移动机器人在移动过程中，单侧视野中至少包含两棵树，左右视野至少覆盖两列树，纵向视野覆盖且不超过树干平均净长；

S2.2对裁剪后的数据进行降维；

将裁剪后的三维数据投影到一个给定三维空间平面，获得降维后的点云数据，实现数据由三维到二维的降维；

S3.欧式聚类；

S3.1创建数据搜索方式kd-tree；

创建一个kd-tree的搜索算法指针tree，将步骤S2.2获得的降维后的点云数据的指针作为搜索算法指针tree的成员函数SetInputCloud()的输入参数，由此完成点云数据搜索方式kd-tree的创建；

S3.2使用欧式聚类算法对搜索后的有效点进行聚类；

欧式聚类算法有5个输入参数，即需聚类的目标点云数据、聚类点与点之间的距离阈值、最少聚类点数目、最多聚类点数目和搜索方式；将需聚类的目标点云数据设为步骤S2.2中降维后的点云数据，根据需要分别设置聚类点与点之间的距离阈值、最少聚类点数目和最多聚类点数目，将搜索方式设为步骤S3.1中创建的kd-tree方式；经过该算法将产生j个数组，即j个类别(j＝0,1,2,…)，每个数组包含m_j个点；

S3.3求取步骤S3.2中第j个数组中m_j个点的中点用以替代相应的第j个类别，即该点为对应类别的等效点；

不同等效点的坐标表示为：

(j＝0,1,2,…)；式中，xc_j、yc_j分别为每个类别数据的横、纵坐标的中点，亦即对应的第j个类别的等效点，m_j为第j个类别对应的数据点总数，下标j表示第j个类别，下标i表示第j个类别中的第i个点，(i＝0,1,…,m_j)，x_i、y_i分别为第j个类别中第i个数据点对应的横、纵坐标；

S4.树行拟合、导航线生成；

S4.1对步骤S3.3获得的聚类后的数据的等效点(xc_j、yc_j)按照点与坐标系OXYZ的几何位置特征进行分类，分离出左树行等效点、障碍物等效点和右树行等效点；

S4.2使用最小二乘法分别拟合左树行直线、右树行直线，获得左树行直线方程和右树行直线方程；

S4.3求取左右树行的中线得到所述导航线，使用步骤S4.2中得到的左树行方程和右树行方程，等距截取一定数量点对，求取点对的中点作为导航线上的点，再次使用最小二乘法拟合即得到左右树行的中线亦即所述导航线。

2.根据权利要求1所述的基于3D Lidar的果园行间导航线提取方法，其特征在于：所述步骤S1中，移动机器人的几何中心和3D Lidar的几何中心位于垂直于水平面的Z轴上，以3DLidar的几何中心为三维直角坐标系OXYZ的原点O，移动机器人的运动方向为X轴，在水平面内垂直于X轴为Y轴。

3.根据权利要求1所述的基于3D Lidar的果园行间导航线提取方法，其特征在于：所述步骤S2.1中，对三维原始点云数据的X轴、Y轴、Z轴进行数据裁剪：根据果树单行株距、果树行距、树干平均净长分别设定X轴、Y轴、Z轴的裁剪阈值，其中，X轴的裁剪阈值至少为果树单行株距的3倍，Y轴的裁剪阈值至少为果树行距的1.5倍，Z轴的裁剪阈值不超过树干平均净长，然后根据各裁剪阈值对三维原始点云数据进行裁剪，获得裁剪后的三维点云数据。

4.根据权利要求1所述的基于3D Lidar的果园行间导航线提取方法，其特征在于：所述步骤S4.1的具体过程为：以X轴为中心线，按照各等效点(xc_j、yc_j)相对于中心线的位置分别提取左树行点、障碍物点、右树行点，通过两次矩形选择算法实现；矩形选择算法有4个输入参数，包括X轴方向的两个边界值(x_n,x_p)和y轴方向的两个边界值(y_n,y_p)，其中，下标n表示负方向，下标p表示正方向；

第一次矩形选择算法以步骤S3.3获得的所有等效点(xc_j、yc_j)(j＝0,1,2,…)为目标点，X轴方向的两个边界值(x_n,x_p)的取值范围至少为果树单行株距的3倍，y_n和y_p的绝对值小于果树行距的二分之一，构建第一次矩形选择框，框内点为障碍物点，框外点为树行点；

第二次矩形选择算法以第一次分离得到的树行点为目标点，X轴方向的两个边界值(x_n,x_p)的取值范围至少为果树单行株距的3倍，y_n的绝对值大于果树行距的二分之一，y_p取值0，构建第二次矩形选择框，框内点为右树行点，框外点为左树行点。

5.根据权利要求1所述的基于3D Lidar的果园行间导航线提取方法，其特征在于：所述步骤S4.2的具体拟合过程为：

最小二乘法算法包含4个输入参数：待拟合的数据点指针*data、直线方程参数a、直线方程参数b和直线方程参数c。对于左树行直线的拟合，将步骤S4.1中聚类后的左树行等效点作为最小二乘法的第一个输入参数*data，分别定义a₀、b₀、c₀作为后面3个参数，该算法将拟合的直线方程3个参数分别写入后3个输入参数a₀、b₀、c₀中，由此得到左树行的直线方程为a₀x+b₀y+c₀＝0；对于右树行的拟合，将步骤S4.1中聚类后的右树行等效点作为最小二乘法的第一个输入参数*data，分别定义a₁、b₁、c₁作为后面3个参数，该算法将拟合的直线方程3个参数分别写入后3个输入参数a₁、b₁、c₁中，由此得到右树行直线方程为a₁x+b₁y+c₁＝0。

6.根据权利要求5所述的基于3D Lidar的果园行间导航线提取方法，其特征在于：所述步骤S4.3的具体过程为：

对于直线方程的点对截取，该算法有6个输入参数：直线方程参数a、直线方程参数b、直线方程参数c、点对个数num、坐标平移步长d和起始点横坐标x_start，对于左树行直线方程的点对截取，将步骤S4.2中获得的左树行直线方程a₀x+b₀y+c₀＝0的3个参数a₀、b₀、c₀作为点对截取算法的前三个输入参数，将点对个数num设为2，将坐标平移步长d设为0.5，将起始点横坐标x_start设为0，该算法将返回2个点对的横纵坐标(x_l0,y_l0)、(x_l1,y_l1)，将前3个输入参数换成步骤S4.2中获得的右树行直线方程a₁x+b₁y+c₁＝0的3个参数a₁、b₁、c₁，其他3个参数不变，用相同的方法得到右树行直线方程的2个点对(x_r0,y_r0)、(x_r1,y_r1)；

对于导航线上的点对计算，将获得的4个点对进行如下求均值运算，

由此得到导航线上的两个点对(x₀,y₀)、(x₁,y₁)，然后将这两个点对存放于一个数组作为最小二乘法拟合算法的第一个参数，定义导航线直线方程参数A、B、C并代入最小二乘法拟合算法，该算法将拟合后的直线方程3个参数分别存入A、B、C，由此得到导航线的直线方程Ax+By+C＝0。

Images