CN114326717A - 一种智能农机转场与作业的融合路径规划方法及系统 - Google Patents

Abstract

本发明提出一种智能农机转场与作业的融合路径规划方法及系统，包括：根据农机位姿信息，全局搜索栅格地图，以规划一条从起点位置到作业区域的转场路径；以作业区域最短的一条边作为目标边，以目标边的边长和方向，生多条平行的线段路径，线段间隔为作业耕宽，并根据最小转弯半径，在每条线段路径的末端添加转弯路径，得到作业路径；以目标边中距离转场路径末端最近的点为作业起点，添加一段连接转场路径终点和作业起点的平滑路径，并在作业路径的末端添加一段与线段路径平行等长的退出路径，全局搜索栅格地图，以规划从退出路径末端到起点位置的离场路径；连接转场路径、作业路径、离场路径和退出路径，作为农机最终的行驶与作业路径。

Description

一种智能农机转场与作业的融合路径规划方法及系统

技术领域

本发明涉及智能农机、无人驾驶、人工智能和路径规划技术领域，并特别涉及一种应用于智能农机转场与作业的融合路径规划方法及系统。

背景技术

智能农机无人驾驶系统能够显著地提升农业生产的效率和质量，解放农业生产劳动力，加快我国农业的信息化与智能化进程。路径规划技术是智能农机无人驾驶技术中的一项关键技术，合理有效的行驶与作业路径规划是保证智能农机稳定高效作业的基础。目前的智能路径规划方法主要有以下几种：(1)最短路径搜索，基于已有地图生成栅格地图，使用深度优度或广度优先的方法在栅格地图中搜索最短的可行路径，这种方法适用于农机转场的路径规划，且最终生成路径的精度依赖于栅格大小的选择；(2)路径全覆盖规划，基于多边形，依据一定的评价指标生成全覆盖的农机作业路线，这种方法适用于农机作业路线的规划，生成的路径需根据具体应用场景作出优化，难以设置统一的优化指标；(3)调头路径规划，基于全覆盖规划算法规划出的作业路线，根据农机的运动模型添加适合的调头路线，这种方法适用于农机作业过程中切换耕道时的路径规划。

现有智能农机路径规划算法，大多针对智能农机行驶作业过程中的单一环节，无法覆盖农机行驶作业的整个流程，这是由于目前农机的智能化程度不高，用于作业的智能农机大多只能实现作业的无人化，而不能实现从较为遥远的机库自动行驶至作业地头；此外，现有路径规划算法可配置性与适应性较差，无法适用于多种农机作业场景与作业模式，但往往农机作业场景境以及作业模式都是多变的。

农机作业场景错综复杂，在如此场景下进行农机的路径规划，农机行驶作业的自主性与环境适应性是需要着重考虑优化的指标，为了提升规划方法在这些指标下的性能，需要解决以下问题：

(1)智能农机的作业流程较为复杂，现有技术需要在不同的行驶作业阶段进行多种不同形式的路径规划。应用上述方法进行路径规划，需要人为地加入一些判断因素，无法满足智能农机行驶作业的完全自主性，也不能保证行驶作业的效率，让智能农机显得不够“智能”。其中的技术难点在于如何精准判断农机行驶作业阶段拼接不同阶段的路径，从而提升智能农机的行驶作业自主性；

(2)智能农机的路径规划方法需要符合农机的运动学以及动力学模型，同时要结合农机作业的场景，保证最终规划出的路径合理有效，能够精确引导农机完成各项任务。应用上述方法进行路径规划时，仅仅考虑的是规划路径的效率以及代价成本，不能够根据农机与作业场景的实际情况对路径做出进一步优化，造成规划出的路径不能够很好地匹配之后的路径跟踪算法。其中的难点在于如何优化路径适配多场景多运动模型从而提升智能农机的环境适应性。

发明内容

本发明的目的是解决现有智能农机路径规划算法在农机无人驾驶过程中存在的一些功能性和适应性上的局限，提出一种将转场路线与作业路线相融合的统一路径规划方法。

针对现有技术的不足，本发明提出一种智能农机转场与作业的融合路径规划方法，其中包括：

步骤1、获取农机的位姿信息、作业耕宽、最小转弯半径，以及包含多边形作业区域的栅格地图；

步骤2、根据该位姿信息中农机的起点位置和朝向，全局搜索该栅格地图，以规划一条从该起点位置到该作业区域的转场路径；以该作业区域最短的一条边作为目标边，以该目标边的边长和方向，生多条平行的线段路径，线段间隔为该作业耕宽，并根据该最小转弯半径，在每条线段路径的末端添加转弯路径，得到作业路径；

步骤3、以该目标边中距离该转场路径末端最近的点为作业起点，添加一段连接该转场路径终点和该作业起点的平滑路径，并在该作业路径的末端添加一段与该线段路径平行等长的退出路径，以该退出路径末端的朝向和在该栅格地图中的位置，全局搜索该栅格地图，以规划一条从该退出路径末端到该起点位置的离场路径，连接该转场路径、该作业路径、该离场路径和该退出路径，作为农机最终的行驶与作业路径。

所述的智能农机转场与作业的融合路径规划方法，其中对该平滑路径的路径点间插入多个等间距的路径点，且该平滑路径包含两部分：第一部分是由作业起点为切点，以农机最小转弯半径为半径的圆弧，该圆弧另一个切点由以农机结束转场时位置和朝向作出的射线所决定，第二部分是由该射线与圆相切所截取的线段。

所述的智能农机转场与作业的融合路径规划方法，其中该全局搜索为广度优先搜索或启发式搜索。

所述的智能农机转场与作业的融合路径规划方法，其中根据该作业耕宽和该最小转弯半径，选择鱼尾型或灯泡型或弓型路径作为该转弯路径。

本发明还提出了一种智能农机转场与作业的融合路径规划系统，其中包括：

初始化模块，用于获取农机的位姿信息、作业耕宽、最小转弯半径，以及包含多边形作业区域的栅格地图；

第一路径规划模块，用于根据该位姿信息中农机的起点位置和朝向，全局搜索该栅格地图，以规划一条从该起点位置到该作业区域的转场路径；以该作业区域最短的一条边作为目标边，以该目标边的边长和方向，生多条平行的线段路径，线段间隔为该作业耕宽，并根据该最小转弯半径，在每条线段路径的末端添加转弯路径，得到作业路径；

第二路径规划模块，用于以该目标边中距离该转场路径末端最近的点为作业起点，添加一段连接该转场路径终点和该作业起点的平滑路径，并在该作业路径的末端添加一段与该线段路径平行等长的退出路径，以该退出路径末端的朝向和在该栅格地图中的位置，全局搜索该栅格地图，以规划一条从该退出路径末端到该起点位置的离场路径，连接该转场路径、该作业路径、该离场路径和该退出路径，作为农机最终的行驶与作业路径。

所述的智能农机转场与作业的融合路径规划系统，其中对该平滑路径的路径点间插入多个等间距的路径点，且该平滑路径包含两部分：第一部分是由作业起点为切点，以农机最小转弯半径为半径的圆弧，该圆弧另一个切点由以农机结束转场时位置和朝向作出的射线所决定，第二部分是由该射线与圆相切所截取的线段。

所述的智能农机转场与作业的融合路径规划系统，其中该全局搜索为广度优先搜索或启发式搜索。

所述的智能农机转场与作业的融合路径规划系统，其中根据该作业耕宽和该最小转弯半径，选择鱼尾型或灯泡型或弓型路径作为该转弯路径。

本发明还提出了一种存储介质，用于存储执行所述任意一种智能农机转场与作业的融合路径规划方法的程序。

本发明还提出了一种客户端，用于上述任意一种智能农机转场与作业的融合路径规划系统。

由以上方案可知，本发明的优点在于：本发明解决了智能农机转场以及作业过程中的路径规划问题，效果是用户只需指定作业区块的位置，便可实现智能农机完全自主的路径规划，规划路线能够覆盖整个行驶作业流程，适应多种作业场景并且精准有效，能够有效地提升智能农机作业效率，加深农机智能化程度。

附图说明

图1为转场路径规划流程图；

图2为作业路径规划流程图；

图3为路径融合与平滑流程图；

图4为具有的作业区域的栅格地图；

图5为起点到作业区域路径规划图；

图6为返程路径规划图；

图7为采用鱼尾型调头时的作业路径规划图；

图8为加入平滑融合后的最终路径规划图。

具体实施方式

本发明基于以下关键技术点，提出并实现了一种覆盖农机作业整个流程的、适用于多场景多作业模式的融合路径规划方法。

关键点1，转场最短路径规划机制；

技术效果，在作业任务确定之后，智能农机能够自主规划从机库前往作业区块的最短路径，在作业任务结束之后，智能农机能够自主规划从作业区块返回机库的最短路径；

关键点2，灵活作业路径覆盖机制；

技术效果，智能农机到达作业区块地头之后，能够自主规划覆盖整个区块的作业路径，作业路径方向与掉头路径模式会根据区块大小与农机模型自主确定，也可按照用户需求进行修改；

关键点3，转场与作业路径融合规划机制；

技术效果，规划算法能够自主确定转场与作业路径的交接点，并根据农机模型进行局部平滑，交接点位置的也可由用户配置和指定；

为让本发明的上述特征和效果能阐述的更明确易懂，下文特举实施例，并配合说明书附图作详细说明如下。

1、系统输入

如图4所示，利用GPS定位技术获取农机的实时位姿信息，位姿信息包括位置和姿态。利用激光雷达扫描得到带有代价数值的栅格地图。获取农机的作业耕宽，用于规划往返路径的次数。获取农机的最小转弯半径，用于转场和作业路径融合时规划平滑路径。绘制作业区块多边形时预设的顶点。

2、转场路径规划

转场路径包含两段，第一段是从农机启动位置转场到作业区域，第二段是由农机完成作业时返回出发位置。如图1所示，首先，通过激光雷达扫描整个试验环境，根据得到的点云数据通过SLAM算法拼接生成带有代价的栅格地图。在此地图中，选取适当的位置作为原点，并利用GPS定位获取该原点的位置信息，此时，我们获得了世界坐标系，之后讨论的位置和朝向信息默认都是关于此坐标系。农机启动伊始，通过GPS定位获取当下农机的位置信息。在操作面板上，按顺序绘制农机作业区块的顶点，每个顶点带有位置和朝向信息。此时根据农机起始点和区块顶点的位置和朝向信息，使用全局搜索算法在代价地图上规划出一条从起始点到进入作业区域的最佳转场路径。返回时同样使用搜索算法，此时是根据农机完成作业时的位置和朝向信息规划出至起始点的路径。

根据需求，有Dijkstra和A*两种全局搜索算法供用户选择。两种算法各有自身优缺点和适用场景。简单来说，Dijkstra是一种广度优先的搜索算法，该算法会遍历所有路径并从中选择最优，因此，该算法会提供最优解但耗费时间。A*是一种启发式搜索算法，由估价函数f(n)＝g(n)+h(n)决定，其中g(n)代表从起点到n节点的实际代价,h(n)代表n节点到目标节点最佳路径的估计代价。相比Dijkstra，A*不能保证提供最优路径但提高了搜索效率，满足了大部分场景的需求。最终得到的效果如图5和6所示。

3、作业路径规划

如图2所示，当绘制完作业区块的目标顶点时，按顶点绘制的顺序依次连接会生成一个目标多边形。

此时需注意绘制的顶点需要在有效区域内，即在代价不到阈值的栅格内，否则多边形无法生成，需重新绘制。在目标多边形内，选择最短的一条边作为目标边，以该边边长和方向，生若干条平行的线段，线段间间隔为作业耕宽。这类线段为农机往返的作业路径，此时还需在两两线段间添加转弯路径使农机可以连续作业。所添加的转弯路径朝向需要按奇偶性放置，即同为奇数次的转弯路径同向并与同为偶数次的转弯路径反向。考虑到农机实际的转置方式，转弯路径的形状还需要根据农机的最小转弯半径与耕宽间的数值关系，自动调整为鱼尾型(Fishtail)，灯泡型(Bulb)，弓型(Bow)三种类型中的合适路径。最终得到的效果如图7所示

根据用户需求，作业路径规划的模式分为往返路径和回型路径，用户可以在操作面板上自行切换调整。

4、路径融合与平滑

如图3所示，由于转场和作业的路径规划是相对独立完成的，在两者的融合上则需要进行平滑处理使农机在实际跟踪路径时不会出现过大偏离。融合部分主要分为两处：

第一处在农机完成转场到进入作业时。由于农机起始朝向与进入作业点的朝向存在偏差，农机必定会存在转向问题，因此需要添加一段平滑路径使其顺利地转入作业区块。该平滑路径包含两部分，第一部分是由作业起始点为切点，以农机最小转弯半径为半径的圆弧，该圆弧另一个切点是由以农机结束转场时的位置和朝向作的射线决定。第二部分是由该射线与圆相切所截取的线段组成。由于这些切点是关于农机的位置信息，因此在规划此圆时需要先把世界坐标系转换成农机局部坐标系，此坐标系是以作业起始点为原点，作业起始点朝向为y正半轴的正交坐标系。

第二处融合是在农机完成作业到进入返场时。此时同样需要加入一段平滑路径调整农机的姿态使其顺利地进入返场路径。不同的是，此时还需要使农机避开作业区块。考虑到作业路径规划最终是以一段转弯路径结束，因此需要在此结束路径点后再添加一段与之前往返线段平行等长的路径，以确保农机驶出作业区块再调整自身朝向。之后，再添加一段转弯路径用以调整农机朝向与进入返场路径点的朝向一致。规划这段转弯路径同样需要先将坐标系转化成以农机当前位置为原点，农机当前朝向为y正半轴的坐标系。在此坐标系上，以原点为切点作以农机最小转弯半径为半径的圆弧，另一切点为以进入返场路径点朝向为方向的切线与此圆相切点。针对每种作业路径规划情况，为了避免转向作业区块，将以农机当前朝向作为判断条件，规划出不同方向的转弯路径。具体来说，若农机在世界坐标系下的偏航角在0～180°间，圆弧将规划在农机坐标系下的第一象限中，即农机以右转的方式调整。反之，若农机的偏航角在0～-180°间，圆弧将规划在农机坐标系下的第二象限中，即农机以左转的方式调整。最终得到的效果如图8所示

为了提升农机跟踪效果，将这两处平滑路径做插值处理，即在原有的路径点间插入若干新的等间距的路径点。

以下为与上述方法实施例对应的系统实施例，本实施方式可与上述实施方式互相配合实施。上述实施方式中提到的相关技术细节在本实施方式中依然有效，为了减少重复，这里不再赘述。相应地，本实施方式中提到的相关技术细节也可应用在上述实施方式中。

本发明还提出了一种智能农机转场与作业的融合路径规划系统，其中包括：

初始化模块，用于获取农机的位姿信息、作业耕宽、最小转弯半径，以及包含多边形作业区域的栅格地图；

第一路径规划模块，用于根据该位姿信息中农机的起点位置和朝向，全局搜索该栅格地图，以规划一条从该起点位置到该作业区域的转场路径；以该作业区域最短的一条边作为目标边，以该目标边的边长和方向，生多条平行的线段路径，线段间隔为该作业耕宽，并根据该最小转弯半径，在每条线段路径的末端添加转弯路径，得到作业路径；

第二路径规划模块，用于以该目标边中距离该转场路径末端最近的点为作业起点，添加一段连接该转场路径终点和该作业起点的平滑路径，并在该作业路径的末端添加一段与该线段路径平行等长的退出路径，以该退出路径末端的朝向和在该栅格地图中的位置，全局搜索该栅格地图，以规划一条从该退出路径末端到该起点位置的离场路径，连接该转场路径、该作业路径、该离场路径和该退出路径，作为农机最终的行驶与作业路径。

所述的智能农机转场与作业的融合路径规划系统，其中对该平滑路径的路径点间插入多个等间距的路径点，且该平滑路径包含两部分：第一部分是由作业起点为切点，以农机最小转弯半径为半径的圆弧，该圆弧另一个切点由以农机结束转场时位置和朝向作出的射线所决定，第二部分是由该射线与圆相切所截取的线段。

所述的智能农机转场与作业的融合路径规划系统，其中该全局搜索为广度优先搜索或启发式搜索。

所述的智能农机转场与作业的融合路径规划系统，其中根据该作业耕宽和该最小转弯半径，选择鱼尾型或灯泡型或弓型路径作为该转弯路径。

本发明还提出了一种存储介质，用于存储执行所述任意一种智能农机转场与作业的融合路径规划方法的程序。

本发明还提出了一种客户端，用于上述任意一种智能农机转场与作业的融合路径规划系统。

Claims (10)

1.一种智能农机转场与作业的融合路径规划方法，其特征在于，包括：

步骤1、获取农机的位姿信息、作业耕宽、最小转弯半径，以及包含多边形作业区域的栅格地图；

步骤2、根据该位姿信息中农机的起点位置和朝向，全局搜索该栅格地图，以规划一条从该起点位置到该作业区域的转场路径；以该作业区域最短的一条边作为目标边，以该目标边的边长和方向，生多条平行的线段路径，线段间隔为该作业耕宽，并根据该最小转弯半径，在每条线段路径的末端添加转弯路径，得到作业路径；

步骤3、以该目标边中距离该转场路径末端最近的点为作业起点，添加一段连接该转场路径终点和该作业起点的平滑路径，并在该作业路径的末端添加一段与该线段路径平行等长的退出路径，以该退出路径末端的朝向和在该栅格地图中的位置，全局搜索该栅格地图，以规划一条从该退出路径末端到该起点位置的离场路径，连接该转场路径、该作业路径、该离场路径和该退出路径，作为农机最终的行驶与作业路径。

2.如权利要求1所述的智能农机转场与作业的融合路径规划方法，其特征在于，对该平滑路径的路径点间插入多个等间距的路径点，且该平滑路径包含两部分：第一部分是由作业起点为切点，以农机最小转弯半径为半径的圆弧，该圆弧另一个切点由以农机结束转场时位置和朝向作出的射线所决定，第二部分是由该射线与圆相切所截取的线段。

3.如权利要求1所述的智能农机转场与作业的融合路径规划方法，其特征在于，该全局搜索为广度优先搜索或启发式搜索。

4.如权利要求1所述的智能农机转场与作业的融合路径规划方法，其特征在于，根据该作业耕宽和该最小转弯半径，选择鱼尾型或灯泡型或弓型路径作为该转弯路径。

5.一种智能农机转场与作业的融合路径规划系统，其特征在于，包括：

初始化模块，用于获取农机的位姿信息、作业耕宽、最小转弯半径，以及包含多边形作业区域的栅格地图；

第一路径规划模块，用于根据该位姿信息中农机的起点位置和朝向，全局搜索该栅格地图，以规划一条从该起点位置到该作业区域的转场路径；以该作业区域最短的一条边作为目标边，以该目标边的边长和方向，生多条平行的线段路径，线段间隔为该作业耕宽，并根据该最小转弯半径，在每条线段路径的末端添加转弯路径，得到作业路径；

第二路径规划模块，用于以该目标边中距离该转场路径末端最近的点为作业起点，添加一段连接该转场路径终点和该作业起点的平滑路径，并在该作业路径的末端添加一段与该线段路径平行等长的退出路径，以该退出路径末端的朝向和在该栅格地图中的位置，全局搜索该栅格地图，以规划一条从该退出路径末端到该起点位置的离场路径，连接该转场路径、该作业路径、该离场路径和该退出路径，作为农机最终的行驶与作业路径。

6.如权利要求1所述的智能农机转场与作业的融合路径规划系统，其特征在于，对该平滑路径的路径点间插入多个等间距的路径点，且该平滑路径包含两部分：第一部分是由作业起点为切点，以农机最小转弯半径为半径的圆弧，该圆弧另一个切点由以农机结束转场时位置和朝向作出的射线所决定，第二部分是由该射线与圆相切所截取的线段。

7.如权利要求1所述的智能农机转场与作业的融合路径规划系统，其特征在于，该全局搜索为广度优先搜索或启发式搜索。

8.如权利要求1所述的智能农机转场与作业的融合路径规划系统，其特征在于，根据该作业耕宽和该最小转弯半径，选择鱼尾型或灯泡型或弓型路径作为该转弯路径。

9.一种存储介质，用于存储执行如权利要求1到4所述任意一种智能农机转场与作业的融合路径规划方法的程序。

10.一种客户端，用于权利要求5至8中任意一种智能农机转场与作业的融合路径规划系统。

Images