基于二维激光雷达的果园车辆地头导航方法

1.本发明涉及果园车辆导航领域，尤其是果园车辆地头导航，具体地说是一种基于二维激光雷达的果园车辆地头导航方法


背景技术：

2.果园车辆导航技术是实现果园精细化管理的核心技术之一，广泛应用于植保、除草、采摘等果园生产过程，可有效提高果园作业的精度效率，减轻操作人员的劳动强度。
3.二维激光雷达(light detection and ranging，lidar)具有测距精度高、分辨率高、抗干扰能力强等优点，广泛应用于果园环境感知和果树信息提取，成为果园车辆导航领域的研究热点。安装时令二维激光雷达扫描面与地面平行，实时获取车辆周围果树树干的轮廓信息，从中提取导航路径，计算车辆当前位置与导航路径的偏差，引导车辆沿导航路径行驶。
4.上述基于二维激光雷达的果园车辆导航方法仅应用于果园行间导航，在车辆地头转弯时，通常采用开环控制，令车辆按照固定的速度和航向角行驶一段圆弧。该方法简单易行，但抗干扰能力差，难以适应车辆在地头有较大横向偏差/航向偏差以及地面粗糙度较大的复杂情况。


技术实现要素：

5.本发明的目的是针对果园车辆地头导航问题，本发明提出一种基于2d lidar的果园车辆地头导航方法，在地头转弯过程中根据实时获取的车辆位置与圆弧导航路径的偏差调整车辆姿态，引导车辆沿圆弧导航路径行驶。
6.本发明的技术方案是：
7.本发明提供一种基于二维激光雷达的果园车辆地头导航方法，该方法包括：
8.s1、地头导航开始检测：获取二维激光雷达数据，判断车辆是否到达果园行末，如已到达，执行s2，开始地头导航；
9.s2、地头导航路径提取：从二维激光雷达数据中提取圆弧导航路径；
10.s3、地头导航偏差计算：计算当前车辆位置与圆弧导航路径的横向偏差e
l
和航向偏差eh；
11.s4、地头导航路径跟踪：根据横向偏差e
l
和航向偏差eh，计算左右轮期望车速引导车辆沿圆弧导航路径行驶，经过δt时刻，执行s5；
12.s5、地头导航结束检测：获取二维激光雷达数据，判断车辆是否到达果园行首，如已到达，结束地头导航，否则，执行s2。
13.进一步地，s1具体为：
14.s1-1、根据二维激光雷达数据提取导航路径，计算车辆横向偏差e
l
和航向偏差eh；
15.s1-2、根据车辆横向偏差e
l
和航向偏差eh，对二维激光雷达数据进行坐标校正；
[0016][0017]
其中；二维激光雷达数据的坐标系原点位于雷达中心，y轴正方向指向车辆前进方向，x轴正方向指向车辆右侧；(x,y)和(x
′
,y
′
)分别为校正前、后二维激光雷达的数据坐标；
[0018]
s1-3、对校正后二维激光雷达的数据坐标进行判断，若横坐标[-2dr,2dr]、纵坐标[0,2dr]围成的长方形区域内无二维激光雷达数据，则车辆到达行末，dr表示果园行矩。
[0019]
进一步地，所述的导航路径提取方法为行间导航路径提取算法。
[0020]
进一步地，圆弧导航路径以转弯侧行末树干所在位置c为圆心，果树行距dr为直径；其中圆心c的位置采用下述步骤获取：
[0021]
首次执行时：
[0022]
筛选出以原点为中心、4dr为边长的正方形区域内的二维激光雷达数据，聚类得到若干个树干簇；计算各树干簇的重心，若为右转弯，取横坐标与纵坐标之和最大的树干簇重心作为圆心c，若为左转弯，取纵坐标与横坐标之差最大的树干簇重心作为圆心c；
[0023]
第二次执行开始：
[0024]
根据上一时刻圆心c的坐标估计其在当前时刻的位置
[0025][0026]
其中：分别表示s4获取的上一时刻左右轮期望车速，δt表示相邻两次导航的时间间隔，d表示车辆轮距，d表示车辆质心与两个驱动轮几何中心之间的距离，δθ表示在δt时间间隔内车辆的航向角变化量，
[0027]
筛选出以为中心、4d
t
为边长的正方形区域内的二维激光雷达数据，获取该数据的重心作为当前时刻圆心c的实际位置(xc,yc)，d
t
为果园果树最大胸径。
[0028]
进一步地，所述的聚类采用具有噪声的基于密度的聚类方法dbscan，参数扫描半径取0.1m，最小邻域点数取1。
[0029]
进一步地，s3具体为：
[0030]
设雷达中心即原点o与圆弧导航路径的圆心c的连线与圆弧导航路径的交点为g，横向偏差是原点o与g的距离，以o在g的右边为正，左边为负；过g点作圆弧切线，航向偏差是车辆方向与该切线的夹角，以车辆右偏为正，左偏为负；
[0031]
采用下述公式计算横向偏差e
l
和航向偏差eh：
[0032][0033]
[0034]
进一步地，s4中，采用模糊自适应纯追踪算法计算左右轮期望车速
[0035]
进一步地，s5包括：
[0036]
s5-1、根据s4获取的横向偏差e
l
和航向偏差eh，按照s1-2对二维激光雷达数据进行坐标校正；
[0037]
s5-2、行首检测：右转弯时，若横坐标[-2dr,0]、纵坐标[-2d
t
,2d
t
]围成的长方形区域内有二维激光雷达数据，代表车辆到达行首；左转弯时，若横坐标[0,2dr]、纵坐标[-2d
t
,2d
t
]围成的长方形区域内有二维激光雷达数据，代表车辆到达行首。
[0038]
进一步地，s1和s5中，获取的二维激光雷达数据每次采集1帧。
[0039]
本发明的有益效果：
[0040]
本发明提出的基于2d lidar的果园车辆地头导航方法通过2d lidar实时获取车辆位置与圆弧导航路径的偏差，引导车辆沿圆弧导航路径行驶。这种闭环控制方法的抗干扰能力强，能够适应车辆在地头有较大横向偏差/航向偏差以及地面粗糙度较大的复杂情况，是对已有基于2d lidar的果园车辆导航方法的补充和完善。
[0041]
本发明的其它特征和优点将在随后具体实施方式部分予以详细说明。
附图说明
[0042]
通过结合附图对本发明示例性实施方式进行更详细的描述，本发明的上述以及其它目的、特征和优势将变得更加明显，其中，在本发明示例性实施方式中，相同的参考标号通常代表相同部件。
[0043]
图1示出了本发明的果园车辆地头导航流程图；
[0044]
图2示出了实施例中2d lidar数据坐标校正前后对比示意图。
[0045]
图3示出了实施例中导航偏差位置示意图；
[0046]
图4示出了本发明的导航方法与开环控制方法导航的车辆轨迹对比示意图。
具体实施方式
[0047]
下面将参照附图更详细地描述本发明的优选实施方式。虽然附图中显示了本发明的优选实施方式，然而应该理解，可以以各种形式实现本发明而不应被这里阐述的实施方式所限制。
[0048]
本发明提供一种基于二维激光雷达的果园车辆地头导航方法，如图1所示，该方法包括：
[0049]
s1、地头导航开始检测：采集1帧2d lidar数据，判断车辆是否到达果园行末，如已到达，执行s2，开始地头导航，具体为：
[0050]
s1-1、采用行间导航路径提取算法(选择一种已有算法，如李秋洁,丁旭东,邓贤.基于激光雷达的果园行间路径提取与导航[j].农业机械学报,2020,51(s2):344-350.)提取导航路径，计算车辆横向偏差e
l
和航向偏差eh；
[0051]
s1-2、根据车辆横向偏差e
l
和航向偏差eh，对二维激光雷达数据进行坐标校正，如图2所示，为2d lidar数据坐标校正前后对比示意图，左：校正前的2d lidar数据右：校正后的2d lidar数据；
[0052][0053]
其中；二维激光雷达数据的坐标系原点位于雷达中心，y轴正方向指向车辆前进方向，x轴正方向指向车辆右侧；(x,y)和(x
′
,y
′
)分别为校正前、后二维激光雷达的数据坐标；
[0054]
s1-3、对校正后二维激光雷达的数据坐标进行判断，若横坐标[-2sr,2dr]、纵坐标[0,2dr]围成的长方形区域内无二维激光雷达数据，则车辆到达行末，dr表示果园行矩。
[0055]
s2、地头导航路径提取：从二维激光雷达数据中提取圆弧导航路径；圆弧导航路径以转弯侧行末树干所在位置c为圆心，果树行距dr为直径。由于dr由行间导航方法测得，此处只需确定c的位置：
[0056]
首次执行时：
[0057]
筛选出以原点为中心、4dr为边长的正方形区域内的二维激光雷达数据，采用具有噪声的基于密度的聚类(density-based spatial clustering of applications with noise，dbscan)方法对上述2d lidar数据进行聚类，得到若干个树干簇；dbscan算法参数扫描半径取0.1m，最小邻域点数取；
[0058]
计算各树干簇的重心，若为右转弯，取横坐标与纵坐标之和最大的树干簇重心作为圆心c，若为左转弯，取纵坐标与横坐标之差最大的树干簇重心作为圆心c；
[0059]
第二次执行开始：
[0060]
根据上一时刻圆心c的坐标估计其在当前时刻的位置
[0061][0062]
其中：分别表示s4获取的上一时刻左右轮期望车速，δt表示相邻两次导航的时间间隔，d表示车辆轮距，d表示车辆质心与两个驱动轮几何中心之间的距离，δθ表示在δt时间间隔内车辆的航向角变化量，
[0063]
筛选出以为中心、4d
t
为边长的正方形区域内的二维激光雷达数据，获取该数据的重心作为当前时刻圆心c的实际位置(xc,yc)，d
t
为果园果树最大胸径。
[0064]
s3、地头导航偏差计算：计算当前车辆位置与圆弧导航路径的横向偏差e
l
和航向偏差eh，具体为：
[0065]
如图3所示，设雷达中心即原点o与圆弧导航路径的圆心c的连线与圆弧导航路径的交点为g，横向偏差是原点o与g的距离，以o在g的右边为正，左边为负；过g点作圆弧切线，航向偏差是车辆方向与该切线的夹角，以车辆右偏为正，左偏为负；
[0066]
采用下述公式计算横向偏差e
l
和航向偏差eh：
[0067]
[0068][0069]
s4、地头导航路径跟踪：根据横向偏差e
l
和航向偏差eh，采用模糊自适应纯追踪算法(参考文献：陈军,蒋浩然,刘沛,等.果园移动机器人曲线路径导航控制[j].农业机械学报,2012,43(4):179-182.)计算左右轮期望车速引导车辆沿圆弧导航路径行驶，s4执行后经过δt时刻，执行s5。
[0070]
s5、地头导航结束检测：采集1帧2d lidar数据，判断车辆是否到达果园行首，如已到达，结束地头导航，否则，执行s2；具体为：
[0071]
s5-1、根据s4获取的横向偏差e
l
和航向偏差eh，按照s1-2对二维激光雷达数据进行坐标校正；
[0072]
s5-2、行首检测：右转弯时，若横坐标[-2dr,0]、纵坐标[-2d
t
,2d
t
]围成的长方形区域内有二维激光雷达数据，代表车辆到达行首；左转弯时，若横坐标[0,2dr]、纵坐标[-2d
t
,2d
t
]围成的长方形区域内有二维激光雷达数据，代表车辆到达行首。
[0073]
具体实施时：
[0074]
采用matlab进行仿真实验，仿真参数如表1所示。
[0075]
表1仿真参数
[0076][0077]
为对比本文方法与开环控制的地头转弯算法，以右转为例，选取车辆初始横向偏差、初始航向偏差和路面粗糙度3个参数作为试验因子，分别进行单因素实验，对某一个试验因子进行实验时，其他试验因子设为0。参数具体取值如表2所示。为模拟粗糙地面，在理想车辆位置的坐标值上添加加性白噪声，用白噪声功率谱密度作为地面粗糙度参数。表2给出了单因素实验结果，图4给出了车辆轨迹。当车辆在地头有较大横向偏差/航向偏差以及地面粗糙度较大时，与开环控制方法相比，本文方法有更小的横向偏差，能较为准确地引导车辆沿圆弧导航路径行驶。
[0078]
表2单因素实验结果
[0079][0080]
以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。