[发明专利]一种基于空间和时间协同的多无人机动态航路规划方法

权利要求书

1.一种基于空间和时间协同的多无人机动态航路规划方法，其特征在于包括以下步骤：

步骤M01，设定共有N架无人机参与要求有空间约束和时间协同的多无人机动态航路规划，N架无人机间可以相互或与基地进行实时通讯，N架无人机的起点分别是P_start,i，任务点分别是P_end,i，P_start,i、P_end,i均为三维坐标，其中i＝1,2,…,N；

步骤M02，无人机相互通讯或同基地进行通讯，在基地的任务计算机或某架计算能力满足要求的无人机上作集中式计算，计算时需要获知规划环境信息和每架无人机的机动性能信息及当前位置、任务点信息；

步骤M03，在任务计算机中，为每架无人机建立虚拟的数据结构，设置稀疏A*算法代价函数中已规划航路长度代价、高度代价、地形威胁代价、沿途威胁代价、需求姿态角代价、估计航路长度代价以及时间协同代价的系数，以及依据无人机性能所建立的性能约束模型中的参数，初始化重规划模式标记为false，设置稀疏A*算法的竖直分划数目为n_V，水平分划数目为n_H；

步骤M04，设置所有无人机的规划步长均为L_step，设置无人机的已规划航迹点数目n_planned,i＝1，整个规划系统的协同需规划航迹点数目n_cooperation＝2，为每架无人机设置相应的Open表和Closed表结构，设置无人机的可规划标记W_i＝true，各架无人机以各自的起点作为第一个当前扩展节点P_expanding,i，并填充至各自的Open表当中；

步骤M05，如果存在无人机的可规划标记W_i为true，继续步骤M06，如果所有无人机的可规划标记W_i都为false，跳转至步骤R01；

步骤M06，找出所有无人机中已规划航迹点数目n_planned最小的作为当前进行航路扩展的无人机j，如果存在相同的具有已规划航点数目的无人机，则在这些无人机中随机选择一个作为当前进行航路扩展的无人机j；跳转至步骤S01；

步骤S01，判断无人机j的Open表是否为空，若不为空，继续步骤S02，若为空，转至步骤F01；

步骤S02，从无人机j的Open表中选取代价函数值最小的节点作为当前扩展节点P_expanding,j，继续步骤S03；

步骤S03，判断当前扩展节点P_expanding,j是否在无人机j的任务点P_end,j的一个规划步长L_step范围内，如果在，继续步骤S04，否则转至步骤S05；

步骤S04，设置无人机j的可规划标记W_i＝false，无人机j的航路计算成功，其航路结果数据不再参与除防碰撞约束之外的其他计算，并转至步骤M05；

步骤S05，依据当前扩展节点P_expanding,j扩展节点，生成待选节点集合S＝{P_choice,j,k}，其中k＝1,2,…,n_Vn_H+3，将当前扩展节点P_expanding,j从无人机j的Open表中删除，放入无人机j的Closed表当中；遍历S中的待选节点，删除不满足无人机性能约束及不满足防碰撞约束的待选节点，继续步骤S06；

步骤S06，如果S为空，转至步骤S01；如果S不为空，继续步骤S07；

步骤S07，计算S中元素的代价函数值，并将其放入无人机j的Open表中，无人机j的已规划航迹点数目n_planned,j加1，继续步骤S08；

步骤S08，判断无人机j的已规划航迹点数目n_planned,j是否和规划系统的协同需规划航迹点数目n_cooperation相等，如果相等，跳转至步骤S02，否则，转至步骤M07；

步骤M07，判断所有无人机的已规划航迹点数目n_planned,i是否和规划系统的协同需规划航迹点数目n_cooperation相等，如果均相等，继续步骤M08，否则，跳转至步骤S02；

步骤M08，n_cooperation加1，跳转至步骤S02；

步骤F01，因无人机j的Open表为空，置本次计算为失败，释放本次计算相关的计算资源；

步骤F02，计算失败次数是否超过设定值，如未超过，跳转至步骤M03，设置不为重规划模式，重新设置稀疏A*算法的备用的各项参数后重新计算，如超过设定值，继续步骤F03；

步骤F03，认为航路规划失败，无人机应进行紧急制动躲避障碍，并发出更改任务的需求，算法结束；

步骤R01，判断重规划模式标记为是否true，如果为重规划模式，继续步骤R02，否则，跳转至步骤R05；

步骤R02，航路规划算法成功，从每个无人机的Open表代价值最小节点回溯，输出所有得到成功规划的无人机的航迹点，并发送到各自对应的无人机中，以此时无人机的位置，结合规划出的航迹点序列，判断当前位置对新规划出的航路的可达性，如可达，以当前位置结合规划出的航迹点序列得到新的航路信息，各无人机计算自身建议平均飞行速率，置新航路计算结果为成功，否则置为失败，继续步骤R03；

步骤R03，判断新航路计算是否成功，若成功，跳转至步骤R06，若失败，继续步骤R04；

步骤R04，以此时各无人机的位置为新起点，设置重规划标记为true，转至步骤M04，进行重规划，重规划过程中，各无人机按照原有航路飞行；

步骤R05，航路规划算法成功，从每个无人机的Open表代价值最小节点回溯，输出所有得到成功规划的无人机的航迹点，根据所有得到成功规划的无人机的航迹长度计算建议平均飞行速率v_c,i，v_c,i为无人机i已规划航路与所有无人机中规划航路最大值的比值乘以参考飞行速率，继续步骤R06；

步骤R06，各架无人机按照规划好的航路和建议平均飞行速率飞向任务点，转至步骤R07；

步骤R07，实时接收环境更新信息，继续步骤R08；

步骤R08，如果出现需规避区域，则跳转至步骤R04，否则继续步骤R09；

步骤R09，判断各无人机是否到达任务点，如均已到达，跳转至步骤M09，否则，跳转至步骤R06；

步骤M09，航路规划任务成功，任务计算机释放相关计算资源，结束计算。