一种三维环境下多无人机同时到达指定地的路径规划方法与流程
本发明涉及无人机路径规划与控制相关技术领域，尤其涉及一种在三维环境下的多无人机同时到达指定地的路径规划方法。
背景技术：
无人机具有灵活性高、成本低、安全性高以及隐蔽性强等特点，这些特有的优越性能使无人机技术快速发展，逐渐成为世界前沿科技的代表性技术，被广泛应用于各种灾后救援、城市快递、地形勘探、环境监测等领域。无人机的航迹规划是影响无人机完成任务效能的重要技术；航迹规划指的是在考虑环境约束、无人机机动性能约束的前提下规划出从起始点至目标点的最优飞行路径，最优的航迹规划能够将无人机在整个航程中的总代价降到最低，权衡各类约束间的重要性，在此基础上最小化航迹长度，降低无人机的燃料代价，以最大程度提高无人机自身的续航能力与任务完成率；单架无人机因其势单力薄，作业范围小，容错率低等缺点，往往难以满足复杂任务的要求，因此，多无人机协同作业被越来越多地应用于现实生活；单架无人机执行任务时，若遇到突发故障将直接导致任务的失败；而多架无人机协同作业时，若某架无人机失去控制，仍有机群内的其它无人机继续执行任务，大大提高任务的完成率，多无人机机群比单无人机更适应于复杂多变的现实环境，将成为未来生产生活中的一种重要的协同作业样式。因此，多无人机协同作业已成为一个重要的研究方向；
三维环境下多无人机同时到达目的地的路径规划问题是指在满足某些约束条件(多无人机间避碰、避开障碍物、同时到达目标点、无人机动力学特性)的前提下，通过优化某些性能指标，为每架无人机分别规划出一条能够同时到达目标位置的可行且安全的路径；这种路径规划技术在多无人机集结进行抢险救灾、数据交换、跟踪监测等方面有非常典型的应用价值；
现有的技术中关于三维环境下无人机的协同路径规划多集中于几何方法生成路径或协同运动模型的建立，缺少三维环境下同时到达这一应用背景：sahingo等人提出了一种基于遗传算法(ga)的多无人机飞行路径规划方法，首先用气体法计算出一条可行路径，然后利用贝塞尔曲线对该路径进行平滑处理；madhavanshanmugavel等人研究了一组无人机的协同路径规划问题，使用具有clothoid弧的dubin路径为每个无人机生成路径；christophrasche等人提出了一种三维环境下的多无人机路径规划方法，为探索灾害区域提供了一种解决方案；本发明主要针对多无人同时到达目标点这一特定的应用场景，结合无人机动力学约束、安全性约束、地形条件约束提出一种新型且实用的多无人机协同路径规划方案。
技术实现要素：
为了解决多无人机集结并实现同时到达任务点的技术目标，尽可能的减少时间误差，本发明提供了一种基于遗传算法的三维环境下多无人机同时到达指定地的路径规划方法，使其能在满足约束的前提下，最小化飞行代价，找到从起始点到目标点的最优航迹。
为了解决上述技术问题，本发明是这样实现的：
一种三维环境下多无人机同时到达指定地的路径规划方法，该方法采用遗传算法实现路径规划，包括：
步骤1、建立种群：种群中每个染色体对应一个解；染色体表示一条综合航迹，所述综合航迹由所有无人机的航迹串接而成；航迹由一系列离散的航迹点组成，所述航迹点的三维坐标构成染色体中的基因编码；
步骤2、设定遗传算法的适应度由所有无人机总航迹长度l、不同无人机到达时间的差异度t以及航迹对所设定的无人机机动性能约束的违背度w进行综合评价；
步骤3、采用遗传算法优化染色体；在进行遗传算法的交叉操作时，仅允许属于同一无人机的航迹点进行交叉；完成遗传算法迭代后得到最优染色体；
步骤4、采用最优染色体分解得到各个无人机的航迹。
优选地，所述步骤3在执行遗传算法时，种群初始化过程为：
针对每一个无人机i，在二维平面内，以该无人机i对应的起始点ai与所有无人机共同的目标点b的连线为对称中线画一矩形，该矩形是无人机i的航迹规划区域；根据给无人机i设定的航迹点个数ni，将起始点ai与目标点b间的连线等分为ni份，进而将规划区域等分为ni个小矩形，每个小矩形是对应的单个航迹点的规划区域；在初始化航迹点时，在可飞行环境的约束下，于航迹点对应的规划区域内随机生成航迹点初始值，得到航迹点在二维平面下的坐标，再在可飞行环境的约束下随机生成航迹点的高度值，从而得到航迹点的初始三维坐标；按照起始点ai到目标点b的顺序生成ni个初始三维坐标构成无人机i的初始航迹；所有无人机的初始航迹串联后得到初始染色体；生成多个初始染色体，完成种群的初始化。
优选地，所述步骤3在进行遗传算法的变异操作时，对于需变异的航迹点，在该航迹点对应的所述规划区域内重新随机生成一个新的航迹点。
优选地，所述步骤3在执行遗传算法时，在变异操作之后进一步进行扰动操作；所述扰动操作仅对除起始点与目标点外的每个航迹点有效；
所述扰动操作包括：根据设定的扰动概率确定染色体是否执行扰动操作；如果判定需要执行，则根据扰动比例，从除起始点与目标点外的航迹点中选取部分扰动点施加微扰动；所述微扰动的扰动量为航迹点原值的1％-5％；对染色体进行扰动后计算新染色体的适应度，若大于扰动前的适应度，则用扰动后染色体取代扰动前染色体；否则，不进行替换。
优选地，所述扰动概率为1％-5％。
优选地，所述施加微扰动为：对于可行航迹的扰动较小，对于不可行航迹的扰动较大；所述可行航迹是指满足无人机机动性能约束的航迹，不满足则为不可行航迹。
优选地，所述步骤3在执行遗传算法的交叉操作和变异操作时，使用自适应交叉概率pc和自适应变异概率pm：
其中，k1，k2为常数，分别决定交叉与变异概率的最大值；fmax表示当前种群内染色体的最大适应度，favg表示当前种群的平均适应度，fc表示当前需要进行交叉操作的染色体的适应度，fm表示当前需要进行变异操作的染色体的适应度；k3与k4为设定的常数。
优选地，所述步骤2为：针对每条染色体，计算三个适应度分量e1、e2、e3，包括：
计算每个无人机的航迹长度，并求和得到l；l越短，则适应度分量e1越大；
计算每个无人机按照航迹点飞行到达目标点的时间；最长时间与最短时间之差即为不同无人机到达时间的最大差异度t；最大差异度t越小，则适应度分量e2越大；
计算每个无人机按照航迹点飞行过程中的实际无人机机动性能是否违背了相应的无人机机动性能约束；对于违背者，获取各约束违背量分量，将所有约束违背量分量分别归一化后加权求和，得到单个无人机的约束违背量；并针对所有无人机的约束违背量加权求和，获得总违背量；如果所有无人机均没有违背约束，则总违背量采用设定值；总违背量越小，则适应度分量e3越大；
将三个适应度分量e1、e2、e3进行归一化操作后再加权求和，获得本染色体的适应度。
优选地，获得所述各个无人机的航迹后，进一步利用三维dubins曲线进行连接，生成无人机的可飞行路径。
有益效果：
(1)本发明将多无人机的航迹优化问题合并为综合路径的规划问题，并设计了适合综合路径规划的适应度计算方法，从而获得飞行代价最小、约束违背量最少且能够同时到达的路径。
(2)本发明对初始化方法、交叉方案、变异方案的选取方案均进行了改进。其中，采用特殊的初始化方法，航迹点严格地由起始点朝目标点的方向依次生成，避免多余的飞行，航迹质量更高，大大缩减了搜索空间，显著提高算法的搜索效率；交叉操作要分别应用在不同无人机的航迹上，避免不同无人机之间进行交叉；变异操作在航迹点对应的规划区域内完成，减少变异带来了劣化。
(3)本发明增加了扰动操作。扰动使不可行航迹朝着可行方向进化，使可行航迹朝着代价更低的方向进化，对航迹起到纠错与修正的作用，还能够促进新染色体的生成，扩大种群范围，增强种群的多样性。
(4)本发明还增加了交叉变异概率的自适应更新方案，使适应度最高的染色体也具有一定的交叉与变异概率，能够更好地保持种群的多样性，防止早熟与局部收敛。
附图说明
图1为最大偏航角约束示意图；
图2为山峰建模效果图；
图3为禁飞区范围示意图；
图4为环境整体建模效果图；
图5为基因编码示意图；
图6为航迹初始化示意图；
图7为基于遗传算法的航迹规划算法流程图；
图8为三维dubins路径示意图；
图9为三架无人机产生等长度dubins路径示意图；
图10为三架无人机在存在障碍物和禁飞区地形中的路径示意图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
下面结合附图和实施例对本发明作详细说明；
本发明提供了一种三维环境下多无人机同时到达指定地的路径规划方法，该方法将多无人机的航迹优化问题合并为综合路径的规划问题，并设计了适合综合路径规划的适应度计算方法，从而获得飞行代价最小、约束违背量最少且能够同时到达的路径。
本发明的三维环境下多无人机同时到达指定地的路径规划方法具体步骤如下：
步骤1、建立各种约束和适应度函数评价方案。
所述约束包括无人机机动性能约束和飞行三维环境约束。本实施例中，无人机机动性能约束包括：最大偏航角约束、最大俯仰角约束、飞行高度约束、最远飞行距离约束。其中，如图1所示，最大偏航角约束是相邻航迹段在水平地面oxy内的投影向量间的夹角要小于最大偏航角θ，角的具体大小根据无人机的性能确定。最大俯仰角约束是航迹段与其在水平面oxy上投影间的夹角应小于最大俯仰角
飞行三维环境建模：如图2、图3、图4所示，主要包括山峰地形建模、禁飞区建模，以模拟无人机的实际飞行环境。禁飞区可表示为一种有理光滑型的分布模型。
本申请适应度函数评价方式采用三个适应度分量e1、e2和e3来表达。考虑所有无人机总航迹长度l、不同无人机到达时间的差异度t，以及航迹对所设定的无人机机动性能约束的违背度w。总航迹长度l越短，则对应的适应度分量e1越大；不同无人机到达时间的差异度t越小，则对应的适应度分量e2越大；无人机机动性能约束的违背度w总和越小，则适应度分量e3越大。
步骤2：运行基于遗传算法的多无人机航迹规划算法。该多无人机航迹规划算法的主干流程与现有遗传算法相同，本发明针对染色体构建方案、初始化方法、交叉方案、变异方案、交叉变异概率的选取方案、适应度计算方案均进行了改进。同时，增加了扰动步骤。如图7流程图所示，改进后的遗传算法包括如下步骤：
step1：按照特定的初始化方法对航迹进行初始化，生成初始种群；step2：计算种群内所有染色体的适应度值；step3：根据适应度的大小，采用锦标赛选择策略选取一定数量的染色体组成繁殖池；step4：计算繁殖池内的最大适应度与平均适应度，确定每个染色体的交叉与变异概率，根据概率对繁殖池内染色体进行交叉与变异操作。step5：对繁殖池内的染色体进行扰动操作，比较新染色体与原染色体的适应度大小，保留适应度大的染色体。step6：判断是否达到最大迭代次数，如果达到，则转入step8，否则进入下一步；step7：清空繁殖池，转入step2；step8：选择繁殖池内适应度最大的染色体为最优航迹。
下面针对本申请的改进部分，包括染色体构建方案、初始化方法、交叉方案、变异方案、交叉变异概率的选取方案、适应度计算方案、扰动操作进行详细描述。
(1)染色体构建方案：种群中的每条染色体表示问题的一个解，问题的一个解由多个无人机的飞行航迹共同组成，也就是说每条染色体表示一条综合航迹，所述综合航迹由所有无人机的航迹串接而成，每条飞行航迹由一系列离散的航迹点组成。例如，3个无人机航迹均包括3条航迹点，则串联后的综合航迹点包括顺序连接的d11、d12、d13、d21、d22、d23、d31、d32、d33。dij表示无人机i的第j个航迹点。
对基因编码则是对航迹点的编码。利用定长的实值基因编码方式对每条染色体进行编码，染色体的基因对应航迹点，基因编码是航迹点的三维坐标值，如图5所示。
(2)种群初始化：三维环境下的航迹规划具有较大的搜索空间，会造成搜索效率低，规划时间长等问题，为在缩减搜索空间的同时尽量不降低最优解的质量，本申请采用特殊的初始化方法对种群进行初始化。
使航迹点在根据一定规律划分的区域内随机生成，且之后对航迹点的变异操作也在规定区域内进行；针对每一个无人机i，在二维平面内，以该无人机i对应的起始点ai与目标点b的连线为对称中线画一矩形，矩形的长度等于起始点ai到目标点b间的距离大小，矩形的宽度可以为设定值，优选地可以也等于起始点ai到目标点b间的距离大小。该矩形是无人机i的航迹规划区域。根据对路径要求的精度高低，给无人机i设定航迹点个数ni。不同无人机的航迹点数可以不同或相同。将起始点ai与目标点间b的连线等分为ni份，以此为基础将规划区域等分为ni个小矩形，每个小矩形是对应的单个航迹点的规划区域。那么在初始化航迹点时，在飞行三维环境的约束下(即在可飞行范围的约束下)，每个航迹点在各自对应的规划区域内随机生成，得到航迹点在二维平面下的坐标，再在可飞行范围内随机生成航迹点的高度值，得到航迹点的三维坐标，作为初始三维坐标。按照起始点ai到目标点b的顺序，从ni个小矩形中生成共ni个初始三维坐标构成无人机i的初始航迹。针对每个无人机生成初始轨迹，拼接后，获得初始染色体。获得多个初始染色体，完成种群的初始化。用此方法对航迹进行初始化需要用到坐标变换。
如图6所示，在地面平面坐标系的基础上，以起始点s为原点，起始点朝向目标点g的方向为x轴方向，x轴逆时针旋转90°为y轴，建立新的坐标系。根据起始点与目标点间的距离长度以及航迹点的个数，可以确定每个航迹点横轴坐标值的随机范围，确定航迹点的矩形规划区域的长度，由此可知航迹点纵轴坐标值的随机范围。利用随机函数确定每个航迹点在新建立的二维坐标系下的坐标后，进行坐标变换，得到航迹点在地面平面坐标系下的坐标，根据山峰建模中储存的山峰地形信息，可得到每个航迹点所处位置的地形高度，使航迹点在无人机能够安全飞行的高度范围内随机生成高程值，满足一定的环境约束。若航迹点在整个规划空间内随机生成，连接各个航迹点后的整条航迹将会是杂乱无章的，而本申请使用的初始化方法使航迹点严格地由起始点朝目标点的方向依次生成，避免多余的飞行，航迹质量更高，大大缩减了搜索空间，显著提高算法的搜索效率。
(3)适应度函数的设计：在遗传算法中，适应度是评判种群染色体质量的唯一标准，应包含染色体的所有评判信息，包括约束条件与航迹代价函数。在满足约束条件的前提下，规划的航迹才是有效航迹。染色体适应度应该包括以下三个指标：
①无人机的总航迹长度l尽可能短；
②不同无人机到达目标点所需时间的最大差异度t尽可能小；
③航迹对所设定的无人机机动性能约束的违背量w尽可能小。
如前所述，总航迹长度l越短，则对应的适应度分量e1越大；不同无人机到达时间的差异度t越小，则对应的适应度分量e2越大；无人机机动性能约束的违背度w总和越小，则适应度分量e3越大。
综合考量上述因素，采用最简单的对应关系即倒数关系设计适应度函数f，可表述为：
其中，wi(i＝1,2,3)是各性能指标的权重值。适应度函数f中的三个分量依次对应三个适应度分量e1、e2、e3。在计算之前e1、e2、e3也要进行归一化处理。在实际中还可以采用其他对应关系实现l/t/w越小，e越大的对应。
此处说明l,t,w的计算方法：
l用来实现对无人机总航迹长度的评价。计算每个无人机的航迹长度，并求和得到，即
t用来评价无人机是否能够同时到达目标地。计算每个无人机按照航迹点飞行到达目标点的时间，则t＝max(ti)-min(ti),i＝1,2…n。其中，ti表示第i架无人机到达目标点所需的时间，max(ti)表示所有无人机采用时间ti的最大值，min(ti)表示所有无人机采用时间ti的最小值。
w用以表示航迹违背约束量的大小。计算每个无人机按照航迹点飞行过程中的实际无人机机动性能是否违背了相应的无人机机动性能约束；对于违背者，获取各约束违背量分量，将所有约束违背量分量分别归一化后加权求和，得到单个无人机的约束违背量；并针对所有无人机的约束违背量加权求和，获得总违背量；如果所有无人机均没有违背约束，则总违背量采用设定值。实现对违背约束条件的航迹段的评价，
其中，wi为第i架无人机的约束违背量。这里采用直接求和的方式，优选地，可以采用加权求和方式，获得总违背量w。μ是常数，δcos、δtan和δh均为本实施例所例举出的约束违背量分量，这里采用归一化后的直接求和方式，优选地，可以采用加权求和方式。δcos代表航迹对于最大偏航角的违背量大小，计算每相邻航迹段间偏航角的余弦值，与最大偏航角的余弦值作比较，若违背约束，则对二者做差得到航迹段偏航角违背量的大小，δtan代表航迹对于最大俯仰角的违背量大小，计算每条航迹段的俯仰角的正弦值，与最大俯仰角的正弦值作比较，若违背约束，则对二者做差得到航迹段俯仰角违背量的大小。δh代表航迹对于地形环境的违背量大小，描述的是航迹被地形阻碍的程度，对航迹段进行采样，通过计算采样航迹点处的高度与山峰高度的差值确定每个采样点的违背量。这些约束违背量分量取平均值，代表整个航迹段对于地形环境的违背量。
不同的约束违背量具有不同的数量级，使数量级较小的因子无法对函数值产生影响，失去该部分对于航迹规划的优化作用，因此需要对指标因子与约束违背量进行归一化处理，将其转化为同一数量级。本发明中所有归一化方案均可以采用最大最下值归一化方案。以航迹长度li为例，
(4)交叉算子的改进。针对航迹规划问题的具体操作方式为，将繁殖池中的航迹染色体随机两两配对，根据事先设定好的交叉概率决定每对航迹是否进行交叉操作，对于需要交叉的航迹对执行交叉操作，得到两条新的航迹染色体。由于一条染色体由多条无人机的航迹共同组成，所以交叉操作要分别应用在不同无人机的航迹上，避免不同无人机之间进行交叉。因此本发明需要限定，在进行遗传算法的交叉操作时，仅允许属于同一无人机的航迹点进行交叉。
(5)变异算子。针对航迹规划问题的具体操作方式为：对于种群内的每条染色体，根据事先设定好的变异概率决定是否对其进行变异操作。对于选中的染色体，随机选取某一航迹点，即变异点，本发明改进点在于，在该航迹点对应的规划区域(即小矩形)内重新随机生成一个新的航迹点，代替原来的航迹点，完成基因变异。同理，同一染色体要对不同无人机代表的航迹分别进行变异。
(6)扰动算子。该扰动算子的使用处于变异操作之后。扰动算子仅对除起始点与目标点外的每个航迹点起作用。扰动操作具体为：根据设定的扰动概率确定染色体是否执行扰动操作，扰动概率设定为一个小概率，从而增加种群多样性，改善不可行解，该小概率可以选择1％-5％。如果判定需要执行，则根据扰动比例，从除起始点与目标点外的航迹点中选取部分扰动点施加微扰动。施加微扰动的方式可以是针对每个方向的坐标均施加相同的扰动量。也可以根据航迹是否可行来确定扰动量大小，对于可行航迹的扰动较小，对于不可行航迹的扰动较大；所述可行航迹是指满足无人机机动性能约束的航迹，不满足则为不可行航迹。微扰动的扰动量可以设为航迹点原值的1％-5％。对染色体进行扰动后计算新染色体的适应度，若大于扰动前的适应度，则用新染色体取代原来的染色体；若染色体扰动后的适应度低于扰动前，则保留原染色体，不进行替换。扰动使不可行航迹朝着可行方向进化，使可行航迹朝着代价更低的方向进化，对航迹起到纠错与修正的作用，还能够促进新染色体的生成，扩大种群范围，增强种群的多样性。
(7)遗传算子的改进。遗传算法存在容易因种群失去多样性而过早收敛的缺点，为克服这一缺点，采用自适应的遗传算法进行航迹规划，改进了一种常用的概率计算方法，它会根据自身适应度与种群平均适应度大小调整交叉概率与变异概率，防止早熟收敛。因此在执行遗传算法的交叉操作和变异操作时，使用自适应交叉概率pc和自适应变异概率pm如下：
其中，pc为自适应交叉概率，pm为自适应变异概率，k1，k2为常数，分别决定交叉与变异概率的最大值。fmax表示当前种群内染色体的最大适应度，favg表示当前种群的平均适应度，fc表示当前需要进行交叉操作的染色体的适应度，fm表示当前需要进行变异操作的染色体的适应度；pc与pm在原有的基础上加上k3与k4，使适应度最高的染色体也具有一定的交叉与变异概率，能够更好地保持种群的多样性，防止早熟与局部收敛。如果去掉k3与k4，当fc＝fmax时，pc、pm为0，如果这样，在进化初期，适应度最大的染色体几乎不会进行变异，代代会被保留，这样种群很快会收敛，因为最强势的个体会对种群的进化方向造成很大影响。加上k3与k4可以避免最大适应度个体不发生交叉变异的问题。
步骤3：通过前面步骤2的遗传算法得到的最优染色体包含了所有无人机的航迹，按照无人机拼接方案进行反向拆分，从而得到各无人机的航迹。
步骤4：可飞行航迹处理。步骤3得到的无人机的航迹是若干条折线段，指导多无人机的飞行，虽然该航迹满足无人机自身机动性能约束，但仍是多条不可飞行航迹。考虑无人机自身转弯速率的限制，无人机在转向过程中具有最大曲率约束，航机上的任意一点都要满足此约束，该航迹才是可飞行的。为了在起始位姿点与终止位姿点间产生可飞行路径，如图8，设计并采用三维dubins路径连接遗传算法所得到的航迹点，预期可产生如图9的平滑可飞的路径。
下面举个实例：
为验证三维环境下多无人机同时到达指定地的路径规划方法的有效性，本实施例使用matlab编程环境，相关参数设置如下：
种群大小：100；最大迭代次数：100；自适应交叉概率最大值/最小值：0.7/0.1；自适应变异概率最大值/最小值：0.25/0.05；无人机个数：3；航迹段数：5；最大偏航角：60°；最大俯仰角：30°；最低飞行高度：100m；最小转弯半径：20m；最高飞行速度：200km/h；最低飞行速度：50km/h；在有地形约束与禁飞区威胁的500km×500km×2000m的三维环境下进行航迹规划，设置三架无人机的起始点坐标分别为(205,59,1783)、(110,122,594)、(495,480,1133)，
目标点坐标为(188,209,750)，得到三维航迹规划效果如图10。可以看出，在有地形约束与禁飞区威胁的情况下，三维航迹规划在不与地形发生碰撞的前提下向所受威胁小、路径长度短、飞行高度低的方向进化，同时在固定无人机速度的前提下，多条航迹段总长度误差不超过1m。
以上公开的实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于所述的实施例；通过以上所述可知，本发明中的许多内容可作修改和替换，本实施例固定了某些参数取值只是为了更好地说明本发明的原理和应用，从而更易理解和运用；凡在本发明的技术方案的基础上所做的局部改动、等同替换、改进等均应包含在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！