基于滑动窗口算法的收获机器人自主卸粮路径规划方法
文献发布时间：2024-04-18 19:52:40
技术领域
本发明属于路径规划技术领域，具体涉及一种基于滑动窗口算法的收获机器人自主卸粮路径规划方法。
背景技术
近年来，北斗导航系统在农业领域有广泛应用，基于北斗导航系统的无人驾驶收获机器人也陆续在全国各地投入使用，大大提高了农民在抢收时节的作业效率。然而，当前无人收获作业的路径规划方法中，未根据作物收成动态生成卸粮路径，导致无人驾驶收获机器人出现空仓卸粮甚至爆仓的情况，不能最大化利用机器粮仓，直接影响到整个无人收获作业的总效能。
自主卸粮是实现无人驾驶收获机器人自主作业的一项关键技术，能为收获机器人田间作业提供合理的卸粮路径，有效改善卸粮路径成本不可控和机器粮仓利用率低问题，提高收获机的作业效率。目前在全覆盖作业中卸粮路径规划方面没有成熟、通用的方法。在实际应用中，针对全覆盖作业的卸粮路径规划，主要有两种方式：一种是通过预估卸粮次数和作业次数，预先规划卸粮点，使卸粮点都位于地头一侧；另一种是在特定的农田环境下，如矩形农田中，通过预估未满载可作业距离，在梭行法或者螺旋法的作业路径中间隔一定圈数加入卸粮路径。以上两种卸粮路径规划方式的智能化程度、对不同农田的适应性以及作业效率较低。因此，需要对卸粮路径规划方法单独进行研究。
发明内容
本发明的主要目的在于克服现有技术的缺点与不足，提出一种基于滑动窗口算法的收获机器人自主卸粮路径规划方法。
为了达到上述目的，本发明采用以下技术方案：
基于滑动窗口算法的收获机器人自主卸粮路径规划方法，包括以下步骤：
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所连成边
将农田4个顶点所构成的四边形顺时针旋转
S13、建立农田的栅格化地图，在栅格单元中存储状态或属性信息，具体为：
为提高卸粮点定位精度，需避免在卸粮点处产生不完整栅格，因此以定点卸粮点坐标作为栅格地图原点，以卸粮边作为横轴，以垂直于卸粮边且与卸粮点相交的直线作为纵轴，设收获机器人的割幅为
如图3所示，为本实施例中农田的栅格化地图。
假设顶点A、B两点所连成边
；
其中，
S2、在下发作业路径后，收获机器人开始追踪作业路径，在工作过程中实时获取收获机器人当前所处栅格，并将已作业区域栅格的障碍物标志位取消置位；收获机器人粮仓剩余容量小于设定值时，将收获机器人当前位置所处栅格设为可行驶栅格列表的第一个元素，计算当前作业轮次的满仓点位置并将其所处栅格设为可行驶栅格列表的最后一个元素，可行驶栅格列表沿作业路径从第一个元素逐个扩展至最后一个元素，形成可行驶栅格列表；
本实施例中，计算当前作业轮次的满仓点位置具体为：
通过设置在收获机器人粮仓中的容积传感器，测量当前时刻粮仓中的粮食体积
；
其中，
仓内粮食堆积速率计算公式为：
；
其中，
计算出再过多少时间后粮仓满粮，计算公式为：
；
通过GPS获取收获机器人当前作业速度
；
设收获机器人当前所处栅格
每个栅格边长为
；
其中，
；
根据收获机器人可作业的距离
；
其中，
S3、遍历当前作业轮次可行进栅格列表，每次将列表中的第
如图4所示，为收获机器人进行内螺旋作业的作业路径；如图5所示，为收获机器人进行内螺旋作业的作业路径。
改进的启发式搜索算法具体为：
收获机器人作业路径为长直线，在到达满粮点时当前行未作业部分也为直线，结合当前作业行斜率选择性扩展节点搜索，减少启发式搜索算法的扩展节点数量；
在到达满仓点时从作业路径文件中获取当前作业行的斜率，假设当前作业行线性方程为：
；
其中，
；
其中，
；
是斜率信息权重系数，计算方法为：
；
其中，
根据上述方式计算
通过改进的启发式搜索算法获取两点的往返最短路径
遍历当前作业轮次可行进栅格列表，每次按序将队列中的栅格作为起点，以定点卸粮点所在栅格为终点，根据收获机器人转弯半径、转弯次数、末端航向作为约束、以地图边界作为搜索边界，采用
；
其中，
计算方法为：
；
计算方法为：
；
其中，
；
等价于通过Reeds-Shepp方法所规划出的曲线路径长度/>
Reeds-Shepp是一种曲线规划方法，在假设收获机器人可以向前和向后行驶的情况下，通过结合收获机器人最小转弯半径，寻找路径池中的最小长度路径，规划形成的曲线至少由三段组成，每段是收获机器人最小转弯半径单位圆的圆弧或直线段。
计算方法为：
；
其中，
为直线段起点/>
；
为拐过圆弧的长度，计算方法为：
；
其中，
路径成本
；
本实施例中，当卸粮切出点处于图4的③边时，
S4、根据可行进距离判断是否进入卸粮切出点决策，采用滑动窗口算法计算最佳卸粮切出区域；在未到达卸粮切出区域时，收获机保持作业状态追踪作业路径，到达则停止作业，切换为卸粮路径追踪完成卸粮，返回卸粮切出区域后重新进入作业路径进行作业。
其中，当收获机器人可作业距离小于当前作业圈的路径长度时（图6中，内螺旋作业中①②③④组成当前作业圈，⑤⑥⑦⑧组成下一作业圈；图7中，梭行法作业中①②③④组成当前作业圈，⑤⑥⑦⑧组成下一作业圈），则满粮点在当前作业圈内，必须在当前作业圈内完成卸粮，由于作业路径不重复可保证卸粮最短路径唯一，判断方法为可行驶栅格列表中所有栅格均不在收获机器人当前所处栅格的左右邻域（即图8中的阴影区域），满足条件则启动滑动窗口算法计算最佳卸粮切出区域；如图8所示，虚实线区域为待作业区域。
滑动窗口算法计算最佳卸粮切出区域具体为：
将可行驶栅格列表中的第一个元素作为滑动窗口的初始位置，根据收获机器人机身长度确定滑动窗口初始大小，设收获机器人的机身长度为
；
当前窗口内的路径成本之和的计算公式为：
；
以单元栅格作为步长移动窗口，并计算每次移动后窗口的
还需要说明的是，在本说明书中，诸如术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。