一种基于冲突搜索的多无人机的协同路径规划方法与流程

1.本发明属于机器人技术领域，涉及一种基于冲突搜索的多无人机的协同路径规划方法。


背景技术：

2.多机器人系统在监视、搜索和救援以及仓库自动化应用方面具有广泛的应用。相对于单机器人路径规划问题，多机器人路径规划问题所存在的几个特点：
3.1)存在更多约束，例如需要保持一定队形，或者需要满足动的先后顺序约束，即某机器人的行动完成作为另一机器人开始行动的条件；
4.2)机器人团队内部存在规划冲突问题，即在执行规划路径过程中两机器人可能在同一时刻到达同一位置，导致碰撞。
5.多机器人路径规划算法可分为集中式和分布式两大类。集中式是将多机器人看作一个整体，将所有机器人整合成一个多自由度空间。去进行规划和搜索。而分布式是对每个机器人进行单独的路径规划。之后再通过协同和调度方法对单个机器人的路径进行协调和修改来解决冲突问题。
6.现在针对于多无人机的路径规划算法大多是分布式的。对于多无人机的编队问题，就主要是通过分布式的路径规划算法，通过一种完全去中心化的方法，只规划“领头”机器人，其他机器人只是在避障的同时“跟随领导”，来实现无人机的编队问题。而集中式规划算法，sharon等人在2015年提出了一种基于冲突的多机器人协作最优路径规划的方法，利用约束树来解决多个机器人之间的路径冲突问题。
7.对于现有的多无人机路径规划算法，大多是分布式的，能保证有解，但是不是最优解，在复杂狭窄、高冲突风险的空间中的飞行安全性不能得到保证。sharon等提出的基于冲突的多机器人协作最优路径规划的方法能很好解决多机器人在路径规划中轨迹冲突的问题，但是在实际应用中不能完全适用于无人机。该算法只能去解决二维平面上多机器人碰撞的问题，而无人机的运动环境是一个三维空间。并且并没有考虑到机器人的实际模型，在现实应用中很难做到轨迹无碰撞。


技术实现要素：

8.本发明提出一种基于冲突搜索的多无人机的路径规划方法，不仅使搜索出来的路径更符合无人机的飞行特性，更大量减少了路径规划过程中扩展节点的数量，提高了总体的规划效率。
9.为实现上述目的，本发明的技术方案如下：
10.一种基于冲突搜索的多无人机的协同路径规划方法，包括：
11.将无人机采集到的点云地图预处理为体素网格地图；
12.对所述体素网格地图进行低层级搜索，得到单个无人机的路径；
13.根据所述单个无人机的路径，遍历约束树对所有无人机的轨迹进行冲突检测，若
存在冲突则对无人机增加约束，并进行有约束的路径规划，若不存在冲突，则规划成功。
14.本发明有益效果：
15.本发明通过构造约束树对产生碰撞的机体进行的约束以得到安全的飞行序列，极大提高了无人机群在复杂狭窄、高冲突风险的空间中的飞行安全性；同时在低层搜索过程中结合hybridastar算法，通过引入无人机运动学和动力学模型，不仅使搜索出来的路径更符合无人机的飞行特性，更大量减少了路径规划过程中扩展节点的数量，提高了总体的规划效率。
附图说明
16.图1是本发明基于冲突搜索的多无人机的路径规划方法流程图；
17.图2是本具体实施方式中点云地图预处理为体素网格地图流程图；
18.图3是本具体实施方式中路径规划搜索得到单个无人机的路径流程图。
具体实施方式
19.下面结合附图并举实施例，对本发明进行详细描述。
20.如图1所示，本具体实施方式的一种基于冲突搜索的多无人机的协同路径规划方法，具体包括如下步骤：
21.步骤一、将无人机采集到的点云地图预处理为体素网格地图；本实施例中，如图2所示，具体为：
22.1.1输入所述点云地图的环境信息，得到点云数据；其中点云地图的障碍物信息包含于三维点云数据中，所述点云数据在世界坐标系下的三维坐标值(x,y,z)表征该位置上存在障碍；
23.1.2根据步骤1.1所述的点云数据，计算其在体素网格地图下的三维网格坐标；具体为：
24.首先，以物理地图大小为基准设定采样空间，根据预设分辨率对所述采样空间进行分割采样，形成三维的稠密体素网格图，其中分辨率表征地图的精细程度。本实施例中使用的世界地图大小为50
×
50
×
5m，设定分辨率为0.1，则生成的网格地图包含125,000个体素。
25.其次，将体素网格图的点云数据由物理坐标值转换到网格坐标值；以x轴坐标为例：
26.p
index
＝[p
real
÷
λ]
[0027]
其中p
index
和p
real
分别为数据点的网格坐标值和物理坐标值，λ为预设分辨率，括号“[]”为取整运算符，本实施例采用为向下取整。在具体实施时，该转换过程中的取整运算会导致两个相邻紧密的数据点具有相同网格坐标值，但通过修改分辨率可对其进行调整。
[0028]
1.3根据所述体素网格地图以及点云数据的网格坐标，设定并存储每个体素的占据信息；具体为：
[0029]
创建一个占据队列，以存储所述体素网格地图中的占据信息，其队列长度与体素网格总数相等，其中所有元素值初始化为0以代表非障碍；存储障碍信息时，将云点数据由物理坐标转换为网格坐标，进而计算其队列索引值，计算公式如下：
[0030]
x
index
*y
size
*z
size
y
index
*z
size
z
index
[0031]
其中x
index
、y
index
、z
index
代表该点网格坐标，y
size
、z
size
代表地图在y轴和z轴上的最大网格数；该队列索引值是三维数据点在队列中的唯一表征，置该处元素为1以表示该点为障碍；当需判断体素占据情况时，只需查询占据队列中该点队列索引处的元素是否为1即可。
[0032]
1.4依据步骤1.3得到的含占据信息的体素网格地图进行地图膨胀；具体实施时，所述地图膨胀的目的是为了将无人机的物理模型近似为一个质点，方便后续的路径规划工作，也即完成无人机由工作空间(working space)到构型空间(configuration space)的转换。本实施例中，采用依据无人机的体积参数对地图障碍物进行膨胀处理。具体为：
[0033]
将无人机近似为一个半径为n的球体，在步骤1.3所得的体素网格地图中，每个被设定为占据的体素则以膨胀半径n为限值向周围的区域进行扩展，得到n3个单元，在占据队列中将所有扩展的节点设定为障碍物，完成地图膨胀。
[0034]
步骤二、对步骤一得到的体素网格地图进行低层级搜索，得到单个无人机的路径；本实施例采用hybridastar算法进行路径规划搜索得到单个无人机的路径；如图3所示，具体为：
[0035]
2.1通过heuristic函数计算无人机起始点的代价f值，并把无人机的起始点加入到open列表中；
[0036]
2.2取所述open列表中f值最小的节点作为父节点，放入close列表中；
[0037]
2.3判断当前节点是否已经到达终点附近，若到达则跳出循环，否则继续循环，进入下一步；
[0038]
2.4利用父节点在动力与运动学约束下去扩展子节点，并判断所述子节点是否在close列表中，若不在则判断节点是否被扩展过，若被扩展过，但新的节点的f值比之前小，则更新节点，并放入open列表中，若未被扩展过，则直接放入open列表中；扩展完子节点后返回步骤2.3；
[0039]
步骤三、根据步骤二得到的单个无人机的轨迹，遍历约束树对所有无人机的轨迹进行冲突检测，若存在冲突则对无人机增加约束，并进行有约束的路径规划，若不存在冲突，则规划成功。具体为：
[0040]
3.1构造约束树，其中约束树的每一个节点n中包含：
[0041]
一组约束(n.constraints)，其中的每个约束都属于一架单独的无人机，约束树的根节点是一组空的约束，约束树中节点的子节点继承了父节点的约束，并为一架无人机添加了一个新的约束；
[0042]
一个解决方案(n.solution)：一组k条路径，每架无人机对应其中一条路径，代理的路径与其约束相一致，所述路径为所述采用hybridastar算法进行路径规划搜索得到单个无人机的路径；
[0043]
总代价(n.cost)：当前解决方案的总代价，称为节点n的f
‑
value值。
[0044]
3.2在所述约束树中处理节点，其处理过程具体包括如下步骤：
[0045]
a)给定约束树中节点n的约束列表，调用采用hybridastar搜索为每个无人机a
i
返回一条最短的路径，使其满足节点n中与代理a
i
相关的所有约束；
[0046]
b)将所述路径对其他无人机分别验证其有效性，本实施例中通过在所有时间步长
迭代并匹配全部无人机当前的位置进行验证；
[0047]
c)如果不存在两个无人机同时位于同一位置，则该约束树节点n设为目标节点，并返回包含此路径的当前解决方案(n.solution)；相反，如果在验证过程中发现两个或以上的无人机a
i
和a
j
存在冲突c＝(a
i
，a
j
，v，t)，则停止验证，节点设置为非目标节点；
[0048]
在具体实施时，上述验证过程中当产生冲突的无人机数k＞2时，采用以下方式处理：生成k个子节点，每个子节点都为k 1个无人机添加约束，即每个子节点只允许一个无人机在时间t时占据冲突顶点v。
[0049]
3.3重新进行低层级的hybridastar搜索为冲突无人机a
i
返回一条最短的路径，使其满足节点n中与代理a
i
相关的所有约束。
[0050]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。