CN113485360A - 基于改进搜索算法的agv机器人路径规划方法及系统 - Google Patents

Abstract

本发明涉及一种基于改进搜索算法的AGV机器人路径规划方法及系统，该方法基于搜索算法采用双向迭代方式进行路径搜索，即从起点与终点分别开始迭代搜索直至双向搜索汇合，其中双向搜索采用不同的启发式函数，以避免双向搜索受同一环境因素干扰；对获取的一系列路径轨迹节点进行关键节点提取，形成并输出路径信息。该方法及系统有利于提高路径搜索效率。

Description

基于改进搜索算法的AGV机器人路径规划方法及系统

技术领域

本发明属于机器人导航规划领域，具体涉及一种基于改进搜索算法的AGV机器人路径规划方法及系统。

背景技术

AGV是一种无人驾驶物流搬运车，属于智能型移动机器人。可以按照上位机下达的指令，通过车载传感器确定位置与环境信息，并根据事先设计的程序，沿着规划的路线自动行驶并且在指定位置停靠，具有自动避障导航等功能，极大程度解放了劳动力，提高了运输效率。同时其较为完善的安全防护及导航避障功能搭配智能化的调度管理，使得工业物流运输的安全性和可靠性得到大大提高，而其中的路径规划功能是移动机器人在复杂环境下完成任务实现自主导航的核心支撑。

通常认为路径规划方法分为基于搜索的路径规划方法、基于采样的路径规划算法以及智能算法。基于搜索的路径规划，如Dijkstra、A*及其经典的变体算法，如ARA*、Weighted A*、Theta*、lazy Theta*等，其本质都是在已经构建好的地图上，首先定义成本函数，然后搜索每个节点和边以找到成本最小路径，其核心在于启发式函数的设计，该类算法容易扩展许多具有相同代价的节点。而基于采样的路径规划算法，RRT、RRT*等，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。通过对状态空间进行采样，可以保证得到连接起始点与终点的可行解，但是由于采样过程是对整个空间进行不稳定采样，因此效率较低，在复杂场景下很难实时求解并且保证解的最优性。智能算法如禁忌搜索算法，粒子群优化、灰狼算法等往往需要大量时间进行迭代技术，实时性较差，通常与其他算法结合使用。其中基于搜索的路径规划方法由于导向性好，实时性较高，适合在低维的状态空间下使用，在AGV的工作环境中得到了广泛应用。但是现有基于搜索的算法仍存在在较大场景路径搜索内存消耗大、效率低、路径未考虑机器人执行难度等问题。

发明内容

本发明的目的在于提供一种基于改进搜索算法的AGV机器人路径规划方法及系统，该方法及系统有利于提高路径搜索效率。

为实现上述目的，本发明采用的技术方案是：一种基于改进搜索算法的AGV机器人路径规划方法，基于A^*搜索算法采用双向迭代方式进行路径搜索，即从起点与终点分别开始迭代搜索直至双向搜索汇合，其中双向搜索采用不同的启发式函数，以避免双向搜索受同一环境因素干扰；对获取的一系列路径轨迹节点进行关键节点提取，形成并输出路径信息。

进一步地，该方法包括以下步骤：

1)初始化搜索，获取路径的起点、终点信息，并将起点、终点分别放入正向搜索开启列表OpenList1、反向搜索开启列表OpenList2中，所述OpenList1列表、OpenList2列表分别用于存放可以搜索的节点；

2)进行正向搜索扩展节点，判断当前节点是否在反向搜索关闭列表CloseList2中，是则转下一步，否则将当前节点放入正向搜索关闭列表CloseList1中，然后判断此时OpenList1列表是否为空，是则搜索结束，否则在OpenList1列表中选取代价f(n)最小节点，继续进行正向搜索扩展节点；同时进行反向搜索扩展节点，判断当前节点是否在正向搜索关闭列表CloseList1中，是则转下一步，否则将当前节点放入反向搜索关闭列表CloseList2中，然后判断此时OpenList2列表是否为空，是则搜索结束，否则在OpenList2列表中选取代价f(n)最小节点，继续进行反向搜索扩展节点；所述CloseList1列表、CloseList2列表分别用于存放已搜索过的节点；

3)初步路径搜索完成；对获取的一系列路径轨迹节点进行关键节点提取，然后基于获得的关键节点形成并输出路径信息，路径搜索结束。

进一步地，A^*算法搜索路径的代价函数为：

f(n)＝g(n)+h(n)

其中，n为当前节点状态，g(n)为从起始状态到当前节点状态的成本累计，h(n)为启发式函数；

在不考虑环境中障碍物的情况下，通过考虑对角线运动方向，使h(n)＝h^*(n)，实现效率与最优性的兼顾：

dx＝abs(node.x-goal.x)

dy＝abs(node.y-goal.y)

其中，node.x、node.y分别表示当前节点的x、y坐标信息，goal.x、goal.y分别表示目标节点的x、y坐标信息；

对于正向搜索的启发式函数，引入效率因子，通过对比父节点与子节点关于当前节点的直线度，设置惩罚项，对h*(n)进行干涉，减少扩展不必要的转弯路径，增加搜索的深度，正向搜索的启发式函数如下：

其中，p为常数项，

(x_i,y_i)为当前节点，(x_i-1,y_i-1)为父节点，(x,_i+1,y_i+1)为子节点，a表示从父节点指向当前节点的向量，b表示从当前节点指向子节点的向量；

因此，正向搜索的代价函数为：

f_f(n)＝g(n)+h^s(n)。

进一步地，对于反向搜索，扩展的过程中需要兼顾起点与正向搜索当前节点信息，其代价函数为：

其中，h^s(n₁)为反向搜索当前节点关于反向搜索终点的启发项，h^s(n₂)为反向搜索当前节点关于正向搜索当前节点的启发项，c为常数项。

进一步地，采用映射算法对获取的一系列路径轨迹节点进行映射处理，以提取关键节点。

进一步地，采用映射算法对路径轨迹节点进行映射处理，包括以下步骤：

301)维护关键节点队列WAYPOINTS，将起点(x_s,y_s)作为映射算法的第一个点计为q_s，加入队列；

302)从起点开始依次与搜索算法的路径节点做连线，直到找到经过障碍物的连线L_i，对应路径节点q_i(x_i,y_i)，将q_i(x_i,y_i)的前一路径节点q_i-1(x_i-1,y_i-1)加入WAYPOINTS队列；

303)若连接至终点q_g(x_g,y_g)均未经过障碍物，则将终点加入WAYPOINTS，映射算法结束；

304)以q_i-1为起点重复操作302、303，直至终点q_g加入WAYPOINTS队列，结束映射算法。

本发明还提供了一种基于改进搜索算法的AGV机器人路径规划系统，其特征在于，包括存储器、处理器以及存储于存储器上并能够被处理器运行的计算机程序指令，当处理器运行该计算机程序指令时，能够实现上述的方法步骤。

相较于现有技术，本发明具有以下有益效果：本发明基于A^*搜索算法，通过采用不同的启发式函数进行双向路径搜索，提高了AGV机器人路径搜索的效率，并通过设计路径直线度的惩罚项，减少了不必要的节点扩展。此外，对获取的一系列路径轨迹节点进行特征提取，减少因储存与维护导致的算法效率低下，同时为后续优化机器人实际运动轨迹，提供关键节点。

附图说明

图1是本发明实施例的方法实现流程图。

图2是本发明实施例中节点之间的位置关系。

图3是本发明实施例中本方法与现有方法的路径搜索效果比较图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

本实施例提供了一种基于改进搜索算法的AGV机器人路径规划方法，基于A^*搜索算法采用双向迭代方式进行路径搜索，即从起点与终点分别开始迭代搜索直至双向搜索汇合，其中双向搜索采用不同的启发式函数，以避免双向搜索受同一环境因素干扰；对获取的一系列路径轨迹节点进行关键节点提取，形成并输出路径信息。通过采用不同的启发式函数，可以提升搜索效率及环境适用性，而通过对路径节点的关键点提取，可以减少需要储存的节点数。如图1所示，该方法具体包括以下步骤：

1)初始化搜索，获取路径的起点、终点信息，即获取地图上所有节点的初始信息，如该节点是否为障碍物等，并将起点、终点分别放入正向搜索开启列表OpenList1、反向搜索开启列表OpenList2中。

所述OpenList1列表、OpenList2列表分别用于存放可以搜索的节点。进行搜索时，从OpenList中弹出代价最低的节点进行扩展相邻节点后，该节点即被弹出OpenList队列，加入CloseList列表，后续搜索就不会再考虑该节点。当双向搜索路径汇合后，从CloseList中找到节点及父节点信息，回溯即可得到路径。

2)进行正向搜索扩展节点，判断当前节点是否在反向搜索关闭列表CloseList2中，是则转下一步，否则将当前节点放入正向搜索关闭列表CloseList1中，然后判断此时OpenList1列表是否为空，是则搜索结束，否则在OpenList1列表中选取代价f(n)最小节点，继续进行正向搜索扩展节点；同时进行反向搜索扩展节点，判断当前节点是否在正向搜索关闭列表CloseList1中，是则转下一步，否则将当前节点放入反向搜索关闭列表CloseList2中，然后判断此时OpenList2列表是否为空，是则搜索结束，否则在OpenList2列表中选取代价f(n)最小节点，继续进行反向搜索扩展节点；所述CloseList1列表、CloseList2列表分别用于存放已搜索过的节点。

其中，正向搜索和反向搜索是并行的，在初始化的时候，正向搜索和反向搜索都获得地图信息以及起点终点信息，双向A*就是从起点和终点开始分别进行A*搜索，直到两个OpenList中的点出现重复。

假设正向搜索发现当前节点已在CloseList2中，那就结束搜索，不会再执行接下来的反向搜索，搜索程序会跳出搜索的这个循环。搜索程序的终止条件是：直到两个OpenList中的点出现重复。

3)初步路径搜索完成；采用映射算法对获取的一系列路径轨迹节点进行映射处理，提取关键节点，然后基于获得的关键节点形成并输出路径信息，路径搜索结束。

1.代价函数设计

对于A^*等搜索算法，关键在于启发式策略的设计。A^*算法搜索路径的代价函数为：

f(n)＝g(n)+h(n)

其中，n为当前节点状态，g(n)为从起始状态到当前节点状态的成本累计，h(n)为启发式函数，是基于当前点对目标点进行的代价估计，其中启发式函数通常需要满足准许性原则，即当h(n)≤h^*(n)，其中h^*(n)为真实的路径代价时，A^*算法仍能保持算法最优性，即当存在路径时能保障路径的最优性；但当h(n)＜＜h^*(n)，A^*算法容易扩展许多无意义节点，这也是A^*算法效率低下的原因。当h(n)＝h^*(n)时，适当增大h(n)权重或者预估代价略大于实际代价h^*(n)时，A^*算法搜索结果是尽管是次优的，但是搜索速度得到了大幅提升。

由于AGV的工作环境是通过栅格法构建的二维环境地图，地图环境高度结构化，在不考虑环境中障碍物的情况下，通过考虑对角线运动方向，可以使得h(n)＝h^*(n)，实现效率与最优性的兼顾：

dx＝abs(node.x-goal.x)

dy＝abs(node.y-goal.y)

其中，node.x、node.y分别表示当前节点的x、y坐标信息，goal.x、goal.y分别表示目标节点的x、y坐标信息。

然而对AGV设备来说高效的工作路径不应仅满足路径最短，在实际执行过程中路径中过多的转向会使得AGV需要频繁的改变速度，因此综合考虑距离和转向角构造启发信息来改进启发函数，兼顾距离最短和出行复杂度具有重要意义。

通过对搜索算法的启发函数项进行改进，考虑路径实际执行难度，引入效率因子，对h^*(n)稍加干涉，增加搜索的深度。同时为了更好的实现双向搜索算法对环境的兼容性，对正反搜索设计不同的启发式函数，在反向搜索过程中考虑起点与正向搜索当前点信息，共同引导整体搜索的进行。

(1)正向搜索启发式函数

对于正向搜索的启发式函数，引入效率因子，通过对比父节点与子节点关于当前节点的直线度，设置惩罚项，对h*(n)进行干涉，减少扩展不必要的转弯路径，增加搜索的深度，正向搜索的启发式函数如下：

其中，p为常数项，

(x_i,y_i)为当前节点，(x_i-1,y_i-1)为父节点，(x,_i+1,y_i+1)为子节点，a表示从父节点指向当前节点的向量，b表示从当前节点指向子节点的向量。本实施例中节点之间的位置关系如图2所示。

因此，正向搜索的代价函数为：

f_f(n)＝g(n)+h^s(n)。

(2)反向搜索启发式函数

对于反向搜索，搜索任务不仅是从终点寻找到起点的路径，更重要的是配合正向搜索完成路径的汇合，因此扩展的过程中需要兼顾起点与正向搜索当前节点信息，其代价函数为：

其中，h^s(n₁)为反向搜索当前节点关于反向搜索终点的启发项，h^s(n₂)为反向搜索当前节点关于正向搜索当前节点的启发项，c为常数项。

2.路径特征点提取

A^*算法最为人诟病的是精度越高，需要储存节点数据越多，在栅格地图进行路径搜索工作时，一旦地图尺寸偏大或者精度过高则需要维护大量节点造成性能浪费，而对于实际的机器人运动轨迹，A^*算法获取的大多数节点并没有意义，繁杂的数据量反而给后期机器人执行任务带来了困扰。因此需要对获取的一系列路径轨迹节点，进行特征提取，减少因储存与维护导致的算法效率低下，同时为后续优化机器人实际运动轨迹，提供关键节点。对此，在本实施例中，采用映射算法对获取的一系列路径轨迹节点进行映射处理，以提取关键节点，具体包括以下步骤：

301)维护关键节点队列WAYPOINTS，将起点(x_s,y_s)作为映射算法的第一个点计为q_s，加入队列；

302)从起点开始依次与搜索算法的路径节点做连线，直到找到经过障碍物的连线L_i，对应路径节点q_i(x_i,y_i)，将q_i(x_i,y_i)的前一路径节点q_i-1(x_i-1,y_i-1)加入WAYPOINTS队列；

303)若连接至终点q_g(x_g,y_g)均未经过障碍物，则将终点加入WAYPOINTS，映射算法结束；

304)以q_i-1为起点重复操作302、303，直至终点q_g加入WAYPOINTS队列，结束映射算法。

本发明还相应提供了一种基于改进搜索算法的AGV机器人路径规划系统，包括存储器、处理器以及存储于存储器上并能够被处理器运行的计算机程序指令，当处理器运行该计算机程序指令时，能够实现上述的方法步骤。

图3示出了本发明方法与现有的A*搜索算法在路径搜索效果上的差别。图3(a)为A*搜索算法，图3(b)为本发明方法。从图3可以看出，相较于现有的A*搜索算法，本发明方法可以有效缩短搜索路径，进一步得到更优的路径规划。

随着应用场景复杂性不断提升，对机器人路径规划方法的环境适应性要求也越来越高，与此同时，随着搜索空间的增加，需要搜索、以及储存的节点数也大大增加。本发明通过设计不同启发式函数的双向搜索方法，能提升机器人前端路径搜索效率，减少不必要的节点扩展。同时由于反向搜索的启发式函数参考了正向搜索的进程，增强了本方法在不同工作环境中的普适性。

由于规划的路径是一系列折线路径，未考虑机器人运动学约束容易导致路径出现不必要的转向。为了解决该类现象，本发明引入了效率因子的概念，通过设计路径直线度的惩罚项，减少了不必要的扩展。

现有的搜索算法最为人诟病的是精度越高，需要储存节点数据越多，在栅格地图进行路径搜索工作时，一旦地图尺寸偏大或者精度过高则需要维护大量节点造成性能浪费，而对于实际的机器人运动轨迹，算法获取的大多数节点并没有意义，繁杂的数据量反而给后期机器人执行任务带来了困扰。因此，本发明对获取的一系列路径轨迹节点，进行特征提取，减少因储存与维护导致的算法效率低下，同时为后续优化机器人实际运动轨迹，提供关键节点。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (7)

1.一种基于改进搜索算法的AGV机器人路径规划方法，其特征在于，基于A^*搜索算法采用双向迭代方式进行路径搜索，即从起点与终点分别开始迭代搜索直至双向搜索汇合，其中双向搜索采用不同的启发式函数，以避免双向搜索受同一环境因素干扰；对获取的一系列路径轨迹节点进行关键节点提取，形成并输出路径信息。

2.根据权利要求1所述的基于改进搜索算法的AGV机器人路径规划方法，其特征在于，包括以下步骤：

1)初始化搜索，获取路径的起点、终点信息，并将起点、终点分别放入正向搜索开启列表OpenList1、反向搜索开启列表OpenList2中，所述OpenList1列表、OpenList2列表分别用于存放可以搜索的节点；

2)进行正向搜索扩展节点，判断当前节点是否在反向搜索关闭列表CloseList2中，是则转下一步，否则将当前节点放入正向搜索关闭列表CloseList1中，然后判断此时OpenList1列表是否为空，是则搜索结束，否则在OpenList1列表中选取代价f(n)最小节点，继续进行正向搜索扩展节点；同时进行反向搜索扩展节点，判断当前节点是否在正向搜索关闭列表CloseList1中，是则转下一步，否则将当前节点放入反向搜索关闭列表CloseList2中，然后判断此时OpenList2列表是否为空，是则搜索结束，否则在OpenList2列表中选取代价f(n)最小节点，继续进行反向搜索扩展节点；所述CloseList1列表、CloseList2列表分别用于存放已搜索过的节点；

3)初步路径搜索完成；对获取的一系列路径轨迹节点进行关键节点提取，然后基于获得的关键节点形成并输出路径信息，路径搜索结束。

3.根据权利要求2所述的基于改进搜索算法的AGV机器人路径规划方法，其特征在于，A^*算法搜索路径的代价函数为：

f(n)＝g(n)+h(n)

其中，n为当前节点状态，g(n)为从起始状态到当前节点状态的成本累计，h(n)为启发式函数；

在不考虑环境中障碍物的情况下，通过考虑对角线运动方向，使h(n)＝h^*(n)，实现效率与最优性的兼顾：

dx＝abs(node.x-goal.x)

dy＝abs(node.y-goal.y)

其中，node.x、node.y分别表示当前节点的x、y坐标信息，goal.x、goal.y分别表示目标节点的x、y坐标信息；

对于正向搜索的启发式函数，引入效率因子，通过对比父节点与子节点关于当前节点的直线度，设置惩罚项，对h*(n)进行干涉，减少扩展不必要的转弯路径，增加搜索的深度，正向搜索的启发式函数如下：

其中，p为常数项，

(x_i,y_i)为当前节点，(x_i-1,y_i-1)为父节点，(x,_i+1,y_i+1)为子节点，a表示从父节点指向当前节点的向量，b表示从当前节点指向子节点的向量；

因此，正向搜索的代价函数为：

f_f(n)＝g(n)+h^s(n)。

4.根据权利要求3所述的基于改进搜索算法的AGV机器人路径规划方法，其特征在于，对于反向搜索，扩展的过程中需要兼顾起点与正向搜索当前节点信息，其代价函数为：

其中，h^s(n₁)为反向搜索当前节点关于反向搜索终点的启发项，h^s(n₂)为反向搜索当前节点关于正向搜索当前节点的启发项，c为常数项。

5.根据权利要求2所述的基于改进搜索算法的AGV机器人路径规划方法，其特征在于，采用映射算法对获取的一系列路径轨迹节点进行映射处理，以提取关键节点。

6.根据权利要求5所述的基于改进搜索算法的AGV机器人路径规划方法，其特征在于，采用映射算法对路径轨迹节点进行映射处理，包括以下步骤：

301)维护关键节点队列WAYPOINTS，将起点(x_s,y_s)作为映射算法的第一个点计为q_s，加入队列；

302)从起点开始依次与搜索算法的路径节点做连线，直到找到经过障碍物的连线L_i，对应路径节点q_i(x_i,y_i)，将q_i(x_i,y_i)的前一路径节点q_i-1(x_i-1,y_i-1)加入WAYPOINTS队列；

303)若连接至终点q_g(x_g,y_g)均未经过障碍物，则将终点加入WAYPOINTS，映射算法结束；

304)以q_i-1为起点重复操作302、303，直至终点q_g加入WAYPOINTS队列，结束映射算法。

7.一种基于改进搜索算法的AGV机器人路径规划系统，其特征在于，包括存储器、处理器以及存储于存储器上并能够被处理器运行的计算机程序指令，当处理器运行该计算机程序指令时，能够实现如权利要求1-6所述的方法步骤。

Images