CN112327939A - 一种城市街区环境下高层消防多无人机协同路径规划方法 - Google Patents

Abstract

本发明公开了一种城市街区环境下高层消防多无人机协同路径规划方法，所述方法首先利用栅格法构建城市街区的三维环境模型，并对所述三维环境模型进行筛选，得到可飞行区域；使用三维A*算法搜索筛选可飞行区域，规划出全局参考路径；再利用高层消防无人机搭载的传感器实时采集环境信息并实时更新三维环境模型，通过粒子群优化算法进行在线局部路径规划，确定当前路径上是否出现未知障碍物，并判断高层消防无人机是否需要动态避障。另外，通过设置最小安全距离，实现多架高层消防无人机的协同飞行。本方法实现了高层消防无人机利用混合算法进行协同路径规划与自主飞行，路径规划合理，响应速度快，在飞行途中灵活避障，具有更高的安全性。

Description

一种城市街区环境下高层消防多无人机协同路径规划方法

技术领域

本发明涉及无人机路径规划领域，更具体地，涉及一种城市街区环境下高层消防多无人机协同路径规划方法。

背景技术

我国高层建筑的数量位居世界前列，但由于高层建筑人员密集、可燃物多，导致高层建筑失火事故频繁发生。为了保障民生、经济和维护人民生命财产安全，国家高度重视高层建筑的消防工作，并且鼓励使用消防无人机等新兴智能设备发挥在抢险救灾等领域内的作用。在高层建筑失火时，传统的消防灭火模式和设备面临着在空间、时间和操作上的困难，而消防无人机可以快速出动并以最少时间抵达失火楼层率先开展抢险救灾工作，消防无人机的路径规划就成了最重要的一环。消防无人机的路径规划中既需要根据先验信息进行全局参考路径规划，又需要基于无人机传感器信息的局部路径动态规划。所以在路径规划中采用混合算法相比于单一算法在消防无人机飞行的快速性和安全性上具有更加明显的优势。在面临消防任务时，多架无人机进行协同的工作效率要比单独一架的更高。

2019年3月22日公开的中国专利CN109506654A提供了一种低空航路规划方法及装置、飞行器。其中方法包括：对飞行区域进行栅格化建模，得到飞行区域的空间模型；获取空间模型中的各个栅格的代价值，其中，代价值用于表示对飞行器在对应栅格的空间中航行的影响；根据各个栅格的代价值规划飞行区域的三维航路。该发明解决了无人机飞行航路的规划方法，但该方法中使用的算法单一，规划的飞行路径不够合理，无法应对无人机复杂多变的飞行环境。

发明内容

本发明为克服现有方法规划的路径不合理，无法应对城市街区飞行环境复杂多变的缺陷，提供一种城市街区环境下高层消防多无人机协同路径规划方法。

本发明的技术方案如下：

本发明提供一种城市街区环境下高层消防多无人机协同路径规划方法，所述方法包括以下步骤：

S1：利用栅格法构建城市街区的三维环境模型，在所述三维环境模型中设高层高层消防无人机的坐标为D(x,y,z)，姿态信息为D(θ,ψ)，其中θ为高层消防无人机的俯仰角，ψ为高层消防无人机的航向角；

S2：对所述三维环境模型进行筛选，得到包含高层消防无人机起点和终点的可飞行区域；

S3：设置高层消防无人机之间的最小安全距离，记为d_safe，使任意两架高层消防无人机间的距离d_ij满足d_ij＞2*d_safe；

设任意两架高层消防无人机的实时坐标为D_i＝(x_i,y_i,z_i)、D_j＝(x_j,y_j,z_j)，则

S4：基于三维A*算法做出全局参考路径规划，用于寻找起点与终点之间的全局参考路径；

S5：在全局参考路径的基础上，基于粒子群优化算法做出局部路径动态规划，得到全局最优路径；

S6：高层消防无人机依据全局最优路径完成飞行，抵达终点。

优选地，所述S2中的可飞行区域不包括无人机禁飞区、人员密集的露天场所和信号强干扰区域。

优选地，所述S4的具体步骤为：

S4.1：利用三维A*算法进行节点搜索，设定搜索方向；

S4.2：在所述节点搜索过程中，利用高层消防无人机的飞行限制条件对节点进行筛选；

S4.3：使用估价函数对S4.2中筛选后的节点进行加权评估，将代价最小的节点进行保存并视为下一次搜索的目标点；

所述评估函数为：f(n)＝g(n)+h(n)+ω_HH(n)，其中，f(n)为当前节点的总代价，g(n)为搜索时起点到当前节点的欧几里德距离，h(n)为当前节点到目标点的欧几里德距离，ω_H为高度权重系数，H(n)为当前节点的上一节点与当前节点的高度差；

S4.4：当S4.3中h(n)＜＜g(n)时，得到最优解，保存的节点构成高层消防无人机飞行完整的全局参考路径。

优选地，所述S4.1中，三维A*算法正向搜索时从起点到终点进行节点搜索，反向搜索时从终点到起点进行节点搜索。

优选地，所述S4.2中，飞行限制条件包括高层消防无人机的俯仰角θ和航向角ψ；

正向搜索时，建立OPEN1表，依据飞行限制条件筛选的节点放进OPEN1表中；

反向搜索时，建立OPEN2表，依据飞行限制条件筛选的节点放进OPEN2表中。

优选地，所述S4.3中，正向搜索时，建立CLOSE1表，使用评估函数计算出OPEN1表中代价最小的节点保存在CLOSE1表中，并在OPEN1表中将其删除；

反向搜索时，建立CLOSE2表，使用评估函数计算出OPEN2表中代价最小的节点保存在CLOSE2表中，并在OPEN2表中将其删除。

优选地，所述S5的具体步骤为：

S5.1：利用高层消防无人机上搭载的传感器实时采集环境信息并实时更新城市街区的三维环境模型；

S5.2：利用粒子群优化算法初始化城市街区的三维环境模型内的粒子群，设定粒子规模和最大迭代次数k_max；

S5.3：赋予每个粒子初始位置和速度；

S5.4：设置粒子群优化算法中的加速度系数c₁与c₂；

S5.5：设置粒子群优化算法中的惯性因子ω；

S5.6：粒子群优化算法在每次迭代中更新每个粒子的速度与位置，直到迭代结束或找到全局最佳位置，完成局部路径动态规划。

优选地，所述S5.5中，采用线性递减权值策略设置惯性因子ω，并在算法迭代过程中对其进行动态调整，其中

其中ω_max表示惯性因子的最大值，ω_min表示惯性因子的最小值，

表示当前迭代次与最大迭代次数的比值。

优选地，所述S5.6的具体步骤为：

设在D维的算法维度中，粒子群由N个粒子组成，在第k次迭代时，第i个粒子的位置表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；第i个粒子的速度表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；第i个粒子的个体最佳位置表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；整个粒子群的全局最佳位置表示为

其中d＝1,2,…,D,k＝1,2,…k_max；则：

当k＝1时，

即为粒子的初始速度和初始位置；c₁与c₂为加速度系数，ω为惯性因子，rand₁()与rand₂()为随机函数；

优选地，所述S5.6中，粒子群优化算法在迭代过程中出现全局最佳位置的更新停滞，则给予粒子一个新的随机速度和随机位置：

其中ω₁和ω₂为任意惯性因子，rand₁()与rand₂()为随机函数。

与现有技术相比，本发明技术方案的有益效果是：

本发明首先利用三维A*算法搜索筛选已构建的城市街区的三维环境模型，得到全局参考路径，再利用粒子群优化算法进行局部路径动态规划，实现动态避障。高层消防无人机依据两个算法共同规划出的路径飞行，在全局参考路径的基础上，遇到障碍物时可以及时发现障碍物位置，进行局部路径动态规划后，依据全局最优路径飞行，路径规划更加合理，响应速度快，具有更高的安全性。

附图说明

图1为实施例1所述的一种城市街区环境下高层消防多无人机协同路径规划方法的流程图；

图2为实施例1所述的高层消防多无人机之间防避撞流程图；

图3为实施例1所述的三维A*算法流程图；

图4为实施例1所述的粒子群优化算法流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

本实施例提供一种城市街区环境下高层消防多无人机协同路径规划方法，如图1所示，所述方法包括以下步骤：

S1：利用栅格法构建城市街区的三维环境模型，在所述三维环境模型中设高层消防无人机的坐标为D(x,y,z)，姿态信息为D(θ,ψ)，其中θ为高层消防无人机的俯仰角，ψ为高层消防无人机的航向角；在所述三维环境模型中确定高层消防无人机的起点和终点的三维坐标；

S2：对所述三维环境模型进行筛选，得到包含高层消防无人机起点和终点的可飞行区域；

S3：设置高层消防无人机之间的最小安全距离，记为d_safe，使任意两架高层消防无人机间的距离d_ij满足d_ij＞2*d_safe；

确定任意两架高层消防无人机的实时坐标为D_i＝(x_i,y_i,z_i)、D_j＝(x_j,y_j,z_j)，则

S4：基于三维A*算法做出全局参考路径规划，用于寻找起点与终点之间的全局参考路径；

S5：在全局参考路径的基础上，基于粒子群优化算法做出局部路径动态规划，得到全局最优路径；

S6：高层消防无人机依据全局最优路径完成飞行，抵达终点。

所述S2中的可飞行区域不包括无人机禁飞区、人员密集的露天场所和信号强干扰区域。

所述S4的具体步骤为：

S4.1：利用三维A*算法进行节点搜索，设定搜索方向；

S4.2：在所述节点搜索过程中，利用高层消防无人机的飞行限制条件对节点进行筛选；

S4.3：使用估价函数对S4.2中筛选后的节点进行加权评估，将代价最小的节点进行保存并视为下一次搜索的目标点；

所述评估函数为：f(n)＝g(n)+h(n)+ω_HH(n)，其中，f(n)为当前节点的总代价，g(n)为搜索时起点到当前节点的欧几里德距离，h(n)为当前节点到目标点的欧几里德距离，ω_H为高度权重系数，H(n)为当前节点的上一节点与当前节点的高度差；

S4.4：当S4.3中h(n)＜＜g(n)时，得到最优解，保存的节点构成高层消防无人机飞行完整的全局参考路径。

所述S4.1中，三维A*算法正向搜索时从起点到终点进行节点搜索，反向搜索时从终点到起点进行节点搜索，正向搜索和反向搜索交替进行。

所述S4.2中，飞行限制条件包括高层消防无人机的俯仰角θ和航向角ψ；

正向搜索时，建立OPEN1表，依据飞行限制条件筛选的节点放进OPEN1表中；

反向搜索时，建立OPEN2表，依据飞行限制条件筛选的节点放进OPEN2表中。

所述S4.3中，正向搜索时，建立CLOSE1表，使用评估函数计算出OPEN1表中代价最小的节点保存在CLOSE1表中，并在OPEN1表中将其删除；

反向搜索时，建立CLOSE2表，使用评估函数计算出OPEN2表中代价最小的节点保存在CLOSE2表中，并在OPEN2表中将其删除。

所述S5的具体步骤为：

S5.1：利用高层消防无人机上搭载的传感器实时采集环境信息并实时更新城市街区的三维环境模型；

S5.2：利用粒子群优化算法初始化城市街区的三维环境模型内的粒子群，设定粒子规模和最大迭代次数k_max；

S5.3：赋予每个粒子初始位置和速度；

S5.4：设置粒子群优化算法中的加速度系数c₁与c₂；

S5.5：设置粒子群优化算法中的惯性因子ω；

S5.6：粒子群优化算法在每次迭代中更新每个粒子的速度与位置，直到迭代结束或找到全局最佳位置，完成局部路径动态规划。

所述S5.5中，采用线性递减权值策略设置惯性因子ω，并在算法迭代过程中对其进行动态调整，其中

其中ω_max表示惯性因子的最大值，ω_min表示惯性因子的最小值，

表示当前迭代次与最大迭代次数的比值。

所述S5.6的具体步骤为：

设在D维的算法维度中，粒子群由N个粒子组成，在第k次迭代时，第i个粒子的位置表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；第i个粒子的速度表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；第i个粒子的个体最佳位置表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；整个粒子群的全局最佳位置表示为

其中d＝1,2,…,D,k＝1,2,…k_max；则：

当k＝1时，

即为粒子的初始速度和初始位置；c₁与c₂为加速度系数，ω为惯性因子，rand₁()与rand₂()为取值范围为[0，1]随机函数；

所述S5.6中，粒子群优化算法在迭代过程中出现全局最佳位置的更新停滞，则给予粒子一个新的随机速度和随机位置：

其中ω₁和ω₂为任意惯性因子，rand₁()与rand₂()为取值范围为[0，1]的随机函数。

本实施例的有益效果是：

本实施例首先利用三维A*算法搜索筛选已构建的城市街区的三维环境模型，得到全局参考路径，再利用粒子群优化算法进行局部路径动态规划，实现动态避障。高层消防无人机依据两个算法共同规划出的路径飞行，在全局参考路径的基础上，遇到障碍物时可以及时发现障碍物位置，进行局部路径动态规划后，依据全局最优路径飞行，路径规划更加合理，响应速度快，具有更高的安全性。

另外，本实施例还具备以下有益效果：通过设定高层消防无人机之间的最小安全距离，实现多架高层消防无人机的协同飞行，提高了消防救援的效率；三维A*算法搜索方向从单向改为双向，提高了算法的工作效率；搜索节点时加入了高层消防无人机的飞行条件限制，减轻了节点计算工作量，规划出的路径符合无人机的动力学特征；粒子群优化算法容易出现全局最佳位置的更新停滞，通过赋予粒子新的随机速度和随机位置，增加了粒子的多样性，使其的位置和速度跳跃摆脱更新停滞。

相同或相似的标号对应相同或相似的部件；

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述方法包括以下步骤：

S1：利用栅格法构建城市街区的三维环境模型，在所述三维环境模型中，设高层消防无人机的三维坐标为D(x,y,z)，姿态信息为D(θ,ψ)，其中θ为高层消防无人机的俯仰角，ψ为高层消防无人机的航向角；

S2：对所述三维环境模型进行筛选，得到包含高层消防无人机起点和终点的可飞行区域；

S3：设置高层消防无人机之间的最小安全距离，记为d_safe，使任意两架高层消防无人机间的距离d_ij满足d_ij＞2*d_safe；

S4：基于三维A*算法进行全局参考路径规划，用于寻找起点与终点之间的全局参考路径；

S5：在全局参考路径的基础上，基于粒子群优化算法做出局部路径动态规划，得到全局最优路径；

S6：高层消防无人机依据全局最优路径完成飞行，抵达终点。

2.根据权利要求1所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S2中的可飞行区域不包括无人机禁飞区、人员密集的露天场所和信号强干扰区域。

3.根据权利要求2所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S4的具体步骤为：

S4.1：利用三维A*算法进行节点搜索，设定搜索方向；

S4.2：在所述节点搜索过程中，利用高层消防无人机的飞行限制条件对节点进行筛选；

S4.3：使用估价函数对S4.2中筛选后的节点进行加权评估，将代价最小的节点进行保存并视为下一次搜索的目标点；

所述评估函数为：f(n)＝g(n)+h(n)+ω_HH(n)，其中，f(n)为当前节点的总代价，g(n)为搜索时起点到当前节点的欧几里德距离，h(n)为当前节点到目标点的欧几里德距离，ω_H为高度权重系数，H(n)为当前节点的上一节点与当前节点的高度差；

S4.4：当S4.3中h(n)＜＜g(n)时，得到最优解，保存的节点构成高层消防无人机飞行完整的全局参考路径。

4.根据权利要求3所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S4.1中，三维A*算法正向搜索时从起点到终点进行节点搜索，反向搜索时从终点到起点进行节点搜索。

5.根据权利要求4所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S4.2中，飞行限制条件包括高层消防无人机的俯仰角θ和航向角ψ；

正向搜索时，建立OPEN1表，依据飞行限制条件筛选的节点放进OPEN1表中；

反向搜索时，建立OPEN2表，依据飞行限制条件筛选的节点放进OPEN2表中。

6.根据权利要求5所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S4.3中，正向搜索时，建立CLOSE1表，使用评估函数计算出OPEN1表中代价最小的节点保存在CLOSE1表中，并在OPEN1表中将其删除；

反向搜索时，建立CLOSE2表，使用评估函数计算出OPEN2表中代价最小的节点保存在CLOSE2表中，并在OPEN2表中将其删除。

7.根据权利要求6所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S5的具体步骤为：

S5.1：利用高层消防无人机上搭载的传感器实时采集环境信息并实时更新城市街区的三维环境模型；

S5.2：利用粒子群优化算法初始化城市街区的三维环境模型内的粒子群，设定粒子规模和最大迭代次数k_max；

S5.3：赋予每个粒子初始位置和初始速度；

S5.4：设置粒子群优化算法中的加速度系数c₁与c₂；

S5.5：设置粒子群优化算法中的惯性因子ω；

S5.6：粒子群优化算法在每次迭代中更新每个粒子的速度与位置，直到迭代结束或找到全局最佳位置，完成局部路径动态规划。

8.根据权利要求7所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S5.5中，采用线性递减权值策略设置惯性因子ω，并在算法迭代过程中对其进行动态调整，其中

其中ω_max表示惯性因子的最大值，ω_min表示惯性因子的最小值，

表示当前迭代次与最大迭代次数的比值。

9.根据权利要求8所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S5.6的具体步骤为：

设在D维的算法维度中，粒子群由N个粒子组成，在第k次迭代时，第i个粒子的位置表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；第i个粒子的速度表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；第i个粒子的个体最佳位置表示为

其中i＝1,2,…,N,d＝1,2,…,D,k＝1,2,…k_max；整个粒子群的全局最佳位置表示为

其中d＝1,2,…,D,k＝1,2,…k_max；依据以下公式更新每个粒子的速度与位置：

当k＝1时，

即为粒子的初始速度和初始位置；c₁与c₂为加速度系数，ω为惯性因子，rand₁()与rand₂()为随机函数。

10.根据权利要求9所述的一种城市街区环境下高层消防多无人机协同路径规划方法，其特征在于，所述S5.6中，粒子群优化算法在迭代过程中出现全局最佳位置的更新停滞，则给予粒子一个新的随机速度和随机位置：

其中ω₁和ω₂为任意惯性因子，rand₁()与rand₂()为随机函数。

Images