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

一种基于二维激光雷达的果园车辆地头导航方法，该方法包括地头导航开始检测步骤、地头导航路径提取步骤、地头导航偏差计算步骤、地头导航路径跟踪步骤以及地头导航结束检测步骤；本发明专利技术提出的基于2D LiDAR的果园车辆地头导航方法通过2DLiDAR实时获取车辆位置与圆弧导航路径的偏差，引导车辆沿圆弧导航路径行驶。这种闭环控制方法的抗干扰能力强，能够适应车辆在地头有较大横向偏差/航向偏差以及地面粗糙度较大的复杂情况，是对已有基于2D LiDAR的果园车辆导航方法的补充和完善。LiDAR的果园车辆导航方法的补充和完善。LiDAR的果园车辆导航方法的补充和完善。

【技术实现步骤摘要】
基于二维激光雷达的果园车辆地头导航方法


[0001]本专利技术涉及果园车辆导航领域，尤其是果园车辆地头导航，具体地说是一种基于二维激光雷达的果园车辆地头导航方法

技术介绍

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

技术实现思路

[0005]本专利技术的目的是针对果园车辆地头导航问题，本专利技术提出一种基于2D LiDAR的果园车辆地头导航方法，在地头转弯过程中根据实时获取的车辆位置与圆弧导航路径的偏差调整车辆姿态，引导车辆沿圆弧导航路径行驶。
[0006]本专利技术的技术方案是：
[0007]本专利技术提供一种基于二维激光雷达的果园车辆地头导航方法，该方法包括：
[0008]S1、地头导航开始检测：获取二维激光雷达数据，判断车辆是否到达果园行末，如已到达，执行S2，开始地头导航；
[0009]S2、地头导航路径提取：从二维激光雷达数据中提取圆弧导航路径；
[0010]S3、地头导航偏差计算：计算当前车辆位置与圆弧导航路径的横向偏差e
l
和航向偏差e
h
；
[0011]S4、地头导航路径跟踪：根据横向偏差e
l
和航向偏差e
h
，计算左右轮期望车速引导车辆沿圆弧导航路径行驶，经过Δt时刻，执行S5；
[0012]S5、地头导航结束检测：获取二维激光雷达数据，判断车辆是否到达果园行首，如已到达，结束地头导航，否则，执行S2。
[0013]进一步地，S1具体为：
[0014]S1
‑
1、根据二维激光雷达数据提取导航路径，计算车辆横向偏差e
l
和航向偏差e
h
；
[0015]S1
‑
2、根据车辆横向偏差e
l
和航向偏差e
h
，对二维激光雷达数据进行坐标校正；
[0016][0017]其中；二维激光雷达数据的坐标系原点位于雷达中心，y轴正方向指向车辆前进方向，x轴正方向指向车辆右侧；(x,y)和(x
′
,y
′
)分别为校正前、后二维激光雷达的数据坐标；
[0018]S1
‑
3、对校正后二维激光雷达的数据坐标进行判断，若横坐标[
‑
2d
r
,2d
r
]、纵坐标[0,2d
r
]围成的长方形区域内无二维激光雷达数据，则车辆到达行末，d
r
表示果园行矩。
[0019]进一步地，所述的导航路径提取方法为行间导航路径提取算法。
[0020]进一步地，圆弧导航路径以转弯侧行末树干所在位置C为圆心，果树行距d
r
为直径；其中圆心C的位置采用下述步骤获取：
[0021]首次执行时：
[0022]筛选出以原点为中心、4d
r
为边长的正方形区域内的二维激光雷达数据，聚类得到若干个树干簇；计算各树干簇的重心，若为右转弯，取横坐标与纵坐标之和最大的树干簇重心作为圆心C，若为左转弯，取纵坐标与横坐标之差最大的树干簇重心作为圆心C；
[0023]第二次执行开始：
[0024]根据上一时刻圆心C的坐标估计其在当前时刻的位置
[0025][0026]其中：分别表示S4获取的上一时刻左右轮期望车速，Δt表示相邻两次导航的时间间隔，D表示车辆轮距，d表示车辆质心与两个驱动轮几何中心之间的距离，Δθ表示在Δt时间间隔内车辆的航向角变化量，
[0027]筛选出以为中心、4d
t
为边长的正方形区域内的二维激光雷达数据，获取该数据的重心作为当前时刻圆心C的实际位置(x
C
,y
C
)，d
t
为果园果树最大胸径。
[0028]进一步地，所述的聚类采用具有噪声的基于密度的聚类方法DBSCAN，参数扫描半径取0.1m，最小邻域点数取1。
[0029]进一步地，S3具体为：
[0030]设雷达中心即原点O与圆弧导航路径的圆心C的连线与圆弧导航路径的交点为G，横向偏差是原点O与G的距离，以O在G的右边为正，左边为负；过G点作圆弧切线，航向偏差是车辆方向与该切线的夹角，以车辆右偏为正，左偏为负；
[0031]采用下述公式计算横向偏差e
l
和航向偏差e
h
：
[0032][0033][0034]进一步地，S4中，采用模糊自适应纯追踪算法计算左右轮期望车速
[0035]进一步地，S5包括：
[0036]S5
‑
1、根据S4获取的横向偏差e
l
和航向偏差e
h
，按照S1
‑
2对二维激光雷达数据进行坐标校正；
[0037]S5
‑
2、行首检测：右转弯时，若横坐标[
‑
2d
r
,0]、纵坐标[
‑
2d
t
,2d
t
]围成的长方形区域内有二维激光雷达数据，代表车辆到达行首；左转弯时，若横坐标[0,2d
r
]、纵坐标[
‑
2d
t
,2d
t
]围成的长方形区域内有二维激光雷达数据，代表车辆到达行首。
[0038]进一步地，S1和S5中，获取的二维激光雷达数据每次采集1帧。
[0039]本专利技术的有益效果：
[0040]本专利技术提出的基于2D LiDAR的果园车辆地头导航方法通过2D LiDAR实时获取车辆位置与圆弧导航路径的偏差，引导车辆沿圆弧导航路径行驶。这种闭环控制方法的抗干扰能力强，能够适应车辆在地头有较大横向偏差/航向偏差以及地面粗糙度较大的复杂情况，是对已有基于2D LiDAR的果园车辆导航方法的补充和完善。
[0041]本专利技术的其它特征和优点将在随后具体实施方式部分予以详细说明。
附图说明
[0042]通过结合附图对本本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于二维激光雷达的果园车辆地头导航方法，其特征在于，该方法包括：S1、地头导航开始检测：获取二维激光雷达数据，判断车辆是否到达果园行末，如已到达，执行S2，开始地头导航；S2、地头导航路径提取：从二维激光雷达数据中提取圆弧导航路径；S3、地头导航偏差计算：计算当前车辆位置与圆弧导航路径的横向偏差e
l
和航向偏差e
h
；S4、地头导航路径跟踪：根据横向偏差e
l
和航向偏差e
h
，计算左右轮期望车速引导车辆沿圆弧导航路径行驶，经过Δt时刻，执行S5；S5、地头导航结束检测：获取二维激光雷达数据，判断车辆是否到达果园行首，如已到达，结束地头导航，否则，执行S2。2.根据权利要求1所述的基于二维激光雷达的果园车辆地头导航方法，其特征在于，S1具体为：S1
‑
1、根据二维激光雷达数据提取导航路径，计算车辆横向偏差e
l
和航向偏差e
h
；S1
‑
2、根据车辆横向偏差e
l
和航向偏差e
h
，对二维激光雷达数据进行坐标校正；其中；二维激光雷达数据的坐标系原点位于雷达中心，y轴正方向指向车辆前进方向，x轴正方向指向车辆右侧；(x,y)和(x
′
,y
′
)分别为校正前、后二维激光雷达的数据坐标；S1
‑
3、对校正后二维激光雷达的数据坐标进行判断，若横坐标[
‑
2d
r
,2d
r
]、纵坐标[0,2d
r
]围成的长方形区域内无二维激光雷达数据，则车辆到达行末，d
r
表示果园行矩。3.根据权利要求2所述的基于二维激光雷达的果园车辆地头导航方法，其特征在于，所述的导航路径提取方法为行间导航路径提取算法。4.根据权利要求2所述的基于二维激光雷达的果园车辆地头导航方法，其特征在于，S2中，圆弧导航路径以转弯侧行末树干所在位置C为圆心，果树行距d
r
为直径；其中圆心C的位置采用下述步骤获取：首次执行时：筛选出以原点为中心、4d
r
为边长的正方形区域内的二维激光雷达数据，聚类得到若干个树干簇；计算各树干簇的重心，若为右转弯，取横坐标与纵坐标之和最大的树干簇重心作为圆心C，若为左转弯，取纵坐标与横坐标之差最大...

【专利技术属性】
技术研发人员：王诗瑶，李秋洁，朱泓逸，
申请(专利权)人：南京林业大学，
类型：发明
国别省市：