[发明专利]一种编队无人机追踪与规避移动目标的算法

权利要求书

1.一种编队无人机追踪与规避移动目标的算法，其特征在于，包括以下步骤：

1)建立三维空间惯性坐标系O_g，并将三维空间内移动目标和障碍进行简化和抽象；

2)通过引入空间移动目标的位置和速度量，分别建立三维空间内无人机移动目标跟踪函数和移动目标规避函数；

3)分别将跟踪与规避函数对相应的位置和速度求负梯度，得到对应的跟踪引力和规避斥力，并进行动态无人机受力分析；

4)将无人机的空间运动问题看作受到以虚拟合外力为输入的正向动力学问题，并建立运动学模型；

5)将编队无人机的空间运动看作一个受队形约束的约束动力学系统，并建立约束动力学方程，然后求解该约束动力学方程；

所述步骤2)中的无人机移动目标跟踪函数为：

其中:ε_q,ε_v分别为位置和速度追踪因子，位置ρ_xy＝||q_g-q_c||为移动目标在编队中各无人机的当前位置x,y平面内的投影与编队无人机之间的欧式距离，q_g,q_c分别表示无人机和移动目标点的当前位置，m,k为正常数比例因子，取m＝2,k＝2，K_p是与编队中无人机性能相关的比例因子；ρ_z＝||q_gz-q_z||表示编队无人机在垂直方向上受到目标点的引力，q_gz,q_z分别表示无人机和移动目标当前位置的z轴坐标；ρ_v＝||v_g(t)-v_c(t)||表示编队无人机与移动目标在时间t时的相对速度，v_g(t),v_c(t)分别表示无人机和移动目标点在t时刻的速度；

所述步骤2)中的无人机移动目标规避函数为：

其中:i表示空间内第i个移动障碍物，η_q,η_v分别为位置和速度规避因子，ρ_xy(q)＝||q_c-q_oi||表示在当前无人机所在的x,y平面内，无人机与移动障碍物边界的最短距离，q_c,q_oi表示无人机和障碍物i的当前位置，ρ₀表示移动障碍的影响范围，m为正常数比例因子，取m＝2，v_co表示无人机与移动障碍i的相对速度，K_s为常值系数；

所述步骤3)中的跟踪引力为：

F(q,v)_a＝F(q)_a+F(v)_a (2)

其中:F(q)_a,F(v)_a分别为公式(6),(7)所示:

其中:e_cg表示在x,y平面内由无人机指向移动目标点的单位向量；e_zg表示在z轴方向上由无人机指向目标点的单位向量；e_vcg表示由无人机指向移动目标点的单位速度向量；

所述步骤3)中的规避斥力为：

其中:分别为公式(14),(15)所示:

其中:分别为公式(16),(17)所示：

其中，▽ _v为速度v的梯度，▽ _q为位置q的梯度，e_co是v_co对q和v的梯度；

所述步骤4)建立的运动学模型如公式(19)所示：

其中，q,分别为三维空间内的位置、速度、加速度向量，分别如下：q＝[x y z]^T,m表示无人机权重矩阵，K_p是与编队中无人机性能相关的比例因子；F_T为无人机所受到的虚拟合外力；

所述步骤5)建立编队无人机约束动力学方程如公式(20)所示：

ρ(x,y,z)＝0 (22)

式中:q表示编队中各个无人机的当前位置矩阵；为编队中各无人机的当前速度矩阵；m为各无人机的权重矩阵；F_T为无人机所受到的虚拟合外力；f是关于无人机位置、速度、虚拟合外力的函数；L为比例因子；λ为拉格朗日乘子；ρ编队无人机队形约束方程，是一个关于无人机当前三维空间位置x,y,z相关的函数；A^T为队形约束方程的雅克比矩阵，如公式(23)所示:

在对约束动力学方程组中的λ求解时，采用罚函数法并结合鲍姆加特稳定性得到λ的表达式，如公式(24)所示:

式中:δ为阻尼因子,γ为弹性因子。