[发明专利]基于3D LiDAR的果园行末、行首识别方法

权利要求书

1.一种基于3D LiDAR的果园行末、行首识别方法，其特征在于：包括如下步骤：

S1、果园行间原始点云数据采集；

以果园作业机具上搭载的3D LiDAR的几何中心为原点，构建果园作业机具的机体坐标系{V}；其中，在该坐标系中，x轴垂直于轮轴且正向指向果园作业机具的前进方向，y轴平行于轮轴且正向指向果园作业机具左侧，z轴垂直于x轴、y轴所在平面且正向指向上方；

在果园作业机具行进过程中，3D LiDAR以一定的频率采集目标果园行间的三维原始点云数据，在机体坐标系{V}下，第k帧三维原始点云数据记为P_k；

S2、兴趣区域选择；

以机体坐标系{V}的原点为中心，x轴正方向保留1.5倍株距d_tre，负方向保留0.5倍株距d_tre；y轴正、负方向均保留0.75倍行距d_row；z轴正方向取正无穷，负方向取0.5倍3D LiDAR的安装高度h_lidar；对三维原始点云数据P_k进行裁剪，作为所选兴趣区域的三维原始点云数据P_k′；

S3、果树位置等效、定位直线拟合与分组；

S3.1、欧几里得聚类，获得每一棵果树点云数据集

S3.1.1、创建一个kd-tree的搜索算法指针tree，将步骤S2获得的兴趣区域的三维原始点云数据P_k′所对应的指针，作为搜索算法指针tree的成员函数SetInputCloud()的输入参数，由此完成点云数据搜索方式kd-tree的创建；

S3.1.2、将需聚类的目标点云设为步骤S2获得的兴趣区域的三维原始点云数据P_k′，分别设置最小距离阈值为聚类点的最小数目为12、聚类点的最大数目为5000，将搜索方式设为步骤S3.1.1创建的kd-tree方式；其中，d_tre为株距；

S3.1.3、经过该算法将产生i个数组，即i个类别，亦即i棵果树(i＝0，1，2，...)，获得每一棵果树点云数据集

S3.2、树干的体心等效与投影；

S3.2.1、求解步骤S3.1获得的每一棵果树点云数据集中分别沿x轴、y轴、z轴三个方向的最值坐标

S3.2.2、分别求解三个轴向最值坐标的中点，即得到该果树的体心坐标其中，

S3.2.3、令求得的所有树干体心坐标的z值为0，则对于第i棵树，其体心坐标表示为获得第k帧的所有树干体心集合Q_k；

S3.3、定位直线拟合与分组；

S3.3.1、首先以机体坐标系{V}的x轴为分割线，将第k帧的所有树干体心集合Q_k分为左右两组Q_l、Q_r，然后以机体坐标系{V}的y轴为分割线将第k帧的所有树干体心集合Q_k分为前后两组Q_f、Q_b，最后使用最小二乘法分别对四组树干体心集合Q_l、Q_r、Q_f、Q_b进行拟合，分别获得四条直线方程：左直线L_l、右直线L_r、前直线L_f、后直线L_b；

S3.3.2、根据四条直线方程L_l、L_r、L_f、L_b存在的真假状态，将直线存在记为1，不存在记为0，则四条直线存在的真假状态组合方式共有2⁴＝16种，获得四条直线存在的真假状态组合方式与果园作业机具在果园中所处位置的对应关系；所述果园作业机具在果园中所处位置包括进入树行、到达行末、离开树行和踏入树行；

S4、行末、行首识别；

根据步骤S3.3获得的四条直线存在的真假状态组合方式与果园作业机具在果园中所处位置的对应关系；

当四条直线中，L_b为1、L_f为0、L_l为0、L_r为0，或

L_b为1、L_f为0、L_l为0、L_r为1，或

L_b为1、L_f为0、L_l为1、L_r为0，果园作业机具处于到达行末的状态时，即为准确识别了果园行末；

当四条直线中，L_b为0、L_f为1、L_l为0、L_r为0，或

L_b为0、L_f为1、L_l为0、L_r为1，或

L_b为0、L_f为1、L_l为1、L_r为0，果园作业机具处于踏入树行的状态时，即为准确识别了果园行首。