CN113220008B - 多火星飞行器的协同动态路径规划方法 - Google Patents

Abstract

本发明公开了一种多火星飞行器的协同动态路径规划方法，属于火星探测技术领域。本发明首先使用离线协同路径规划方法，确定火星飞行器在已知威胁环境中的离线协同路径，实现指定区域覆盖与对已知威胁的离线避障；其次，确定火星飞行器探测到事先未知威胁的条件、局部动态路径规划的起始点和终点；最后使用局部动态路径规划方法，确定火星飞行器对新威胁的局部动态避障路径，并将所得离线协同路径和局部动态路径进行合并，得到火星飞行器最终飞行路径。本发明提供一种既能离线协同搜索探测，又能局部动态避障的多火星飞行器路径规划方法，满足火星飞行器探测任务的需求，具有较好的工程应用价值。

Description

多火星飞行器的协同动态路径规划方法

技术领域

本发明涉及一种飞行器路径规划领域的方法，尤其涉及一种多火星飞行器的协同动态路径规划方法。

背景技术

火星作为太阳系中最类似地球的行星，一直吸引着人类探测器的不断来访，到目前为止，火星是除地球外人类了解最多的行星。

目前的火星探测器主要是轨道器和火星车。轨道器的探测范围大，但是无法做细致的探测；火星车探测精度较高，但是由于其移动范围的限制，其探测能力有限。因为火星表面存在着稀薄的大气，火星飞行器的出现较好地解决了探测范围和探测精度的矛盾，填补了天基与路基探测之间的空白。使用火星飞行器执行火星探测任务已经成为各航天强国密切关注的下一代理想火星探索方式，如美国的“Mars2020”任务成功试飞了人类第一架地外天体动力航空器“机智号”。

随着火星探测的深入、任务多样性程度的增加，使用单飞行器的探测范围和探测效率有限。而如果使用多架火星飞行器有效地协同探测，则可以提高执行任务的效率与灵活性。

火星地形环境复杂，有着巨大的环形山和陡峭的山谷，同时火星大气环境恶劣，大气中存在着龙卷风、热气流、沙尘暴等高机动、时间敏感类威胁区域，这给火星飞行器的探测造成了困难。为了保证火星飞行器的探测任务安全完成，需要一种多火星飞行器的协同动态路径规划方法，实现探测与避障的结合。该方法采用离线协同覆盖搜索路径规划，使多火星飞行器的路径覆盖给定火星表面区域，同时对火星轨道器探测到的危险区进行离线避障。而由于轨道器探测精度的限制，某些突发危险区只能在火星飞行器的飞行途中被探测到，当火星飞行器探测到事先未知的威胁区域时，采用局部动态路径规划，实现避障功能。

发明内容：

发明目的：本发明提供一种既能离线协同搜索探测，又能局部动态避障的多火星飞行器路径规划方法，满足火星飞行器探测任务的需求，具有较好的工程应用价值。

技术方案：本发明所述的一种多火星飞行器的协同动态路径规划方法，包括以下步骤：

(1)使用多火星飞行器的离线协同路径规划方法，确定火星飞行器在已知威胁环境中的离线协同路径；

(2)确定火星飞行器探测到突发危险区的条件、局部动态路径规划的起始点和终点；

(3)使用火星飞行器的局部动态路径规划方法，确定火星飞行器对于新危险区的局部动态路径；并将所得的离线协同路径和局部动态路径进行合并，得到火星飞行器的最终飞行路径。

可选的，所述步骤(1)所述的离线协同路径规划方法，包括以下步骤：

(11)建立地图模型，并根据飞行器初始位置构建生成树；

(12)根据所步骤(11)所构建的生成树设置期望路径；

(13)为集群中各飞行器分配期望路径。

可选的，所述步骤(11)包括以下步骤：

(111)根据飞行器探测半径R_UAV对任务区域进行栅格化，分别以4R_UAV和2R_UAV对待探测区域进行栅格化，分别记录得到的各大、小栅格中心点的坐标，若栅格中心点位于某一危险区内，则标记该栅格为不可行；

(112)对N_UAV个飞行器分别各自生成N_UAV段子树，根据各飞行器发初始位置，确定生成树每段子树的起始点位置，子树的起始点位置即为飞行器所处大栅格的中心点，若栅格中心位于某一危险区内，则标记该栅格为不可行；

(113)逐格添加子树，以当前位置为中心判断上下左右四个方向相邻栅格是否可行，并计算与各相邻栅格的曼哈顿距离，所述曼哈顿距离是指当前位置与其他所有飞行器x、y方向距离之和，按照曼哈顿距离最大的原则逐格添加子树，当曼哈顿距离相同时，按照右>上>左>下的优先级顺序进行添加；

(114)如果当前位置周边相邻栅格均不可添加(即已被添加入其他子树中，或标记不可行区域)，则终止该子树的生成；

(115)寻找所有可行线段，将各子树相互连接成一条完整但不闭合的树，并在所有可行线段中遍历寻优，以最终确定生成树的形状。

可选的，所述步骤(12)包括以下步骤：

(121)以各大栅格中心点为中心，小栅格中心点为顶点，构建一系列中栅格

并按照步骤(11)中生成树的节点添加顺序记录其对应中栅格组合顺序

同时将每一中栅格四个顶点按方位编号，如标记为A(i)，B(i)，C(i)，D(i)；

(122)每一段期望路径的起点为飞行器初始位置所处的小栅格中心，即对应某一中栅格的方位编号；

(123)以当前位置依次添加下一节点位置，当判断无拐角时，方位编号中方位不变、编号递增，即字母顺序不变，数字递增，如A(1)→A(2)；当判断出生成树在下一节点处将产生拐角时，则还应根据拐角方向对应改变中栅格方位编号中的方位，即改变顶点字母，如产生逆时针90°拐角，则顶点字母逆时针变换90°，A(1)→B1。

可选的，步骤(2)所述火星飞行器探测到突发危险区的条件为：

r_t≤R_UAV

其中，r_t为飞行器距未知危险区的距离，R_UAV为飞行器自带传感器的最大探测半径；

局部动态路径规划的起点为，火星飞行器沿着离线协同路径飞行时首次探测到事先未知的新危险区，并且若继续沿着离线路径飞行，则将要穿过新危险区时，火星飞行器所处的坐标；

局部动态路径规划的终点为，火星飞行器避开新危险区后首次探测不到该危险区时，火星飞行器所处的坐标。

可选的，所述步骤(3)包括以下步骤：

(31)使用人工势场法进行局部动态路径规划，得到初步的局部动态路径；

(32)检测该路径是否到达了局部动态路径规划的终点，如果初步的局部动态路径到达了终点，则将初步的局部动态路径作为最终的局部动态路径规划的结果；如果初步的局部动态路径陷入了局部最优陷阱，没有到达终点，则重新换用虚拟目标点法，使火星飞行器逃离局部最优陷阱，然后将虚拟目标点当作新的局部动态规划的起点，继续使用人工势场法进行局部动态路径规划，直到飞行器到达局部动态路径的终点；

(33)将离线路径中局部动态路径规划的起点和终点之间的路径点移除，并用局部动态路径与之拼接，得到最终的飞行路径。

可选的，所述步骤(31)包括以下步骤：

(311)计算目标点对飞行器产生的引力场U_att、危险区对飞行器产生的斥力场U_rep以及势场和U：

U＝U_att+U_rep

其中：X_g＝(x_g,y_g)为目标点的坐标；X＝(x,y)为飞行器的坐标；

为飞行器到目标点的欧几里得距离；k为引力增益系数；U_att为目标点对飞行器产生的力场；m为斥力增益系数；d为飞行器与危险区的距离；d₀为危险区的影响距离；d_a为飞行器与目标点之间的距离；U_rep为危险区对飞行器产生的引力场；

(312)计算目标点对飞行器产生的引力F_att、危险区对飞行器产生的斥力F_rep以及合力F：

其中：分量F_rep1由危险区指向飞行器；分量F_rep2由飞行器指向目标点。

可选的，所述步骤(32)包括以下步骤：

(321)给定相关参数：安全距离r_s、预测距离r_p；

(322)连接当前点和目标点，得到一条直线，判断各危险区到此直线的距离d；

(323)如果危险区在目标点一侧，同时满足危险区i到直线的距离小于安全距离r_s，且危险区i到起始点的距离最短，执行步骤(324)；否则，沿当前方向前进，直至到达终点；

(324)判断步骤(323)中危险区i到当前点的距离，如果小于预测距离r_p，执行步骤(325)；否则，沿当前方向前进，直至小于预测距离r_p；

(325)以危险区i与当前点的距离为半径向危险区少的一边旋转，每次旋转3°，直到目标点一侧所有危险区到新的方向所在直线的距离大于安全距离r_s，则以此时的方向为新的前进方向，半径的另一端点作为新的目标点，并沿新的方向前进；

(326)判断是否到达新的目标点，如果到达新的目标点，则跳到步骤(322)，直至到达终点；否则，沿当前方向继续前进，直至到达新的目标点。

可选的，所述步骤(321)包括：

通过鸽群优化(PIO)算法，对预测距离r_p的大小进行优化，确保飞行器能避开局部最优陷阱，以及获得最小的代价：

首先对鸽群优化算法中的种群数量、迭代次数等信息进行初始化，并确定适应度值的计算方法：

J＝J_length+J_angle

其中，(x,y)为当前点的坐标；(x_i,y_i)为步骤(333)中的危险区i的圆心坐标；J_length为虚拟目标点法产生的路径长度代价；

为从当前点指向危险区i的向量；

为从当前点指向虚拟目标点的向量；J_angle为虚拟目标点法产生的航向角代价；J为虚拟目标点法产生的总代价。

同时，随机初始化鸽群的速度与位置，第i只鸽子的位置X_i、速度V_i可表示为：

X_i＝[X_i(1) X_i(2) ... X_i(n)]^T

V_i＝[V_i(1) V_i(2) ... V_i(n)]^T

算法前期利用地图罗盘算子迭代更新，第i只鸽子在第t次迭代中的速度为V_i(t)，位置为X_i(t)，当前鸽群中最优个体的速度为V_g，位置为X_g，迭代更新方法为：

V_i(t)＝V_i(t-1)·exp(-Rt)+r·(X_g-X_i(t-1))

X_i(t)＝X_i(t-1)+V_i(t)

其中，R为地磁因子，r为[0,1]上均匀分布的随机数；

算法后期利用地标算子迭代更新，第i只鸽子在第t次迭代中的位置为X_i t，当前鸽子的中心位置为X_c，更新后根据适应度值的大小将所有鸽子进行排序，每次迭代淘汰掉一半的鸽子，迭代更新方法为：

X_i(t)＝X_i(t-1)+r·(X_c-X_i(t-1))

式中，N_P(t)为第t次迭代后鸽子种群数量，fitness为适应度函数。

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

1、该方法不仅考虑火星飞行器进行协同搜索，覆盖给定的火星表面区域，而且对于飞行过程中检测到的危险区进行动态避障。

2、该方法采用鸽群优化算法，对人工势场法的虚拟目标点进行优化，较好地防止火星飞行器陷入局部最优陷阱。

附图说明

图1为本发明的流程图；

图2为协同离线路径规划的流程图

图3为局部动态路径规划的流程图。

具体实施方式

以下实施例采用具体示例说明解释本发明的技术方案，根据下述实施例，可以更好的理解本发明。然而，本领域的技术人员容易理解，实施例所描述的具体的技术方案及其结果仅用于说明本发明，而不应当也不会限制权利要求书中所详细描述的本发明。

下面结合附图对本发明作进一步描述：

图1为本发明的流程图，一种多火星飞行器的协同动态路径规划方法，包括如下步骤：

(1)、使用多火星飞行器的离线协同路径规划方法，确定火星飞行器在已知威胁环境中的离线协同路径。

(11)建立地图模型，并根据飞行器初始位置构建生成树。

首先应对任务区域进行初步建模，任务区域的划分应尽量以飞行器探测直径的偶数倍作为矩形边长，如不符合，应在可接受的范围内适当减小模型中R_UAV，以使后续栅格化步骤顺利进行。

(111)根据飞行器的探测半径R_UAV为标准对任务区域进行栅格化：首先以4R_UAV为边长对待探测区域进行初步栅格化，得到一系列大栅格，分别记录其顶点坐标；随后再以2R_UAV为边长对待探测区域进一步栅格化，分别记录得到的各大、小栅格中心点的坐标，若栅格中心点位于某一危险区内，则标记该栅格为不可行。

(112)生成树由各子树连接而成，对N_UAV个飞行器分别各自生成N_UAV段子树：根据各飞行器的初始位置，确定生成树每段子树的起始点位置，子树的起始点位置即为飞行器所处大栅格的中心点，且生成树各节点均为大方格中心点。

(113)自起点始，逐格添加子树，以当前位置为中心判断上下左右四个方向相邻大栅格是否可行，并计算各相邻栅格的曼哈顿距离的大小(与其他所有飞行器x、y方向距离之和)，按照曼哈顿距离最大的原则逐格添加子树，当曼哈顿距离相同时，按照某一约定的顺序对各栅格进行一次添加，此处以右>上>左>下的优先级顺序为例进行添加。

(114)如果当前位置周边相邻栅格均不可添加(即已被添加入其他子树中或因其他原因如禁飞区等被标记不可行)，则终止该子树的生成。

(115)寻找每一组可行线段，将各子树相互连接成一条完整但不闭合的树，并在所有可行线段中遍历寻优，以最终确定生成树的形状。

(12)期望路径的定义为一条包覆在生成树外侧的封闭路径，具体构建步骤如下：

①以各大栅格中心点为中心，小栅格中心点为顶点，构建一系列中栅格

并按照步骤(11)中生成树的节点添加顺序记录其对应中栅格组合顺序

同时将每一中栅格四个顶点按方位编号，在本实施例中，按照顺时针顺序依次标记为A(i)，B(i)，C(i)，D(i)，分别对应当前笛卡尔坐标系下中栅格的左上、右上、右下、左下位置顶点；

②每一段期望路径的起点为飞行器初始位置所处的小栅格中心，即对应某一中栅格的方位编号即顶点字母，并记录该顶点标记字母，同时以当前位置依次添加下一节点位置，此时需判断下一节点处是否产生拐角，判断方法即为预生成的期望路径是否与生成树相交，产生交点即为抵达拐角；

③根据步骤②的拐角检测结果，判断无拐角时，方位编号中方位不变、编号递增，在本实施例中，即为字母顺序不变，数字递增，如A(1)→A(2)；如判断出生成树在下一节点处将产生拐角，则还应根据拐角方向对应改变中栅格的方位编号中的方位，在本实施例中，即为改变顶点字母，如产生顺时针90°拐角，则顶点字母顺时针变换90°，A(1)→B1；

④按照飞行器位置与期望路径形状分配期望路径。一般情况下按照各飞行器均匀探测时间最优的原则分配期望路径，即各段期望路径长度相同，当某一飞行器所分配的期望路径长度大于相邻某飞行器所分配期望路径长度的二倍时，判断应产生“回溯”，即较短路径对应的飞行器应在抵达远点后掉头返回，协助较长路径对应的飞行器进行探测，以达到协同探测时间最短的要求。

(2)、确定火星飞行器探测到事先未知威胁(即突发危险区)的条件、局部动态路径规划的起始点和终点。

飞行器探测到突发危险区的条件为：

r_t≤R_UAV

其中，r_t为飞行器距突发危险区的距离，R_UAV为飞行器自带传感器的最大探测半径。

局部动态路径规划的起点为，火星飞行器沿着离线协同路径飞行时首次探测到事先未知的新危险区，并且若继续沿着离线路径飞行，则将要穿过新危险区时，火星飞行器所处的坐标。

局部动态路径规划的终点为，火星飞行器避开新危险区后首次探测不到该危险区时，火星飞行器所处的坐标。

(3)、使用火星飞行器的局部动态路径规划方法，确定火星飞行器对于新危险区的局部动态路径，并将所得的离线路径和局部动态路径进行合并，得到火星飞行器的最终飞行路径。

(31)使用人工势场法进行局部动态路径规划，得到初步的局部动态路径。

人工势场法是一种虚拟力法，它将飞行器的运动过程视为在虚拟力场中的运动，目标点对飞行器产生引力，危险区对飞行器产生斥力，引力和斥力的合力控制飞行器的运动方向，飞行器在空间中任意位置的运动方向是由目标点的引力场和危险区的斥力场共同合成的总场强的方向决定。

(311)计算目标点对飞行器产生的引力场U_att、危险区对飞行器产生的斥力场U_rep以及势场和

U＝U_att+U_rep

其中：X_g＝(x_g,y_g)为目标点的坐标；X＝(x,y)为飞行器的坐标；

为飞行器到目标点的欧几里得距离；k为引力增益系数；U_att为目标点对飞行器产生的力场；m为斥力增益系数；d为飞行器与危险区的距离；d₀为危险区的影响距离；d_a为飞行器与目标点之间的距离；U_rep为危险区对飞行器产生的引力场。

(312)计算目标点对飞行器产生的引力F_att、危险区对飞行器产生的斥力F_rep以及合力F，引力F_att和斥力F_rep分别为引力场和斥力场的负梯度：

其中：分量F_rep1由危险区指向飞行器；分量F_rep2由飞行器指向目标点。

飞行器受到的合力F＝F_att+F_rep决定了飞行器的运动方向。正常情况下，飞行器将在合力的作用下到达目标点，但由于人工势场法自身的缺陷，存在局部最优点导致无法到达目标点的问题。局部最优点是飞行器移动过程中的某一点，目标点对飞行器的引力和危险区对目标点的斥力的合力为零，受力平衡导致飞行器在该点徘徊不前。

由于火星表面地形环境复杂，当火星飞行器面对复杂的危险区(如U型危险区)时，按照传统的人工势场法飞行，容易被危险区环绕，无法逃出，导致无法到达目标点。所以得到初步局部动态路径后还需要进行以下步骤：

(32)检测该路径是否到达了局部动态路径规划的终点，如果初步的局部动态路径到达了终点，则将初步的局部动态路径作为最终的局部动态路径规划的结果；如果初步的局部动态路径陷入了局部最优陷阱，没有到达终点，则重新换用虚拟目标点法，使火星飞行器逃离局部最优陷阱，然后将虚拟目标点当作新的局部动态规划的起点，继续使用人工势场法进行局部动态路径规划，直到飞行器到达局部动态路径的终点。具体包括以下步骤：

Step1:给定相关参数：安全距离r_s、预测距离r_p；

Step2:连接当前点和目标点，得到一条直线，判断各危险区到此直线的距离d；

Step3:如果危险区在目标点一侧，同时满足危险区i到直线的距离小于安全距离r_s，且危险区i到起始点的距离最短，执行Step4；否则，沿当前方向前进，直到到达终点；

Step4：判断Step3中危险区i到当前点的距离，如果小于预测距离r_p，执行Step5；否则，沿当前方向前进，直至小于预测距离r_p；

Step5：以危险区i与当前点的距离为半径向危险区少的一边旋转，每次旋转3°，直到目标点一侧所有危险区到新的方向所在直线的距离大于安全距离r_s，则以此时的方向为新的前进方向，半径的另一端点作为新的目标点，并沿新的方向前进；

Step6：判断是否到达新的目标点，如果到达新的目标点，则跳到Step2，直至到达终点；否则，沿当前方向继续前进，直至到达新的目标点；

由于预测距离r_p的选择会影响虚拟目标点法的成败，以及路径的性能。r_p选择过小会导致飞行器进入复杂危险区内部后再掉头返回，导致航向角代价过大，甚至规划失败；r_p选择过大会导致飞行器过早进行虚拟目标点法规划，导致路径长度代价过大。所以上述Step1采用鸽群优化(PIO)算法对r_p的大小进行优化，确保飞行器能避开局部最优陷阱，以及获得最小的代价：

首先对鸽群优化算法中的种群数量、迭代次数等信息进行初始化，并确定适应度值的计算方法：

J＝J_length+J_angle

其中，(x,y)为当前点的坐标；(x_i,y_i)为步骤(333)中的危险区i的圆心坐标；J_length为虚拟目标点法产生的路径长度代价；

为从当前点指向危险区i的向量；

为从当前点指向虚拟目标点的向量；J_angle为虚拟目标点法产生的航向角代价；J为虚拟目标点法产生的总代价。

同时，随机初始化鸽群的速度与位置，第i只鸽子的位置X_i、速度V_i可表示为：

X_i＝[X_i(1) X_i(2) ... X_i(n)]^T

V_i＝[V_i(1) V_i(2) ... V_i(n)]^T

算法前期利用地图罗盘算子迭代更新，第i只鸽子在第t次迭代中的速度为V_i(t)，位置为X_i(t)，当前鸽群中最优个体的速度为V_g，位置为X_g，迭代更新方法为：

V_i(t)＝V_i(t-1)·exp(-Rt)+r·(X_g-X_i(t-1))

X_i(t)＝X_i(t-1)+V_i(t)

其中，R为地磁因子，其值视具体问题而定，r为[0,1]上均匀分布的随机数；

算法后期利用地标算子迭代更新，第i只鸽子在第t次迭代中的位置为X_i t，当前鸽子的中心位置为X_c，更新后根据适应度值的大小将所有鸽子进行排序，每次迭代淘汰掉一半的鸽子，迭代更新方法为：

X_i(t)＝X_i(t-1)+r·(X_c-X_i(t-1))

式中，N_P(t)为第t次迭代后鸽子种群数量，fitness为适应度函数。

综上所述，本发明实施例提供了一种多火星飞行器的协同动态路径规划方法，首先使用离线协同路径规划方法，确定火星飞行器在已知威胁环境中的离线协同路径，实现指定区域覆盖与对已知威胁的离线避障；其次，确定火星飞行器探测到事先未知威胁的条件、局部动态路径规划的起始点和终点；最后使用局部动态路径规划方法，确定火星飞行器对新威胁的局部动态避障路径，并将所得离线协同路径和局部动态路径进行合并，得到火星飞行器最终飞行路径。本发明实施例提供一种既能离线协同搜索探测，又能局部动态避障的多火星飞行器路径规划方法，满足火星飞行器探测任务的需求，具有较好的工程应用价值。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种多火星飞行器的协同动态路径规划方法，其特征在于，包括以下步骤：

(1)使用多火星飞行器的离线协同路径规划方法，确定火星飞行器在已知威胁环境中的离线协同路径；

(2)确定火星飞行器探测到突发危险区的条件、局部动态路径规划的起始点和终点；

(3)使用火星飞行器的局部动态路径规划方法，确定火星飞行器对于突发危险区的局部动态路径；并将所得的离线协同路径和局部动态路径进行合并，得到火星飞行器的最终飞行路径，包括以下步骤：

(31)使用人工势场法进行局部动态路径规划，得到初步的局部动态路径；

(32)检测该路径是否到达了局部动态路径规划的终点，如果初步的局部动态路径到达了终点，则将初步的局部动态路径作为最终的局部动态路径规划的结果；如果初步的局部动态路径陷入了局部最优陷阱，没有到达终点，则重新换用虚拟目标点法，使火星飞行器逃离局部最优陷阱，然后将虚拟目标点当作新的局部动态规划的起点，继续使用人工势场法进行局部动态路径规划，直到飞行器到达局部动态路径的终点，包括以下步骤：

(321)给定相关参数：安全距离r_s、预测距离r_p，包括：

通过鸽群优化算法，对预测距离r_p的大小进行优化，确保飞行器避开局部最优陷阱，以及获得最小的代价：

首先对鸽群优化算法中包括但不限于种群数量、迭代次数在内的信息进行初始化，并确定适应度值的计算方法：

J＝J_length+J_angle

其中，(x,y)为飞行器的当前坐标；(x_i,y_i)为危险区i的圆心坐标；J_length为虚拟目标点法产生的路径长度代价；

为从当前点指向危险区i的向量；

为从当前点指向虚拟目标点的向量；J_angle为虚拟目标点法产生的航向角代价；J为虚拟目标点法产生的总代价；

同时，随机初始化鸽群的速度与位置，第i只鸽子的位置X_i、速度V_i可表示为：

算法前期利用地图罗盘算子迭代更新，第i只鸽子在第t次迭代中的速度为V_i(t)，位置为X_i(t)，当前鸽群中最优个体的速度为V_g，位置为X_g，迭代更新方法为：

V_i(t)＝V_i(t-1)·exp(-Rt)+r·(X_g-X_i(t-1))

X_i(t)＝X_i(t-1)+V_i(t)

其中，R为地磁因子，r为[0,1]上均匀分布的随机数；

算法后期利用地标算子迭代更新，第i只鸽子在第t次迭代中的位置为X_i(t)，当前鸽子的中心位置为X_c，更新后根据适应度值的大小将所有鸽子进行排序，每次迭代淘汰掉一半的鸽子，迭代更新方法为：

X_i(t)＝X_i(t-1)+r·(X_c-X_i(t-1))

式中，N_P(t)为第t次迭代后鸽子种群数量，F为适应度函数；

(322)连接当前点和目标点，得到一条直线，判断各危险区到此直线的距离d；

(323)如果危险区在目标点一侧，同时满足危险区i到直线的距离小于安全距离r_s，且危险区i到起始点的距离最短，执行步骤(324)；否则，沿当前方向前进，直至到达终点；

(324)判断步骤(323)中危险区i到当前点的距离，如果小于预测距离r_p，执行步骤(325)；否则，沿当前方向前进，直至小于预测距离r_p；

(325)以危险区i与当前点的距离为半径向危险区少的一边旋转，每次旋转3°，直到目标点一侧所有危险区到新的方向所在直线的距离大于安全距离r_s，则以此时的方向为新的前进方向，半径的另一端点作为新的目标点，并沿新的方向前进；

(326)判断是否到达新的目标点，如果到达新的目标点，则跳到步骤(322)，直至到达终点；否则，沿当前方向继续前进，直至到达新的目标点；

(33)将离线路径中局部动态路径规划的起点和终点之间的路径点移除，并用局部动态路径与之拼接，得到最终的飞行路径。

2.根据权利要求1所述的一种多火星飞行器的协同动态路径规划方法，其特征在于，步骤(1)中所述的离线协同路径规划方法，包括以下步骤：

(11)建立地图模型，并根据飞行器初始位置构建生成树；

(12)根据所步骤(11)所构建的生成树设置期望路径；

(13)为各飞行器分配期望路径。

3.根据权利要求2所述的一种多火星飞行器的协同动态路径规划方法，其特征在于，所述步骤(11)包括以下步骤：

(111)根据飞行器自带传感器的最大探测半径R_UAV对任务区域进行栅格化，分别以4R_UAV和2R_UAV为边长对待探测区域进行栅格化，分别记录得到的各大、小栅格中心点的坐标，若栅格中心点位于某一危险区内，则标记该栅格为不可行；

(112)对N_UAV个飞行器分别各自生成N_UAV段子树，根据各飞行器的初始位置，确定生成树每段子树的起始点位置，子树的起始点位置即为飞行器所处大栅格的中心点；

(113)逐格添加子树，以当前位置为中心判断上下左右四个方向相邻栅格是否可行，并计算与各相邻栅格的曼哈顿距离，所述曼哈顿距离是指当前位置与其他所有飞行器x、y方向距离之和，按照曼哈顿距离最大的原则逐格添加子树，当曼哈顿距离相同时，按照右>上>左>下的优先级顺序进行添加；

(114)如果当前位置周边相邻栅格均不可添加，则终止该子树的生成；

(115)寻找所有可行线段，将各子树相互连接成一条完整但不闭合的树，并在所有可行线段中遍历寻优，以最终确定生成树的形状。

4.根据权利要求2所述的一种多火星飞行器的协同动态路径规划方法，其特征在于，所述步骤(12)包括以下步骤：

(121)以各大栅格中心点为中心，小栅格中心点为顶点，构建一系列中栅格N_g3，并按照步骤(11)中生成树的节点添加顺序记录其对应中栅格组合顺序N_g3(i)，同时将每一中栅格四个顶点按方位编号；

(122)每一段期望路径的起点为飞行器初始位置所处的小栅格中心，即对应某一中栅格的方位编号；

(123)以当前位置依次添加下一节点位置，当判断无拐角时，方位编号中方位不变、编号递增；当判断出生成树在下一节点处将产生拐角时，则根据拐角方向对应改变中栅格的方位编号中的方位。

5.根据权利要求1所述的一种多火星飞行器的协同动态路径规划方法，其特征在于，步骤(2)所述火星飞行器探测到突发危险区的条件为：

r_t≤R_UAV

其中，r_t为飞行器距突发危险区的距离，R_UAV为飞行器自带传感器的最大探测半径；

局部动态路径规划的起点为，火星飞行器沿着所述离线协同路径飞行时首次探测到与离线协同路径相交的突发危险区时，火星飞行器所处的坐标；

局部动态路径规划的终点为，火星飞行器避开新危险区后首次探测不到该危险区时，火星飞行器所处的坐标。

6.根据权利要求1所述的一种多火星飞行器的协同动态路径规划方法，其特征在于，所述步骤(31)包括以下步骤：

(311)计算目标点对飞行器产生的引力场U_att、危险区对飞行器产生的斥力场U_rep以及势场和U：

U＝U_att+U_rep

其中：X_g＝(x_g,y_g)为目标点的坐标；X＝(x,y)为飞行器的当前坐标；

为飞行器到目标点的欧几里得距离；k为引力增益系数；U_att为目标点对飞行器产生的引力场；m为斥力增益系数；d为飞行器与危险区的距离；d₀为危险区的影响距离；d_a为飞行器与目标点之间的距离；U_rep为危险区对飞行器产生的斥力场；

(312)计算目标点对飞行器产生的引力F_att、危险区对飞行器产生的斥力F_rep以及合力F：

其中：分量F_rep1由危险区指向飞行器；分量F_rep2由飞行器指向目标点。

Images