一种多机器人路径规划方法

1.本发明涉及机器人全局路径规划领域，特别涉及一种多机器人路径规划方法。


背景技术：

2.机器人具有节约成本、方便监管和安全性高等优点，在从事体力劳动程度大，工作重复性高或者危险性高等工作方面表现出了卓越的性能，对于促进社会生产力的发展具有重要意义。相比单机器人执行任务，多机器人协同执行任务具有协作配合能力强，工作效率高，可以完成更加复杂繁琐的工作等优点。例如，在快递物流领域，多机器人协同配合可以快速处理不同类型的快递物品，提高物流分拣运输速度；在国防军事领域，多机器人协同配合执行军事任务，既可以同时对单目标进行多角度全方位军事打击，也可以同时对多目标发起攻击，提高了军事打击的攻击力和时效性。
3.机器人路径规划技术作为机器人核心技术，对于机器人技术的发展具有重要意义。特别对于移动机器人而言，路径规划是移动机器人完成工作任务的首要前提。移动机器人可以通过路径规划在有障碍物的工作环境中，找到一条从起点到终点适当的运动路径，从而在运动过程中能安全、无碰撞地绕过所有障碍物。
4.相比单机器人路径规划，多机器人路径规划更加注重多机器人的整体利益，通过多机器人协同配合，规划得到具有最优整体利益的多机器人路径。现有的多机器人路径规划方法较少，计算复杂度较大，协同配合能力较差，规划得到的路径平滑度较低，实用性不高。


技术实现要素：

5.本发明的目的是针对目前技术的弊端，提供一种多机器人路径规划方法，降低多机器人路径规划的时间和空间复杂度，提高多机器人路径的平滑度，增强多机器人路径规划的协同配合能力，提高多机器人路径规划方法的可行性和实用性。
6.为了实现上述目的，一种多机器人路径规划方法，包括：s100:获取所述多机器人的环境地图、起点和终点坐标；s200:将所述多机器人的起点和终点坐标输入路径规划策略得到多机器人中每个单机器人的起点和终点坐标；s300:将所述的环境地图进行环境建模得到可视图；s400:将所述单机器人的起点及终点坐标和所述的可视图输入单机器人路径规划算法得到所述单机器人的路径；s500:将所述多机器人中所有的单机器人循环步骤s300-s400，循环结束后多机器人路径规划完成。
7.进一步地，所述多机器人的环境地图包括所述多机器人路径规划区域的全局地图和所述多机器人路径规划区域中的所有障碍物。
8.进一步地，所述多机器人的起点是指所述多机器人中所有单机器人的起点坐标的
集合。
9.进一步地，所述多机器人的终点坐标是指所述多机器人完成路径规划任务需要所述多机器人中的一个单机器人到达的终点坐标的集合。
10.进一步地，所述步骤s200中的路径规划策略是采用深度强化学习算法中的sac算法预先训练得到的多机器人路径规划策略。
11.进一步地，所述多机器人中每个单机器人的起点和终点坐标是多机器人中每个单机器人需要规划路径的起点和终点坐标。
12.进一步地，所述步骤s300中环境建模采取的方法是可视图法。
13.进一步地，所述多机器人路径规划区域中的所有障碍物在所述多机器人环境地图的可视图中均采用凸多边形表示。
14.进一步地，所述步骤s300中的可视图由若凸干个多边形列表组成的。
15.进一步地，所述步骤s400中单机器人路径规划算法是一种基于a*算法和漏斗算法的单机器人路径规划算法，具体步骤如下：s410:初始化；s411:设置一个存储单元的数据结构（p,b,k,s,c），其中p表示机器人路径上当前节点v
now
的前驱节点；b是一个布尔值，表示起点路径是否修改；k表示过往路径的斜率或方向；s表示当前凸多边形的起始点；c表示正在搜索的凸多边形；s412:设置机器人路径的起点和终点分别设置为v
start
和v
end
，起点所在的凸多边形表示为c
start
，终点的凸多边形为c
end
，初始路径的斜率为k0，即点v
start
与点v
end
之间连线的斜率；s413:初始化一个节点d
start
（-1,true,k0,v
end
,c
start
）；s420:使用二叉搜索树创建空列表open list和closed list，并将节点d
start
添加到open list中，其中open list表示待访问节点列表，closed list表示已访问节点列表；s430:若open list为空，且路径未能到达终点，则寻路失败未能找到最优路径；s440:将open list按照估价函数f值进行升序排序，取出并且移除open list中f值最小的节点，第一个节点为dn（pn,bn,kn,sn,cn），并且将节点dn插入closed list中；s450:若cn为终点凸多边形，则找到最优路径，最优路径的凸多边形序列可以通过cn的p反向推演得到，直至p=-1。若cn不为终点凸多边形，则搜索所有相邻的拓展凸多边形c
expend
,得到节点d
expend
（p
expend
,b
expend
,k
expend
,s
expend
,c
expend
）；s460:若节点d
expend
在closed list中，则搜索下一个相邻的拓展凸多边形c
expend
。若节点d
expend
不在closed list中，但在open list中，则计算经由当前节点cn到达c
expend
的路径是否比原路径长度小，若小则转至步骤s470，否则搜索下一个相邻的拓展凸多边形c
expend
；s470:将节点d
expend
加入open list中，且计算其估价函数f值，转至步骤s430；s480:使用漏斗算法对获得的凸多边形列表进行平滑化处理，规划得到一条平滑的单机器人最优避障路径，单机器人路径规划完成。
16.进一步地，步骤s440和s470所述估价函数f值的计算方法如下：f*(n)=g*(n)+h*(n)，其中g*(n)表示从起点到当前凸多边形的最小代价值，h*(n)表示当前凸多边形到终点的最小估价值。
17.进一步地，所述表示从起点到当前凸多边形的最小估价值的g*(n)的具体计算步骤包括：步骤一：使用路径斜率kn检测前驱节点pn是否可以连通；步骤二：若存在通过路径斜率线连通的凸多边形，则将所述的凸多边形设为c
n+1
，并将斜率线与该凸多边形c
expend
的交点即后驱节点设为p
n+1
。若不存在，则为g*(n)设值为无穷大；步骤三：计算得到前驱节点pn到后驱节点p
n+1
的距离，并且加上前驱节点pn的g*(n)值，即为后驱节点p
n+1
的g*(n)值。
18.进一步地，所述表示从当前凸多边形到终点的最小估价值的h*(n)的具体计算步骤包括：步骤一：若凸多边形没有改变路径方向，h*(n)值为前驱节点pn到终点v
end
的直线距离；步骤二：若凸多边形改变了路径方向，h*(n)值为前驱节点pn到终点v
end
的直线距离乘以1.1。
19.进一步地，所述步骤s400得到的单机器人路径是所述单机器人从起点坐标到终点坐标的一条平滑可行且避障的路径。
20.进一步地，所述多机器人路径规划方法规划得到的路径包括所述多机器人中所有单机器人从起点坐标到终点坐标的路径。
21.进一步地，所述多机器人路径规划方法规划得到的路径是具有所述多机器人最优整体利益的多机器人路径。
22.与现有技术相比，本发明对待多机器人路径规划首先通过预先训练的sac算法策略输出多机器人中的每个单机器人路径的起点和终点，然后对多机器人所处区域的环境地图使用可视图法进行环境建模，最后使用a*算法和漏斗算法规划得到多机器人中的每个单机器人的路径，实现了多机器人协同配合规划路径，提高了多机器人路径的整体利益和平滑度，降低了路径规划的复杂度，具有很高的可行性和实用性。
附图说明
23.图1是本发明的一种多机器人路径规划方法的流程图。
24.图2是本发明使用的sac算法的模型结构图。
25.图3是本发明提供的一张环境建模后的可视图。
26.图4是本发明使用的一种单机器人路径规划算法流程图。
27.图5是本发明使用的a*算法的流程图。
28.图6是本发明是漏斗算法平滑化处理得到路径的示意图。
具体实施方式
29.为使本发明的目的、特征、技术方案和优点更加清楚易懂，下面结合附图对本发明的具体实施案例进行详细的说明。
30.实施例一。
31.本实施例提供一种多机器人路径规划方法，如图1所示，包括步骤：
s100:获取所述多机器人的环境地图、起点和终点坐标；s200:将所述多机器人的起点和终点坐标输入路径规划策略得到所述多机器人中每个单机器人的起点和终点坐标；s300:将所述的环境地图进行环境建模得到可视图；s400:将所述单机器人的起点及终点坐标和所述多机器人环境地图的可视图输入单机器人路径规划算法得到所述单机器人的路径；s500:将所述多机器人中所有的单机器人循环步骤s300-s400，循环结束后多机器人路径规划完成。
32.本实施例的多机器人路径规划方法是计算机通过多机器人仿真系统来实现的，执行主体是计算机，所述步骤s100至s500都是通过计算机中的软件和工具进行实现的，所述的软件和工具包括unity、visual studio和ml-agent等。
33.步骤100，计算机获取所述多机器人的环境地图和多机器人的起点坐标及终点坐标。
34.具体的，所述多机器人的环境地图包括所述多机器人路径规划区域的全局地图和所述多机器人路径规划区域中的所有障碍物。
35.具体的，所述多机器人的起点是指所述多机器人中所有单机器人的起点坐标。
36.具体的，所述多机器人的终点坐标是指所述多机器人完成路径规划任务需要所述多机器人中的一个单机器人到达的终点坐标的集合。
37.具体的，多机器人的起点坐标及终点坐标是计算机随机产生的。
38.步骤200，获取所述多机器人的起点坐标及终点坐标之后，将所述多机器人的起点坐标及终点坐标输入路径规划策略得到所述多机器人中每个单机器人的起点和终点坐标。
39.具体的，所述多机器人中每个单机器人的起点和终点坐标是多机器人中每个单机器人需要规划路径的起点坐标和终止坐标。
40.具体的，所述的路径规划策略是采用深度强化学习算法中的sac算法预先训练得到的多机器人路径规划策略。
41.需要说明的是，sac算法是一种稳定而有效的深度强化学习算法，同时sac算法也是一种面向最大熵强化学习的off-policy算法，使用的是随机策略，sac算法的主要特征是熵正则化，可以被训练去最大程度上权衡期望回报和熵，熵是策略随机性的一个衡量，增加熵也就说明策略的随机性增强，所以会增加更多的探索，从而可以加快后续的学习速度，也可以防止策略过早的收敛到局部最优值，sac算法的模型结构如图2所示。
42.步骤300，将所述多机器人的环境地图进行环境建模得到可视图。
43.具体的，所述的环境建模采取的方法是可视图法。
44.具体的，所述多机器人路径规划区域中的所有障碍物在所述多机器人环境地图的可视图中均采用凸多边形表示。
45.具体的，所述步骤s300中的可视图由若凸干个多边形列表组成的。
46.需要说明的是，可视图法是路径规划中常用的一种环境建模的方法，可视图的建立是很多路径搜索算法的前提。可视即可通行，是将所有障碍物的顶点、起始点及目标点用直线组合相连，要求三者（即障碍物的顶点、起始点及目标点）之间连线均不能穿越障碍物，即直线是“可视的”，故称该方法为可视图法。例如，建模后得到的可视图如图3所示。
47.步骤400，将所述单机器人的起点和终点坐标和所述多机器人环境地图的可视图输入单机器人路径规划算法得到所述单机器人的路径。
48.具体的，所述步骤s400中单机器人路径规划算法是一种基于a*算法和漏斗算法的单机器人路径规划算法，参考图4，所述单机器人路径规划算法步骤包括：s410:初始化；s411:设置一个存储单元的数据结构（p,b,k,s,c）。其中p表示机器人路径上当前节点v
now
的前驱节点;b是一个布尔值，表示起点路径是否修改;k表示过往路径的斜率或方向;s表示当前凸多边形的起始点;c表示正在搜索的凸多边形；s412:设置机器人路径的起点和终点分别设置为v
start
和v
end
，起点所在的凸多边形表示为c
start
，终点的凸多边形为c
end
，初始路径的斜率为k0，即点v
start
与点v
end
之间连线的斜率；s413:初始化一个节点d
start
（-1,true,k0,v
end
,c
start
）；s420:使用二叉搜索树创建空列表open list和closed list，并将节点d
start
添加到open list中。其中open list表示待访问节点列表，closed list表示已访问节点列表；s430:若open list为空，且路径未能到达终点，则寻路失败未能找到最优路径；s440:将open list按照估价函数f值进行升序排序，取出并且移除open list中f值最小的节点，第一个节点为dn（pn,bn,kn,sn,cn），并且将节点dn插入closed list中；s450:若cn为终点凸多边形，则找到最优路径，最优路径的凸多边形序列可以通过cn的p反向推演得到；若cn不为终点凸多边形，则搜索所有相邻的拓展凸多边形c
expend
,得到节点d
expend
（p
expend
,b
expend
,k
expend
,s
expend
,c
expend
）；s460:若节点d
expend
在closed list中，则搜索下一个相邻的拓展凸多边形c
expend
；若节点d
expend
不在closed list中，但在open list中，则计算经由当前节点cn到达c
expend
的路径是否比原路径长度小，若小则转至步骤s470，否则搜索下一个相邻的拓展凸多边形c
expend
；s470:将节点d
expend
加入open list中，且计算其估价函数f值，转至步骤s430；s480:使用漏斗算法对获得的凸多边形列表进行平滑化处理，规划得到一条平滑的单机器人最优避障路径，单机器人路径规划完成。
49.需要说明的是，所述的a*算法是一种启发式搜索算法，也是一种静态路网中求解最短路径最有效的直接搜索方法，a*算法估价函数的表达式是f
*
(n)= g
*
(n)+ h
*
(n)，其中f
*
(n)为在状态空间图中从初始节点出发，约束经由中间节点n到达目标节点的的最小代价值，g
*
(n)代表从初始节点到中间节点n的最小代价值，h
*
(n)代表从中间节点n到目标节点的最小估价值，算法中的距离估算值与实际值越接近，最终搜索速度越快，本实施例中a
*
算法的流程如图5所示。
50.需要说明的是，所述的漏斗算法是一种解决路径平滑问题的算法，主要应用于使用几何法进行环境建模后的地图中，对通过搜索算法得到的三角形/多边形路径队列进行平滑化处理，本实施例中漏斗算法平滑化处理得到的路径如图6所示。
51.具体的，所述表示从起点到当前凸多边形的最小估价值的g
*
(n)的具体计算步骤包括：步骤一：使用路径斜率kn检测前驱节点pn是否可以连通；
步骤二：若存在通过路径斜率线连通的凸多边形，则将所述的凸多边形设为c
n+1
，并将斜率线与该凸多边形c
expend
的交点即后驱节点设为p
n+1
。若不存在，则为g
*
(n)设值为无穷大；步骤三：计算得到前驱节点pn到后驱节点p
n+1
的距离，并且加上前驱节点pn的g
*
(n)值，即为后驱节点p
n+1
的g
*
(n)值。
52.具体的，所述表示从当前凸多边形到终点的最小估价值的h*(n)的具体计算步骤包括：步骤一：若凸多边形没有改变路径方向，h
*
(n)值为前驱节点pn到终点v
end
的直线距离；步骤二：若凸多边形改变了路径方向，h
*
(n)值为前驱节点pn到终点v
end
的直线距离乘以1.1。
53.具体的，所述步骤s400得到的单机器人路径是所述单机器人从起点坐标到终点坐标的一条平滑可行且避障的路径。
54.具体的，所述多机器人路径规划方法规划得到的路径包括所述多机器人中所有单机器人从起点坐标到终点坐标的路径。
55.具体的，所述多机器人路径规划方法规划得到的路径是具有所述多机器人最优整体利益的多机器人路径。
56.本实施例对待所述的多机器人路径规划首先通过预先训练的sac算法策略输出多机器人中的每个单机器人路径的起点和终点，然后对多机器人所处区域的环境地图使用可视图法进行环境建模，最后使用a
*
算法和漏斗算法规划得到多机器人中的每个单机器人的路径，实现了多机器人协同配合规划路径，提高了多机器人整体利益和路径的平滑度，降低了路径规划的复杂度，具有很高的可行性和实用性。
57.注意，以上所述的实施例展现的仅是本发明较佳实施例和具体实施的技术方案而已，并非用于限制本发明的保护范围，凡是本领域技术人员在本发明的精神和原则之内所做的简易修改、等同替代和改进等，都应蕴涵在本发明的保护范围之内，因此，本发明的保护范围应以权利要求的保护范围为准。