一种对角耙地路径规划方法与流程

本发明涉及自动驾驶，具体为一种对角耙地路径规划方法。

背景技术：

1、车辆路径规划是实现自动驾驶车辆自主导航的关键环节。在农机自动驾驶导航系统中，需要根据农田信息生成全局的覆盖路径，作为控制层的输入，指导农机作业。针对耙地作业任务，生成高效的对角耙地覆盖路径是实现农机自动作业的重要前提之一。

2、目前已有的耙地路径规划方法主要分为两类，一类是以空间分析为基础的几何方法，一类是以启发式搜索为基础的搜索算法。其中，几何方法是先获取地块的外接矩形，在外接矩形内按照对角连接的顺序生成覆盖路径，再根据地块边界裁剪所生成的路径，从而得到最终路径。虽然几何方法可以直接生成带遍历顺序的路径，算法效率高，但是该方法对不规则地块的适应性较差，容易在不规则地块的边界附近出现过度重叠和超出边界的情况。

3、与几何方法不同，基于启发式搜索的方法是先在地块边界内生成全覆盖的对角耙地条带，再按照一定规则对所生成的离散条带进行路径搜索，得到条带的遍历顺序，最后根据遍历顺序依次连接所有条带，得到最终路径。该方法相比于几何方法能够更好的适应不规则地块，但是随着地块面积的增大，算法效率快速下降，生成路径的时间过长，不符合实际作业需求。

技术实现思路

1、为解决现有的耙地路径规划方法对不规则地块适应性较差，以及算法效率过低的问题，本发明提供了一种基于最近邻算法来生成近似最优对角耙地路径的方法。

2、本发明的目的在于提供一种对角耙地路径规划方法，以解决上述背景技术中提出的问题。

3、为了解决上述技术问题，本发明提供如下技术方案：

4、一种对角耙地路径规划方法，该规划方法包括：

5、s100：空间预处理；

6、s200：对角条带生成；

7、s300：成本矩阵计算；

8、s400：约束列表计算；

9、s500：基于最邻近算法的路径搜索；

10、s600：路径后处理。

11、优选的，所述的步骤s100中，定义地块边界，根据地块边界生成安全边界、作业区边界、约束内边界以及约束外边界四个边界；

12、1)将地块边界向地块内部等距偏移一个d_s距离，生成安全边界；

13、2)将安全边界向地块内部等距偏移一个r·f1距离，生成作业区边界；

14、3)将作业区边界向地块内部等距偏移一个r·f2距离，生成约束内边界；

15、4)将作业区边界向地块外部等距偏移一个r·f3距离，生成约束外边界；

16、其中d_s为安全距离，r为车辆转弯半径，f1为作业区宽度比例因子，f2为内边界偏移比例因子，f3为外边界偏移比例因子，且f1，f2，f3均大于0。

17、优选的，所述的步骤s200中，选择地块边界上的最长边作为参考边，第一组条带的方向为方向一，与参考边的夹角为β1，第二组条带的方向为方向二，方向二与参考边的夹角β2为：

18、β2＝π-2·β1

19、分别沿方向一和方向二以间隔w生成两组覆盖地块的等间隔平行条带，两组条带为交叉网格状，其中，w为作业幅宽，π为圆周率。

20、优选的，所述的步骤s300中，在条带生成完成后，获取条带端点，计算每两个端点pi和pj之间的转弯成本costij，形成成本矩阵；成本的计算涉及到三层判断，其中第一层为同条带判断，第二层为两点距离判断，第三层包括外边界超出判断以及内边界穿过判断两个部分；

21、首先，判断点pi和点pj是否在同一条带上，若在同一条带上，则costij＝0；若不在同一条带上，则进入第二层判断；

22、在第二层判断，计算点pi和点pj之间的距离dij，若dij>d_m，则costij＝dij·f4，否则进入第三层判断；

23、在第三层判断，计算点pi到点pj的杜宾斯曲线及其长度依次进行以下两个判断：

24、若点pi到点pj的杜宾斯曲线超出了约束外边界，则costij＝m，否则

25、若点pi到点pj的杜宾斯曲线穿过了约束内边界，则否则

26、重复上述判断步骤直到每对端点之间的转弯成本都计算完成，其中，f5为交点距离比例因子，f4，f6，f7为成本权重，f4，f5，f6，f7均大于0，d_m为设定的距离阈值，d_m>0，m为正数，作用是使得到达成本足够大，避免超出约束外边界的路径。

27、优选的，所述的步骤s400中，约束列表用于存储不可通行的路径，约束列表的计算涉及到以下两个判断条件：

28、若点pi到点pj的杜宾斯曲线超出了约束外边界，则点pi到点pj的路径不合理，将[pi,pj]加入约束列表；

29、若点pi到点pj的杜宾斯曲线穿过了约束内边界，则点pi到点pj的路径不合理，将[pi,pj]加入约束列表。

30、优选的，所述的步骤s500中，指定起点为ps，针对不在以ps为起点的约束列表中的其他点进行搜索，选择到达成本最小的点作为下一个点，加入路径，不断以当前到达点为起点，寻找符合约束的下一个目标点，直到所有条带端点都被加入路径，若带搜索的点全部属于当前起点的约束列表，则选择约束列表中到达成本最小的点加入路径。

31、优选的，所述的s600中，通过路径搜索确定条带连接顺序后，按照顺序用杜宾斯曲线连接各条带，生成转弯路径，依次判断所生成的转弯路径是否在安全边界内，若超出安全边界，需要进行以下路径后处理：

32、将转弯路径起点所在条带从该起点处向地块内部按照步长d_step收缩，判断起点收缩后的转弯路径是否在安全边界内，若在，则停止收缩，用收缩后的新起点代替原起点，否则继续下一步处理；

33、将转弯路径终点所在条带从该终点处向地块内部按照步长d_step收缩，判断终点收缩后的转弯路径是否在安全边界内，若在，则停止收缩，用收缩后的新终点代替原终点，否则继续下一步处理；

34、将转弯路径的起点和终点所在条带分别从该起点、终点处向地块内部按照步长d_step收缩，直到起、终点收缩后的转弯路径处于安全边界内，用新的起、终点代替原起、终点；

35、处理完所有转弯路径后，得到最终的耙地路径点集。

36、与现有技术相比，本发明所达到的有益效果是：本发明根据对角耙地作业基本规则来计算不同条带间的转弯成本和约束列表，在满足约束的条件下，依次在当前点的给定邻域范围内搜索成本最小的路径，直到所有条带都被加入路径。本发明深度融合了实际对角耙地作业的农艺需求，提高了对不规则地块的适应性，结合最近邻局部搜索的特点提高算法效率，能够在合理时间内生成覆盖率较高、重叠率较低的对角耙地路径。

技术特征：

1.一种对角耙地路径规划方法，其特征在于，该规划方法包括：

2.根据权利要求1所述的一种对角耙地路径规划方法，其特征在于，所述的步骤s100中，定义地块边界，根据地块边界生成安全边界、作业区边界、约束内边界以及约束外边界四个边界；

3.根据权利要求1所述的一种对角耙地路径规划方法，其特征在于，所述的步骤s200中，选择地块边界上的最长边作为参考边，第一组条带的方向为方向一，与参考边的夹角为β1，第二组条带的方向为方向二，方向二与参考边的夹角β2为：

4.根据权利要求1所述的一种对角耙地路径规划方法，其特征在于，所述的步骤s300中，在条带生成完成后，获取条带端点，计算每两个端点pi和pj之间的转弯成本costij，形成成本矩阵；成本的计算涉及到三层判断，其中第一层为同条带判断，第二层为两点距离判断，第三层包括外边界超出判断以及内边界穿过判断两个部分；

5.根据权利要求1所述的一种对角耙地路径规划方法，其特征在于，所述的步骤s400中，约束列表用于存储不可通行的路径，约束列表的计算涉及到以下两个判断条件：

6.根据权利要求1所述的一种对角耙地路径规划方法，其特征在于，所述的步骤s500中，

7.根据权利要求1所述的一种对角耙地路径规划方法，其特征在于，所述的s600中，通过路径搜索确定条带连接顺序后，按照顺序用杜宾斯曲线连接各条带，生成转弯路径，依次判断所生成的转弯路径是否在安全边界内，若超出安全边界，需要进行以下路径后处理：

技术总结
本发明涉及自动驾驶技术领域，具体为一种对角耙地路径规划方法。为解决现有的耙地路径规划方法对不规则地块适应性较差，以及算法效率过低的问题，本发明提供了一种基于最近邻算法来生成近似最优对角耙地路径的方法。根据对角耙地作业基本规则来计算不同条带间的转弯成本和约束列表，在满足约束的条件下，依次在当前点的给定邻域范围内搜索成本最小的路径，直到所有条带都被加入路径。本发明深度融合了实际对角耙地作业的农艺需求，提高了对不规则地块的适应性，结合最近邻局部搜索的特点提高算法效率，能够在合理时间内生成覆盖率较高、重叠率较低的对角耙地路径。

技术研发人员：董靓,李晓宇,王锐,具大源
受保护的技术使用者：上海联适导航技术股份有限公司
技术研发日：
技术公布日：2024/3/4