一种基于分解思想的高效多无人机协同航迹规划方法与流程

本发明涉及一种基于分解思想的高效多无人机协同航迹规划方法，属于航迹规划
技术领域：
。
背景技术：
随着无人机技术的快速发展和日趋成熟，无人机在军用和民用领域得到愈发广泛的应用。受限于单架无人机载荷能力和续航能力的不足，通过多无人机协同，能够扩展执行任务功能，提高任务完成效能。多无人机协同航迹规划需在综合考虑任务环境、机间协同和飞行性能等约束下，为每架无人机快速规划出从起始点至目标点的可行飞行航迹，是成功执行协同任务的基础。协同航迹规划的结果是一系列不含时间信息的离散航迹点，对于时空耦合较弱的无人机协同应用，航迹规划结果可以直接作为无人机底层跟踪控制的输入，而对于时空协同约束严格的应用，协同航迹规划的结果则可作为协同轨迹规划的引导点。无人机航迹规划方法主要包括两类：基于数值优化的方法和基于图搜索的方法。基于数值优化的航迹规划方法，是将航迹规划描述为非线性约束优化问题，然后使用序列二次规划、遗传算法、粒子群优化等最优化算法进行求解。采用全局优化算法得到的航迹具有良好的最优性，但是全局优化算法需要大量调用规划模型，导致其求解时间较长。基于图搜索的航迹规划方法，根据规划环境、飞行性能等约束，建立包含无人机备选航迹点的显式或隐式空间图，然后利用图搜索算法可快速确定无人机的飞行航迹。常用的图搜索算法包括dijkstra、a*和快速扩展随机树(rapidly-exploringrandomtree，rrt)。针对协同航迹规划的时效性需求，一般采用图搜索算法求解协同飞行航迹以提高求解效率。然而，对编队内所有无人机进行航迹规划，需要同时考虑所有无人机间的耦合约束，对于图搜索算法，意味着对所有无人机的航迹同时进行节点扩展和可行航迹搜索，在节点搜索过程需考虑当前无人机与其它无人机间的协同约束。随着无人机个数的增加，处理这些耦合约束的计算量呈指数增长，大大降低了航迹规划求解效率。技术实现要素：本发明的目的是为了解决现有多无人机协同航迹规划耗时长，无法实现实时规划的问题，提供一种基于分解思想的高效多无人机协同航迹规划方法。该方法根据实际任务需要，基于分解思想获得满足复杂约束的多无人机协同飞行航迹，具有在短时间内生成可行航迹的优点。所述的复杂约束包括机间避碰约束、无人机机动能力约束、地形约束和禁飞区约束。本发明的目的是通过下述技术方案实现的。本发明公开的一种基于分解思想的高效多无人机协同航迹规划方法，针对协同航迹快速生成问题，建立无人机协同航迹规划模型。基于分解思想，将多无人机协同规划问题分解为单无人机逐次规划问题。对于单机规划问题，将三维航迹规划分解为水平规划和高度规划。提出一种考虑机间碰撞规避约束的稀疏a*搜索(collisionavoidanceconstrainedsas,cac-sas)算法用于解决水平面内多无人机协同航迹规划问题。在铅垂面内，提出一种改进的高度沉降法用于规划航迹点的高度值，从而快速生成满足约束的多无人机协同飞行航迹。一种基于分解思想的高效多无人机协同航迹规划方法，包括如下步骤：步骤一、根据无人机飞行性能参数信息、航迹约束信息和任务环境信息，建立无人机协同航迹规划数学模型。所述的无人机飞行性能参数信息包括无人机最大转弯角、最大爬升/俯冲角和最小航迹段长度。所述的航迹约束信息包括无人机的飞行起点位置和终点位置。所述的任务环境信息包括最小相对地面飞行高度、禁飞区的位置和半径。所述的协同航迹规划数学模型包括协同航迹规划的优化目标和协同航迹规划约束。无人机协同航迹规划的优化目标根据应用不同而有所不同，本发明以最小化无人机飞行航程作为优化目标，如下式所示：其中，n为飞行编队中无人机的数量；li为第i架无人机的飞行航程。无人机协同航迹规划约束需考虑机间避碰约束、无人机飞行性能参数信息、地形和禁飞区约束，具体如下：机间避碰约束：无人机编队在抵达目标点过程中，无人机间的距离需要始终大于预设的安全距离ds，以避免无人机之间发生碰撞。两无人机间的避碰约束可转化为航迹间的避碰约束。以相同时间间隔δt离散两无人机飞行航迹，对时间匹配的每一离散航迹段依次进行比较，若存在两条离散航迹段间的最小距离小于ds，则认为待检测的两条航迹不满足机间避碰约束。另外，从同一起点出发或到达同一目标点的协同航迹规划，在起点或目标点附近的预设范围内不考虑机间避碰约束。机间避碰约束的表达式为：其中，dis(·)表示计算两线段最短距离的函数；表示第i架无人机在第s个δt时间间隔内的飞行航迹段；表示第j架无人机在第s个δt时间间隔内的飞行航迹段；ti,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.1中所述与二维航迹对应的地形高度剖面，还需在航迹点中生成较密集的插值点，插值密度根据期望的分辨率确定；所述生成插值点的方法为线性插值；步骤3.2、初始化航迹高度。将航迹起点与目标点的高度分别初始化为真实航迹起点高度h0和真实航迹目标点高度hf。考虑最小相对飞行高度hmin的约束，将其它航迹点的高度全部初始化为h0、hf和hmax+hmin三者中的最大值。将航迹的起点与终点设置为不可沉降点。步骤3.3、计算航迹中的所有可沉降航迹点的沉降裕度。可沉降航迹点的沉降裕度取地形约束沉降裕度和爬升/俯冲约束沉降裕度中的最小值。地形约束沉降裕度为与航迹点相连两航迹段的最小相对地面高度。为了提高准确度，对航迹段进行线性插值，计算两航迹段的相对地面高度，从中选取最小值。爬升/俯冲约束下的沉降裕度，仅当航迹点的两相邻航迹点中有不可沉降点时才需考虑。根据当前航点与相邻不可沉降点间的水平距离和最大爬升/俯冲角，计算得到爬升/俯冲约束下的沉降裕度。步骤3.4、航迹沉降。选择所有航迹点沉降裕度的最小值作为航迹沉降值，降低所有可沉降航迹点的高度。将沉降裕度等于该最小值的可沉降航迹点设置为不可沉降航迹点。步骤3.5、判断航迹可沉降性。判断航迹中是否仍存在可沉降的航迹点，若存在，则转步骤3.3；否则，算法结束，则得到新的三维可行航迹。步骤四、将单无人机逐次规划后可得到的多条三维可行航迹作为多无人机的协同飞行航迹。有益效果1、本发明公开的一种基于分解思想的高效多无人机协同航迹规划方法，针对协同航迹快速生成问题，建立无人机协同航迹规划模型。基于分解思想，将多无人机协同规划问题分解为单无人机逐次规划问题。对于单机规划问题，将三维航迹规划分解为水平规划和高度规划，协同航迹规划结果具有最优性好的优点。无人机协同数量为四架时，与传统的三维稀疏a*算法相比，基于分解思想的协同航迹规划方法求解效率提升了近40倍。2、本发明公开的一种基于分解思想的高效多无人机协同航迹规划方法，对于单机规划，为了提高求解效率，提出一种考虑机间碰撞规避约束的稀疏a*搜索(collisionavoidanceconstrainedsas,cac-sas)算法用于求解水平面内多无人机协同航迹规划问题。在铅垂面内，提出一种改进的高度沉降法用于规划航迹点的高度值，从而快速生成满足约束的多无人机协同飞行航迹。附图说明图1为cac-sas算法流程图；图2为高度规划示意图；图3为最小化航程的航迹规划结果；图4为无人机航迹的高度规划结果；其中(a)为无人机1高度规划结果；(b)为无人机2高度规划结果；(c)为无人机3高度规划结果；(d)为无人机4高度规划结果。具体实施方式为了更好地说明本发明的目的与优点，下面通过多无人机协同航迹规划实例，结合附图与表格对本发明做进一步说明。实施例1：仿真硬件为intelcorei7-6700cpu3.40ghz，8g内存，仿真环境为matlabr2016b。无人机编队在10km×10km×1km的三维环境中执行任务，地形采用模拟函数法生成。无人机协同航迹规划要求编队中的无人机从各自的起始点抵达同一目标点，飞行过程中需保持安全的飞行高度和规避环境中的禁飞区。本实施例公开的一种基于分解思想的高效多无人机协同航迹规划方法，具体实现步骤如下：步骤一、根据无人机飞行性能参数信息、航迹约束信息和任务环境信息，建立无人机协同航迹规划数学模型。设定无人机的最大转弯角为90°，最大爬升/俯冲角为8°，最小航迹段长度lmin＝1km。无人机的飞行起点/终点位置和任务环境中禁飞区的位置、半径如表1所列。最小相对地面飞行高度为50m。预设的安全距离ds＝100m。表1无人机和禁飞区信息无人机信息起点(km)终点(km)禁飞区信息位置(km)半径(km)无人机1(3.72,1.38,0.35)(8,4,0.055)禁飞区1[4.6,2.1]0.65无人机2(3.28,3.47,0.47)(8,4,0.055)禁飞区2[3.0,5.1]0.70无人机3(3.61,5.80,0.45)(8,4,0.055)禁飞区3[7.2,3.1]0.60无人机4(4.03,9.25,0.42)(8,4,0.055)禁飞区4[7.0,7.5]0.70禁飞区5[8.0,7.3]0.91禁飞区6[6.9,5.4]1.11建立的无人机协同航迹规划的数学模型如式(9)-(16)所示：无人机协同航迹规划的优化目标函数为机间避碰约束：最小航迹段长度约束：最大转弯角约束：最大爬升角/俯冲角约束：地形约束：禁飞区约束：步骤二、基于分解思想，将多无人机协同规划问题分解为单无人机逐次规划问题，减少耦合协同约束。对于单机规划问题，设计改进的高度沉降法，将三维航迹规划分解为水平规划和高度规划。水平规划时不考虑高度约束，以最小化航程为目标，基于cac-sas算法对每一架无人机依次进行二维航迹规划，得到水平面内多无人机协同飞行航迹。在规划第i架无人机的二维航迹时，需要将前i-1架无人机协同航迹作为输入信息。图3给出了最小化航程得到的各无人机航迹结果，航迹长度分别为6.32km，4.89km，5.18km和9.60km。因此，这四条航迹即为水平面内多无人机协同飞行航迹。cac-sas算法具体实现方法如下：步骤2.1、若当前规划第i架无人机，则需输入前i-1架无人机协同飞行航迹；步骤2.2、初始化open表和closed表。创建open表和closed表，同时将规划的起点插入open表，此时closed表为空。步骤2.3、判断open表是否为空。若open表为空，则搜索失败并结束搜索，要求用户修改算法参数后，重新启动算法；若open表非空，则执行步骤2.4。步骤2.4、更新当前节点。从当前的open表中取出代价值最小的节点作为新的当前节点，则open表中不再含有该节点，并将该节点放入closed表。步骤2.5、判断当前节点能否满足收敛条件。若当前节点能够在满足公式(11)至(16)所有约束的条件下到达规划的终点，则结束节点扩展循环，转而执行步骤2.9；若不收敛，则执行步骤2.6。步骤2.6、节点扩展。以当前节点为中心进行节点扩展，获得当前节点的子节点。水平扩展数为5。步骤2.7、机间避碰约束检测。根据公式(10)依次判断当前扩展航迹与步骤2.1的输入航迹是否发生碰撞，若发生碰撞，则舍弃当前扩展节点。步骤2.8、判断扩展子节点的可行性。根据公式(11)至(16)判断扩展子节点是否满足约束条件(无人机机动能力、地形和禁飞区)。计算所有可行节点的代价值，并将其存入open表中。然后执行步骤2.3。步骤2.9、创建目标节点，将规划的终点设置为目标节点，目标节点的父节点设置为当前节点，并将目标节点压入closed表。步骤2.10、反溯最终规划航迹。根据closed表中的目标节点和已扩展节点，利用节点间的扩展关系，从目标节点向上回溯直至起始节点，得到从起始点到目标点的航迹。步骤三、在水平航迹规划结果的基础上，利用改进的高度沉降法对每一架无人机进行高度规划。高度规划的具体实现方法如下：步骤3.1、生成地形高度剖面。计算与二维航迹对应的地形高度剖面，并确定地形高度剖面中的最大地形高度hmax＝450m。为了精确计算步骤三的3.1中所述与二维航迹对应的地形高度剖面，在计算航迹点处高度的同时，还需在航迹点中生成较密集的插值点用于生成高度剖面。插值密度根据期望的分辨率确定，文中的分辨率取为100m。所述生成插值点的方法为线性插值；步骤3.2、初始化航迹高度。将航迹起点与目标点的高度分别初始化为真实高度h0＝600m和hf＝600m。考虑最小相对地面飞行高度hmin＝50m的约束，将其它航迹点的高度全部初始化为h0、hf和hmax+hmin三者中的最大值600m。将航迹的起点与终点设置为不可沉降点。步骤3.3、计算航迹中的所有可沉降航迹点的沉降裕度。可沉降航迹点的沉降裕度取地形约束沉降裕度和爬升/俯冲约束沉降裕度中的最小值。地形约束沉降裕度为与航迹点相连两航迹段的最小相对地面高度。为了提高准确度，对航迹段进行线性插值，计算两航迹段的相对地面高度，从中选取最小值。爬升/俯冲约束下的沉降裕度，仅当航迹点的两相邻航迹点中有不可沉降点时才需考虑。根据当前航点与相邻不可沉降点间的水平距离和最大爬升/俯冲角，计算得到爬升/俯冲约束下的沉降裕度。步骤3.4、航迹沉降。选择所有航迹点沉降裕度的最小值作为航迹沉降值，降低所有可沉降航迹点的高度。将沉降裕度等于该最小值的可沉降航迹点设置为不可沉降航迹点。步骤3.5、判断航迹可沉降性。判断航迹中是否仍存在可沉降的航迹点，若存在，则转步骤3.3；否则，算法结束。则得到新的三维可行航迹。高度规划结果如图4所示。在高度规划结果图中，下方的曲线为二维航迹对应的地形高程剖面，上方的折线为航迹高度规划的结果。由图4可知，改进沉降法得到的航迹结果能在满足爬升/下滑约束和地形约束的条件下，保持较低的飞行高度，具有良好的地形跟随特性。步骤四、将单无人机逐次规划后可得到的多条三维可行航迹作为多无人机的协同飞行航迹。根据前述的二维航迹规划结果和高度规划结果分析，协同航迹规划结果满足无人机编队的机间避碰、禁飞区规避、地形和无人机机动能力等约束，是一组可行的无人机编队协同航迹，能够引导无人机编队安全抵达目标点。为检验本发明提出的协同航迹规划方法的效率，对上述协同航迹规划的时间进行统计，并与三维稀疏a*算法对比，结果如表2所示。表2协同航迹规划时间统计基于分解思想，利用本发明提出的航迹规划方法能够为无人机编队规划出满足约束的协同航迹。与三维稀疏a*算法对比，基于分解思想的协同航迹规划方法总耗时更短，能够满足无人机协同航迹规划的时效性需求。根据前述的多无人机协同航迹规划实例仿真结果与分析可见，本实施例所述的基于分解思想的高效多无人机协同航迹规划方法能够为多无人机提供满足实际约束的可行协同飞行航迹，航迹生成速度具有很高的效率，因此本发明具有很强的工程实用性，并且能够实现预期的发明目的。以上的具体描述，是对发明的目的、技术方案和有益效果的进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施实例，仅用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。当前第1页12