[发明专利]一种基于激光雷达的果园车辆自动导航控制系统

权利要求书

1.一种基于激光雷达的果园车辆自动导航控制系统，其特征是它采用激光雷达获取树干点云数据并进行预处理；对预处理结果进行树干起止点识别，计算树干中心坐标；结合K-Means算法和最小二乘法拟合树行直线，根据拟合树行直线的中心线导航路径构建增量式PD控制器作为导航控制器。

2.根据权利要求1所述的基于激光雷达的果园车辆自动导航控制系统，其特征在具体包括以下步骤：

步骤1、将激光雷达以及电机安装在履带式果园车上，履带式果园车按照预设的采样频率经过两行果树进行采样，得到树干点云数据；

步骤2、预处理当前采样的树干点云数据，得到预处理结果；

步骤3、遍历预处理结果，捕获并存储树干起始点坐标和树干终止点坐标；配对树干起始点坐标和树干终止点坐标得到配对后树干起始点坐标与树干终止点坐标，配对后树干起始点坐标与树干终止点坐标取平均值，得到树干中心点坐标；

步骤4、采用K-Means聚类算法将树干中心坐标分为左右两列,即计算当前树干中心坐标到K-Means聚类算法中两个质心的欧几里得距离，进行分列；采用最小二乘法拟合得到左右两侧树行直线；获取树行直线的中心线；

步骤5、根据树行直线的中心线作为导航路径，计算车辆期望航向θ′；

步骤6、构建增量式PD控制器作为导航控制器，对增量式PD控制器输入车辆期望航向θ′，输出车辆电机转速Δv，纠正车辆姿态至期望航向角度，车辆沿树行中线行驶。

3.根据权利要求1所述的基于激光雷达的果园车辆自动导航控制系统，其特征在于步骤1具体为：根据树行行距设置激光雷达观测半径ρ_max并形成圆形感兴趣区域，激光雷达获取树干点云数据其中N为扫描角度总数，i表示扫描点编号，ρ_i表示雷达发射点到目标的距离，θ_i表示扫描角度。

4.根据权利要求1所述的基于激光雷达的果园车辆自动导航控制系统，其特征在于步骤2具体为：

S21、对当前采样的树干点云数据进行可视化转化，得到可视化树干点云图像，该树干点云图像的高为H、宽为W的图像，白色表示背景，黑色像素点表示树干；

S22、以激光雷达中心位置为坐标系原点，以小车前进方向为y轴建立直角坐标系，采用下述公式将树干点云数据的极坐标转换为树干点云图像的直角坐标：其中，x_i、y_i为第i个扫描点在直角坐标系上x、y轴的坐标；

S23、采用下述公式对树干点云图像的直角坐标做归一化处理，得到归一化处理后的点云数据：

S24、采用一阶滞后滤波法对归一化处理后的点云数据进行滤波,得到的滤波后的点云数据作为预处理结果。

5.根据权利要求1所述的基于激光雷达的果园车辆自动导航控制系统，其特征在于步骤3具体为：

S31、设置圆形感兴趣区域外的点，以及圆形感兴趣区域以内未获取采样值的点返回值为0；遍历预处理结果，若出现零、零、非零、非零的数据块，判断数据块中的第一个非零数据点为树干起始点；若出现非零、非零、零、零的数据块，判断数据块中的最后一个非零数据为树干终止点，存储树干起始点坐标和树干终止点坐标；

S32、配对树干起始点坐标和树干终止点坐标，计算任意两个起始点坐标和终止点坐标之间的角度差，标记角度差在阈值范围内的点并完成配对，配对成功的树干起始点和终止点坐标取平均值，得到树干中心点坐标。

6.根据权利要求1所述的基于激光雷达的果园车辆自动导航控制系统，其特征在于步骤5具体包括：将树行直线的中心线设置成导航路径，导航路径为实线L，计算实线L与y轴的夹角，作为航向偏差θ；计算激光雷达中心位置与实线L的平行距离，作为横向偏差ΔP；

ΔP＝a₂-a₁，

其中，h为履带式果园车的中心位置前后预设像素点之间的直线像素距离；a₁为车前像素至左侧树行直线的直线像素距离，a₂为车前像素至右侧树行直线的直线像素距离，b₁车为后像素至左侧树行直线的直线像素距离。