一种考虑运动附加耗时的多机器人路径规划方法、系统

本发明涉及一种考虑运动附加耗时的多机器人路径规划方法、系统，属于移动机器人路径规划。

背景技术：

1、多机器人路径规划作为多机器人系统研究中的重要内容，其目的在于为多机器人系统中的每一个机器人规划从其起点至终点的路径，且任意两个机器人的路径之间不能存在冲突。

2、与单机器人路径规划问题相比，多机器人路径规划问题的状态空间规模不仅与路径长度成指数关系，还与机器人的数量成指数关系。因而，多机器人路径规划问题的复杂度被极大地增加。传统的多机器人路径规划方法建立在均匀离散的定速质点模型基础上。该模型假设机器人速度恒定且能够瞬时加速或转向，由此，在单元化的离散栅格场景中，不同机器人同步移动且在任意两个栅格之间的移动耗时相同。然而，在现实应用中，机器人的加速、减速和转向均无法瞬时完成，需要增加相应的附加时间。该附加时间将使机器人在不同栅格之间的移动耗时变得不一致，导致所有机器人的运动不同步，从而极大地增加系统运行风险。传统的解决方法是将这些附加耗时同样设定为一个单位时间，从而使得问题仍保持均匀离散状态下的同步属性。或者，为机器人的路径之间设定较大的安全距离，用以缓冲这些附加耗时。然而，这将导致规划方案的路径长度被迫增加，造成较大的时间浪费。

3、有鉴于此，特提出本发明。

技术实现思路

1、本发明提供了一种考虑运动附加耗时的多机器人路径规划方法、系统，该方法考虑机器人的起步、减速和转向耗时，并进一步在冲突约束的基础上引入干涉约束，以实现在机器人不同步状态下的多机器人路径规划。

2、本发明的技术方案是：

3、根据本发明的第一方面，提供了一种考虑运动附加耗时的多机器人路径规划方法，包括：步骤一：对多机器人进行优先级排序；步骤二：按优先级顺序依次选择一个机器人作为当前待规划机器人，在移动耗时考虑运动附加耗时的基础上进行路径规划；步骤三：当前待规划机器人的规划过程中，将已规划的高优先级机器人路径作为障碍物进行避让，以实现当前待规划机器人的扩展路径节点与已规划高优先级路径满足不存在冲突和干涉；步骤四：当前待规划机器人规划完成后，将其添加到已规划路径列表中作为后续待规划机器人规划过程中的障碍物，返回步骤二继续规划后续待规划机器人；步骤五：当所有机器人均规划结束时，输出整体路径。

4、机器人路径规划过程中，对路径节点进行搜索与扩展时，运动状态包括：起点与终点仅存在v＝0一种速度模式，中间的扩展栅格点均存在v＝0和v＝1两种速度模式；其中，速度模式参数v＝0时，机器人能执行转向操作；速度模式参数v＝1时，机器人只能沿当前朝向继续运动。

5、依据机器人在前后栅格的具体运动状态进行移动耗时赋值：当机器人从当前栅格出发时速度模式参数v＝1，到达下一个栅格时速度模式参数也为v＝1，则移动耗时为单位化标准耗时tu；当机器人从当前栅格出发时速度模式参数v＝0且无转向，到达下一栅格时速度模式参数v＝1，则移动耗时为tu+tac；当机器人从当前栅格出发时速度模式参数v＝1，到达下一栅格时速度模式参数v＝0，则移动耗时为tu+tdc；当机器人从当前栅格出发时速度模式参数v＝0且无转向，到达下一栅格时速度模式参数v＝0，则移动耗时为tu+tac+tdc；当机器人从当前栅格出发时速度模式参数v＝0且有转向，到达下一栅格时速度模式参数v＝1，则移动耗时为tu+tac+ttr；当机器人从当前栅格出发时速度模式参数v＝0且有转向，到达下一栅格时速度模式参数v＝0，则移动耗时为tu+tac+tdc+ttr；其中，tac表示起步附加耗时，tdc表示减速附加耗时，ttr表示转向附加耗时。

6、多机器人路径规划过程中，机器人之间的冲突分为点冲突、边冲突，干涉分为交叉干涉和共线干涉；其中，点冲突指两个机器人在相同的时间占用同一栅格；边冲突指在同一时间段内，两个机器人在两个相邻栅格点之间发生相向移动；交叉干涉指两个机器人在未发生点冲突的情况下，在垂直方向上发生了实体干涉；共线干涉指两个机器人在未发生点冲突的情况下，在相同运动方向上发生了实体干涉。

7、多机器人路径规划过程中，需要避免路径发生点冲突，存在点冲突表达式为：

8、

9、其中，为机器人ai的第m个路径位置，为机器人aj的第n个路径位置；为机器人ai到达位置的时刻，为机器人ai到达位置的时刻；为机器人ai到达位置的时刻，为机器人ai到达位置的时刻。

10、多机器人路径规划过程中，需要避免路径发生边冲突，存在边冲突表达式为：

11、

12、其中，为机器人ai的第m个路径位置，为机器人aj的第n个路径位置；为机器人ai到达位置的时刻，为机器人ai到达位置的时刻；为机器人ai到达位置的时刻，为机器人ai到达位置的时刻。

13、多机器人路径规划过程中，为避免交叉干涉和共线干涉，需对栅格的边长lg和机器人的边长la添加如下边界约束：

14、

15、当两者满足关系时，机器人之间将不存在干涉冲突；

16、其中，tu表示单位化标准耗时，tac表示起步附加耗时，tdc表示减速附加耗时。

17、根据本发明的第二方面，提供了一种考虑运动附加耗时的多机器人路径规划系统，包括：排序模块，用于执行步骤一：对多机器人进行优先级排序；第一规划模块，用于执行步骤二：按优先级顺序依次选择一个机器人作为当前待规划机器人，在移动耗时考虑运动附加耗时的基础上进行路径规划；第二规划模块，用于执行步骤三：当前待规划机器人的规划过程中，将已规划的高优先级机器人路径作为障碍物进行避让，以实现当前待规划机器人的扩展路径节点与已规划高优先级路径满足不存在冲突和干涉；循环模块，用于执行步骤四：当前待规划机器人规划完成后，将其添加到已规划路径列表中作为后续待规划机器人规划过程中的障碍物，返回步骤二继续规划后续待规划机器人；输出模块，用于执行步骤五：当所有机器人均规划结束时，输出整体路径。

18、根据本发明的第三方面，提供了一种终端，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器被配置为执行上述中任一项所述的方法。

19、根据本发明的第四方面，提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的程序，该程序被处理器执行时使处理器实现上述中任一项所述的方法。

20、本发明的有益效果是：本发明在多机器人路径规划过程中考虑了机器人由于加速、减速和转向等运动状态引起的运动耗时，并将其添加到机器人的移动耗时计算中。所得多机器人路径方案更符合实际运行状态，具有更高的安全性。通过定义具备非均匀离散性质的点冲突、边冲突约束，以及交叉与共线干涉的边界约束，使得运动状态不同步情况下的多机器人路径规划得以离散化，从而有效降低了问题求解的复杂度。由于允许机器人的不同步运动，并进行了非均匀离散化处理，使得机器人的在各路径节点的时间得以充分利用，从而降低了方案的路径长度，减少了时间浪费。本发明原理清晰、路径时间浪费少、求解效率高，能有效提高现有多机器人路径规划的安全性和可靠性。