路径规划方法及装置与流程

1.本技术涉及路径规划技术领域，特别是涉及一种路径规划方法及装置。


背景技术：

2.近年来，自动引导小车(automated guided vehicle，agv)在工业领域的飞速拓展，大力推动了工业4.0的进程。自动引导小车越来越频繁地亮相于各大工厂和车间，逐渐取代人工作业，成为智能工厂里最为关键的设备之一。但是目前应用到自动引导小车的路径规划方法不太智能，导致运行效率较低。


技术实现要素：

3.本技术提供一种路径规划方法及装置，可以防止在规划的路径中货架频繁调整，以尽量保证路径的平滑和流畅运行。
4.为达到上述目的，本技术提供一种路径规划方法，该方法包括：
5.搜索当前节点的子节点；
6.计算每一子节点的代价，其中，在自动引导小车处于负载状态时，每一子节点的代价包括自动引导小车从当前节点移动到每一子节点的过程中的货架调整代价；
7.基于子节点的代价，确定出当前节点的下一节点；
8.基于下一节点，完成路径规划。
9.其中，货架调整代价包括货架运动状态切换代价和/或货架旋转代价；
10.货架旋转代价与货架的旋转角度呈正相关。
11.其中，搜索当前节点的子节点，之前包括：
12.确定能否以原地旋转和/或直线移动的方式，从当前节点移动到目标点；
13.若能，则确认完成路径规划；
14.若否，执行搜索当前节点的子节点；
15.基于下一节点，完成路径规划，包括：
16.将下一节点作为当前节点，并返回确定能否以原地旋转和/或直线移动的方式，从当前节点移动到目标点的步骤，直至完成路径规划。
17.其中，目标点为自动引导小车回到预定轨道线的节点，
18.若能，则确认完成路径规划，包括：
19.若能，则拼接当前节点与目标点之前的搜索路径，并将搜索路径作为自动引导小车回到预定轨道线的路径。
20.其中，确定能否以原地旋转和/或直线移动的方式，从当前节点移动到目标点，之前包括：
21.在自动引导小车处于未负载状态，且自动引导小车的终点为货架中心点的情况下，将货架入口的中点作为目标点；
22.方法还包括：将目标点到货架中心点的直线路径和搜索路径进行拼接，得到自动
引导小车从自动引导小车当前所在位置到终点的路径。
23.其中，基于子节点的代价，确定出当前节点的下一节点，包括：
24.将当前节点的所有子节点中代价最小的子节点，作为下一节点。
25.其中，搜索当前节点的子节点，包括：
26.搜索当前节点的所有候选子节点；
27.筛除所有候选子节点中已经搜索过的节点和存在碰撞风险的节点，得到当前节点的子节点集合。
28.其中，该方法还包括：
29.若基于障碍物数据和自动引导小车的车体状态，确定自动引导小车及其上负载的货架在从当前节点移动到候选子节点的过程中能够碰撞到障碍物，则候选子节点为存在碰撞风险的节点。
30.为达到上述目的，本技术还提供一种电子设备，该电子设备包括处理器；处理器用于执行指令以实现上述方法。
31.为达到上述目的，本技术还提供一种计算机可读存储介质，其用于存储指令/程序数据，指令/程序数据能够被执行以实现上述方法。
32.本技术在路径规划时，确认当前节点后，可以搜索当前节点的子节点，以便基于当前节点的所有子节点的代价确定当前节点的下一节点，继而基于下一节点完成路径规划；并且在自动引导小车处于负载状态时，每一子节点的代价包括自动引导小车从当前节点移动到每一子节点的过程中的货架调整代价，如此在路径规划时考虑到货架的调整代价，可以防止在规划的路径中货架频繁调整，以尽量保证路径的平滑和流畅运行。
附图说明
33.此处所说明的附图用来提供对本技术的进一步理解，构成本技术的一部分，本技术的示意性实施例及其说明用于解释本技术，并不构成对本技术的不当限定。在附图中：
34.图1是本技术路径规划方法一实施方式的流程示意图；
35.图2是本技术路径规划方法另一实施方式的流程示意图；
36.图3是本技术路径规划方法一实施方式的流程框架图；
37.图4是本技术电子设备一实施方式的结构示意图；
38.图5是本技术计算机可读存储介质一实施方式的结构示意图。
具体实施方式
39.下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本技术的一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。另外，除非另外指明(例如，“或另外”或“或在替代方案中”)，否则如本文所使用的术语“或”指代非排他性的“或”(即，“和/或”)。并且，本文所描述的各种实施例不一定相互排斥，因为一些实施例可以与一个或多个其它实施例组合以形成新的实施例。
40.具体如图1所示，本实施方式的路径规划方法包括以下步骤。且本技术的路径规划
方法可以应用于举升式agv或差速式agv在物流仓储工厂等环境中运行的场景中，当然不限于此。需要注意的是，以下步骤编号仅用于简化说明，并不旨在限制步骤的执行顺序，本实施方式的各步骤可以在不违背本技术技术思想的基础上，任意更换执行顺序。
41.s101：搜索当前节点的子节点。
42.在路径规划时，确认当前节点后，可以搜索当前节点的子节点，以便基于当前节点的所有子节点确定当前节点的下一节点，继而基于下一节点完成路径规划。
43.其中，初始可以将起始节点(即自动引导小车的当前位置)作为当前节点。然后利用本实施方式的路径规划方法基于当前节点确定出下一节点后，然后将下一节点作为当前节点；接着在更新后的当前节点不符合预设条件的情况下，可以继续利用本实施方式的路径规划方法确定出当前节点的下一节点，直至最新的当前节点满足预设条件；其中，若能够直接从当前节点抵达目标点，则当前节点满足预设条件，如此通过上述方式可以规划出从起始点到目标点的路径，从而可以完成路径规划。
44.另外，在步骤s101中的当前节点为起始节点时，在步骤s101之前，也可以先确认当前节点是否符合预设条件；在当前节点不符合预设条件的情况下，执行步骤s101，以便基于本技术实施方式的方法规划出从起始节点到目标点的路径；在当前节点符合预设条件的情况下，直接将起始节点和目标点的连接作为从起始节点到目标点的路径。
45.在一实现方式中，可以将当前节点的相邻节点作为当前节点的子节点。
46.在另一实现方式中，可以将当前节点的相邻节点作为当前节点的所有候选子节点；然后筛除所有候选子节点中已经搜索过的节点和存在碰撞风险的节点，将剩余子节点作为当前节点的所有子节点。
47.在自动引导小车为举升式自动引导小车时，确定当前节点的相邻节点的方式可以为：基于x1＝x0 step*cosθ0、y1＝y0 step*sinθ0和θ1＝θ0 δθ这些公式，确定出当前节点的相邻节点；
48.其中，(x0,y0,θ0)分别表示当前节点在世界坐标系下的横坐标、纵坐标和角度朝向，(x1,y1,θ1)分别表示相邻节点在世界坐标系下的横坐标、纵坐标和角度朝向，step表示搜索的步长大小(例如可以设置为0.05m)，δθ(其中，δθ＝θ
1-θ0)表示相邻节点的旋转角度值。示例性地，当前节点的前进方向上的相邻节点数目可设置为3，在此情况下，δθ可分别为(δθ
t
,0,-δθ
t
)，δθ
t
可取0.08等弧度值。只要将上述公式中的step取相反数，即可生成当前节点的后退方向上的相邻节点。
49.另外，若存在一候选子节点位于已确定的从起始节点(可以为自动引导小车的当前位置)到当前节点的路径中，则该候选子节点为已经搜索过的节点。
50.而若自动引导小车从当前节点移动到候选子节点的过程中会碰撞到固定障碍物或其他自动引导小车等移动障碍物，则该候选子节点为存在碰撞风险的节点。
51.进一步地，考虑到在自动引导小车处于负载状态时，自动引导小车上放置的货架也可能存在着碰撞障碍物的风险，从而在确认当前节点的子节点时，可以基于障碍物数据和自动引导小车的车体状态，确定自动引导小车及其上负载的货架在从当前节点移动到候选子节点的过程中是否会碰撞到障碍物；若自动引导小车及其上负载的货架会碰撞到障碍物，则候选子节点为存在碰撞风险的节点，如此在路径规划时考虑到货架的运动，可以使得规划的路径能让货架不碰撞到障碍物，以保证自动引导小车上货物的安全。
52.其中，自动引导小车的车体状态可以包括自动引导小车(agv)的姿态(在世界坐标系的绝对坐标和朝向角)和车体轮廓，如果为负载状态的话，还可包括负载的货架姿态(在世界坐标系的绝对坐标和朝向角)和负载货架的轮廓。
53.另外，上述的障碍物数据可以包括基于避障传感器或激光传感器采集的障碍物的点云数据。
54.具体地，在步骤s101之前，可以获取当前一帧障碍物的点云数据，并将点云数据转换为世界坐标系下的坐标点云，并存储下来，以便后续利用障碍物的点云数据确认候选子节点是否存在碰撞风险，此外还可便于后续利用障碍物的点云数据确认当前节点是否符合预设条件。
55.另外，上述的障碍物数据还可包括其他自动引导小车的位置信息(可包括姿态信息和/或轮廓信息)，如此可避免自动引导小车碰撞到附近运行的自动引导小车。
56.s102：计算每一子节点的代价。
57.在搜索出当前节点的子节点后，可以计算当前节点的所有子节点的代价，以便后续基于当前节点的子节点的代价确定出当前节点的下一节点。
58.其中，在自动引导小车处于负载状态时，每一子节点的代价可包括自动引导小车从当前节点移动到每一子节点的过程中的货架调整代价，如此在路径规划时考虑到货架的调整代价，可以防止在规划的路径中货架频繁调整，以尽量保证路径的平滑和流畅运行。
59.其中，货架调整代价可以包括货架运动状态切换代价和/或货架旋转代价。
60.在从当前节点移动到每一子节点的过程中，每次货架旋转均可产生货架旋转代价。
61.而每次货架旋转代价可与货架的每次旋转角度呈正相关。具体地，可通过ks*δθh公式计算出每次货架旋转代价。其中，ks为代价系数，ks可根据实际情况进行设置，在此不做限制，例如ks可为5或10等值。δθh为货架的旋转角度。具体地，货架旋转可包括两种状态：保持货架朝向在世界坐标系下不变(shelf_keep)或者跟随agv摆动(shelf_follow)；每次生成子节点，如果子节点是shelf_keep，货架旋转的角度δθh为-δθ(即与车体旋转方向相反)；如果子节点是shelf_follow，则货架不做旋转，跟随车体摆动。
62.另外，在从当前节点移动到每一子节点的过程中，若货架出现从跟随agv摆动变换为保持货架朝向在世界坐标系下不变等切换运动状态的情况，则会产生货架运动状态切换代价。其中，货架运动状态切换代价可为预设值，而预设值可根据实际情况进行设定，在此不做限制，例如可为5。
63.此外，每一子节点的代价还可包括路径距离代价和/或启发式函数。其中，子节点的路径距离代价可以为从当前节点或起始节点到子节点的代价。启发式函数可以为从子节点到目标点的最短路径代价的启发式函数。
64.具体地，每一子节点的代价可以等于货架运动状态切换代价、货架旋转代价、路径距离代价以及启发式函数的和。
65.s103：基于子节点的代价，确定出当前节点的下一节点。
66.确定当前节点的每一子节点的代价后，可以基于子节点的代价，确定出当前节点的下一节点。
67.可选地，可以将所有子节点中代价最低的子节点作为当前节点的下一节点，以尽
量降低规划出来的路径的代价，而使得自动引导小车顺畅且快速地运行到目标点。
68.在其他实施方式中，可以确定当前节点的所有子节点中是否存在符合预设条件的子节点；若存在，将当前节点的所有符合预设条件的子节点中代价最小的子节点作为当前节点的下一节点。
69.s104：基于下一节点，完成路径规划。
70.基于上述步骤确定当前节点的下一节点后，可以基于所确定的下一节点完成路径规划。
71.可选地，可以将当前节点更新为下一节点，然后确定更新后的当前节点是否符合预设条件，若符合，则返回执行步骤s101，直至当前节点符合预设条件；其中，若能够直接从当前节点抵达目标点，则当前节点满足预设条件，如此通过上述方式可以规划出从起始点到目标点的路径，从而可以完成路径规划。
72.具体地，可以确认能否以原地旋转和/或直线移动的方式，从当前节点移动到目标点；若能，则确认当前节点符合预设条件，否则当前节点不符合预设条件，如此通过原地旋转和直线移动的方式避免在有限的空间下由于遇到障碍物而规划失败，可以提高路径规划的成功率和效率，并且该方法可以应用到具有容易原地旋转的特性的差速agv中，适用性强。
73.其中，目标点可以是自动引导小车的终点位置，即可通过全程搜索规划的方式确定出从自动引导小车的当前位置到终点位置的路径。
74.当然，目标点也可以是自动引导小车回到预定轨道线的节点，如此通过本实施方式路径规划方法确定出符合预设条件的当前节点后，可以拼接当前节点和目标点之前的搜索路径，即将当前节点之前的搜索路径和当前节点和目标点之间的路径拼接起来，接着将搜索路径作为自动引导小车回到预定轨道线的路径。其中，预定轨道线可以是由后续任务的连通节点及连通性构成的拓扑路线图。预定轨道线信息中还可包含节点信息，例如货架点和普通路径点信息。而相邻两个节点又是由直线或者三阶贝塞尔曲线构成他们的连通性。如此，将目标点设置为自动引导小车回到预定轨道线上的节点，可以在自动引导小车遇到障碍物时或被人为操作到非轨道线上时，利用本实施方式的路径规划方法自动规划出从自动引导小车当前位置回到预定轨道线上的路径，使得自动引导小车可以绕过障碍物，并达到轨道线上的目标状态；且采用了拓扑路线和局部规划结合的方式，在效率上有较大提升，也降低了局部规划带来的性能消耗。
75.另外，在由节点和节点间的连通性构成的拓扑路线图的基础上，还可对每段路线赋予左右的宽度值，即引出了车道的概念。根据设置的宽度值，以及节点坐标信息，即可生成一张灰度通道地图，该地图使用pgm格式存储。如此在确定当前节点是否符合预设条件，可利用每条路线的宽度值确定当前节点能否直接无碰撞到移动到目标点。另外，在搜索当前节点的子节点时，也可以利用每条路线的宽度值确定候选子节点是否存在碰撞风险。
76.另外，考虑到终点位置和自动引导小车的负载状态，可以将自动引导小车的移动细化为多个场景；
77.(1)未负载状态的进货架场景
78.由于货架大小一般略大于agv大小，因此在进货架下时需要保证路线的笔直。在规划时需要根据货架的大小和姿态，计算出货架入口的中间目标点，并将货架入口的中间目
标点作为目标点。如此，可将该场景的规划分为两段，第一段为自动引导小车的当前位置到目标点的路径，第二段为目标点到货架中心路径。第一段使用本实施方式的路径规划方式进行搜索规划，第二段将目标点和货架中心点直线连接生成路径；如此搜索路径和目标点到货架中心点的直线路径拼接而成的路径，即为自动引导小车从自动引导小车当前所在位置到终点位置的路径。
79.(1.2)未负载状态的出货架场景
80.考虑到当前姿态不一定在货架中心，需要根据激光传感器的数据避开货架腿，因此直接采用本实施方式的路径规划方式进行搜索规划，目标点即为自动引导小车的终点位置。
81.(1.3)负载状态进出货架场景
82.考虑到货架区的货架密集摆放，规划的路线需要保证进出负载货架时能直进直出，而不碰撞到边上的货架。因此在该场景下，在出货架的第一段路径或者进去的最后一段路径，均需要直线路径；其他段路径采用本实施方式的路径规划方式进行搜索规划，目标点也根据实际情况进行设置。
83.(1.4)普通运行区场景：采用搜索规划，目标点即为自动引导小车的终点位置。
84.在本实施方式中，在路径规划时，确认当前节点后，可以搜索当前节点的子节点，以便基于当前节点的所有子节点的代价确定当前节点的下一节点，继而基于下一节点完成路径规划；并且在自动引导小车处于负载状态时，每一子节点的代价包括自动引导小车从当前节点移动到每一子节点的过程中的货架调整代价，如此在路径规划时考虑到货架的调整代价，可以防止在规划的路径中货架频繁调整，以尽量保证路径的平滑和流畅运行。
85.另外，具体如图2所示，另一实施方式的路径规划方法包括以下步骤。需要注意的是，以下步骤编号仅用于简化说明，并不旨在限制步骤的执行顺序，本实施方式的各步骤可以在不违背本技术技术思想的基础上，任意更换执行顺序。
86.s201：将起始节点放入优先级列表和未搜索节点列表。
87.在步骤s201之前，可以先定义优先级队列。其中，优先级队列中的节点是按照节点的代价进行排序的，且优先级队列的首值为代价最小的节点。还可以定义未搜索节点列表和已搜索节点列表。
88.s202：若未搜索节点列表不为空，将优先级列表的首值设置为当前节点，并放入到搜索节点列表中，且在优先级列表中删除。
89.将当前节点放入到搜索节点列表中的目的是：为了表示当前节点已经被搜索过，可以避免后续再次将当前节点作为搜索到的子节点，可以减少代价计算的节点数，提高路径规划效率。
90.另外，若未搜索节点列表为空，则代表搜索失败，未成功规划出路径，则本实施方式的路径规划方法执行完毕。未成功规划出路径的情况下，可以返回失败报警，以便让管理员尽快处理该故障。
91.s203：确认能否以原地旋转和/或直线移动的方式，从当前节点移动到目标点。
92.若确认能以原地旋转和/或直线移动的方式，从当前节点移动到目标点，即确认当前节点符合预设条件，进入步骤s205，以拼接形成从起始节点到目标点的路径；若确认不能以原地旋转和/或直线移动的方式，从当前节点移动到目标点，即确认当前节点不符合预设
条件，进入步骤s204，以搜索当前节点的子节点，进而找到当前节点的下一节点，直至完成路径规划。
93.s204：搜索当前节点的子节点，将子节点放入到优先级列表和未搜索节点列表。
94.可以先生成当前节点的候选子节点，然后对每个候选子节点进行下一步检测，以确定出当前节点的子节点，然后将子节点放入到优先级列表和未搜索节点列表，接着返回执行步骤s202。
95.具体地，若候选子节点已经在已搜索节点列表中，则说明该候选子节点已被搜索过，则筛除该候选子节点；若候选子节点存在碰撞风险，即自动引导小车从当前节点到候选子节点的过程中，自动引导小车的轮廓(货架和车体)会碰撞到障碍物，则候选子节点存在碰撞风险，则筛除该候选子节点；将剩余的候选子节点作为当前节点的子节点。
96.s205：拼接当前节点与目标点之前的搜索路径，并将搜索路径作为自动引导小车从起始节点到目标点的路径。
97.在当前节点符合预设条件的情况下，则拼接当前节点与目标点之前的搜索路径，并将搜索路径作为自动引导小车从起始节点到目标点的路径，即成功利用本实施方式的路径规划方法规划出路径，如此可输出搜索到的路径节点序列。
98.进一步地，本技术的路径规划方法的流程可如图3所示。首先通过数据处理与管理层对先验通道地图(如拓扑路线图和/或灰度通道地图等)、避障传感器数据、其他设备轮廓信息、参考路径信息和当前车身状态等输入信息进行校验和处理，然后由规划决策层利用图1或图2所示的混合a*搜索路径规划方法规划并输出从起始节点到终点的路径。
99.请参阅图4，图4是本技术电子设备20一实施方式的结构示意图。本技术电子设备20包括处理器22，处理器22用于执行指令以实现本技术上述任一实施方式的方法及任意不冲突的组合所提供的方法。
100.处理器22还可以称为cpu(central processing unit，中央处理单元)。处理器22可能是一种集成电路芯片，具有信号的处理能力。处理器22还可以是通用处理器、数字信号处理器(dsp)、专用集成电路(asic)、现场可编程门阵列(fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器，或者该处理器22也可以是任何常规的处理器等。
101.电子设备20还可进一步包括存储器21，用于存储处理器22运行所需的指令和数据。
102.请参阅图5，图5为本技术实施方式中计算机可读存储介质的结构示意图。本技术实施例的计算机可读存储介质30存储有指令/程序数据31，该指令/程序数据31被执行时实现本技术上述方法任一实施例以及任意不冲突的组合所提供的方法。其中，该指令/程序数据31可以形成程序文件以软件产品的形式存储在上述存储介质30中，以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本技术各个实施方式方法的全部或部分步骤。而前述的存储介质30包括：u盘、移动硬盘、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质，或者是计算机、服务器、手机、平板等设备。
103.在本技术所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，
仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
104.另外，在本技术各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
105.还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。
106.以上仅为本技术的实施方式，并非因此限制本技术的专利范围，凡是利用本技术说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本技术的专利保护范围内。