CN117308965A - 基于滑动窗口算法的收获机器人自主卸粮路径规划方法 - Google Patents

Abstract

本发明公开了一种基于滑动窗口算法的收获机器人自主卸粮路径规划方法，包括：S1、采集农田地块边界关键点信息，建立农田的栅格化地图；S2、将收获机器人当前位置所处栅格设为可行驶栅格列表的第一个元素，计算当前作业轮次的满仓点位置，可行驶栅格列表沿作业路径从第一个元素逐个扩展至满仓点，形成可行驶栅格列表；S3、遍历可行进栅格列表，将可行驶栅格列表中的元素逐个作为起点，以卸粮点所在栅格为终点，通过改进的启发式搜索算法获取两点的往返最短路径；S4、根据可行进距离判断是否进入卸粮切出点决策，采用滑动窗口算法计算最佳卸粮切出区域。本发明可提高收获机粮仓容量使用率，减少卸粮路径成本，提升无人驾驶收获作业效率。

Description

基于滑动窗口算法的收获机器人自主卸粮路径规划方法

技术领域

本发明属于路径规划技术领域，具体涉及一种基于滑动窗口算法的收获机器人自主卸粮路径规划方法。

背景技术

近年来，北斗导航系统在农业领域有广泛应用，基于北斗导航系统的无人驾驶收获机器人也陆续在全国各地投入使用，大大提高了农民在抢收时节的作业效率。然而，当前无人收获作业的路径规划方法中，未根据作物收成动态生成卸粮路径，导致无人驾驶收获机器人出现空仓卸粮甚至爆仓的情况，不能最大化利用机器粮仓，直接影响到整个无人收获作业的总效能。

自主卸粮是实现无人驾驶收获机器人自主作业的一项关键技术，能为收获机器人田间作业提供合理的卸粮路径，有效改善卸粮路径成本不可控和机器粮仓利用率低问题，提高收获机的作业效率。目前在全覆盖作业中卸粮路径规划方面没有成熟、通用的方法。在实际应用中，针对全覆盖作业的卸粮路径规划，主要有两种方式：一种是通过预估卸粮次数和作业次数，预先规划卸粮点，使卸粮点都位于地头一侧；另一种是在特定的农田环境下，如矩形农田中，通过预估未满载可作业距离，在梭行法或者螺旋法的作业路径中间隔一定圈数加入卸粮路径。以上两种卸粮路径规划方式的智能化程度、对不同农田的适应性以及作业效率较低。因此，需要对卸粮路径规划方法单独进行研究。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提出一种基于滑动窗口算法的收获机器人自主卸粮路径规划方法。

为了达到上述目的，本发明采用以下技术方案：

基于滑动窗口算法的收获机器人自主卸粮路径规划方法，包括以下步骤：

S1、采集农田地块边界关键点信息，以农田边界点关键点信息作为边界值，建立农田的栅格化地图，根据作业幅宽确定栅格大小和分辨率，在栅格单元中存储状态或属性信息；

S2、在下发作业路径后，收获机器人开始追踪作业路径，收获机器人粮仓剩余容量小于设定值时，将收获机器人当前位置所处栅格设为可行驶栅格列表的第一个元素，计算当前作业轮次的满仓点位置并将其所处栅格设为可行驶栅格列表的最后一个元素，可行驶栅格列表沿作业路径从第一个元素逐个扩展至最后一个元素，形成可行驶栅格列表；

S3、遍历可行进栅格列表，将可行驶栅格列表中的元素逐个作为起点，以定点卸粮点所在栅格为终点，根据收获机器人转弯半径、转弯次数、末端航向作为约束、以地图边界作为搜索边界，通过改进的启发式搜索算法获取两点的往返最短路径，将最短路径添加至卸粮路径集合并将路径成本写入对应栅格中；

S4、根据可行进距离判断是否进入卸粮切出点决策，采用滑动窗口算法计算最佳卸粮切出区域；在未到达卸粮切出区域时，收获机保持作业状态追踪作业路径，到达则停止作业，切换为卸粮路径追踪完成卸粮，返回卸粮切出区域后重新进入作业路径进行作业。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明建立农田的栅格化地图是根据卸粮点位置及卸粮边方向求解出划分后的最小外接矩形，栅格地图分辨率随收获机器人作业幅宽变化而自适应变化，保证栅格地图上卸粮点处的位置精度。

2、本发明收获机器人与卸粮点两点路径规划算法，结合收获机器人农艺特点进行改进，由于其作业路径通常为长直线，在到达满粮点时当前行未作业部分也为直线，结合当前作业行斜率选择性扩展节点搜索，可大大减少启发式搜索算法的扩展节点数量，减少收获机器人计算等待时间，省时省油。

3、本发明中，满粮点是根据收获机器人粮仓中的容积传感器及GPS实时计算所得，而现有无人驾驶收获机卸粮路径规划方法是先通过稻田五点采样法粗略估产，再人工在作业路径中分段插入相应卸粮路径，然而实际情况中水稻产量并不均匀甚至差别很大，导致容易产生两种问题：预估产量偏低易导致粮仓爆满损坏机器，预估产量偏高则频繁卸粮浪费粮仓容量；而本发明的卸粮路径规划方法通过实时监测粮仓容积情况动态决策卸粮切出点，提高作业安全性、作业效率和全过程无人化占比。

4、现有无人驾驶收获机卸粮切出点都选择在作业行终点，这种方法不但适用性有限在长田块中无法使用，而且冗余路径多，本发明通过滑动窗口算法决策最优卸粮切出点，选取卸粮路径列表中路径长度最小值所对应的路径起点栅格作为卸粮切出点，提高收获机粮仓容量使用率，缩短卸粮路径，降本增效，提升无人驾驶收获整体作业效率。

附图说明

图1是本发明方法的流程图；

图2是本发明实施例中的农田边界及其最小外接矩形示意图；

图3是本发明实施例中农田的栅格化地图；

图4是收获机器人内螺旋作业方式路径示意图；

图5是收获机器人梭行法作业方式路径示意图；

图6是实施例中的内螺旋作业卸粮切出点处于图4中的③边时的卸粮路径示意图；

图7是实施例中的内螺旋作业卸粮切出点处于图4中的④边时的卸粮路径示意图；

图8是收获机器人当前所处栅格的左右邻域示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

如图1所示，本发明，基于滑动窗口算法的收获机器人自主卸粮路径规划方法，包括以下步骤：

S1、采集农田地块边界关键点信息，以农田边界点关键点信息作为边界值，建立农田的栅格化地图，根据作业幅宽确定栅格大小和分辨率，在栅格单元中存储状态或属性信息；具体包括：

S11、采集农田地块边界关键点及定点卸粮点的经纬度信息，建立高斯平面坐标系，具体为：

获取农田地块边界所有关键点以及定点卸粮点的经纬度信息，对经纬度信息进行坐标转换运算，以第一个关键点作为坐标系原点，将所有关键点转化至高斯平面坐标系，求得对应横坐标x 、纵坐标y；

设农田的四个顶点为A、B、C和D，根据四点的高斯平面坐标，解出农田边界的数学表达式。

S12、选择卸粮边作为最小外接矩形的一条边并求解出最小外接矩形，具体为：

卸粮点一般位于边界边缘，为提高卸粮点定位精度，选择卸粮边（卸粮边为农田四个顶点所构成的四边形中距离卸粮点最近的边）作为最小外接矩形的一条边并求解出最小外接矩形，方法为：

假设顶点A、B所连成边为卸粮边，计算出/>与x轴夹角/>；

将农田4个顶点所构成的四边形顺时针旋转，使卸粮边与x轴平行，旋转后得四边形所有顶点中横坐标最小值/>、横坐标最大值/>、纵坐标最小值/>以及纵坐标最大值/>，则/>，/>分别为最小外接矩形的左下顶点和右上顶点，由于该矩形四条边平行于x轴和y轴，得矩形另外两个顶点为/>，，完成对最小外接矩形的求解。如图2所示，为本实施例中的农田边界及其最小外接矩形示意图。

S13、建立农田的栅格化地图，在栅格单元中存储状态或属性信息，具体为：

为提高卸粮点定位精度，需避免在卸粮点处产生不完整栅格，因此以定点卸粮点坐标作为栅格地图原点，以卸粮边作为横轴，以垂直于卸粮边且与卸粮点相交的直线作为纵轴，设收获机器人的割幅为，则将坐标系单元长度设为/>，建立栅格地图坐标系，把最小外接矩形分割为若干边长为/>的栅格单元，并剔除分割过程中产生的不完整栅格；建立栅格化地图时将未作业区域栅格的障碍物标志位置位。

如图3所示，为本实施例中农田的栅格化地图。

假设顶点A、B两点所连成边为卸粮边，最小外接矩形作为栅格地图纵向跨度，则将一系列栅格行表示为：

；

其中，为AD边的斜率，/>为AD边的截距，/>为AD边的长度，/>为栅格行在栅格地图坐标系中的横坐标值，/>为栅格行在栅格地图坐标系中的纵坐标值，/>为作业行的序号；栅格以行号和列号组成的坐标作为索引，以栅格几何中心的大地坐标代表该栅格坐标，在栅格表中存储状态或属性信息(如障碍物、高程、作业状态等，该信息可以根据实际应用需求来定义)。

S2、在下发作业路径后，收获机器人开始追踪作业路径，在工作过程中实时获取收获机器人当前所处栅格，并将已作业区域栅格的障碍物标志位取消置位；收获机器人粮仓剩余容量小于设定值时，将收获机器人当前位置所处栅格设为可行驶栅格列表的第一个元素，计算当前作业轮次的满仓点位置并将其所处栅格设为可行驶栅格列表的最后一个元素，可行驶栅格列表沿作业路径从第一个元素逐个扩展至最后一个元素，形成可行驶栅格列表；

本实施例中，计算当前作业轮次的满仓点位置具体为：

通过设置在收获机器人粮仓中的容积传感器，测量当前时刻粮仓中的粮食体积，粮仓剩余容量计算公式为：

；

其中，为收获机器人粮仓的额定容积；

仓内粮食堆积速率计算公式为：

；

其中，为上一时刻粮仓中的粮食体积，/>为容积传感器采样时间间隔；

计算出再过多少时间后粮仓满粮，计算公式为：

；

通过GPS获取收获机器人当前作业速度，则收获机器人可作业的距离计算公式为：

；

设收获机器人当前所处栅格，根据作业路径中当前作业行的起点和终点计算出当前作业行的航向角为/>；

每个栅格边长为，则第/>个所处栅格坐标为：

；

其中，的含义为判断括号内容正负值，具体计算方法为：

；

根据收获机器人可作业的距离，则收获机器人在栅格地图中可行驶的栅格数为：

；

其中，从0到/>遍历，/>时，即为当前作业轮次的收获机器人满仓点，依次将第/>个栅格扩展至窗口中，形成可行驶栅格列表。

S3、遍历当前作业轮次可行进栅格列表，每次将列表中的第个栅格作为起点，以定点卸粮点所在栅格为终点，根据收获机器人转弯半径、转弯次数、末端航向作为约束、以地图边界作为搜索边界，通过改进的启发式搜索算法获取两点的往返最短路径/>，将添加至卸粮路径集合同时将对应路径成本/>写入对应栅格/>中。

如图4所示，为收获机器人进行内螺旋作业的作业路径；如图5所示，为收获机器人进行内螺旋作业的作业路径。

改进的启发式搜索算法具体为：

收获机器人作业路径为长直线，在到达满粮点时当前行未作业部分也为直线，结合当前作业行斜率选择性扩展节点搜索，减少启发式搜索算法的扩展节点数量；

在到达满仓点时从作业路径文件中获取当前作业行的斜率，假设当前作业行线性方程为：

；

其中，为当前作业行在栅格地图坐标系中的横坐标值，/>为当前作业行在栅格地图坐标系中的纵坐标值，/>为当前行的斜率，/>为当前行的截距，将当前行斜率信息用于指导搜索方向，定义启发式函数计算公式为：

；

其中，是当前节点/>到卸粮点的欧几里得距离，/>是监测节点的数量，/>是当前节点/>点到/>节点间连续节点的平均斜率，/>为节点/>到上一节点所连直线的斜率，/>计算公式为：

；

是斜率信息权重系数，计算方法为：

；

其中，为斜率信息权重系数的预设值，/>越大当前行斜率信息指导搜索能力越强，/>为平行判断阈值，当斜率差小于平行判断阈值则认为当前段卸粮路径与当前作业行平行，可利用当前作业行斜率进入快速搜索；

根据上述方式计算，减少必须考虑的节点数量选择性地扩展节点，同时避免与未作业区域相交的路径。

通过改进的启发式搜索算法获取两点的往返最短路径具体为：

遍历当前作业轮次可行进栅格列表，每次按序将队列中的栅格作为起点，以定点卸粮点所在栅格为终点，根据收获机器人转弯半径、转弯次数、末端航向作为约束、以地图边界作为搜索边界，采用算法进行路径规划，通过收获机器人最小转弯半径和单步最小步长约束进行节点扩展，其启发函数为：

；

其中，表示从起点/>经过节点/>到目标/>的总估计成本，表示从起点/>到节点/>的实际成本，/>表示从节点/>到目标/>的启发式估计成本；节点/>的坐标表示为/>；

计算方法为：

；

计算方法为：

；

其中，计算方法为：

；

等价于通过Reeds-Shepp方法所规划出的曲线路径长度/>，需根据当前节点和目标节点的相对位置和朝向，选择相应的Reeds-Shepp路径类型；

Reeds-Shepp是一种曲线规划方法，在假设收获机器人可以向前和向后行驶的情况下，通过结合收获机器人最小转弯半径，寻找路径池中的最小长度路径，规划形成的曲线至少由三段组成，每段是收获机器人最小转弯半径单位圆的圆弧或直线段。

计算方法为：

；

其中，为直线段的长度之和，/>为圆弧段的长度之和；

为直线段起点/>和终点/>的欧氏距离，计算方法为：

；

为拐过圆弧的长度，计算方法为：

；

其中，为圆弧的半径，/>为圆弧的角度；

路径成本计算方法为：

；

本实施例中，当卸粮切出点处于图4的③边时，算法规划的卸粮路径如图6所示，当卸粮切出点处于图4的④边时，/>算法规划的卸粮路径如图7所示。

S4、根据可行进距离判断是否进入卸粮切出点决策，采用滑动窗口算法计算最佳卸粮切出区域；在未到达卸粮切出区域时，收获机保持作业状态追踪作业路径，到达则停止作业，切换为卸粮路径追踪完成卸粮，返回卸粮切出区域后重新进入作业路径进行作业。

其中，当收获机器人可作业距离小于当前作业圈的路径长度时（图6中，内螺旋作业中①②③④组成当前作业圈，⑤⑥⑦⑧组成下一作业圈；图7中，梭行法作业中①②③④组成当前作业圈，⑤⑥⑦⑧组成下一作业圈），则满粮点在当前作业圈内，必须在当前作业圈内完成卸粮，由于作业路径不重复可保证卸粮最短路径唯一，判断方法为可行驶栅格列表中所有栅格均不在收获机器人当前所处栅格的左右邻域（即图8中的阴影区域），满足条件则启动滑动窗口算法计算最佳卸粮切出区域；如图8所示，虚实线区域为待作业区域。

滑动窗口算法计算最佳卸粮切出区域具体为：

将可行驶栅格列表中的第一个元素作为滑动窗口的初始位置，根据收获机器人机身长度确定滑动窗口初始大小，设收获机器人的机身长度为，则滑动窗口长度计算公式为：

；

当前窗口内的路径成本之和的计算公式为：

；

以单元栅格作为步长移动窗口，并计算每次移动后窗口的，直到窗口到达可行驶栅格列表的末尾，完成路径集合构建，选取/>最小的窗口作为最佳卸粮切出区域。

还需要说明的是，在本说明书中，诸如术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，包括以下步骤：

S1、采集农田地块边界关键点信息，以农田边界点关键点信息作为边界值，建立农田的栅格化地图，根据作业幅宽确定栅格大小和分辨率，在栅格单元中存储状态或属性信息；

S2、在下发作业路径后，收获机器人开始追踪作业路径，收获机器人粮仓剩余容量小于设定值时，将收获机器人当前位置所处栅格设为可行驶栅格列表的第一个元素，计算当前作业轮次的满仓点位置并将其所处栅格设为可行驶栅格列表的最后一个元素，可行驶栅格列表沿作业路径从第一个元素逐个扩展至最后一个元素，形成可行驶栅格列表；

S3、遍历可行进栅格列表，将可行驶栅格列表中的元素逐个作为起点，以定点卸粮点所在栅格为终点，根据收获机器人转弯半径、转弯次数、末端航向作为约束、以地图边界作为搜索边界，通过改进的启发式搜索算法获取两点的往返最短路径，将最短路径添加至卸粮路径集合并将路径成本写入对应栅格中；

S4、根据可行进距离判断是否进入卸粮切出点决策，采用滑动窗口算法计算最佳卸粮切出区域；在未到达卸粮切出区域时，收获机保持作业状态追踪作业路径，到达则停止作业，切换为卸粮路径追踪完成卸粮，返回卸粮切出区域后重新进入作业路径进行作业。

2.根据权利要求1所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，步骤S1具体包括：

S11、采集农田地块边界关键点及定点卸粮点的经纬度信息，建立高斯平面坐标系；

S12、选择卸粮边作为最小外接矩形的一条边并求解出最小外接矩形；

S13、建立农田的栅格化地图，在栅格单元中存储状态或属性信息。

3.根据权利要求2所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，步骤S11具体为：

获取农田地块边界所有关键点以及定点卸粮点的经纬度信息，对经纬度信息进行坐标转换运算，以第一个边界关键点作为坐标系原点，将所有边界关键点转化至高斯平面坐标系，并求出对应横坐标和纵坐标；

设农田的四个顶点为A、B、C和D，根据四点的高斯平面坐标，解出农田边界的数学表达式。

4.根据权利要求3所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，步骤S12具体为：

选择卸粮边作为最小外接矩形的一条边并求解出最小外接矩形，方法为：

假设顶点A、B所连成边 为卸粮边，计算出/>与x轴夹角/>；

将农田4个顶点所构成的四边形顺时针旋转，使卸粮边与x轴平行，旋转后得四边形所有顶点中横坐标最小值/>、横坐标最大值/>、纵坐标最小值/>以及纵坐标最大值/>，则/>，/>分别为最小外接矩形的左下顶点和右上顶点，由于该矩形四条边平行于x轴和y轴，得矩形另外两个顶点为/>，/>，完成对最小外接矩形的求解；

其中，卸粮边为农田4个顶点所构成的四边形中距离卸粮点最近的边。

5.根据权利要求4所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，步骤S13具体为：

以定点卸粮点坐标作为栅格地图原点，以卸粮边作为横轴，以垂直于卸粮边且与卸粮点相交的直线作为纵轴，设收获机器人的割幅为，则将坐标系单元长度设为/>，建立栅格地图坐标系，把最小外接矩形分割为若干边长为/>的栅格单元，并剔除分割过程中产生的不完整栅格；

假设顶点A、B所连成边为卸粮边，最小外接矩形作为栅格地图纵向跨度，则将一系列栅格行表示为：

；

其中，为AD边的斜率，/>为AD边的截距，/>为AD边的长度，/>为栅格行在栅格地图坐标系中的横坐标值，/>为栅格行在栅格地图坐标系中的纵坐标值，/>为作业行的序号；栅格以行号和列号组成的坐标作为索引，以栅格几何中心的大地坐标代表该栅格坐标，在栅格表中存储状态或属性信息。

6.根据权利要求1所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，步骤S2中，计算当前作业轮次的满仓点位置具体为：

通过收获机器人粮仓中的容积传感器，测量当前时刻粮仓中的粮食体积，粮仓剩余容量计算公式为：

；

其中，为收获机器人粮仓的额定容积；

仓内粮食堆积速率计算公式为：

；

其中，为上一时刻粮仓中的粮食体积，/>为容积传感器采样时间间隔；

计算出再过多少时间后粮仓满粮：

；

通过GPS获取收获机器人当前作业速度，则收获机器人可作业的距离计算公式为：

；

设收获机器人当前所处栅格为，根据作业路径中当前作业行的起点和终点计算出当前作业行的航向角为/>；

每个栅格边长为，则第/>个所处栅格坐标为：

；

其中，的含义为判断括号内容正负值，具体计算方法为：

；

根据收获机器人可作业的距离，则收获机器人在栅格地图中可行驶的栅格数为：

；

其中，从0到/>遍历，/>时，即为当前作业轮次的收获机器人满仓点，依次将第/>个栅格扩展至窗口中，形成可行驶栅格列表。

7.根据权利要求1所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，步骤S3中，改进的启发式搜索算法具体为：

收获机器人作业路径为长直线，在到达满粮点时当前行未作业部分也为直线，结合当前作业行斜率选择性扩展节点搜索，减少启发式搜索算法的扩展节点数量；

在到达满仓点时从作业路径中获取当前作业行的斜率，假设当前作业行线性方程为：

；

其中，为当前作业行在栅格地图坐标系中的横坐标值，/>为当前作业行在栅格地图坐标系中的纵坐标值，/>为当前行的斜率，/>为当前行的截距，将当前行斜率信息用于指导搜索方向，定义启发式函数计算公式为：

；

其中，是当前节点/>到卸粮点的欧几里得距离，/>是监测节点的数量，是当前节点/>点到/>节点间连续节点的平均斜率，/>为节点/>到上一节点所连直线的斜率，/>计算公式为：

；

是斜率信息权重系数，计算方法为：

；

其中，为斜率信息权重系数的预设值，/>越大当前行斜率信息指导搜索能力越强，/>为平行判断阈值，当斜率差小于平行判断阈值则认为当前段卸粮路径与当前作业行平行，利用当前作业行斜率进入快速搜索；

根据上述方式计算，减少必须考虑的节点数量选择性地扩展节点，同时避免与未作业区域相交的路径。

8.根据权利要求7所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，通过改进的启发式搜索算法获取两点的往返最短路径具体为：

遍历当前作业轮次可行进栅格列表，每次按序将队列中的栅格作为起点，以定点卸粮点所在栅格为终点，根据收获机器人转弯半径、转弯次数、末端航向作为约束、以地图边界作为搜索边界，采用算法进行路径规划，通过收获机器人最小转弯半径和单步最小步长约束进行节点扩展，其启发函数为：

；

其中，表示从起点/>经过节点/>到目标/>的总估计成本，/>表示从起点/>到节点/>的实际成本，/>表示从节点/>到目标/>的启发式估计成本；节点/>的坐标表示为/>；

计算方法为：

；

计算方法为：

；

其中，计算方法为：

；

等价于通过Reeds-Shepp方法所规划出的曲线路径长度/>，需根据当前节点和目标节点的相对位置和朝向，选择相应的Reeds-Shepp路径类型；

Reeds-Shepp是一种曲线规划方法，在假设收获机器人可向前和向后行驶的情况下，通过结合收获机器人最小转弯半径，寻找路径池中的最小长度路径，规划形成的曲线至少由三段组成，每段为收获机器人最小转弯半径单位圆的圆弧或直线段；

计算方法为：

；

其中，为直线段的长度之和，/>为圆弧段的长度之和；

为直线段起点/>和终点/>的欧氏距离，计算方法为：

；

为拐过圆弧的长度，计算方法为：

；

其中，为圆弧的半径，/>为圆弧的角度；

路径成本计算方法为：

。

9.根据权利要求1所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，步骤S4中，当收获机器人可作业距离小于当前作业圈的路径长度时，则满粮点在当前作业圈内，必须在当前作业圈内完成卸粮，由于作业路径不重复，卸粮最短路径唯一，判断方法为可行驶栅格列表中所有栅格均不在收获机器人当前所处栅格的左右邻域，满足条件则启动滑动窗口算法计算最佳卸粮切出区域。

10.根据权利要求8所述的基于滑动窗口算法的收获机器人自主卸粮路径规划方法，其特征在于，滑动窗口算法计算最佳卸粮切出区域具体为：

将可行驶栅格列表中的第一个元素作为滑动窗口的初始位置，根据收获机器人机身长度确定滑动窗口初始大小，设收获机器人的机身长度为，则滑动窗口长度计算公式为：

；

当前窗口内的路径成本之和的计算公式为：

；

以单元栅格作为步长移动窗口，并计算每次移动后窗口的，直到窗口到达可行驶栅格列表的末尾，完成路径集合构建，选取/>最小的窗口作为最佳卸粮切出区域。