[发明专利]基于3D Lidar的果园行间导航线提取方法

权利要求书

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轴的裁剪阈值不超过树干平均净长，然后根据各裁剪阈值对三维原始点云数据进行裁剪，获得裁剪后的三维点云数据。