一种路径规划及管制方法与流程

1.本发明涉及移动机器人领域，特别涉及一种路径规划及管制方法。

背景技术：

2.在多辆移动机器人行驶过程中，为确保小车不会相撞，首先会对车辆行驶的路径进行规划，现阶段经常使用的方法为交通管制方法，交通管制基本原则是小车如果要驶入下一路段，那么下一路段的所有关联站点和所有关联路段不能被其他小车占用。现阶段主要采用a*路径规划方法，但在很多实际应用过程中，仍会出现路径死锁、车辆碰撞等现象。

技术实现要素：

3.本发明目的是：提供一种路径规划及管制方法，在路径规划算法中添加时间窗原理，防止机器人在行驶过程中与其他机器人发生路径冲突。
4.本发明的技术方案是：
5.一种路径规划及管制方法，包括步骤：
6.s1、初始化：出以初始点为起点的所有路段，并且验算每条路段的空闲时间窗，将时间窗t
star
＝0,的路段放入open列表，其中path.lenth 为路段长度，v表示机器人行走的速度；
7.s2、从open列表中到代价最小的节点n
last
，代价包括到达时间、是否有空闲时间窗；将节点n
last
从open列表中移除，加入到close列表中，并占用节点n
last
的空闲时间窗；
8.s3、到节点n
last
的所有不发生冲突的子节点列表，从列表中选择一个子节点n
next
，若子节点n
next
的路段终点为目标点，跳到步骤s6；否则，继续下一步；若子节点列表为空，跳到步骤s2；
9.s4、判断子节点n
next
是否已在open列表中，若不在，继续下一步；若在，再比较子节点n
next
的代价是否比原有的要小，若是，移除原有的节点，继续下一步，若不是，返回步骤s3；
10.s5、将子节点n
next
添加至open列表中，返回步骤s2；
11.s6、路径规划成功，并为终点添加一个安全时间窗。
12.优选的，如果机器人的终点为一个窄巷道，对终点进行反向预约路径：
13.首先获取小车生成路径的终点所生成的节点，获取该节点对应的反向预约路径，预约到巷道出口点；
14.再以小车终点所在的时间窗结束时间为起始时间，依次为预约路径添加时间窗。
15.优选的，子节点列表寻方法包括：
16.(1)通过地图，到所有以当前机器人站点为起点的所有子路段列表，从中选择一条路段p_next执行以下操作；
17.(2)判断p_next路段的终点是否与路径的起始点和目标点的区域类型相同，若不同，返回第(1)步，若相同，继续下一步；
18.(3)从p_next路段的空闲时间窗中选择一个时间窗tw进行以下操作， tw＝[a,b]，若已选择完毕，跳到第(1)步；
[0019]
(4)判断空闲时间窗tw的开始时间a是否小于机器人到达路段p_next 起始节点的时间c，如果小于，继续第(5)步；如果不小于，跳转第(6)步；
[0020]
(5)判断时间窗tw_1是否被时间窗tw包含：如果包含，即机器人通过p_next不被其他机器人干扰，进行第(9)步；如果不包含，即机器人通过 p_next会被其他机器人干扰，则返回第(3)步；
[0021]
时间窗tw_1＝[c,d]，tw_1的起始时间c为机器人到达路段p_next起始节点的时间，时间长度d-c为机器人通过p_next路段的时间，其中机器人通过 p_next路段的时间＝p_next路段的长度/机器人行驶的速度；
[0022]
(6)判断时间窗tw_2是否能被tw包含，若是，即机器人在等待一段时间后，通过路段p_next，不被其他机器人干扰；若不是，即机器人在等待一段时间后，仍然无法通过路段p_next，会被其他机器人干扰，返回第(3)步；
[0023]
时间窗tw_2＝[a,a+d-c],时间窗tw_2的起始时间为空闲时间窗tw的起始时间a，时间长度为机器人通过p_next路段的时间，即d-c，其中机器人通过p_next路段的时间＝p_next路段的长度/机器人行驶的速度；
[0024]
(7)判断时间窗tw_3是否被机器人停靠的站点的空闲时间窗n_tw包含，若是，即机器人在站点等待时，不会与其他机器人发生干扰，进行下一步；若不是，即机器人在站点等待时，会与其他机器人发生干扰，返回第(3)步；
[0025]
时间窗tw_3＝[c,a]，即机器人在站点等待的时间窗，时间窗tw_3的起始时间为机器人到达路段p_next起始节点的时间，时间窗tw_3的结束时间为空闲时间窗tw的起始时间；
[0026]
(8)以(p_next,tw_2)生成节点node_next,将node_next添加至节点列表；
[0027]
(9)以(p_next,tw_1)生成节点node_next,将node_next添加至节点列表；
[0028]
(10)返回节点列表。
[0029]
步骤(6)中，机器人到达p_next路段起始节点时，p_next路段还有其他机器人通过，等待一段时间，其他机器人行驶过后，该机器人再行走。
[0030]
本发明的优点是：
[0031]
本发明的路径规划及管制方法，在路径规划算法中添加时间窗原理，防止机器人在行驶过程中与其他机器人发生路径冲突，如果机器人的终点为一个窄巷道，还对终点进行反向预约路径，获取小车生成路径的终点所生成的节点，获取该节点对应的反向预约路径，预约到巷道出口点，以小车终点所在的时间窗结束时间为起始时间，依次为预约路径添加时间窗。
具体实施方式
[0032]
发明提出的路径规划及管制方法，其整体路径规划方法包括步骤：
[0033]
s1、初始化：出以初始点为起点的所有路段，并且验算每条路段的空闲时间窗，将时间窗t
star
＝0,的路段放入open列表，其中path.lenth 为路段长度，v表
示机器人行走的速度；
[0034]
s2、从open列表中到代价最小的节点n
last
，代价包括到达时间、是否有空闲时间窗；将节点n
last
从open列表中移除，加入到close列表中，并占用节点n
last
的空闲时间窗；
[0035]
s3、到节点n
last
的所有不发生冲突的子节点列表，从列表中选择一个子节点n
next
，若子节点n
next
的路段终点为目标点，跳到步骤s6；否则，继续下一步；若子节点列表为空，跳到步骤s2；
[0036]
s4、判断子节点n
next
是否已在open列表中，若不在，继续下一步；若在，再比较子节点n
next
的代价是否比原有的要小，若是，移除原有的节点，继续下一步，若不是，返回步骤s3；
[0037]
s5、将子节点n
next
添加至open列表中，返回步骤s2；
[0038]
s6、路径规划成功，并为终点添加一个安全时间窗，以防机器人停靠时，与其他机器行驶路径相冲突。
[0039]
如果机器人的终点为一个窄巷道，对终点进行反向预约路径：
[0040]
首先获取小车生成路径的终点所生成的节点，获取该节点对应的反向预约路径，预约到巷道出口点；
[0041]
再以小车终点所在的时间窗结束时间为起始时间，依次为预约路径添加时间窗。
[0042]
本实施例的子节点列表寻方法，包括以下步骤：
[0043]
(1)通过地图，到所有以当前机器人站点为起点的所有子路段列表，从中选择一条路段p_next执行以下操作；
[0044]
(2)判断p_next路段的终点是否与路径的起始点和目标点的区域类型相同，若不同，返回第(1)步，若相同，继续下一步；
[0045]
(3)从p_next路段的空闲时间窗中选择一个时间窗tw进行以下操作， tw＝[a,b]，若已选择完毕，跳到第(1)步；
[0046]
(4)判断空闲时间窗tw的开始时间a是否小于机器人到达路段p_next 起始节点的时间c，如果小于，继续第(5)步；如果不小于，跳转第(6)步；
[0047]
(5)判断时间窗tw_1是否被时间窗tw包含：如果包含，即机器人通过p_next不被其他机器人干扰，进行第(9)步；如果不包含，即机器人通过 p_next会被其他机器人干扰，则返回第(3)步；
[0048]
时间窗tw_1＝[c,d]，tw_1的起始时间c为机器人到达路段p_next起始节点的时间，时间长度d-c为机器人通过p_next路段的时间，其中机器人通过 p_next路段的时间＝p_next路段的长度/机器人行驶的速度；
[0049]
(6)判断时间窗tw_2是否能被tw包含，若是，即机器人在等待一段时间后，通过路段p_next，不被其他机器人干扰；若不是，即机器人在等待一段时间后，仍然无法通过路段p_next，会被其他机器人干扰，返回第(3)步；
[0050]
时间窗tw_2＝[a,a+d-c],时间窗tw_2的起始时间为空闲时间窗tw的起始时间a，时间长度为机器人通过p_next路段的时间，即d-c，其中机器人通过p_next路段的时间＝p_next路段的长度/机器人行驶的速度；
[0051]
(7)判断时间窗tw_3是否被机器人停靠的站点的空闲时间窗n_tw包含，若是，即机器人在站点等待时，不会与其他机器人发生干扰，进行下一步；若不是，即机器人在站点等
待时，会与其他机器人发生干扰，返回第(3)步；
[0052]
时间窗tw_3＝[c,a]，即机器人在站点等待的时间窗，时间窗tw_3的起始时间为机器人到达路段p_next起始节点的时间，时间窗tw_3的结束时间为空闲时间窗tw的起始时间；
[0053]
机器人到达p_next路段起始节点时，p_next路段还有其他机器人通过，等待一段时间，其他机器人行驶过后，该机器人再行走。
[0054]
(8)以(p_next,tw_2)生成节点node_next,将node_next添加至节点列表；
[0055]
(9)以(p_next,tw_1)生成节点node_next,将node_next添加至节点列表；
[0056]
(10)返回节点列表。
[0057]
为方便理解，本实施例假设空闲时间窗tw＝[5,8]；
[0058]
如果机器人到达路段p_next起始节点的时间为6，即c＝6，则进行第(5) 步；
[0059]
如果机器人到达路段p_next起始节点的时间为4，即c＝4，则进行第(6) 步；
[0060]
由第(4)步，机器人到达路段p_next起始节点的时间为6，即c＝6；
[0061]
如果机器人通过p_next路段的时间为2，则tw_1＝[6,8]，tw_1被tw包含，即机器人通过p_next不被其他机器人干扰，跳转第(9)步；
[0062]
如果机器人通过p_next路段的时间为4，则tw_1＝[6,10]，tw_1不被tw 包含，即机器人通过p_next会被其他机器人干扰，返回第(3)步；
[0063]
由第(4)步，机器人到达路段p_next起始节点的时间为4，即c＝4；
[0064]
如果机器人通过p_next路段的时间为2，则tw_2＝[5,7]，tw_2被tw包含，即机器人可以在等待一段时间后，通过路段p_next，不被其他机器人干扰，跳转下一步；
[0065]
如果机器人通过p_next路段的时间为4，则tw_1＝[5,9]，tw_2不被tw 包含，即机器人在等待一段时间后，仍然无法通过路段p_next，会被其他机器人干扰，返回第(3)步。
[0066]
tw_3＝[4,5]为机器人等待时间。
[0067]
如果站点空闲时间窗n_tw＝[3,10],tw_3被n_tw包含，机器人在站点等待时，不会与其他机器人发生干扰，跳转下一步；
[0068]
如果站点空闲时间窗n_tw＝[3,4],tw_3不被n_tw包含，机器人在站点等待时，会与其他机器人发生干扰，返回第(3)步。
[0069]
上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明主要技术方案的精神实质所做的修饰，都应涵盖在本发明的保护范围之内。

技术特征：

1.一种路径规划及管制方法，其特征在于，包括步骤：s 1、初始化：出以初始点为起点的所有路段，并且验算每条路段的空闲时间窗，将时间窗的路段放入open列表，其中path.lenth为路段长度，v表示机器人行走的速度；s2、从open列表中到代价最小的节点n
last
，代价包括到达时间、是否有空闲时间窗；将节点n
last
从open列表中移除，加入到close列表中，并占用节点n
last
的空闲时间窗；s3、到节点n
last
的所有不发生冲突的子节点列表，从列表中选择一个子节点n
next
，若子节点n
next
的路段终点为目标点，跳到步骤s6；否则，继续下一步；若子节点列表为空，跳到步骤s2；s4、判断子节点n
next
是否已在open列表中，若不在，继续下一步；若在，再比较子节点n
next
的代价是否比原有的要小，若是，移除原有的节点，继续下一步，若不是，返回步骤s3；s5、将子节点n
next
添加至open列表中，返回步骤s2；s6、路径规划成功，并为终点添加一个安全时间窗。2.根据权利要求1所述的路径规划及管制方法，其特征在于，如果机器人的终点为一个窄巷道，对终点进行反向预约路径：首先获取小车生成路径的终点所生成的节点，获取该节点对应的反向预约路径，预约到巷道出口点；再以小车终点所在的时间窗结束时间为起始时间，依次为预约路径添加时间窗。3.根据权利要求2所述的路径规划及管制方法，其特征在于，步骤s3中，子节点列表寻方法包括：(1)通过地图，到所有以当前机器人站点为起点的所有子路段列表，从中选择一条路段p_next执行以下操作；(2)判断p_next路段的终点是否与路径的起始点和目标点的区域类型相同，若不同，返回第(1)步，若相同，继续下一步；(3)从p_next路段的空闲时间窗中选择一个时间窗tw进行以下操作，tw＝[a,b]，若已选择完毕，跳到第(1)步；(4)判断空闲时间窗tw的开始时间a是否小于机器人到达路段p_next起始节点的时间c，如果小于，继续第(5)步；如果不小于，跳转第(6)步；(5)判断时间窗tw_1是否被时间窗tw包含：如果包含，即机器人通过p_next不被其他机器人干扰，进行第(9)步；如果不包含，即机器人通过p_next会被其他机器人干扰，则返回第(3)步；时间窗tw_1＝[c,d]，tw_1的起始时间c为机器人到达路段p_next起始节点的时间，时间长度d-c为机器人通过p_next路段的时间，其中机器人通过p_next路段的时间＝p_next路段的长度/机器人行驶的速度；(6)判断时间窗tw_2是否能被tw包含，若是，即机器人在等待一段时间后，通过路段p_next，不被其他机器人干扰；若不是，即机器人在等待一段时间后，仍然无法通过路段p_next，会被其他机器人干扰，返回第(3)步；时间窗tw_2＝[a,a+d-c],时间窗tw_2的起始时间为空闲时间窗tw的起始时间a，时间
长度为机器人通过p_next路段的时间，即d-c，其中机器人通过p_next路段的时间＝p_next路段的长度/机器人行驶的速度；(7)判断时间窗tw_3是否被机器人停靠的站点的空闲时间窗n_tw包含，若是，即机器人在站点等待时，不会与其他机器人发生干扰，进行下一步；若不是，即机器人在站点等待时，会与其他机器人发生干扰，返回第(3)步；时间窗tw_3＝[c,a]，即机器人在站点等待的时间窗，时间窗tw_3的起始时间为机器人到达路段p_next起始节点的时间，时间窗tw_3的结束时间为空闲时间窗tw的起始时间；(8)以(p_next,tw_2)生成节点node_next,将node_next添加至节点列表；(9)以(p_next,tw_1)生成节点node_next,将node_next添加至节点列表；(10)返回节点列表。4.根据权利要求3所述的路径规划及管制方法，其特征在于，步骤(6)中，机器人到达p_next路段起始节点时，p_next路段还有其他机器人通过，等待一段时间，其他机器人行驶过后，该机器人再行走。

技术总结

本发明公开了一种路径规划及管制方法，首先出以初始点为起点的所有路段，并且验算每条路段的空闲时间窗，从OPEN列表中到代价最小的节点N

技术研发人员：

周航 葛翔 杨晓光

受保护的技术使用者：

苏州艾吉威机器人有限公司

技术研发日：

2022.06.29

技术公布日：

2022/11/1