一种基于车载激光雷达的山地农田坡度估计方法

本发明涉及非道路路面坡度检测，具体为一种基于车载激光雷达的山地农田坡度估计方法。

背景技术：

1、随着我国丘陵山地农业机械化的发展，丘陵山地对于农机的需求越来越迫切，各类适用于丘陵山地农田工作环境的农机层出不穷。丘陵山地农机常被用于在多坡道环境作业，因此对其在坡地上的稳定性要求极高。现阶段多采用机械自动调平方法较多，没有对坡度进行估计，无法适应坡道作业，也无法满足坡道作业要求，另外，丘陵山地农田的地面特征与道路的路面特征不同。道路路面比较平整，有规律。而农田的地面十分复杂，在使用激光雷达对地面特征进行分析时会更加困难。

2、丘陵山地农田的地形通常呈现起伏不平的特点，地面有高低起伏的坡度，在农机进行作业的土地上，土壤松软，地面存在大量的间隙和形状不规则的土块，在使用激光雷达的激光束对地面进行扫描的过程中，激光束不可避免地会扫描到这些间隙和土块，这就使得采集到的点云数据存在大量的无用点，如何找出最符合的点集进行坡度的估计成为一大难题，从而导致现有的基于激光雷达的无人农机的坡度识别方法，并没有考虑丘陵山地的地面特征，估计出的坡度与实际坡度误差较大，精确度低。

技术实现思路

1、(一)解决的技术问题

2、针对现有技术的不足，本发明提供了一种基于车载激光雷达的山地农田坡度估计方法，具备避免采集过程中的无用点对识别过程产生影响等优点，解决了上述技术问题。

3、(二)技术方案

4、为实现上述目的，本发明提供如下技术方案：一种基于车载激光雷达的山地农田坡度估计方法，包括以下步骤：

5、s1、对农机进行安装激光雷达传感器，并建立激光雷达传感器的观测模型，标定激光雷达，并返回采集的坐标值p(x，y，z)；

6、s2、采集激光雷达点云数据，获取点云数据点的坐标信息、法向量信息，通过坐标关系，剪切出农机前方指定区域点云数据作为待用点云数据；

7、s3、对激光点云数据集进行滤波，对离群点和噪声进行去除；

8、s4、通过算法公式对进行滤波后的激光点云数据集进行坡道平面拟合；

9、s5、通过拟合平面方程对农机前方指定区域进行坡度估计，并对估计结果进行处理。

10、作为本发明的优选技术方案，所述步骤s1中的观测模型建立步骤如下：

11、s1.1、对激光雷达坐标系进行定义，确定三维坐标系，以当前农机前进方向为x轴正方向，当前农机前进方向的左侧为y轴正方向，当前农机的竖直向上的方向作为z轴正方向；

12、s1.2、对z轴坐标进行非负化处理，即对采集到的所有坐标点上的z轴坐标加上激光雷达传感器相对于地面的安装高度；

13、s1.3、激光雷达传感器返回坐标值为p(x，y，z)。

14、作为本发明的优选技术方案，所述步骤s2中的具体步骤如下：

15、s2.1、在农机两前轮正前方q米处选取两块全等的矩形；

16、s2.2、对采样矩形的范围进行限制，两块矩形之间相距l米，且长为m米，宽为n米，农机的轴距为l米，轮距为b米，且m≥l，n的长度大于车轮宽度；

17、s2.3、剪切出的两块矩形区域的点云数据满足以下条件：

18、

19、其中，x、y、z分别为采集点的坐标，|*|表示绝对值操作，之后将所有符合条件的点云数据保存为待用点云数据集。

20、作为本发明的优选技术方案，所述步骤s3中的离群点和噪声包括激光束达到土壤缝隙中和小土块等非真实地面上的激光点，具体的去噪步骤如下：

21、s3.1、设置点云数据集r＝(r1,r2,…,rn)，其中n表示的集合中数据个数，之后从点云数据集r中随机选取三个数据点，通过选定的三个点建立平面模型；

22、s3.2、对点云数据集r中每个点进行测试，判断其是否与当前平面模型匹配，若匹配则命名为内点，若不匹配则命名为外点，同时计算剩余点到该平面的距离，若小于初始设定的阈值t则更改其对应的命名为内点，仍不满足的条件的点作为外点；

23、s3.3、在每一次迭代中的判定完成后，重新选择数据点构建新的平面模型，并重复步骤s3.2的过程，之后将所有匹配的内点的数量进行统计，之后选取内点数最大的一个模型，同时将所有点针对选取的模型进行点到平面模型的距离计算，将小于阈值t的点作为新的合集；

24、s3.4、对正确的概率进行计算；

25、s3.5、若迭代次数已经达到m次或以及检索到满足条件的模型，则停止迭代，返回内点最多的模型，并执行步骤s3.6，若未找到满足条件的模型，则重新迭代，直到找到满足条件的模型；

26、s3.6、在得到模型参数后，根据该平面模型保留所有的内点，滤除所有的外点，将内点构成的点云数据保存为滤波后的点云数据集。

27、作为本发明的优选技术方案，所述步骤s3.4中的正确概率p的计算公式如下：

28、p＝1-[1-(1-e)s]m

29、其中，e表示外点率，即外点个数除以整个点集的个数，m为迭代次数。

30、作为本发明的优选技术方案，所述步骤s4中对平面拟合的具体步骤如下：

31、s4.1、建立拟合坡道区域平面方程：

32、ax+by+cz+d＝0

33、a2+b2+c2＝1

34、其中，a、b、c、d为拟合平面参数，a2+b2+c2＝1为约束条件；

35、s4.2、根据得到的拟合平面参数a、b、c、d对点到平面的距离平方和er进行表达；

36、s4.3、用拉格朗日乘子法求解极值，构造拉格朗日函数f：

37、f＝er-μ(a2+b2+c2-1)

38、其中，er为点到平面的距离平方和，μ为拉格朗日乘子；

39、s4.4、对平面的质心进行计算，通过特征方程对特征值和特征向量进行求解，最终对a、b、c的值求解，分别满足如下表达式：

40、

41、

42、

43、其中，xi、yi、zi分别表示点云数据中的任意数据点pi(xi,yi,zi)中的坐标，n表示数据集中的数据个数，表示对内部数据进行求和运算；

44、s4.5、最后得到该点坡道拟合平面。

45、作为本发明的优选技术方案，所述步骤s4.2中的表达式如下：

46、

47、

48、其中，di表示点云数据中任意数据点pi(xi,yi,zi)到这个平面的距离，er表示点到平面的距离平方和，且需要保证er的值最小。

49、作为本发明的优选技术方案，所述步骤s4.4中的计算公式如下：

50、ax0＝μx0

51、

52、

53、

54、

55、

56、其中，x0表示对a、b、c的解组成的矩阵，a为特征矩阵，δxi、δyi、δzi分别表示第i个点三维坐标与质心坐标之间的差值，同时求解公式表达式如下：

57、

58、其中，上标t表示的是对矩阵的转置，并在约束条件a2+b2+c2＝1下进行求解，具体表达式如下：

59、

60、最后求出a、b、c以及di的值。

61、作为本发明的优选技术方案，所述步骤s5坡度估计的具体过程如下：

62、s5.1、在拟合平面中随机找不共线的三个点分别为q1(x1,y1,z1)、q2(x2,y2,z2)和q3(x3,y3,z3)；

63、s5.2、根据步骤s5.1中拟合平面内的不共线的三个点的出两个直线向量和并对其法向量进行计算；

64、s5.3、根据农机所处的xoy平面的法向量对坡度角进行计算。

65、作为本发明的优选技术方案，所述坡度角θ计算公式如下：

66、

67、

68、其中，表示向量点乘，和分别表示向量的模。

69、与现有技术相比，本发明提供了一种基于车载激光雷达的山地农田坡度估计方法，具备以下有益效果：

70、1、本发明通过采用了对农机前方地面指定区域坡度的检测算法，可以为农机底盘调平提供数据基础，有利于提高农机在复杂山地环境下的作业精度，也有利于智能驾驶的发展，在坡度变化较为激烈的环境下，对农机前方指定区域坡度的检测，有利于农机提前作出反应，调整速度等，提高了农机作业过程中的安全性和稳定性。

71、2、本发明通过算法拟合指定区域点云平面，通过剪切出有用区域的点云信息，过滤区域点云数据中的无用点，剔除无用点后，再拟合平面，最后利用拟合平面的法向量估计坡度，最终得到农机前面指定区域的坡度信息。