[发明专利]一种基于分解思想的高效多无人机协同航迹规划方法

摘要

本发明涉及一种基于分解思想的高效多无人机协同航迹规划方法，属于航迹规划技术领域。本发明针对协同航迹快速生成问题，建立无人机协同航迹规划模型。基于分解思想，将多无人机协同规划问题分解为单无人机逐次规划问题。对于单机规划问题，将三维航迹规划分解为水平规划和高度规划。提出一种考虑机间碰撞规避约束的稀疏A*搜索(Collision Avoidance Constrained SAS,CAC‑SAS)算法用于解决水平面内多无人机协同航迹规划问题。在铅垂面内，提出一种改进的高度沉降法用于规划航迹点的高度值，从而快速生成满足约束的多无人机协同飞行航迹。本发明要解决的技术问题为：根据实际任务需要，基于分解思想获得满足复杂约束的多无人机协同飞行航迹，具有在短时间内生成可行航迹的优点。

主权项

1.一种基于分解思想的高效多无人机协同航迹规划方法，其特征在于：包括如下步骤：步骤一、根据无人机飞行性能参数信息、航迹约束信息和任务环境信息，建立无人机协同航迹规划数学模型；所述的无人机飞行性能参数信息包括：无人机最大转弯角、最大爬升/俯冲角和最小航迹段长度；所述的航迹约束信息包括无人机的飞行起点位置和终点位置；所述的任务环境信息包括最小相对地面飞行高度、禁飞区的位置和半径；所述的协同航迹规划数学模型包括协同航迹规划的优化目标和协同航迹规划约束；无人机协同航迹规划的优化目标根据应用不同而有所不同，本发明以最小化无人机飞行航程作为优化目标，如下式所示：其中，N为飞行编队中无人机的数量；Li为第i架无人机的飞行航程；无人机协同航迹规划约束需考虑机间避碰约束、无人机飞行性能参数信息、地形和禁飞区约束，具体如下：机间避碰约束：无人机编队在抵达目标点过程中，无人机间的距离需要始终大于预设的安全距离Ds，以避免无人机之间发生碰撞；两无人机间的避碰约束能够转化为航迹间的避碰约束；以相同时间间隔Δt离散两无人机飞行航迹，对时间匹配的每一离散航迹段依次进行比较，若存在两条离散航迹段间的最小距离小于Ds，则认为待检测的两条航迹不满足机间避碰约束；另外，从同一起点出发或到达同一目标点的协同航迹规划，在起点或目标点附近的预设范围内不考虑机间避碰约束；机间避碰约束的表达式为：其中，DIS(·)表示计算两线段最短距离的函数；表示第i架无人机在第s个Δt时间间隔内的飞行航迹段；表示第j架无人机在第s个Δt时间间隔内的飞行航迹段；T_i,j表示第i架无人机和第j架无人机航程对应飞行时长中的较小值；最小航迹段长度约束：受机动能力限制，无人机每次改变航迹方向前，必须沿原方向飞行一段距离，即要求每一段航迹段不小于最短直飞距离lmin，其表达式为：其中，ni表示第i架无人机的航迹所包含航迹点总数量；li,k为第i架无人机第k段航迹的长度，其表达式如下所示：其中，表示第i架无人机第k个航迹点的x位置坐标；表示第i架无人机第k+1个航迹点的x位置坐标；表示第i架无人机第k个航迹点的y位置坐标；表示第i架无人机第k+1个航迹点的y位置坐标；最大转弯角约束：受无人机机动能力的约束，规划的航迹需要避免过大的转弯角，以保证航迹可行；无人机的最大转弯角为Δχmax，则要求：其中，Δχi,k为第i架无人机在第k个航迹点处的转弯角；最大爬升角/俯冲角约束：受无人机动能力的约束，无人机的爬升/俯冲能力存在极限值；记无人机的最大爬升/俯冲角为θmax，则要求：其中，θi,k为第i架无人机在第k个航迹点处的爬升/俯冲角；地形约束：为了保证无人机飞行安全，需与地面保持安全距离；设允许的最小相对地面飞行高度为hmin，则地形约束表示为：其中，hi,k为第i架无人机在第k个航迹点处的相对地面飞行高度；禁飞区约束：无人机飞行过程中，需对环境中的威胁、障碍等禁飞区进行规避，即要求无人机的航迹不与禁飞区相交，则表示为：其中，disi,j表示第i架无人机的航迹与第j个禁飞区之间的最小距离，nNFZ为禁飞区的数量；步骤二、基于分解思想，将多无人机协同规划问题分解为单无人机逐次规划问题，减少耦合协同约束；对于单机规划问题，设计改进的高度沉降法，将三维航迹规划分解为水平规划和高度规划；水平规划时不考虑高度约束，以最小化航程为目标，基于CAC‑SAS算法对每一架无人机依次进行二维航迹规划，得到水平面内多无人机协同飞行航迹；在规划第i架无人机的二维航迹时，需要将前i‑1架无人机协同航迹作为输入信息；CAC‑SAS算法具体实现方法如下：步骤2.1、若当前规划第i架无人机，则需输入前i‑1架无人机协同飞行航迹；步骤2.2、初始化OPEN表和CLOSED表；创建OPEN表和CLOSED表，同时将规划的起点设置为起始节点，并插入OPEN表，此时CLOSED表为空；步骤2.3、判断OPEN表是否为空；若OPEN表为空，则搜索失败并结束搜索，要求用户修改算法参数后，重新启动SAS算法；若OPEN表非空，则执行步骤2.4；步骤2.4、更新当前节点；从当前的OPEN表中取出代价值最小的节点作为新的当前节点，则OPEN表中不再含有该节点，并将该节点放入CLOSED表；步骤2.5、判断当前节点能否满足收敛条件；若当前节点能够在满足公式(3)至(8)所有约束的条件下到达规划的终点，则结束节点扩展循环，转而执行步骤2.9；若不收敛，则执行步骤2.6；步骤2.6、节点扩展；以当前节点为中心进行节点扩展，获得当前节点的子节点；扩展节点数量由水平扩展数决定；步骤2.7、机间避碰约束检测；根据公式(2)依次判断当前扩展航迹与步骤2.1的输入航迹是否发生碰撞，若发生碰撞，则舍弃当前扩展节点；步骤2.8、判断扩展子节点的可行性；根据公式(3)至(8)判断扩展子节点是否满足约束条件；计算所有可行节点的代价值，并将其存入OPEN表中；然后执行步骤2.3；步骤2.9、创建目标节点；将规划的终点设置为目标节点，目标节点的父节点设置为当前节点，并将目标节点压入CLOSED表；步骤2.10、反溯最终规划航迹；根据CLOSED表中的目标节点和已扩展节点，利用节点间的扩展关系，从目标节点向上回溯直至起始节点，得到从起始点到目标点的航迹；步骤三、在完成水平面内航迹规划之后，每架无人机需要考虑无人机最小相对地面飞行高度和最大爬升/俯冲角度约束进行高度规划以得到三维航迹；高度规划具体实现方法如下：步骤3.1、计算与二维航迹对应的地形高度剖面，并确定地形高度剖面中的最大地形高度Hmax；步骤3.2、初始化航迹高度；将航迹起点与目标点的高度分别初始化为真实航迹起点高度h0和真实航迹目标点高度hf；考虑最小相对飞行高度hmin的约束，将其它航迹点的高度全部初始化为h0、hf和Hmax+hmin三者中的最大值；将航迹的起点与终点设置为不可沉降点；步骤3.3、计算航迹中的所有可沉降航迹点的沉降裕度；可沉降航迹点的沉降裕度取地形约束沉降裕度和爬升/俯冲约束沉降裕度中的最小值；地形约束沉降裕度为与航迹点相连两航迹段的最小相对地面高度；为了提高准确度，对航迹段进行线性插值，计算两航迹段的相对地面高度，从中选取最小值；爬升/俯冲约束下的沉降裕度，仅当航迹点的两相邻航迹点中有不可沉降点时才需考虑；根据当前航点与相邻不可沉降点间的水平距离和最大爬升/俯冲角，计算得到爬升/俯冲约束下的沉降裕度；步骤3.4、航迹沉降；选择所有航迹点沉降裕度的最小值作为航迹沉降值，降低所有可沉降航迹点的高度；将沉降裕度等于该最小值的可沉降航迹点设置为不可沉降航迹点；步骤3.5、判断航迹可沉降性；判断航迹中是否仍存在可沉降的航迹点，若存在，则转步骤3.3；否则，算法结束，则得到新的三维可行航迹；步骤四、将单无人机逐次规划后得到的多条三维可行航迹作为多无人机的协同飞行航迹。