CN105652838B - 一种基于时间窗的多机器人路径规划方法 - Google Patents

Abstract

本发明涉及一种基于时间窗的多机器人路径规划方法，该方法通过已有机器人占用节点的点时间窗和边时间窗，规划新机器人的可行路径，并且在机器人时间窗被打乱时，重新动态规划各个机器人的路径，从而可以避免系统内多机器人的冲突。本发明的多机器人路径规划方法不需要多次迭代，只是在有限的路径集上计算可能性，相对计算量比较小，更适合真实的调度场景应用。

Description

一种基于时间窗的多机器人路径规划方法

【技术领域】

本发明涉及机器人设计领域，尤其涉及一种基于时间窗的多机器人路径规划方法。

【背景技术】

目前，随着新技术的快速发展，机器人的应用也越来越广泛，并且同一个系统中的机器人数量也在不断增加，多机器人调度问题日趋重要。由于多个机器人在同一个系统内执行任务，它们有可能在行走路径上产生冲突。冲突有三种基本类型，即路口冲突、赶超冲突和相向冲突。其中路口冲突是指两台机器人同时到达路口，赶超冲突是指两台机器人同向行驶，后面机器人速度快，会在行驶过程中产生追尾事件，相向冲突是指两台机器人相向行驶的碰撞。目前解决冲突的策略主要有等待策略、更改路径、速度调节等。其中更改路径策略就涉及对机器人的路径规划。

路径规划指在具有一定障碍物的环境中(此环境可以通过机器人与环境的交互感知),给定一个起始点和目标点,参照一定的准则,如最短路径因素、最少花费时间、最少拐弯次数,最少花费费用等等,在所处环境中搜索出一条最优的无碰撞路径。

而多机器人系统路径规划是建立在单个机器人基础之上,在保证单个机器人完成从起始点到终点的路径规划,同时避免机器人和周围环境障碍物以及其它机器人发生冲突碰撞,完成多机器人之间的协调,避免碰撞和死锁等情况发生,使得多台机器人能共同顺利的完成指定任务。

目前，对于多机器人系统路径规划已经提出了一些算法，典型的算法通过将单台机器人的算法和时间窗原理相结合来规划路径。例如将Dijkstra算法和时间窗原理相结合，顺序规划各个机器人的路径，在已规划机器人路径的基础上，运用基于Dijkstra的算法继续规划下一台机器人，实现机器人的无碰撞路径规划。

但是，现有算法很多是基于精确的时间窗，或应用于特定的场景来进行，通用性较差；对冲突的判断比较繁琐，并且冲突的解决方案多需要等待，停车次数多，用户体验较差。

【发明内容】

基于现有技术的上述问题，本发明提出了一种基于时间窗的多机器人路径规划方法。该方法按照机器人优先级顺序，依次进行路径规划，对于待规划机器人，搜索所有可行路径作为搜索路径集，从中找到一条能在已有空闲时间窗内执行完的可行路径作为其路径，最后进行机器人死锁判断。

其中，死锁判断通过对每一个非正常执行任务状态的机器人，根据前方的阻塞/停止机器人的位置和方向，增加一条有向边，如果最后图中出现环，则为死锁。

其中，对于新加入的系统的机器人，执行预先规划方法，计算各条可行路径的执行时间，将执行时间最短的可行路径作为新机器人的路径。

本发明采用的技术方案为：

一种基于时间窗的多机器人路径规划方法，包括如下步骤：

1)在多机器人的系统地图中设定节点和短路径，并保存；

2)初始化，清除节点和路径的时间窗，清除所有机器人的已规划标记、阻塞标记和循环标记；

3)获取各个机器人的速度、位置和状态信息，所述状态包括待规划状态和无需规划状态；使用预定路径搜索算法在预设条件下为各个机器人搜索若干路径，搜索到的路径作为相应机器人的可选路径；

4)清除所有机器人的时间窗、已规划标记和循环标记；

5)根据所有具有无需规划状态的机器人和具有阻塞标记的机器人的位置和状态更新时间窗；

6)选择一个无阻塞标记且无已规划标记的待规划状态的机器人，计算该机器人的每一条可选路径的空闲时间窗，包括该可选路径上各个节点、短路径的空闲时间窗；

7)判断步骤6所选择的机器人是否具有可选路径能在对应的空闲时间窗内完成任务。如果有，则选择其中耗时最短的可选路径作为该机器人的行进路径，并对该机器人打上已规划标记，同时更新所有的点时间窗和边时间窗，如果没有，则放宽预设条件，使用所述预定路径搜索算法为该机器人搜索其他路径，如果没能搜索到新的可选路径，则对该机器人打上阻塞标记和循环标记，如果搜索到新的可选路径，则将新路径加入该机器人的可选路径并返回步骤6；

8)判断所有无阻塞标记的待规划状态的机器人中，是否还有未被打上已规划标记的机器人，如果有则返回步骤6，如果没有则继续执行以下步骤；

9)判断是否还有打上循环标记的机器人，如果有则返回步骤4，如果没有则继续执行以下步骤；

10)检测目前系统中是否存在机器人死锁，如果有，则报警。

优选的，每隔预设时间重复执行步骤2至步骤10。

优选的，所述预设时间为100ms。

优选的，所述待规划状态包括任务待分配状态、任务运行状态和任务阻塞状态；所述无需规划状态包括机器人故障状态和任务结束状态。

优选的，其中步骤5更新时间窗的方法为对无需规划状态机器人和具有阻塞标记机器人所在的短路径或节点进行封锁，以表明该短路径或节点被占用。

优选的，其中步骤6中按照机器人优先级从高到低选择。

优选的，其中步骤3和7中所述的预定路径搜索算法为Di jkstra算法、A*算法或者BFS算法。

优选的，其中步骤3中所述的预设条件为预先设定的路径搜索的上限时间或者搜索到的路径的上限数量，若路径搜索的时间达到上限时间或者搜索到的路径数量达到上限数量，则结束本次搜索。

优选的，其中步骤7中所述的放宽预设条件是指增加路径搜索的上限时间，或者增加搜索到的路径的上限数量。

优选的，所述步骤10中检测机器人死锁的具体步骤如下：

10.1)统计所有具有阻塞标记的机器人的关键路径和/或关键点；所述关键路径是指机器人所有可选路径中都经过的路径，所述关键点是指机器人所有可选路径中都经过的节点；

10.2)对于每一个具有关键路径和/或关键点的机器人，判断在其关键路径和/或关键点上是否有其他具有阻塞标记的机器人，如果有，则建立一有向边，该有向边从该具有关键路径和/或关键点的机器人指向该其他具有阻塞标记的机器人；

10.3)判断所有有向边是否有形成环，如果有，则为死锁；如果没有，则没有死锁。

优选的，对于死锁中无解的机器人，记录其每条可行路径上的冲突短路径和冲突节点，并记录冲突依赖关系，对被依赖最多的机器人给出预警。

本发明的有益效果为：

1)可以预先估算整条路的时间窗，如果机器人可以按照预定的时间窗完成任务，则中途不会出现死锁情况；

2)和遗传算法，蚁群算法，粒子群算法这些需要自学习/随机量的调度算法等比较，时间窗算法不需要多次迭代，只是在有限的路径集上计算可能性，相对计算量比较小，而且不会陷入局部最小，更适合真实的调度场景应用。

3)可更合理并且更容易的检测冲突。

4)对存在冲突的机器人进行死锁检测，并给出预警。。

【附图说明】

此处所说明的附图是用来提供对本发明的进一步理解，构成本申请的一部分，但并不构成对本发明的不当限定，在附图中：

图1是本发明的动态规划方法的流程图。

【具体实施方式】

下面将结合附图以及具体实施例来详细说明本发明，其中的示意性实施例以及说明仅用来解释本发明，但并不作为对本发明的限定。

首先说明本发明使用的一些术语：

时间窗：时间窗指的是机器人占用某个资源的时间段，即机器人从该时间段的起始时间到终止时间占用该资源。当不同机器人占用资源的时间窗没有重叠时，则表明没有发生碰撞和冲突。如果有潜在时间窗重叠，则调整原有时间窗，使时间窗相互分离，就可以避免冲突。

点时间窗：机器人占用路径上某个节点的时间窗。

边时间窗：机器人占用某条短路径(即相邻节点之间的路径)的时间窗和方向。所述方向是指机器人在该短路径上的行走方向，用于在同一段短路径上多台机器人的冲突判断。

空闲时间窗：即某个资源空闲的时间段。

机器人优先级：为了避免路径改变的频次，本发明对同一系统内的多个机器人定义优先级，以保证不会因为新任务的增加导致的其他已运行任务的找不到合理路径的情况。即正在执行任务的机器人优先级比拟执行任务的机器人的优先级高。同一批任务中，按照执行任务的顺序从小到大给机器人编号，从而机器人编号决定了其优先级别，机器人编号较小者优先级高。

死锁：几个机器人互相占用资源，导致均无法移动的情况。

预先规划方法：

在正常情况下，同一系统内的执行各自任务的机器人是按照顺序逐个加入到系统里的，当一个执行新任务的机器人加入时，需要先根据已有的机器人状态，对新任务进行路径规划，以避免新机器人与已有机器人的冲突，本发明将这一路径规划称为预先规划方法，新机器人根据预先规划方法的结果行进。预先规划方法的具体步骤如下：

1)初始化：将系统地图用节点和短路径来表示并保存，同时保存系统中所有已有机器人的点时间窗和边时间窗。

2)对于新任务，按照任务的起始时间、起始节点、目标节点，搜索获得空间上所有可行的路径集合作为备选集合。

3)从备选集合中选择一条可行路径，根据所有已有机器人的点时间窗和边时间窗，计算该可行路径的各个空闲时间窗。

4)选择第一个空闲时间窗作为当前空闲时间窗，判断新任务能否在当前空闲时间窗内执行完毕，如果能执行完，则合并该可行路径上每段短路径的执行时间，得到该可行路径的执行时间；如果不能执行完，则选择下一个空闲时间窗作为当前空闲时间窗，重复上述判断步骤，直到找到一个可以执行完任务的空闲时间窗。

5)对于备选集合中的所有可行路径，循环执行步骤3-4，得到每条可行路径的执行时间。将执行时间最短的可行路径作为新机器人的行进路径。

动态规划方法：

预先规划方法正确执行的前提是每个机器人均按照规定的时间到达和离开相应节点，但实际应用中可能会有各种因素导致各个机器人的时间窗被打乱。基于此，本发明进一步提出了一种动态规划方法，每隔一段时间根据各个机器人的实时位置，进行动态规划，实时在线进行路径协调。动态规划方法的具体步骤如下：

1)在多机器人的系统地图中设定节点和短路径，并保存。

所述系统地图是指机器人在室内或室外运动的实际地图，机器人在此地图上移动以完成相关任务。对机器人而言，所述地图可以人为的加入节点和短路径以限定机器人的运动线路，即本地图中的机器人的运动路径只能是由这些设定的节点和路径组成。其中节点是指机器人运动路径中的某个点位置，短路径是指由两个节点为端点形成的直线路径。

机器人在执行任务时，通过合理地选择地图中的节点和短路径可以寻找到若干条路径使机器人从起点运动到终点以完成任务。

路径搜索算法就是通过这种算法能够得到若干可行的路径以使机器人能够完成其任务，并按照一定的排列规则将这些路径进行排序。

2)初始化，清除节点和路径的时间窗，清除所有机器人的已规划标记、阻塞标记和循环标记。

上述初始化过程将地图中所有机器人的相关标记进行清除，所有路径的时间窗进行清除，以进行相关路径规划。所述路径包括短路径，也包括由若干条短路径前后依次相连而成的运动路径。所述时间窗是指机器人占用某个资源的时间段，即机器人从该时间段的起始时间到终止时间占用该资源。所述已规划标记用于表示机器人已经完成了路径规划，所述阻塞标记用于表示机器人处于阻塞状态；所述循环标记用于后续算法的循环控制。

3)获取各个机器人的速度、位置和状态信息，所述状态包括待规划状态和无需规划状态；使用预定路径搜索算法在预设条件下为各个机器人搜索若干路径，搜索到的路径作为相应机器人的可选路径。

上述预定的算法可以是本领域已知的路径搜索算法，例如Dijkstra算法、A*算法或者BFS算法。所述待规划状态包括任务待分配状态、任务运行状态和任务阻塞状态；所述无需规划状态包括机器人故障状态和任务结束状态。通过当前各个机器人的速度、位置和状态信息，已知的路径搜索算法都可以为该机器人搜索出多条可行的路径，但是为了提高算法运行的效率，控制路径规划时间，本发明在上述步骤3中为该路径搜索算法设置了预设条件，该预设条件可是预先设定的路径搜索的上限时间或者搜索到的路径的上限数量，换言之，一旦该路径搜索算法的路径搜索的时间达到上限时间或者搜索到的路径数量达到上限数量，则立即结束该路径搜索算法，继续执行以下步骤。

4)清除所有机器人的时间窗、已规划标记和循环标记；

步骤4是本算法循环步骤中的第一步，只清除时间窗、已规划标记和循环标记，但是保留了后面给机器人打上的阻塞标记。

5)根据所有具有无需规划状态的机器人和具有阻塞标记的机器人的位置和状态更新时间窗；

此处的更新时间窗指的是对上述具有无需规划状态的机器人和具有阻塞标记的机器人所在的短路径或节点进行封锁，以表明该短路径或节点已被占用。由于具有阻塞标记的机器人实际上已经处于阻塞状态，其所在的短路径和节点已经无法被其他机器人使用，因此当发现阻塞机器人后，需要在排除其占用短路径和节点的情况下，重新进行计算。

6)选择一个无阻塞标记且无已规划标记的待规划状态的机器人，计算该机器人的每一条可选路径的空闲时间窗，包括该可选路径上各个节点、短路径的空闲时间窗。

上述选择可以是随机选择，也可以按照机器人优先级从高到低选择。上述可选路径就是步骤4中搜索得到的可选路径。但是，步骤4中得到的可选路径只是对单个机器人而言可选的路径，并没有考虑其他机器人，因此步骤6中需要考察可选路径上的空闲时间窗，从而判断该机器人是否能在不和其他机器人冲突的情况下，真正走完该可选路径。

7)判断步骤6所选择的机器人是否具有可选路径能在对应的空闲时间窗内完成任务。如果有，则选择其中耗时最短的可选路径作为该机器人的行进路径，并对该机器人打上已规划标记，同时更新所有的点时间窗和边时间窗，如果没有，则放宽预设条件，使用所述预定路径搜索算法为该机器人搜索其他路径，如果没能搜索到新的可选路径，则对该机器人打上阻塞标记和循环标记，如果搜索到新的可选路径，则将新路径加入该机器人的可选路径并返回步骤6。

步骤7通过判断可选路径是否能在对应空闲时间窗内完成，从而可以获得机器人不与其他机器人冲突的真正可选路径，但是，这些可选路径中有可能并不存在无冲突的可选路径，在这种情况下，首先是为该机器人重新搜索路径，搜索路径的算法与步骤3所使用的算法相同。步骤3中的算法是在预定条件限制下的完成的算法，为了搜索其他路径，这里放宽了该预定条件，以检查是否还有其他可能的路径，从而能够在后续步骤中从新路径中找到无冲突的路径。所述的放宽预设条件是指增加路径搜索的上限时间，或者增加搜索到的路径的上限数量。但是，如果在完全放宽条件后，仍然不能找到新路径，实际上意味着该机器人在该算法下的所有可选路径都被其他机器人堵塞了，冲突已经不可避免，该机器人就是一个处于阻塞状态的机器人。

步骤7中的循环标记用于后续步骤判断是否结束本算法的循环；

8)判断所有无阻塞标记的待规划状态的机器人中，是否还有未被打上已规划标记的机器人，如果有则返回步骤6，如果没有则继续执行以下步骤。

步骤8对阻塞标记和待规划状态进行判断，从而找出还需要规划的机器人，通过到步骤6-8的循环，让所有待规划机器人都完成路径规划。

9)判断是否还有打上循环标记的机器人，如果有则返回步骤4，如果没有则继续执行以下步骤。

步骤9通过判断循环标记，再次进行步骤4-9的循环，由于循环标记和阻塞标记是在步骤7中一起打上的，这意味着如果步骤7发现了新的阻塞机器人，就再重新循环计算一遍，在每次循环时，都是在排除了所有已发现的阻塞机器人所占用的短路径或节点的情况下重新计算的，直到不再发现新的阻塞机器人，就终止循环。

10)检测目前系统中是否存在机器人死锁，如果有，则报警。

动态规划方法的最后一步需要判断系统中是否存在死锁。实际上，在系统运行过程中，如果所有机器人都按照规划正常执行任务，就不可能出现死锁，但是实际应用中，机器人可能由于各种原因进入非正常执行任务状态，例如在路上被阻挡，或者故障，都有可能会造成死锁情况。所以必须及时的给出预警，以免机器人大面积瘫痪罢工，影响正常工作。

根据本发明的一个实施例，可以每隔预设时间重复执行上述步骤2-10，优选的，该预设时间是100ms。

死锁判断方法：

针对动态规划方法的最后一步，本发明还提出一种检测系统中是否存在机器人死锁的方法，可以用于动态规划方法中，也可以在系统运行时随时判断。该死锁判断的方法的具体步骤如下：

10.1)统计所有具有阻塞标记的机器人的关键路径和/或关键点；所述关键路径是指机器人所有可选路径中都经过的路径，所述关键点是指机器人所有可选路径中都经过的节点。

如果机器人具有阻塞标记，说明该机器人的所有可选路径上都具有冲突的机器人，如果所有可选路径中有共同的路径或节点，那么这些无法回避的路径或节点就是导致冲突乃至死锁的关键之处。

10.2)对于每一个具有关键路径和/或关键点的机器人，判断在其关键路径和/或关键点上是否有其他具有阻塞标记的机器人，如果有，则建立一有向边，该有向边从该具有关键路径和/或关键点的机器人指向该其他具有阻塞标记的机器人。

如果在机器人的关键路径或关键点上还具有阻塞机器人，说明该机器人的阻塞状态是否能够解除就依赖于该阻塞机器人的阻塞状态十分解除，因此通过上述建立的有向边来表示这一依赖关系。

10.3)判断所有有向边是否有形成环，如果有，则为死锁；如果没有，则没有死锁。

通过有向边表示的阻塞状态依赖关系如果形成了一个环，说明依赖关系链最终指向了机器人自身，这种情况下显然是无解的，这就构成了机器人死锁。

如果发生死锁，就应发出报警。另外，对于死锁中无解的机器人，记录其每条可行路径上的冲突短路径和冲突节点，并记录冲突依赖关系。对被依赖最多的机器人，也应给出预警。

以上所述仅是本发明的较佳实施方式，故凡依本发明专利申请范围所述的构造、特征及原理所做的等效变化或修饰，均包括于本发明专利申请范围内。

Claims (11)

1.一种基于时间窗的多机器人路径规划方法，其特征在于，包括如下步骤：

1)在多机器人的系统地图中设定节点和短路径，并保存；其中，短路径是指由两个节点为端点形成的直线路径；

2)初始化，清除节点和路径的时间窗，清除所有机器人的已规划标记、阻塞标记和循环标记；

3)获取各个机器人的速度、位置和状态信息，所述状态包括待规划状态和无需规划状态；使用预定路径搜索算法在预设条件下为各个机器人搜索若干路径，搜索到的路径作为相应机器人的可选路径；

4)清除所有机器人的时间窗、已规划标记和循环标记；

5)根据所有具有无需规划状态的机器人和具有阻塞标记的机器人的位置和状态更新时间窗；其中，如果机器人具有阻塞标记，说明该机器人的所有可选路径上都具有冲突的机器人；

6)选择一个无阻塞标记且无已规划标记的待规划状态的机器人，计算该机器人的每一条可选路径的空闲时间窗，包括该可选路径上各个节点、短路径的空闲时间窗；

7)判断步骤6所选择的机器人是否具有可选路径能在对应的空闲时间窗内完成任务；如果有，则选择其中耗时最短的可选路径作为该机器人的行进路径，并对该机器人打上已规划标记，同时更新所有的点时间窗和边时间窗，如果没有，则放宽预设条件，使用所述预定路径搜索算法为该机器人搜索其他路径，如果没能搜索到新的可选路径，则对该机器人打上阻塞标记和循环标记，如果搜索到新的可选路径，则将新路径加入该机器人的可选路径并返回步骤6；

其中，点时间窗是指机器人占用路径上某个节点的时间窗，边时间窗是指机器人占用相邻节点之间的路径的时间窗和方向；

8)判断所有无阻塞标记的待规划状态的机器人中，是否还有未被打上已规划标记的机器人，如果有则返回步骤6，如果没有则继续执行以下步骤；

9)判断是否还有打上循环标记的机器人，如果有则返回步骤4，如果没有则继续执行以下步骤；

10)检测目前系统中是否存在机器人死锁，如果有，则报警。

2.如权利要求1所述的基于时间窗的多机器人路径规划方法，其特征在于，每隔预设时间重复执行步骤2至步骤10。

3.如权利要求2所述的基于时间窗的多机器人路径规划方法，其特征在于，所述预设时间为100ms。

4.如权利要求1-3任意一项所述的基于时间窗的多机器人路径规划方法，其特征在于，所述待规划状态包括任务待分配状态、任务运行状态和任务阻塞状态；所述无需规划状态包括机器人故障状态和任务结束状态。

5.如权利要求1-3任意一项所述的基于时间窗的多机器人路径规划方法，其特征在于，步骤5更新时间窗的方法为对无需规划状态机器人和具有阻塞标记机器人所在的短路径或节点进行封锁，以表明该短路径或节点被占用。

6.如权利要求1所述的基于时间窗的多机器人路径规划方法，其特征在于，步骤6中按照机器人优先级从高到低选择。

7.如权利要求1所述的基于时间窗的多机器人路径规划方法，其特征在于，步骤3和7中所述的预定路径搜索算法为Dijkstra算法、A*算法或者BFS算法。

8.如权利要求1所述的基于时间窗的多机器人路径规划方法，其特征在于，步骤3中所述的预设条件为预先设定的路径搜索的上限时间或者搜索到的路径的上限数量，若路径搜索的时间达到上限时间或者搜索到的路径数量达到上限数量，则结束本次搜索。

9.如权利要求1所述的基于时间窗的多机器人路径规划方法，其特征在于，步骤7中所述的放宽预设条件是指增加路径搜索的上限时间，或者增加搜索到的路径的上限数量。

10.如权利要求1所述的基于时间窗的多机器人路径规划方法，其特征在于，所述步骤10)中检测机器人死锁的具体步骤如下：

10.1)统计所有具有阻塞标记的机器人的关键路径和/或关键点；所述关键路径是指机器人所有可选路径中都经过的路径，所述关键点是指机器人所有可选路径中都经过的节点；

10.2)对于每一个具有关键路径和/或关键点的机器人，判断在其关键路径和/或关键点上是否有其他具有阻塞标记的机器人，如果有，则建立一有向边，该有向边从该具有关键路径和/或关键点的机器人指向该其他具有阻塞标记的机器人；

10.3)判断所有有向边是否有形成环，如果有，则为死锁；如果没有，则没有死锁。

11.如权利要求10所述的基于时间窗的多机器人路径规划方法，其特征在于，对于死锁中无解的机器人，记录其每条可行路径上的冲突短路径和冲突节点，并记录冲突依赖关系，对被依赖最多的机器人给出预警。