[发明专利]多火星飞行器的协同动态路径规划方法

权利要求书

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)将离线路径中局部动态路径规划的起点和终点之间的路径点移除，并用局部动态路径与之拼接，得到最终的飞行路径。