CN113295166A - Agv路径规划方法、电子装置和计算机可读存储介质 - Google Patents

Abstract

本发明公开一种AGV路径规划方法、电子装置和计算机可读存储介质，该方法包括：S1、在接收到起点和终点后，将起点作为四叉树的根节点，计算起点的F值，并按F值升序保存到双向指针链表；S2、从链表表头取出节点，确定是否有预设标记；S3、若没有，则确定取出的节点可到达的相邻节点，判断各相邻节点中是否存在终点；S4、若是，则找出是终点的节点，打上预设标记、计算F值，再保存到链表中以及添加到四叉树中，转S2；S5、若否，则计算出各相邻节点的F值，并将各相邻节点保存到双向指针链表以及添加到四叉树中，转S2；S6、若有，则在四叉树中由取出的节点回溯到根节点，以得到起点到终点的路径。本发明技术方案，提升智能停车场的车辆存取效率。

Description

AGV路径规划方法、电子装置和计算机可读存储介质

技术领域

本发明涉及智能停车场领域，特别涉及一种AGV路径规划方法、电子装置和计算机可读存储介质。

背景技术

在智能停车场领域，车辆的存放和取出过程都是通过AGV(Automated GuidedVehicle，自动导引运输车)进行运输。目前，AGV的运输路径通常是采用从可通行的若干路径中随机选取一条，AGV的路径通常不是最优的路径，AGV的运输时间长，造成智能停车场的车辆存取效率低。

发明内容

本发明提供一种AGV路径规划方法、电子装置和计算机可读存储介质，旨在提升智能停车场的车辆存取效率。

为实现上述目的，本发明提出的AGV路径规划方法，所述AGV路径规划方法包括：

S1、在接收到起点和终点后，在预先建立的栅格地图模型中建立四叉树，将所述起点作为所述四叉树的根节点，计算出所述起点的总代价F值，并将所述起点保存到新建的双向指针链表中，所述双向指针链表中的节点按F值升序保存，其中，所述F值的计算公式为F＝F0+F1+F2，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价；

S2、从所述双向指针链表的表头取出节点，确定所述取出的节点是否有预设标记；

S3、在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点，判断各个确定的相邻节点中是否存在所述终点；

S4、若是，则从各个确定的相邻节点中找出是所述终点的节点，将找出的节点打上预设标记、计算F值后，再保存到所述双向指针链表中以及添加到所述四叉树的根节点的对应分支上，跳转至步骤S2；

S5、若否，则计算出各个确定的相邻节点的F值，将各个确定的相邻节点保存到所述双向指针链表中，以及将各个确定的相邻节点添加到所述四叉树的根节点的对应分支上，跳转至步骤S2；

S6、在确定所述取出的节点有预设标记后，在所述四叉树中由所述取出的节点回溯到所述根节点，以得到所述起点到所述终点的路径。

可选地，所述F值的计算公式替换为F＝F0+F1+F2+F3，其中，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价，F3为所述起点到当前节点的累计变向次数的代价。

可选地，所述步骤S3包括：

在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点；

若确定的相邻节点数为零，则跳转至步骤S2；

若确定的相邻节点数大于为零，则判断各个确定的相邻节点中是否存在所述终点。

可选地，在所述步骤S2中，若无法从所述双向指针链表中的表头取到节点，则反馈无法找到所述起点到所述终点的路径。

可选地，所述确定所述取出的节点可到达的除其父节点以外的其它相邻节点的步骤包括：

通过A*算法启发函数确定所述取出的节点的移动方向，根据确定的移动方向获得可到达的相邻节点，其中，所述确定的移动方向中滤除了所述取出的节点朝其父节点回退的方向。

可选地，所述终点为一个或多个。

本发明还提出一种电子装置，所述电子装置包括存储器、处理器，所述存储器上存储有可在所述处理器上运行的AGV路径规划系统，所述AGV路径规划系统被所述处理器执行时实现如下步骤：

a、在接收到起点和终点后，在预先建立的栅格地图模型中建立四叉树，将所述起点作为所述四叉树的根节点，计算出所述起点的总代价F值，并将所述起点保存到新建的双向指针链表中，所述双向指针链表中的节点按F值升序保存，其中，所述F值的计算公式为F＝F0+F1+F2，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价；

b、从所述双向指针链表的表头取出节点，确定所述取出的节点是否有预设标记；

c、在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点，判断各个确定的相邻节点中是否存在所述终点；

d、若是，则从各个确定的相邻节点中找出是所述终点的节点，将找出的节点打上预设标记、计算F值后，再保存到所述双向指针链表中以及添加到所述四叉树的根节点的对应分支上，跳转至步骤b；

e、若否，则计算出各个确定的相邻节点的F值，将各个确定的相邻节点保存到所述双向指针链表中，以及将各个确定的相邻节点添加到所述四叉树的根节点的对应分支上，跳转至步骤b；

f、在确定所述取出的节点有预设标记后，在所述四叉树中由所述取出的节点回溯到所述根节点，以得到所述起点到所述终点的路径。

可选地，所述F值的计算公式替换为F＝F0+F1+F2+F3，其中，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价，F3为所述起点到当前节点的累计变向次数的代价。

可选地，所述步骤c包括：

在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点；

若确定的相邻节点数为零，则跳转至步骤b；

若确定的相邻节点数大于为零，则判断各个确定的相邻节点中是否存在所述终点。

本发明还提出一种计算机可读存储介质，所述计算机可读存储介质存储有AGV路径规划系统，所述AGV路径规划系统可被一个或多个处理器执行，以实现如下步骤：

S01、在接收到起点和终点后，在预先建立的栅格地图模型中建立四叉树，将所述起点作为所述四叉树的根节点，计算出所述起点的总代价F值，并将所述起点保存到新建的双向指针链表中，所述双向指针链表中的节点按F值升序保存，其中，所述F值的计算公式为F＝F0+F1+F2，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价；

S02、从所述双向指针链表的表头取出节点，确定所述取出的节点是否有预设标记；

S03、在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点，判断各个确定的相邻节点中是否存在所述终点；

S04、若是，则从各个确定的相邻节点中找出是所述终点的节点，将找出的节点打上预设标记、计算F值后，再保存到所述双向指针链表中以及添加到所述四叉树的根节点的对应分支上，跳转至步骤S02；

S05、若否，则计算出各个确定的相邻节点的F值，将各个确定的相邻节点保存到所述双向指针链表中，以及将各个确定的相邻节点添加到所述四叉树的根节点的对应分支上，跳转至步骤S02；

S06、在确定所述取出的节点有预设标记后，在所述四叉树中由所述取出的节点回溯到所述根节点，以得到所述起点到所述终点的路径。

本发明方案，基于栅格图法建立环境地图模型(即栅格地图模型)，在栅格地图模型中新建四叉树和双向指针链表，起点作为四叉树的根节点，通过四叉树保存所有搜索路径，通过双向指针链表按节点的总代价F值升序保存搜索到的节点。循环从双向指针链表的表头取出F值最小的节点搜索下一步将到达的所有可能的下一节点，并将搜索到的将达到的节点按F值保存到双向指针链表中，如此循环搜索以搜索到下一步将到达节点为终点，即搜索到从起点到终点的F值最小的路径终点，根据该路径终点在四叉树中回溯，得到从起点到终点的最优路径。使AGV按照规划的最优路径从起点移动运输到终点，运输时间最短，运输最快，如此，大幅提升了智能停车场的车辆存取效率。

附图说明

图1为本发明AGV路径规划方法较佳实施例的流程示意图；

图2为本发明AGV路径规划系统一实施例的运行环境示意图；

图3为本发明AGV路径规划系统一实施例的程序模块图；

图4为本发明AGV路径规划系统二实施例中判断模块的功能模块图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制，基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提出一种AGV路径规划方法，可用于智能停车场等使用AGV进行运输的一些场景，在本发明的下面实施例中，以在智能停车场中的应用场景为例进行阐述。

如图1所示，图1是本发明AGV路径规划方法较佳实施例的流程示意图。

本实施例的AGV路径规划方法包括：

步骤S10，在接收到起点和终点后，在预先建立的栅格地图模型中建立四叉树，将起点作为四叉树的根节点，计算出起点的总代价F值，并将起点保存到新建的双向指针链表中，双向指针链表中的节点按F值升序保存。

本实施例的AGV路径规划方法基于预先建立的栅格地图模型，栅格地图模型为使用栅格图法预先建立的环境地图模型，本实施例环境地图模型为智能停车场的地图模型。建立栅格地图模型后，初始化该栅格地图模型的模型参数，包括：将人工设置的静态代价导入地图模型的F1参数，将静止的AGV在栅格地图模型中的对应位置设置为障碍点(不可通过)，根据移动AGV的剩余路径，修改栅格地图模型中对应位置的F2参数。另外，栅格地图模型会定时或实时更新其模型参数；栅格地图模型中每个节点都有唯一的标识信息(例如，二维码、条形码等)。

在接收到智能停车场系统发送的起点和终点后，先在栅格地图模型中新建四叉树和双向指针链表。四叉树的节点除了设置数据域外，还设置有5个指针和1个int型的方向参数，5个指针分别为father、up、down、left和right，其中father表示该节点的父节点，up表示该节点的上节点，down表示该节点的下节点，left表示该节点的左节点，right表示该节点的右节点，方向参数主要是记录父节点位于该节点的哪个方向。再将起点作为四叉树的根节点，采用计算公式F＝F0+F1+F2计算出起点的总代价F值，并将起点保存到双向指针链表中，四叉树的根节点也会保存有起点的F值。该双向指针链表中的节点是采用按F值大小进行升序保存的，即F值最小的节点保存在双向指针链表的表头，F值越小的节点则保存在双向指针链表越靠表头的位置，若多个F值相同的节点在双向指针链表中保存时，则可按照保存顺序，先保存的节点保存于表头或后保存的节点保存于表头。

F值的计算公式为F＝F0+F1+F2。其中，F0为当前节点距离起点的距离代价，例如，起点的F0为0，由起点向上下左右任意方向移动一步到达的下一节点D1的F0则为1，假设D1是由起点向下移动一步到达的节点，那么由D1向左、右或下到达的下一节点D2的F0则为2，如此随着不回退的移动，F0则逐渐累加，每次累加1；F1为当前节点的静态代价，栅格地图模型中某些路径为主干道，主干道是其他AGV都要经过路径区域，交通流量大，当AGV选择经过这些主干道时，通常会出现避让其他AGV的情况，导致等待时间过长，造成AGV运输时间更长，因此将这些主干道上的节点分别赋予一个静态代价(例如，5)；F2为当前节点的动态代价，栅格地图模型中，有的路径是正在运输的其它移动的AGV的剩余路径，经过这些路径也是可能会出现需要对其它AGV进行等待避让的情况，相较于主干道，可能等待的时间会较短一些，因此给这些路径上的节点赋予一个动态代价(例如，2)。

步骤S20，从双向指针链表的表头取出节点，确定取出的节点是否有预设标记。

从双向指针链表的表头取出节点，取出节点后，双向指针链表中则删除了该取出的节点，表头的节点则更换为之前排在取出的节点后面的那个节点。第一次从双向指针链表的表头取出节点时，取出的节点即为起点。取出节点后，确定取出的节点是否有预设标记，该预设标记为表示该节点是终点的标记(例如，预设标记为“Finished”、“完成”，等等)；通过确定该取出的节点是否有预设标记，以确认取出的节点是不是已经打上标记了的终点。

步骤S30，在确定取出的节点没有预设标记后，确定取出的节点可到达的除其父节点以外的其它相邻节点，判断各个确定的相邻节点中是否存在终点。

在确定取出的节点没有预设标记后，即确认取出的节点不是已经打上标记了的终点，那么则进一步确认取出的节点可到达的除其父节点以外的其它相邻节点，即确认取出的节点分别从上下左右四个方向中的三个方向(除去了朝其父节点移动的方向，因为往回退一定不可能到达不终点)移动到达的相邻节点。确定了各个相邻节点后，再判断各个确定的相邻节点中是否存在终点，由于每个节点都有各自一一对应的标识，通过将各个确定的相邻节点的标识与终点的标识对比，以确定各个确定的相邻节点中是否有终点。

步骤S40，若是，则从各个确定的相邻节点中找出是终点的节点，将找出的节点打上预设标记、计算F值后，再保存到双向指针链表中以及添加到四叉树的根节点的对应分支上，跳转至步骤S20。

若确定得到的各个确定的相邻节点中有为终点的节点，则从各个确定的相邻节点中找出为终点的节点，将找出的节点打上预设标记和计算F值，然后再将找出的节点按F值升序在双向指针链表中保存，并将找出的节点添加到四叉树的根节点的对应分支上(从根节点确定的可到达的各个相邻节点即为根节点的各个分支)。虽然已经找到了为终点的节点，但双向指针链表中可能还存在F值更小的节点还未搜索，可能存在通过其它分支上节点搜索到终点的情况，因此完成上述处理后，跳转回步骤S20，再次从双向指针链表的表头取出节点，循环上述步骤。

步骤S50，若否，则计算出各个确定的相邻节点的F值，将各个确定的相邻节点保存到双向指针链表中，以及将各个确定的相邻节点添加到四叉树的根节点的对应分支上，并跳转至步骤S20。

当判断各个确定的相邻节点不是终点，则分别计算出各个确定的相邻节点的F值，将各个确定的相邻节点按F值升序保存到双向指针链表中，即双向指针链表中新保存的各个节点和之前剩余的节点一同按升序重新排序保存；并且将各个确定的相邻节点添加到四叉树的根节点的对应分支上。完成上述处理后，跳转回步骤S20，再次从双向指针链表的表头取出节点，循环上述步骤。

步骤S60，在确定取出的节点有预设标记后，在四叉树中由取出的节点回溯到根节点，以得到起点到终点的路径。

当确定取出的节点有预设标记，即取出的节点为已经打上标记了的终点，说明已经搜索完所有可能到达终点的节点；并且由于当前取出的节点为从双向指针链表的表头取出的，是双向指针链表中F值最小的节点，因此确定根据该取出的节点的搜索路径找到的起点到终点的路径为最优路径，如此则结束上述搜索的循环步骤，依据四叉树中节点的方向参数和节点的指针信息，在四叉树中从当前取出的节点(即终点)回溯到根节点(即起点)，以得到起点到终点的路径，该路径则为AGV从起点到终点的最优路径，AGV依照该最优路径移动运输，则从起到到终点所需的时间最短。

本实施例的AGV路径规划方法，基于栅格图法建立环境地图模型(即栅格地图模型)，在栅格地图模型中新建四叉树和双向指针链表，起点作为四叉树的根节点，通过四叉树保存所有搜索路径，通过双向指针链表按节点的总代价F值升序保存搜索到的节点。循环从双向指针链表的表头取出F值最小的节点搜索下一步将到达的所有可能的下一节点，并将搜索到的将达到的节点按F值保存到双向指针链表中，如此循环搜索以搜索到下一步将到达节点为终点，即搜索到从起点到终点的F值最小的路径终点，根据该路径终点在四叉树中回溯，得到从起点到终点的最优路径。使AGV按照规划的最优路径从起点移动运输到终点，运输时间最短，运输最快，如此，大幅提升了智能停车场的车辆存取效率。

本实施例中，起点采用一个，终点为一个或多个。当终点为一个时，在智能停车场中对应的场景，例如：1、需要AGV从某个车位取车，则AGV的出发位置为起点，该车位对应的取车位置为终点；2、AGV从取车位置回到车辆的出车交接口。终点为多个时，在智能停车场中对应的场景，例如：需要将停车交接区的车辆，运输到智能停车场的某个区域的若干车位中的其中一个车位上停放，则该区域内的若干车位均为终点。无论终点为一个还是多个，通过AGV路径规划方法均可找到起点到终点的最优路径，终点为多个时，找到的最优路径的终点也是各个终点中最优的终点。

进一步地，在本实施例的AGV路径规划方法中，其步骤S30包括：

在确定取出的节点没有预设标记后，确定取出的节点可到达的除其父节点以外的其它相邻节点；

若确定的相邻节点数为零，则跳转至步骤S20；

若确定的相邻节点数大于为零，则判断各个确定的相邻节点中是否存在终点。

如果确定的相邻节点数为零，说明取出的节点除朝其父节点方向外，其它方向都无法到达相邻节点，即其它方向均为障碍物，该取出的节点进入死胡同，无法到达终点，此时则跳转至步骤S20，以重新从双向指针链表的表头取出节点进行搜索。如果确定的相邻节点数大于零，说明取出的节点除朝其父节点方向外，至少一个其它方向可到达相邻节点，此时则进一步判断各个确定的相邻节点中是否存在终点。

进一步地，在步骤S20中，若无法从双向指针链表中的表头取到节点，则反馈无法找到起点到终点的路径。

若是无法从双向指针链表的表头取到节点，说明双向指针链表已经空了，双向指针链表中的所有节点已经全部取出搜索完，找不到起点到终点的路径，那么此时则反馈无法找到起点到终点的路径，或反馈对应的标志信号以表示无法找到起点到终点的路径。

进一步地，针对终点为一个的情况，本实施例AGV路径规划方法的步骤S30中，确定取出的节点可到达的除其父节点以外的其它相邻节点的步骤包括：

通过A*算法启发函数确定取出的节点的移动方向，根据确定的移动方向获得可到达的相邻节点，其中，确定的移动方向中滤除了取出的节点朝其父节点回退的方向。

先采用A*算法启发函数确定取出的节点的移动方向，确定的移动方向中滤除了取出的节点朝其父节点回退的方向，确定的移动方向通常为一个或两个；再根据确定的移动方向去获得可到达的相邻节点，如此，可以省掉一个或两个方向的搜索，减少搜索节点，提升搜索速度，更快的找到最优路径。

还有一种特殊情况：在通过A*算法启发函数确定取出的节点的移动方向时，若该取出的节点可到达的除朝其父节点回退方向的各个方向中存在F1值变大的情况，则需要将F1值变大的方向的那个节点和确定的移动方向上的节点保存到双向指针链表中。

在上述实施例中，可能存在以下两种特殊情形：1、通过不同的搜索方向搜索到的节点为同一终点，2、通过当前取出的节点确定的可到达的相邻节点中多个为终点(终点为多个时)。在这两种情形下，双向指针链表中均可能同时保存有多个打上预设标记的节点(即终点)；情形1时，双向指针链表中同时保存的终点的F值可能相等，也可能不等；情形2时，双向指针链表中同时保存的终点的F值相等。而本实施例的AGV路径规划方法中只根据保存在双向指针链表表头的一个打上预设标记的节点进行回溯得到起点到终点路径，没有考虑其它F值相等的打上预设标记的节点所回溯得到的最优路径，不够全面。

本实施例基于上述任一实施例的方案，针对上述两种特殊情形，本实施例将F值的计算公式替换为F＝F0+F1+F2+F3，其中，F0为当前节点距离起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价，F3为起点到当前节点的累计变向次数的代价。

由于AGV的变向动作也会一定程度的影响AGV的运输时间，因此，本实施例中将节点的F值的计算增加了F3参数。变向次数的解释如下：假设当前节点为第二节点，第二节点是由第一节点向下移动到达的节点，第三节点是由第二节点移动到达的相邻节点，那么，如果第三节点是第二节点向左或向右移动到达的，则第三节点的变向次数加1(即由第二节点的变向次数加1)，如果第三节点是第二节点向下移动到达的，则第二节点的变向次数不变(为第二节点的变向次数)，累计变向次数的代价F3的值＝累计变向次数×变向单位代价(可预先设定，例如为1)。通过引入F3参数，降低了在双向指针链表中可能出现同时保存有多个F值相等的终点的情况，确保了根据保存在双向指针链表表头的一个打上预设标记的节点回溯得到的起点到终点路径为最优路径。

另外，在其它实施例中，基于AGV路径规划方法一实施例的方案的基础上，还可包括如下方案：在双向指针链表中同时保存有多个打上预设标记的节点，且在确定取出的节点有预设标记时，比较各个打上预设标记的节点的F值，选取F值最小的打上了预设标记的节点，在四叉树中由该选取的节点回溯到根节点，以得到起点到终点的路径。若F值最小的打上了预设标记的节点有多个时，则再比较变向次数，取变向次数最少的节点，在四叉树中由该选取的节点回溯到根节点，以得到起点到终点的路径。

此外，本发明还提出一种AGV路径规划系统。

请参阅图2，是本发明AGV路径规划系统10一实施例的运行环境示意图。

在本实施例中，AGV路径规划系统10安装并运行于电子装置1中。电子装置1可以是桌上型计算机、笔记本、掌上电脑及服务器等计算设备。该电子装置1可包括，但不仅限于，存储器11、处理器12及显示器13。图2仅示出了具有组件11-13的电子装置1，但是应理解的是，并不要求实施所有示出的组件，可以替代的实施更多或者更少的组件。

存储器11是一种计算机存储介质，在一些实施例中可以是电子装置1的内部存储单元，例如该电子装置1的硬盘或内存。存储器11在另一些实施例中也可以是电子装置1的外部存储设备，例如电子装置1上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，存储器11还可以既包括电子装置1的内部存储单元也包括外部存储设备。存储器11用于存储安装于电子装置1的应用软件及各类数据，例如AGV路径规划系统10的程序代码等。存储器11还可以用于暂时地存储已经输出或者将要输出的数据。

处理器12在一些实施例中可以是一中央处理器(Central Processing Unit，CPU)，微处理器或其他数据处理芯片，用于运行存储器11中存储的程序代码或处理数据，例如执行AGV路径规划系统10等。

显示器13在一些实施例中可以是LED显示器、液晶显示器、触控式液晶显示器以及OLED(Organic Light-Emitting Diode，有机发光二极管)触摸器等。显示器13用于显示在电子装置1中处理的信息以及用于显示可视化的用户界面，例如业务定制界面等。电子装置1的部件11-13通过系统总线相互通信。

请参阅图3，是本发明AGV路径规划系统10一实施例的程序模块图。在本实施例中，AGV路径规划系统10可以被分割成一个或多个模块，一个或者多个模块被存储于存储器11中，并由一个或多个处理器(本实施例为处理器12)所执行，以完成本发明。例如，在图3中，AGV路径规划系统10可以被分割成新建模块101、提取模块102、判断模块103、第一处理模块104、第二处理模块105及获取模块106。本发明所称的模块是指能够完成特定功能的一系列计算机程序指令段，比程序更适合于描述AGV路径规划系统10在电子装置1中的执行过程，其中：

新建模块101，用于在接收到起点和终点后，在预先建立的栅格地图模型中建立四叉树，将起点作为四叉树的根节点，计算出起点的总代价F值，并将起点保存到新建的双向指针链表中，双向指针链表中的节点按F值升序保存。

本实施例的AGV路径规划方法基于预先建立的栅格地图模型，栅格地图模型为使用栅格图法预先建立的环境地图模型，本实施例环境地图模型为智能停车场的地图模型。建立栅格地图模型后，初始化该栅格地图模型的模型参数，包括：将人工设置的静态代价导入地图模型的F1参数，将静止的AGV在栅格地图模型中的对应位置设置为障碍点(不可通过)，根据移动AGV的剩余路径，修改栅格地图模型中对应位置的F2参数。另外，栅格地图模型还会定时或实时更新其模型参数；栅格地图模型中每个节点都有唯一的标识信息(例如，二维码、条形码等)。

智能停车场系统在确定起点和终点集后，将起点和终点集发送给AGV终点筛选系统，AGV终点筛选系统在接收到起点和终点后，先在栅格地图模型中新建四叉树和双向指针链表。四叉树的节点除了设置数据域外，还设置有5个指针和1个int型的方向参数，5个指针分别为father、up、down、left和right，其中father表示该节点的父节点，up表示该节点的上节点，down表示该节点的下节点，left表示该节点的左节点，right表示该节点的右节点，方向参数主要是记录父节点位于该节点的哪个方向。再将起点作为四叉树的根节点，采用计算公式F＝F0+F1+F2计算出起点的总代价F值，并将起点保存到双向指针链表中，四叉树的根节点也会保存有起点的F值。该双向指针链表中的节点是采用按F值大小进行升序保存的，即F值最小的节点保存在双向指针链表的表头，F值越小的节点则保存在双向指针链表越靠表头的位置，若多个F值相同的节点在双向指针链表中保存时，则可按照保存顺序，先保存的节点保存于表头或后保存的节点保存于表头。

F值的计算公式为F＝F0+F1+F2。其中，F0为当前节点距离起点的距离代价，例如，起点的F0为0，由起点向上下左右任意方向移动一步到达的下一节点D1的F0则为1，假设D1是由起点向下移动一步到达的节点，那么由D1向左、右或下到达的下一节点D2的F0则为2，如此随着不回退的移动，F0则逐渐累加，每次累加1；F1为当前节点的静态代价，栅格地图模型中某些路径为主干道，主干道是其他AGV都要经过路径区域，交通流量大，当AGV选择经过这些主干道时，通常会出现避让其他AGV的情况，导致等待时间过长，造成AGV运输时间更长，因此将这些主干道上的节点分别赋予一个静态代价(例如，5)；F2为当前节点的动态代价，栅格地图模型中，有的路径是正在运输的其它移动的AGV的剩余路径，经过这些路径也是可能会出现需要对其它AGV进行等待避让的情况，相较于主干道，可能等待的时间会较短一些，因此给这些路径上的节点赋予一个动态代价(例如，2)。

提取模块102，用于从双向指针链表的表头取出节点，确定取出的节点是否有预设标记。

从双向指针链表的表头取出节点，取出节点后，双向指针链表中则删除了该取出的节点，表头的节点则更换为之前排在取出的节点后面的那个节点。第一次从双向指针链表的表头取出节点时，取出的节点即为起点。取出节点后，确定取出的节点是否有预设标记，该预设标记为表示该节点是终点的标记(例如，预设标记为“Finished”、“完成”，等等)；通过确定该取出的节点是否有预设标记，以确认取出的节点是不是已经打上标记了的终点。

判断模块103，用于在确定取出的节点没有预设标记后，确定取出的节点可到达的除其父节点以外的其它相邻节点，判断各个确定的相邻节点中是否存在终点。

在确定取出的节点没有预设标记后，即确认取出的节点不是已经打上标记了的终点，那么则进一步确认取出的节点可到达的除其父节点以外的其它相邻节点，即确认取出的节点分别从上下左右四个方向中的三个方向(除去了朝其父节点移动的方向，因为往回退一定不可能到达不终点)移动到达的相邻节点。确定了各个相邻节点后，再判断各个确定的相邻节点中是否存在终点，由于每个节点都有各自一一对应的标识，通过将各个确定的相邻节点的标识与终点的标识对比，以确定各个确定的相邻节点中是否有终点。

第一处理模块104，用于在判断各个确定的相邻节点中存在终点后，从各个确定的相邻节点中找出是终点的节点，将找出的节点打上预设标记、计算F值后，再保存到双向指针链表中以及添加到四叉树的根节点的对应分支上，跳转至提取模块102。

若确定得到的各个确定的相邻节点中有为终点的节点，则从各个确定的相邻节点中找出为终点的节点，将找出的节点打上预设标记和计算F值，然后再将找出的节点按F值升序在双向指针链表中保存，并将找出的节点添加到四叉树的根节点的对应分支上(从根节点确定的可到达的各个相邻节点即为根节点的各个分支)。虽然已经找到了为终点的节点，但双向指针链表中可能还存在F值更小的节点还未搜索，可能存在通过其它分支上节点搜索到终点的情况，因此完成上述处理后，跳转至提取模块102，再次从双向指针链表的表头取出节点，循环上述步骤。

第二处理模块105，用于在判断各个确定的相邻节点中不存在终点后，计算出各个确定的相邻节点的F值，将各个确定的相邻节点保存到双向指针链表中，以及将各个确定的相邻节点添加到四叉树的根节点的对应分支上，并跳转至提取模块102。

当判断各个确定的相邻节点不是终点，则分别计算出各个确定的相邻节点的F值，将各个确定的相邻节点按F值升序保存到双向指针链表中，即双向指针链表中新保存的各个节点和之前剩余的节点一同按升序重新排序保存；并且将各个确定的相邻节点添加到四叉树的根节点的对应分支上。完成上述处理后，跳转至提取模块102，再次从双向指针链表的表头取出节点，循环上述步骤。

获取模块106，用于在确定取出的节点有预设标记后，在四叉树中由取出的节点回溯到根节点，以得到起点到终点的路径。

当确定取出的节点有预设标记，即取出的节点为已经打上标记了的终点，说明已经搜索完所有可能到达终点的节点；并且由于当前取出的节点为从双向指针链表的表头取出的，是双向指针链表中F值最小的节点，因此确定根据该取出的节点的搜索路径找到的起点到终点的路径为最优路径，如此则结束上述搜索的循环步骤，依据四叉树中节点的方向参数和节点的指针信息，在四叉树中从当前取出的节点(即终点)回溯到根节点(即起点)，以得到起点到终点的路径，该路径则为AGV从起点到终点的最优路径，AGV依照该最优路径移动运输，则从起到到终点所需的时间最短。

本实施例的AGV路径规划系统，基于栅格图法建立环境地图模型(即栅格地图模型)，在栅格地图模型中新建四叉树和双向指针链表，起点作为四叉树的根节点，通过四叉树保存所有搜索路径，通过双向指针链表按节点的总代价F值升序保存搜索到的节点。循环从双向指针链表的表头取出F值最小的节点搜索下一步将到达的所有可能的下一节点，并将搜索到的将达到的节点按F值保存到双向指针链表中，如此循环搜索以搜索到下一步将到达节点为终点，即搜索到从起点到终点的F值最小的路径终点，根据该路径终点在四叉树中回溯，得到从起点到终点的最优路径。使AGV按照规划的最优路径从起点移动运输到终点，运输时间最短，运输最快，如此，大幅提升了智能停车场的车辆存取效率。

本实施例中，起点采用一个，终点为一个或多个。当终点为一个时，在智能停车场中对应的场景，例如：1、需要AGV从某个车位取车，则AGV的出发位置为起点，该车位对应的取车位置为终点；2、AGV从取车位置回到车辆的出车交接口。终点为多个时，在智能停车场中对应的场景，例如：需要将停车交接区的车辆，运输到智能停车场的某个区域的若干车位中的其中一个车位上停放，则该区域内的若干车位均为终点。无论终点为一个还是多个，通过AGV路径规划方法均可找到起点到终点的最优路径，终点为多个时，找到的最优路径的终点也是各个终点中最优的终点。

如图4所示，图4是本发明AGV路径规划系统二实施例中判断模块103的功能模块图。

在本实施例的AGV路径规划系统中，判断模块103包括：

确定子模块1031，用于在确定取出的节点没有预设标记后，确定取出的节点可到达的除其父节点以外的其它相邻节点；

跳转子模块1032，用于在确定的相邻节点数为零时，跳转至提取模块102；

如果确定的相邻节点数为零，说明取出的节点除朝其父节点方向外，其它方向都无法到达相邻节点，即其它方向均为障碍物，该取出的节点进入死胡同，无法到达终点，此时则跳转至提取模块102，以重新从双向指针链表的表头取出节点进行搜索。

判断子模块1033，用于在确定的相邻节点数大于为零后，判断各个确定的相邻节点中是否存在终点。

如果确定的相邻节点数大于零，说明取出的节点除朝其父节点方向外，至少一个其它方向可到达相邻节点，此时则进一步判断各个确定的相邻节点中是否存在终点。

进一步地，提取模块102还用在无法从双向指针链表中的表头取到节点时，反馈无法找到起点到终点的路径。

若是无法从双向指针链表的表头取到节点，说明双向指针链表已经空了，双向指针链表中的所有节点已经全部取出搜索完，找不到起点到终点的路径，那么此时则反馈无法找到起点到终点的路径，或反馈对应的标志信号以表示无法找到起点到终点的路径。

进一步地，针对终点为一个的情况，本实施例中，判断模块103在确定取出的节点可到达的除其父节点以外的其它相邻节点的处理包括：通过A*算法启发函数确定取出的节点的移动方向，根据确定的移动方向获得可到达的相邻节点，其中，确定的移动方向中滤除了取出的节点朝其父节点回退的方向。

先采用A*算法启发函数确定取出的节点的移动方向，确定的移动方向中滤除了取出的节点朝其父节点回退的方向，确定的移动方向通常为一个或两个；再根据确定的移动方向去获得可到达的相邻节点，如此，可以省掉一个或两个方向的搜索，减少搜索节点，提升搜索速度，更快的找到最优路径。

还有一种特殊情况：在通过A*算法启发函数确定取出的节点的移动方向时，若该取出的节点可到达的除朝其父节点回退方向的各个方向中存在F1值变大的情况，则需要将F1值变大的方向的那个节点和确定的移动方向上的节点保存到双向指针链表中。

在上述实施例中，可能存在以下两种特殊情形：1、通过不同的搜索方向搜索到的节点为同一终点，2、通过当前取出的节点确定的可到达的相邻节点中多个为终点(终点为多个时)。在这两种情形下，双向指针链表中均可能同时保存有多个打上预设标记的节点(即终点)；情形1时，双向指针链表中同时保存的终点的F值可能相等，也可能不等；情形2时，双向指针链表中同时保存的终点的F值相等。而本实施例方案中只根据保存在双向指针链表表头的一个打上预设标记的节点进行回溯得到起点到终点路径，没有考虑其它F值相等的打上预设标记的节点所回溯得到的最优路径，不够全面。

本实施例基于上述任一实施例的方案，针对上述两种特殊情形，本实施例将F值的计算公式替换为F＝F0+F1+F2+F3，其中，F0为当前节点距离起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价，F3为起点到当前节点的累计变向次数的代价。

由于AGV的变向动作也会一定程度的影响AGV的运行时间，因此，本实施例中将节点的F值的计算增加了F3参数。变向次数的解释如下：假设当前节点为第二节点，第二节点是由第一节点向下移动到达的节点，第三节点是由第二节点移动到达的相邻节点，那么，如果第三节点是第二节点向左或向右移动到达的，则第三节点的变向次数加1(即由第二节点的变向次数加1)，如果第三节点是第二节点向下移动到达的，则第二节点的变向次数不变(为第二节点的变向次数)，累计变向次数的代价F3的值＝累计变向次数×变向单位代价(可预先设定，例如为1)。通过引入F3参数，降低了在双向指针链表中可能出现同时保存有多个F值相等的终点的情况，确保了根据保存在双向指针链表表头的一个打上预设标记的节点回溯得到的起点到终点路径为最优路径。

另外，在其它实施例中，基于AGV路径规划系统一实施例的方案的基础上，还可包括如下方案：在双向指针链表中同时保存有多个打上预设标记的节点，且在确定取出的节点有预设标记时，比较各个打上预设标记的节点的F值，选取F值最小的打上了预设标记的节点，在四叉树中由该选取的节点回溯到根节点，以得到起点到终点的路径。若F值最小的打上了预设标记的节点有多个时，则再比较变向次数，取变向次数最小的节点，在四叉树中由该选取的节点回溯到根节点，以得到起点到终点的路径。

另外，本发明还提出一种计算机可读存储介质，该计算机可读存储介质存储有AGV路径规划系统，该AGV路径规划系统可被一个或多个处理器执行，以实现上述任一实施例的AGV路径规划方法。

以上所述的仅为本发明的部分或优选实施例，无论是文字还是附图都不能因此限制本发明保护的范围，凡是在与本发明一个整体的构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明保护的范围内。

Claims (10)

1.一种AGV路径规划方法，其特征在于，所述AGV路径规划方法包括：

S1、在接收到起点和终点后，在预先建立的栅格地图模型中建立四叉树，将所述起点作为所述四叉树的根节点，计算出所述起点的总代价F值，并将所述起点保存到新建的双向指针链表中，所述双向指针链表中的节点按F值升序保存，其中，所述F值的计算公式为F＝F0+F1+F2，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价；

S2、从所述双向指针链表的表头取出节点，确定所述取出的节点是否有预设标记；

S3、在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点，判断各个确定的相邻节点中是否存在所述终点；

S4、若是，则从各个确定的相邻节点中找出是所述终点的节点，将找出的节点打上预设标记、计算F值后，再保存到所述双向指针链表中以及添加到所述四叉树的根节点的对应分支上，跳转至步骤S2；

S5、若否，则计算出各个确定的相邻节点的F值，将各个确定的相邻节点保存到所述双向指针链表中，以及将各个确定的相邻节点添加到所述四叉树的根节点的对应分支上，跳转至步骤S2；

S6、在确定所述取出的节点有预设标记后，在所述四叉树中由所述取出的节点回溯到所述根节点，以得到所述起点到所述终点的路径。

2.根据权利要求1所述的AGV路径规划方法，其特征在于，所述F值的计算公式替换为F＝F0+F1+F2+F3，其中，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价，F3为所述起点到当前节点的累计变向次数的代价。

3.根据权利要求1所述的AGV路径规划方法，其特征在于，所述步骤S3包括：

在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点；

若确定的相邻节点数为零，则跳转至步骤S2；

若确定的相邻节点数大于为零，则判断各个确定的相邻节点中是否存在所述终点。

4.根据权利要求3所述的AGV路径规划方法，其特征在于，在所述步骤S2中，若无法从所述双向指针链表中的表头取到节点，则反馈无法找到所述起点到所述终点的路径。

5.根据权利要求1至4中任意一项所述的AGV路径规划方法，其特征在于，所述确定所述取出的节点可到达的除其父节点以外的其它相邻节点的步骤包括：

通过A*算法启发函数确定所述取出的节点的移动方向，根据确定的移动方向获得可到达的相邻节点，其中，所述确定的移动方向中滤除了所述取出的节点朝其父节点回退的方向。

6.根据权利要求1所述的AGV路径规划方法，其特征在于，所述终点为一个或多个。

7.一种电子装置，其特征在于，所述电子装置包括存储器、处理器，所述存储器上存储有可在所述处理器上运行的AGV路径规划系统，所述AGV路径规划系统被所述处理器执行时实现如下步骤：

a、在接收到起点和终点后，在预先建立的栅格地图模型中建立四叉树，将所述起点作为所述四叉树的根节点，计算出所述起点的总代价F值，并将所述起点保存到新建的双向指针链表中，所述双向指针链表中的节点按F值升序保存，其中，所述F值的计算公式为F＝F0+F1+F2，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价；

b、从所述双向指针链表的表头取出节点，确定所述取出的节点是否有预设标记；

c、在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点，判断各个确定的相邻节点中是否存在所述终点；

d、若是，则从各个确定的相邻节点中找出是所述终点的节点，将找出的节点打上预设标记、计算F值后，再保存到所述双向指针链表中以及添加到所述四叉树的根节点的对应分支上，跳转至步骤b；

e、若否，则计算出各个确定的相邻节点的F值，将各个确定的相邻节点保存到所述双向指针链表中，以及将各个确定的相邻节点添加到所述四叉树的根节点的对应分支上，跳转至步骤b；

f、在确定所述取出的节点有预设标记后，在所述四叉树中由所述取出的节点回溯到所述根节点，以得到所述起点到所述终点的路径。

8.根据权利要求7所述的电子装置，其特征在于，所述F值的计算公式替换为F＝F0+F1+F2+F3，其中，F0为当前节点距离所述起点的距离代价，F1为当前节点的静态代价，F2为当前节点的动态代价，F3为所述起点到当前节点的累计变向次数的代价。

9.根据权利要求7所述的电子装置，其特征在于，所述步骤c包括：

在确定所述取出的节点没有预设标记后，确定所述取出的节点可到达的除其父节点以外的其它相邻节点；

若确定的相邻节点数为零，则跳转至步骤b；

若确定的相邻节点数大于为零，则判断各个确定的相邻节点中是否存在所述终点。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有AGV路径规划系统，所述AGV路径规划系统可被一个或多个处理器执行，以实现如权利要求1至6中任一项所述的AGV路径规划方法的步骤。

Images