CN117214861A - 一种在线检测水稻收割导航线的方法 - Google Patents

Abstract

一种在线检测水稻收割导航线的方法，包括：采用激光雷达与惯性传感器，获取水稻在收割机车身坐标系下的实时坐标并进行坐标变换，得到车身坐标系下的水稻作物的点云信息；提取该点云信息的感兴趣区域，对区域内的数据进行初步滤波处理，得到水稻收割模型；采用布料模拟滤波法滤除水稻收割模型的地面点云，得到水稻点云模型；采用点云数据特征值分析法求取点云的法向量；将点云数据的法向量映射到高斯球面上，得到高斯映射点集；对高斯映射点集进行聚类分析，根据高斯映射点的多维分布规律判断得到特征点作为边界点；采用最小二乘法拟合边界点，得到拟合的收割导航线；对比其与实际收割中水稻的边界线，计算横向误差和航向误差，并验证精度。

Description

一种在线检测水稻收割导航线的方法

技术领域

本发明涉及农作物导航线提取技术，特别是一种3D激光雷达在线检测水稻收割导航线的方法。

背景技术

目前，水稻收割导航线的提取方式主要有机器视觉和激光两种方式。机器视觉方式主要是利用单目相机、双目相机等获取收割过程中的图像，利用图像处理技术获取水稻的高度信息并确定水稻导航线；激光方式主要是通过二维3D激光雷达、3D激光雷达获取收割过程中的点云信息，对点云信息处理获取水稻的高度并进行导航线拟合。

基于机器视觉的图像处理方法易受环境因素的影响，如天气阴晴、杂草遮挡等都容易导致识别精度降低，从而提取导航线失败。而3D激光雷达不受天气等的影响，3D激光雷达还可准确获取水稻的三维信息，再结合3D激光雷达反馈的反射率，即可对水稻进行较为精准的识别，根据高度差确定导航线，为收割机辅助导航提供数据。

发明内容

本发明所要解决的技术问题是针对现有技术的上述缺陷，提供一种在线检测水稻收割导航线的方法。

为了实现上述目的，本发明提供了一种在线检测水稻收割导航线的方法，其中，包括如下步骤：

S100、采用3D激光雷达与IMU惯性传感器，获取水稻在收割机的车身坐标系下的实时坐标并进行坐标变换，得到坐标变换后的车身坐标系下的水稻作物的点云信息；

S200、提取坐标变换后的点云信息的感兴趣区域，对所述感兴趣区域内的数据进行初步滤波处理，得到水稻收割模型；

S300、采用布料模拟滤波法滤除所述水稻收割模型的地面点云，得到水稻点云模型；

S400、采用点云数据特征值分析法，利用三维坐标求得数据的协方差矩阵、特征值和特征向量，以求取点云的法向量；

S500、将点云数据的法向量映射到高斯球面上，得到所述点云的法向量在高斯球上的映射集；

S600、对高斯映射点集进行聚类分析，根据高斯映射点的多维分布规律判断得到特征点作为边界点；

S700、采用最小二乘法拟合所述边界点，得到拟合的收割导航线；以及

S800、对比所述拟合的收割导航线与实际收割过程中水稻的边界线，计算横向误差和航向误差，并验证精度。

上述的在线检测水稻收割导航线的方法，其中，所述横向误差为收割机割台边界延长线与所述拟合的收割导航线之间的距离；所述航向误差为收割机导航线所在直线与所述拟合的收割导航线之间的夹角。

上述的在线检测水稻收割导航线的方法，其中，步骤S100进一步包括：

S110、在收割机上安装3D激光雷达和IMU惯性传感器，并确定所述3D激光雷达和、IMU惯性传感器和收割机的坐标系；

S120、3D激光雷达标定，求得3D激光雷达的初始安装角度；以及

S130、采集待收割区域水稻的点云数据，通过3D激光雷达和IMU惯性传感器数据获取水稻在车身坐标系下的实时坐标并进行坐标变换。

上述的在线检测水稻收割导航线的方法，其中，步骤S130进一步包括：

S131、收割机行进过程中，所述3D激光雷达扫描前方水稻作物的点云信息，所述IMU惯性传感器高频反馈收割机的即时欧拉角；

S132、根据所述IMU惯性传感器提供的姿态信息，包括3D激光雷达坐标系和车身坐标系之间的旋转矩阵R_VC，根据3D激光雷达安装高度信息，得到3D激光雷达坐标系和车身坐标系之间的平移矩阵T_VC；

S133、根据如下公式，将激光坐标系下的点云信息转换到车身坐标系下，得到坐标变换后的车身坐标系下的水稻作物的点云信息：

其中，[x_C y_C z_C]^T为车身坐标系下的水稻点云坐标，[x_V y_V z_V]^T为3D激光雷达坐标系下的水稻点云坐标。

上述的在线检测水稻收割导航线的方法，其中，步骤S200进一步包括：

S210、综合考虑3D激光雷达安装高度和角度、3D激光雷达扫描角度和收割机行进速度确定感兴趣区域；

S220、采用直通滤波、半径滤波和双边滤波对所述感兴趣区域内的点云进行数据预处理，得到预处理后的点云。

上述的在线检测水稻收割导航线的方法，其中，步骤S300进一步包括：

S310、将所述感兴趣区域内的点云数据上下翻转；

S320、设置初始布料，将其置于翻转后点云的最上方；

S330、将所有点云数据与初始布料网格粒子投影至同一平面，寻找每个布料粒子的最近激光点云，并记录该点云的高度值Hs；

S340、布料粒子向下移动，当其高度达到Hs时，停止移动；

S350、重复步骤S340，当所有布料粒子的最大高度变化达到设定的最小值或最大迭代次数时，布料模拟过程终止；以及

S360、若点云中的点与布料粒子间距小于设定的阈值thr时，则归类于地面点，反之为非地面点。

上述的在线检测水稻收割导航线的方法，其中，步骤S500进一步包括：

S510、将曲面上任意一点的单位法向量的起点平移到坐标原点，法向量的起点设定为单位球的球心，终点落在单位球的球面上，投影后形成的投影点的集合为高斯球；

S520、将高斯球面上的映射分为零维、一维和二维，其中，所述零维由重合点组成；所述一维由数据曲线的点组成；所述二维在高斯球面上占据为区域形式的簇；以及

S530、对点云数据中每个采样点p的单位法向量进行单位高斯球映射，得到所述点云的法向量在所述高斯球上的映射集。

上述的在线检测水稻收割导航线的方法，其中，步骤S600进一步包括：

S610、运用K-means对高斯映射点进行聚类，在每一个采样点p高斯映射点的集合中，选取k个元素，作为各自的中心点，再分别计算剩下的元素到k个簇中心的相异度为：

d_ij＝|x_i-x_j|+|y_i-y_j|+|z_i-z_j|,(x_i,y_i,z_i)∈p,(x_j,yj,z_j)∈p^k；

S620、根据曼哈顿距离d_ij最小把各点元素划分到不同的簇中，完成点集的聚类；

S630、通过聚类后的簇，再次求得每个簇内全部点元素在各自维度上的算数平均值，并作为聚类后每个簇的中心；

S640、对该中心重复聚类操作；

S650、循环执行步骤S610-S640，直至聚类生成的簇数量保持恒定时，得到聚类结果。

上述的在线检测水稻收割导航线的方法，其中，以p_i作为单位高斯球的圆心，若球面上只有一个聚类簇，则该映射点非特征点；若球面上涵盖2到4个簇，则该映射点为特征点；在多个面相交边上的点经过高斯映射后，在高斯球上通过聚类会产生多个簇，该类点为特征点；若聚类后所述高斯球上的簇为0或多于4个，则该类点为非特征点。

上述的在线检测水稻收割导航线的方法，其中，步骤S700进一步包括：

S710、设函数x和y之间的关系式为：

y＝ax+b；

其中，a和b分别为该直线的斜率和该直线与y轴的截距；

S720、设所述边界点的坐标为(x_i,y_i),i＝1,2,…n，总误差的平方为：

S730、对上述公式中的a、b分别求偏导数，得到：

S740、将上述公式进一步求解，得到a和b的最佳估计值：

S750、求拟合直线作为所述拟合的收割导航线，定义r为相关系数，表示两个变量之间函数关系与线性的符合程度，r∈[-1,1]：

当|r|→1，则x和y存在线性关系；当|r|→0，则x和y不存在线性关系。

本发明的技术效果在于：

本发明为收割机无人驾驶技术提供辅助导航，一方面，无人驾驶通过卫星进行导航，该种方式往往会因为地球环境、物体遮挡等原因造成卫星信号丢失从而导致导航系统无法正常工作，此时需要根据水稻收割情况提供实时、准确的辅助导航；另一方面，本发明拟合水稻收割边界，获取车辆与导航基准线之间的相对位置关系，从而引导车辆按照规划路径进行作业。3D激光雷达相较于传统机器视觉的方法，具有不受云雾、光照等天气影响的优点；3D激光雷达获取作物的三维坐标，更为精准；3D激光雷达具有更宽广的视野，能够提供更多的细节。3D激光雷达与IMU惯性传感器相结合，能够准确实时地反应收割机在某一时刻内的姿态角，从而提高识别精度，使获取的作物三维信息更加精准。在求取水稻边界点时，结合3D激光雷达的反射率信息，能够消除作物倒伏的情况下高度突变现象导致的误差点的影响。最小二乘法拟合直线具有最优解唯一、求解过程较为简单、准确度较高和响应速度快等优点。能够提高导航线拟合的准确率，提高作业效率。

以下结合附图和具体实施例对本发明进行详细描述，但不作为对本发明的限定。

附图说明

图1为本发明的工作原理图；

图2为本发明一实施例的3D激光雷达和惯性传感器安装位置示意图；

图3为本发明一实施例的车身坐标系、3D激光雷达坐标系和IMU坐标系示意图；

图4为本发明一实施例的坐标系变换图；

图5为本发明一实施例的水稻收割区域示意图；

图6为本发明一实施例的布料滤波原理图；

图7为本发明一实施例的高斯映射过程图；

图8A-8C为本发明一实施例的高斯映射点集图；

图9为本发明一实施例的导航线拟合示意图；

图10为本发明一实施例的横向误差示意图；

图11为本发明一实施例的航向误差示意图。

其中，附图标记

1收割机

2 3D激光雷达

3惯性传感器

4水稻

5割台

6割台边界延长线

7拟合的收割导航线

8横向误差

V前进方向

具体实施方式

下面结合附图对本发明的结构原理和工作原理作具体的描述：

收割机1无人驾驶技术主要通过卫星信号进行车辆自主行驶，但是由于树木遮挡等原因会造成部分卫星信号丢失，因此需要对收割边界实时监测，实现辅助导航。本发明的在线检测水稻4收割导航线的方法，包括如下步骤：

步骤S100、采用3D激光雷达2与IMU惯性传感器3，获取水稻4在收割机1的车身坐标系下的实时坐标并进行坐标变换，得到坐标变换后的车身坐标系下的水稻4作物的点云信息；

步骤S200、提取坐标变换后的点云信息的感兴趣区域，对所述感兴趣区域内的数据进行初步滤波处理，得到水稻4收割模型；

步骤S300、采用布料模拟滤波法滤除所述水稻4收割模型的地面点云，得到水稻4点云模型；

步骤S400、采用点云数据特征值分析法，利用三维坐标求得数据的协方差矩阵、特征值和特征向量，以求取点云的法向量；

步骤S500、将点云数据的法向量映射到高斯球面上，得到所述点云的法向量在高斯球上的映射集；

步骤S600、对高斯映射点集进行聚类分析，根据高斯映射点的多维分布规律判断得到特征点作为边界点；

步骤S700、采用最小二乘法拟合所述边界点，得到拟合的收割导航线7；以及

步骤S800、对比所述拟合的收割导航线7与实际收割过程中水稻4的边界线，计算横向误差8和航向误差，并验证精度。

其中，所述横向误差8为收割机1割台边界延长线6与所述拟合的收割导航线7之间的距离；所述航向误差为收割机1导航线所在直线与所述拟合的收割导航线7之间的夹角。

本实施例中，步骤S100进一步包括：

步骤S110、在收割机1上安装3D激光雷达2和IMU惯性传感器3，并确定所述3D激光雷达2和、IMU惯性传感器3和收割机1的坐标系；

步骤S120、3D激光雷达2标定，求得3D激光雷达2的初始安装角度；以及

步骤S130、采集待收割区域水稻4的点云数据，通过3D激光雷达2和IMU惯性传感器3数据获取水稻4在车身坐标系下的实时坐标并进行坐标变换。

其中，步骤S130进一步包括：

步骤S131、收割机1行进过程中，所述3D激光雷达2扫描前方水稻4作物的点云信息，所述IMU惯性传感器3高频反馈收割机1的即时欧拉角；

步骤S132、根据所述IMU惯性传感器3提供的姿态信息，包括3D激光雷达2坐标系和车身坐标系之间的旋转矩阵R_VC，根据3D激光雷达2安装高度信息，得到3D激光雷达2坐标系和车身坐标系之间的平移矩阵T_VC；

步骤S133、根据如下公式，将激光坐标系下的点云信息转换到车身坐标系下，得到坐标变换后的车身坐标系下的水稻4作物的点云信息：

其中，[x_C y_C z_C]^T为车身坐标系下的水稻4点云坐标，[x_V y_V z_V]^T为3D激光雷达2坐标系下的水稻4点云坐标。

本实施例中，步骤S200进一步包括：

步骤S210、综合考虑3D激光雷达2安装高度和角度、3D激光雷达2扫描角度和收割机1行进速度确定感兴趣区域；以及

步骤S220、采用直通滤波、半径滤波和双边滤波对所述感兴趣区域内的点云进行数据预处理，得到预处理后的点云。

其中，步骤S300进一步包括：

步骤S310、将所述感兴趣区域内的点云数据上下翻转；

步骤S320、设置初始布料，将其置于翻转后点云的最上方；

步骤S330、将所有点云数据与初始布料网格粒子投影至同一平面，寻找每个布料粒子的最近激光点云，并记录该点云的高度值Hs；

步骤S340、布料粒子向下移动，当其高度达到Hs时，停止移动；

步骤S350、重复步骤S340，当所有布料粒子的最大高度变化达到设定的最小值或最大迭代次数时，布料模拟过程终止；以及

步骤S360、若点云中的点与布料粒子间距小于设定的阈值thr时，则归类于地面点，反之为非地面点。

其中，步骤S500进一步包括：

步骤S510、将曲面上任意一点的单位法向量的起点平移到坐标原点，法向量的起点设定为单位球的球心，终点落在单位球的球面上，投影后形成的投影点的集合为高斯球；

步骤S520、将高斯球面上的映射分为零维、一维和二维，其中，所述零维由重合点组成；所述一维由数据曲线的点组成；所述二维在高斯球面上占据为区域形式的簇；以及

步骤S530、对点云数据中每个采样点p的单位法向量进行单位高斯球映射，得到所述点云的法向量在所述高斯球上的映射集。

其中，步骤S600进一步包括：

步骤S610、运用K-means对高斯映射点进行聚类，在每一个采样点p高斯映射点的集合中，选取k个元素，作为各自的中心点，再分别计算剩下的元素到k个簇中心的相异度为：

d_ij＝|x_i-x_j|+|y_i-y_j|+|z_i-z_j|,(x_i,y_i,z_i)∈p,(x_j,yj,z_j)∈p^k；

步骤S620、根据曼哈顿距离d_ij最小把各点元素划分到不同的簇中，完成点集的聚类；

步骤S630、通过聚类后的簇，再次求得每个簇内全部点元素在各自维度上的算数平均值，并作为聚类后每个簇的中心；

步骤S640、对该中心重复聚类操作；以及

步骤S650、循环执行步骤S610-S640，直至聚类生成的簇数量保持恒定时，得到聚类结果。

其中，以p_i作为单位高斯球的圆心，若球面上只有一个聚类簇，则该映射点非特征点；若球面上涵盖2到4个簇，则该映射点为特征点；在多个面相交边上的点经过高斯映射后，在高斯球上通过聚类会产生多个簇，该类点为特征点；若聚类后所述高斯球上的簇为0或多于4个，则该类点为非特征点。

其中，步骤S700进一步包括：

步骤S710、设函数x和y之间的关系式为：

y＝ax+b；

其中，a和b分别为该直线的斜率和该直线与y轴的截距；

步骤S720、设所述边界点的坐标为(x_i,y_i),i＝1,2,…n，总误差的平方为：

步骤S730、对上述公式中的a、b分别求偏导数，得到：

步骤S740、将上述公式进一步求解，得到a和b的最佳估计值：

以及

步骤S750、求拟合直线作为所述拟合的收割导航线7，定义r为相关系数，表示两个变量之间函数关系与线性的符合程度，r∈[-1,1]：

当|r|→1，则x和y存在线性关系；当|r|→0，则x和y不存在线性关系。

参见图1，图1为本发明的工作原理图。本实施例的3D激光雷达2和IMU惯性传感器3安装在同一平面内，二者的安装角度相同，且二者的X轴、Y轴和Z轴方向相同，原点位置不同。车身坐标系原点为安装支架中电在地面的投影，Z轴垂直于地面，Y轴方向为收割机1前进方向V。正式工作前，进行外参标定，寻找一片较为平坦地面，得到拟合公式，即可判断3D激光雷达2的安装高度和角度。收割水稻4过程中，根据IMU惯性传感器3实时提供的姿态角，得到3D激光雷达2下水稻4三维信息和车身坐标系下水稻4三维信息之间的变换公式，获取车身坐标系下实时水稻4信息。结合水稻4高度、雷达安装高度、雷达视场角等因素确定感兴趣区域尺寸大小，对感兴趣区域内的水稻4点云进行预处理，主要包括直通滤波、半径滤波和双边滤波，得到滤波处理后的水稻4收割模型。采用布料滤波的方法，滤除感兴趣区域水稻4模型的地面点云，得到水稻4的数字表面模型；采用点云数据特征值分析法，利用三维坐标求得数据的协方差矩阵、特征值和特征向量以求取点云的法向量；将点云法向量置于高斯球面中，得到高斯映射后的点集；对高斯映射点集进行聚类分析，不同的边界点在高斯球中具有不同的表现形式；根据聚类后的模型，判断点集呈零维、一维或二维分布，不同的分布代表不同的曲面模型，判断得到特征点，即为边界点；采用最小二乘法拟合候选边检点，得到拟合的导航线；根据导航线的公式计算得到其横向误差8和航向误差，验证精度，为收割机1辅助导航提供依据。

参见图2及图3，图2为本发明一实施例的3D激光雷达2和惯性传感器3安装位置示意图，图3为本发明一实施例的车身坐标系、3D激光雷达2坐标系和IMU坐标系示意图。其中，3D激光雷达2和IMU惯性传感器3在收割机1上的安装位置如图2所示，3D激光雷达2和IMU安装在支架上，且位于同一平面，安装支架位于收割机1顶端，固定平面与地面呈一定角度倾斜放置。车身坐标系、3D激光雷达2坐标系和IMU坐标系位置关系如图3所示，车身坐标系的原点为安装支架固定面板中点在地面的投影，车身坐标系Z轴方向垂直地面向上、Y轴方向为收割机1前进方向V；3D激光雷达2和IMU在安装时位于同一平面，二者之间X轴、Y轴和Z轴方向均相同，与地面倾斜角度也相同。

参见图4，图4为本发明一实施例的坐标系变换图。如图4所示，安装支架固定面板中点Q与3D激光雷达2坐标系和IMU坐标系之间的距离均为s，Q与地面之间的距离为H，安装面板与垂直方向所呈夹角为ɑ。寻找一块平坦宽敞的空地，利用3D激光雷达2对其进行扫描，采用最小二乘法进行平面拟合。

假设平面的方程如下：

z＝a₀x+a₁y+a₂；

寻找合适的平面S₀，使得空间内所有点集(x_i,y_i,z_i)到拟合平面的距离平方和最小，即：

空间直角坐标系中，坐标轴OVYV与平面所呈的夹角为3D激光雷达2在垂直平面内的俯仰角，即为3D激光雷达2坐标系绕车身坐标系OCYC轴转动的角度，记为初始俯仰角θ₀，即：

空间直角坐标系中，坐标轴OVXV与平面所呈的夹角为3D激光雷达2在水平平面内的横滚角，即为3D激光雷达2坐标系绕车身坐标系OCXC轴转动的角度，记为初始横滚角ψ₀，即：

拟合平面方程z＝a₀x+a₁y+a₂中，x、y变量为0时的z值，即常数项a₂的绝对值为三维激光坐标系原点OV距地面的初始高度，记作H₀。

由此。可以根据拟合得到的平面公式计算得到3D激光雷达2初始安装的俯仰角θ₀、横滚角ψ₀和初始安装高度H₀。

收割水稻4过程中，IMU高频反馈联合收割机1此时的姿态角，因此可以实时地计算3D激光雷达2坐标系相对于车身坐标系的旋转矩阵。

横滚角roll表示为绕X轴旋转的角度ψ，俯仰角pitch表示为绕Y轴旋转的角度θ，偏航角yaw表示为绕Z轴旋转的角度φ，旋转矩阵分别为：

最终的三维变换矩阵为：

如图4所示，3D激光雷达2安装的高度可以表示为，即为3D激光雷达2坐标系原点OV在车身坐标系下在ZC轴的距离HVC和在YC轴的距离LVC分别表示为：

H_VC＝H-s·cosα

L_VC＝s·sinα；

3D激光雷达2坐标系原点OV在车身坐标系下在XC轴的距离为0。

因此可得到3D激光雷达2坐标系相对于车身坐标系的平移矩阵：

所以，3D激光雷达2坐标系下的点云信息转换到车身坐标系下的公式如下所示：

其中，cθ表示cosθ，cψ表示cosψ，cφ表示cosφ，sθ表示sinθ，sψ表示sinψ，sφ表示sinφ。

参见图5，图5为本发明一实施例的水稻4收割区域示意图。如图5所示，3D激光雷达2扫描收割前方水稻4的点云信息，3D激光雷达2的垂直扫描角为γ，感兴趣区域为长方形，长和宽分别是L和K。将坐标变换后的点云数据按照感兴趣区域的尺寸进行裁剪和数据预处理。

其中，直通滤波包括：

①设置滤波维度为Z轴，高度最小值limit-min为0.1m，高度最大值limit-max为1.4m；

②遍历所有点云，删除不在值域内的点云；

③得到滤波后的点云。

半径滤波包括：

①设置搜索半径Radius Search的值为0.8mm，设置查询点的近邻点个数小于4的会被删除，即Min Neighbors In Radius的值为4；

②遍历所有点云，删除不符合条件的点；

③得到滤波后的点云。

双边滤波后，点云的数量不会发生变化，点云的强度会改变。

参见图6，图6为本发明一实施例的布料滤波原理图。如图6所示，下方序号a表示原始测量值，将其在Z轴方向上进行上下翻转，得到翻转后的图形即为序号b，最终在重力作用下落下的布料即为当前的地形，为序号c，可以反映当前地形的起伏。

布料滤波包括：

①设置网格大小res为0.5mm，设置布料硬度rigidness为1，设置时间步长T为0.65s，设置分类阈值thr为0.6，设置最大迭代次数int为500；

②遍历所有点云，删除不符合条件的点；

③得到滤波后的点云。由此可以得到去除地面点后的水稻4作物模型。

参见图7-图8C，图7为本发明一实施例的高斯映射过程图，图8A-8C为本发明一实施例的高斯映射点集图。对于平面上的点，其法向量为一个常数，且法向量的方向相同，因此在对其进行高斯映射时，平面上的点在高斯面上聚集为如图8A所示的一个点，被称为零维分布。若点云数据模型中存在圆柱面以及圆锥面，则其表面上的点被称为抛物点，对于任意一个抛物点，在求得其法向量后可以发现，法向量和其轴线之间的夹角都是固定的一个常数，因此投影后这些点在高斯面上形成一个如图8B所示的曲线，被称为一维分布。若存在球面或圆环面等曲面，求其法向量后对其进行高斯映射得到的为如图8C所示的分布在高斯面上的二维分布的点，即被称为二维分布。

根据前述步骤获得的目标单位法向量，部分法向量会出现相反方向的簇，因此设定点云数据中若点云单位法向量与Z轴正方向夹角大于90°时，此点法向量旋转180°；若点云单位法向量与Z轴正方向夹角小于90°时，法向量方向不变。

在X_CO_CY_C平面标记各个特征点，利用最小二乘法拟合直线，拟合过后的直线如图7所示。拟合的直线公式为：

y_c＝a_cx_c+b_c；

其中，a_c表示拟合直线的斜率，b_c表示拟合直线在Y_C轴的截距。

参见图9-图11，图9为本发明一实施例的导航线拟合示意图，图10为本发明一实施例的横向误差8示意图，图11为本发明一实施例的航向误差示意图。横向误差8定义为割台5右侧与收割边界点之间的距离。如图10所示，沿收割机1右割台5延长的方向定义了一个作业参考线，该参考线上的一个已知点(x₀,y₀)检测到的作物拟合直线的距离定义为收割机1的横向偏差。航向误差定义为边界直线和收割机1行驶方向所在直线的夹角，车身坐标系的Y轴正轴方向指向收割机1前进方向V，如图9所示，拟合直线与车身坐标系Y_C轴所呈的夹角为ε，可以用拟合直线的斜率表示：

本发明融合3D激光雷达2和IMU惯性传感器3实时提取水稻4收割导航线，3D激光雷达2和IMU惯性传感器3安装在同一平面内，共同集成在雷达安装架上，进一步安装在收割机1上，与收割机1呈一定角度倾斜安装，以便工作过程中更好的扫描前方作物的点云信息。3D激光雷达2坐标系中，以3D激光雷达2的正上方为Z轴，记作Zv；以3D激光雷达2接线缆所在位置为Y轴的负方向，接线缆负向记作Yv；通过空间坐标系右手法则确定X轴，记作Xv。IMU坐标系中，以IMU惯性传感器3的正上方为Z轴，记作ZI；以IMU惯性传感器3接线缆所在位置为Y轴的负方向，接线缆负向记作YI；通过空间坐标系右手法则确定X轴，记作XI。车身坐标系中，以雷达安装支架转轴所在位置在地面上的投影为原点OC，以与地面垂直向上的方向为Z轴，记作ZC；以车身前进方向V为Y轴正方向，记作YC；通过空间坐标系右手法则确定X轴，记作XC。

3D激光雷达2和IMU安装完成后，寻找一块平坦宽敞的地面，用于3D激光雷达2初步标定，利用3D激光雷达2扫描该平面获取点云数据，利用最小二乘法拟合平面，根据拟合得到的平面公式即可得到3D激光雷达2初始安装的俯仰角、横滚角和偏航角以及雷达距地面的安装高度。

收割机1行进过程中，3D激光雷达2扫描前方水稻4作物的点云信息，IMU高频反馈收割机1该时刻的欧拉角。根据IMU提供的姿态信息，包括可以计算得到3D激光雷达2坐标系和车身坐标系两个坐标系之间的旋转矩阵RVC，根据步骤二的3D激光雷达2安装高度信息，可以得到3D激光雷达2坐标系和车身坐标系两个坐标系之间的平移矩阵TVC。由此根据如下公式，即可将激光坐标系下的点云信息转换到车身坐标系下。

上式中，[x_C y_C z_C]^T表示车身坐标系下的水稻4点云坐标，[x_V y_V z_V]^T表示3D激光雷达2坐标系下的水稻4点云坐标，结合IMU反馈的姿态角得到的坐标系之间的旋转矩阵和雷达安装高度得到的坐标间移动矩阵，由此，得到了坐标变换后的车身坐标系下的水稻4作物的点云信息。

综合考虑3D激光雷达2安装高度和角度、3D激光雷达2扫描角度、收割机1行进速度等确定感兴趣区域的大小，并对点云进行裁剪和分析。对感兴趣区域内的点云进行数据预处理，包括直通滤波、半径滤波和双边滤波等，得到预处理过的点云。

直通滤波的作用是过滤掉在指定维度方向上取值不在给定值域内的点，包括：

①指定一个维度以及该维度下的值域；

②遍历点云中的每个点，判断该点在指定维度上的取值是否在值域内，删除取值不在值域内的点；

③遍历结束，留下的点即构成滤波后的点云。

半径滤波的原理是假定原始点云中每个激光点在指定的半径邻域内至少包含一定数量的近邻点，符合条件的激光点进行保留，反之，则视为噪声点进行剔除，作用是剔除悬空的孤立点或无效点，包括：

①设置搜索半径和近邻点个数的阈值；

②遍历所有点云，判断该点半径内近邻点的个数是否符合设定范围，删除不符合条件的点；

③遍历结束，留下的点即构成滤波后的点云。

双边滤波是从单纯考虑空间域点的位置的高斯滤波的基础上，又加上一个维度的权重，在点云处理上，可以叫做为特征域，即当前点的法向量与临近点的法向量。通过改变两个域上的高斯滤波的方差来保持边缘平滑的效果，包括：

①输入某一点p∈P，两个高斯权重σ_s，σ_r；

②查找出p的所有邻域点q，邻域半径为r＝2*σ_s；

③计算点p和其邻域点q的距离d_d，d_d＝||q-p||，强度差异I_d，I_d＝|I_p-I_q|；

④BF＝∑weight*I_p，W＝∑weight；

⑤

布料模拟滤波算法主要原理是将点云翻转，假设翻转过后的区域正上方存在虚拟布料且该布料具有一定硬度，在重力作用下布料的最终形状即是点云的数字高程模型，由此完成非地面点的剔除，包括：

①将感兴趣区域内点云数据上下翻转；

②设置初始布料，将其置于翻转后点云的最上方；

③将所有点云数据与初始布料网格粒子投影至同一平面，寻找每个布料粒子的最近激光点云，并记录该点云的高度值Hs；

④布料粒子向下移动，当其高度达到Hs时，停止移动；

⑤重复步骤④，当所有粒子的最大高度变化足够小或者达到设定的最大迭代次数时，布料模拟过程终止；

⑥若点云中的点与布料粒子间距小于设置的阈值thr时，则归类于地面点，反之为非地面点。

其中，进一步包括：

①数据样本标准化处理：

原始数据样本为：

按列计算样本的均值和标准差S_j，

数据标准化公式为：

由此得到标准化后的数据：

②计算样本的协方差矩阵：

上式中cov(x,y)＝E(xy)-E(x)E(y)。

③计算矩阵C的特征值λ₀≤λ₁≤λ₂≤…≤λ_n和对应的特征向量μ₀≤μ₁≤μ₂≤…≤μ_n。

④表面变化公式如下：

⑤由于平坦平面的协方差矩阵最小特征值为0，其表面变化为0，法向量为最小值λ₀。

高斯映射是指将曲面上任意一点的单位法向量的起点平移到坐标原点的过程，法向量的起点设定为单位球的球心，终点落在单位球的球面上，投影后形成的投影点的集合被称为高斯球。通常将高斯球面上的映射分为零维、一维和二维。零维由重合点组成；一维由数据曲线的点组成；二维在高斯球面上占据为区域形式的簇。

对点云数据中每个采样点p的单位法向量进行单位高斯球映射之后，运用K-means对高斯映射点进行聚类。在每一个采样点p高斯映射点的集合中，选取k个元素，作为各自的中心点，再分别计算剩下的元素到k个簇中心的相异度为：

d_ij＝|x_i-x_j|+|y_i-y_j|+|z_i-z_j|,(x_i,y_i,z_i)∈p,(x_j,yj,z_j)∈p^k；

把此类点元素根据曼哈顿距离d_ij最小划分到不同的簇中，完成点集的聚类。通过聚类后的诸多簇，再次求得每个簇内全部点元素在其维度上的算数平均值，并以此作为聚类后每个簇的中心；对该中心重复聚类操作；循环执行上述步骤，当聚类生成的簇数量保持恒定时，得到聚类结果。

如果pi作为单位高斯球的圆心，球上只有一个聚类簇则说明其潜在区域在一个平面内，映射点呈无维度的分布，此类点不是特征点；如果球面上涵盖2到4个簇，此类点呈一维分布，通过聚类操作能够得到4个簇，判定该点为特征点；在多个面相交边上的点经过高斯映射后，在高斯球上通过聚类会产生若干个簇，此类点同样判定为特征点；若经过聚类过程后高斯球上的簇数量为0或者多余4个，判定此类点并非特征点。

上述步骤中求得的特征点为空间点，由于拟合的直线用于收割机1的辅助导航，因此只需考虑候选边界点在XCOYC平面内的值，而无需考虑在ZC轴上的数值，可在一定程度上降低算法难度，提高响应速度。

最小二乘算法包括：

①设函数x和y之间的关系式为

y＝ax+b；

上式中两个参数a和b分别表示该直线的斜率和该直线与y轴的截距。

②选取的候选边界点坐标为(x_i,y_i),i＝1,2,…n，总误差的平方为：

③对上述公式中的a、b分别求偏导数，可得如下公式：

④将上述公式进一步求解即可得到a和b的最佳估计值，如下式所示：

由此，可求出拟合的直线的公式。

⑤定义r为相关系数，表示两个变量之间函数关系与线性的符合程度，r∈[-1,1]，

当|r|→1，说明x和y的相关性较好存在线性关系；当|r|→0，说明x和y的相关性较差，不存在线性关系。

本发明可在线检测水稻4收割导航线，采用3D激光雷达2与IMU惯性传感器3相结合获取雷达与车身之间的实时变换矩阵，并根据矩阵获取前方收割作物在车身坐标下的点云信息；对坐标变换后的点云信息进行感兴趣(ROI)区域的提取，裁剪去除非感兴趣区域内的数据；对ROI区域内的数据进行初步滤波处理，得到水稻4收割模型；采用布料滤波的方法，滤除感兴趣区域水稻4模型的地面点云，得到水稻4的数字表面模型；采用点云数据特征值分析法，利用三维坐标求得数据的协方差矩阵、特征值和特征向量以求取点云的法向量；将点云法向量置于高斯球面中，得到高斯映射后的点集；对高斯映射点集进行聚类分析，不同的边界点在高斯球中具有不同的表现形式；根据聚类后的模型，判断点集呈零维、一维或二维分布，不同的分布代表不同的曲面模型，判断得到特征点，即为边界点；采用最小二乘法拟合候选边检点，得到拟合的导航线；根据导航线的公式计算得到其横向误差8和航向误差，验证精度，为收割机1辅助导航提供依据，能够提高导航线拟合的准确率，提高作业效率。

当然，本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (10)

1.一种在线检测水稻收割导航线的方法，其特征在于，包括如下步骤：

S100、采用3D激光雷达与IMU惯性传感器，获取水稻在收割机的车身坐标系下的实时坐标并进行坐标变换，得到坐标变换后的车身坐标系下的水稻作物的点云信息；

S200、提取坐标变换后的点云信息的感兴趣区域，对所述感兴趣区域内的数据进行初步滤波处理，得到水稻收割模型；

S300、采用布料模拟滤波法滤除所述水稻收割模型的地面点云，得到水稻点云模型；

S400、采用点云数据特征值分析法，利用三维坐标求得数据的协方差矩阵、特征值和特征向量，以求取点云的法向量；

S500、将点云数据的法向量映射到高斯球面上，得到所述点云的法向量在高斯球上的映射集；

S600、对高斯映射点集进行聚类分析，根据高斯映射点的多维分布规律判断得到特征点作为边界点；

S700、采用最小二乘法拟合所述边界点，得到拟合的收割导航线；以及

S800、对比所述拟合的收割导航线与实际收割过程中水稻的边界线，计算横向误差和航向误差，并验证精度。

2.如权利要求1所述的在线检测水稻收割导航线的方法，其特征在于，所述横向误差为收割机割台边界延长线与所述拟合的收割导航线之间的距离；所述航向误差为收割机导航线所在直线与所述拟合的收割导航线之间的夹角。

3.如权利要求1所述的在线检测水稻收割导航线的方法，其特征在于，步骤S100进一步包括：

S110、在收割机上安装3D激光雷达和IMU惯性传感器，并确定所述3D激光雷达和、IMU惯性传感器和收割机的坐标系；

S120、3D激光雷达标定，求得3D激光雷达的初始安装角度；以及

S130、采集待收割区域水稻的点云数据，通过3D激光雷达和IMU惯性传感器数据获取水稻在车身坐标系下的实时坐标并进行坐标变换。

4.如权利要求1所述的在线检测水稻收割导航线的方法，其特征在于，步骤S130进一步包括：

S131、收割机行进过程中，所述3D激光雷达扫描前方水稻作物的点云信息，所述IMU惯性传感器高频反馈收割机的即时欧拉角；

S132、根据所述IMU惯性传感器提供的姿态信息，包括3D激光雷达坐标系和车身坐标系之间的旋转矩阵R_VC，根据3D激光雷达安装高度信息，得到3D激光雷达坐标系和车身坐标系之间的平移矩阵T_VC；

S133、根据如下公式，将激光坐标系下的点云信息转换到车身坐标系下，得到坐标变换后的车身坐标系下的水稻作物的点云信息：

其中，[x_C y_C z_C]^T为车身坐标系下的水稻点云坐标，[x_V y_V z_V]^T为3D激光雷达坐标系下的水稻点云坐标。

5.如权利要求1所述的在线检测水稻收割导航线的方法，其特征在于，步骤S200进一步包括：

S210、综合考虑3D激光雷达安装高度和角度、3D激光雷达扫描角度和收割机行进速度确定感兴趣区域；

S220、采用直通滤波、半径滤波和双边滤波对所述感兴趣区域内的点云进行数据预处理，得到预处理后的点云。

6.如权利要求1所述的在线检测水稻收割导航线的方法，其特征在于，步骤S300进一步包括：

S310、将所述感兴趣区域内的点云数据上下翻转；

S320、设置初始布料，将其置于翻转后点云的最上方；

S330、将所有点云数据与初始布料网格粒子投影至同一平面，寻找每个布料粒子的最近激光点云，并记录该点云的高度值Hs；

S340、布料粒子向下移动，当其高度达到Hs时，停止移动；

S350、重复步骤S340，当所有布料粒子的最大高度变化达到设定的最小值或最大迭代次数时，布料模拟过程终止；以及

S360、若点云中的点与布料粒子间距小于设定的阈值thr时，则归类于地面点，反之为非地面点。

7.如权利要求1所述的在线检测水稻收割导航线的方法，其特征在于，步骤S500进一步包括：

S510、将曲面上任意一点的单位法向量的起点平移到坐标原点，法向量的起点设定为单位球的球心，终点落在单位球的球面上，投影后形成的投影点的集合为高斯球；

S520、将高斯球面上的映射分为零维、一维和二维，其中，所述零维由重合点组成；所述一维由数据曲线的点组成；所述二维在高斯球面上占据为区域形式的簇；以及

S530、对点云数据中每个采样点p的单位法向量进行单位高斯球映射，得到所述点云的法向量在所述高斯球上的映射集。

8.如权利要求1所述的在线检测水稻收割导航线的方法，其特征在于，步骤S600进一步包括：

S610、运用K-means对高斯映射点进行聚类，在每一个采样点p高斯映射点的集合中，选取k个元素，作为各自的中心点，再分别计算剩下的元素到k个簇中心的相异度为：

d_ij＝|x_i-x_j|+|y_i-y_j|+|z_i-z_j|,(x_i,y_i,z_i)∈p,(x_j,yj,z_j)∈p^k；

S620、根据曼哈顿距离d_ij最小把各点元素划分到不同的簇中，完成点集的聚类；

S630、通过聚类后的簇，再次求得每个簇内全部点元素在各自维度上的算数平均值，并作为聚类后每个簇的中心；

S640、对该中心重复聚类操作；

S650、循环执行步骤S610-S640，直至聚类生成的簇数量保持恒定时，得到聚类结果。

9.如权利要求8所述的在线检测水稻收割导航线的方法，其特征在于，以p_i作为单位高斯球的圆心，若球面上只有一个聚类簇，则该映射点非特征点；若球面上涵盖2到4个簇，则该映射点为特征点；在多个面相交边上的点经过高斯映射后，在高斯球上通过聚类会产生多个簇，该类点为特征点；若聚类后所述高斯球上的簇为0或多于4个，则该类点为非特征点。

10.如权利要求1所述的在线检测水稻收割导航线的方法，其特征在于，步骤S700进一步包括：

S710、设函数x和y之间的关系式为：

y＝ax+b；

其中，a和b分别为该直线的斜率和该直线与y轴的截距；

S720、设所述边界点的坐标为(x_i,y_i),i＝1,2,…n，总误差的平方为：

S730、对上述公式中的a、b分别求偏导数，得到：

S740、将上述公式进一步求解，得到a和b的最佳估计值：

S750、求拟合直线作为所述拟合的收割导航线，定义r为相关系数，表示两个变量之间函数关系与线性的符合程度，r∈[-1,1]：

当|r|→1，则x和y存在线性关系；当|r|→0，则x和y不存在线性关系。