CN117629221A - 一种对角耙地路径规划方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

S100：空间预处理；

S200：对角条带生成；

S300：成本矩阵计算；

S400：约束列表计算；

S500：基于最邻近算法的路径搜索；

S600：路径后处理。

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

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

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

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

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

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

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

β₂＝π-2·β₁

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

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

首先，判断点P_i和点P_j是否在同一条带上，若在同一条带上，则cost_ij＝0；若不在同一条带上，则进入第二层判断；

在第二层判断，计算点P_i和点P_j之间的距离d_ij，若d_ij>d_m，则cost_ij＝d_ij·f₄，否则进入第三层判断；

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

若点P_i到点P_j的杜宾斯曲线超出了约束外边界，则cost_ij＝M，否则

若点P_i到点P_j的杜宾斯曲线穿过了约束内边界，则否则

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

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

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

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

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

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

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

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

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

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

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

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1是本发明的流程示意图；

图2是本发明路径规划生成的结果示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供技术方案，具体流程图参照图1，最后路径规划生成的结果示意图如图2所示，该方法的具体执行过程如下。

本发明的内容主要包括三大部分：耙地条带生成、条带连接以及路径后处理。其中耙地条带生成部分包括空间预处理以及对角条带生成两个步骤；条带连接部分包括成本矩阵计算、约束列表计算以及基于最近邻算法的路径搜索三个步骤。

1空间预处理

在空间预处理部分，需要生成安全边界、作业区边界、约束内边界以及约束外边界四个边界，作为后续步骤的几何基础。

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

将安全边界向地块内部等距偏移一个r·f₁距离，生成作业区边界；

将作业区边界向地块内部等距偏移一个r·f₂距离，生成约束内边界；

将作业区边界向地块外部等距偏移一个r·f₃距离，生成约束外边界；

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

2对角条带生成

选择地块边界上的最长边作为参考边。第一组条带的方向为方向一，与参考边的夹角为β₁，第二组条带的方向为方向二，则方向二与参考边的夹角β₂为：

β₂＝π-2·β₁

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

3成本矩阵计算

在条带生成完成后，获取条带端点，计算每两个端点P_i和P_j之间的转弯成本cost_ij，形成成本矩阵。成本的计算涉及到三层判断，其中第一层为同条带判断，第二层为两点距离判断，第三层则包括外边界超出判断以及内边界穿过判断两个部分。

首先，判断点P_i和点P_j是否在同一条带上，若在同一条带上，则cost_ij＝0；若不在同一条带上，则进入第二层判断。

在第二层判断，计算点P_i和点P_j之间的距离d_ij，若d_ij>d_m，则cost_ij＝d_ij·f₄，否则进入第三层判断。

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

若点P_i到点P_j的杜宾斯曲线超出了约束外边界，则cost_ij＝M，否则

若点P_i到点P_j的杜宾斯曲线穿过了约束内边界，则否则

重复上述判断步骤直到每对端点之间的转弯成本都计算完成。其中，f₅为交点距离比例因子。f₄，f₆，f₇为成本权重，作用是按一定比例加大成本，用于避免不合理路径。f₄，f₅，f₆，f₇均大于0。d_m为设定的距离阈值，用分割不同的距离范围，以设计不同的成本计算方式，d_n>0。M为一个足够大的正数，作用是使得到达成本足够大，避免超出约束外边界的路径。

4约束列表计算

约束列表用于存储不可通行的路径，约束列表的计算涉及到以下两个判断条件：

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

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

5基于最近邻算法的路径搜索

指定起点为P_i，针对不在以P_i为起点的约束列表中的其他点进行搜索，利用3成本矩阵计算中所计算的成本矩阵cost_ij进行成本比较，选择其中cost_ij最小值所对应的P_iP_j路径，则点P_j即为下一个到达点，加入路径。不断以当前到达点为起点，寻找符合约束的下一个目标点，直到所有条带端点都被加入路径。需要注意的是，若待搜索的点全部属于当前起点的约束列表，则选择约束列表中到达成本最小的点加入路径。

6路径后处理

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

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

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

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

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

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

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

S100：空间预处理；

S200：对角条带生成；

S300：成本矩阵计算；

S400：约束列表计算；

S500：基于最邻近算法的路径搜索；

S600：路径后处理。

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

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

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

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

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

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

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

β₂＝π-2·β₁

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

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

首先，判断点P_i和点P_j是否在同一条带上，若在同一条带上，则cost_ij＝0；若不在同一条带上，则进入第二层判断；

在第二层判断，计算点P_i和点P_j之间的距离d_ij，若d_ij＞d_m，则cost_ij＝d_ij·f₄，否则进入第三层判断；

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

若点P_i到点P_j的杜宾斯曲线超出了约束外边界，则cost_ij＝M，否则

若点P_i到点P_j的杜宾斯曲线穿过了约束内边界，则否则

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

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

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

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

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

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

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

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

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

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

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