CN107015571B - 一种编队无人机追踪与规避移动目标的算法 - Google Patents

Abstract

本发明提供一种编队无人机追踪与规避移动目标的算法，首先通过引入空间移动目标的位置和速度，建立三维空间内无人机移动目标跟踪与规避函数，并进行动态无人机受力分析；然后，求解三维空间内无人机所受虚拟合外力，并建立三维空间内无人机运动学方程；最后，针对无人机的队形保持方面的问题，建立一个受队形约束的约束动力学系统，并建立约束动力学方程，然后求解该约束动力学方程。本发明的算法简单，实现了编队无人机准确、实时地追踪与规避移动目标，大大提高了编队无人机的机动性和运动安全性，扩大了编队无人机的应用范围。

Description

一种编队无人机追踪与规避移动目标的算法

技术领域

本发明涉及无人机控制技术领域，具体而言涉及一种编队无人机追踪与规避移动目标的算法。

背景技术

当前，无人机技术已大量运用于空中侦察、地面打击、任务救援、电力巡检等军用和民用领域。相比较于单无人机执行任务，编队无人机无论在复杂环境的适应能力、执行任务的可靠性等方面都具有很大的优势。而与此同时，随着各国装备科技的迅速发展，编队无人机在执行对地移动目标的打击、空中目标的驱离、雷达的规避等任务时也面临前所未有的挑战。因此，三维空间内编队无人机对移动目标的跟踪与规避问题越来越成为一个广泛关注和研究的问题。

发明内容

本发明所要解决的技术问题是提供一种编队无人机追踪与规避移动目标的算法，该方法能够很好地实现编队无人机准确、及时地追踪空间内的移动目标和准确、及时地规避移动障碍。

为实现上述目的，本发明采用以下技术方案：

一种编队无人机追踪与规避移动目标的算法，包括以下步骤：

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)所示：

无人机所受的虚拟合外力F_T等于将无人机所受到空间内所有虚拟跟踪引力F(q,v)_a和规避斥力

的线性叠加。如公式(18)所示：

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

其中，

分别为三维空间内的位置、速度、加速度向量，分别如下：

m表示无人机权重矩阵，K_p是与编队中无人机性能相关的比例因子。

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

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

式中:q表示编队中各个无人机的当前位置矩阵；

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

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

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

本发明的一种编队无人机追踪与规避移动目标的算法，首先通过引入空间移动目标的位置和速度，建立三维空间内无人机移动目标跟踪与规避函数，并进行动态无人机受力分析；然后，求解三维空间内无人机所受虚拟合外力，并建立三维空间内无人机运动学方程；最后，针对无人机的队形保持方面的问题，建立一个受队形约束的约束动力学系统，并建立约束动力学方程，然后求解该约束动力学方程。本发明的算法简单，实现了编队无人机准确、实时地追踪与规避移动目标，大大提高了编队无人机的机动性和运动安全性，扩大了编队无人机的应用范围。

附图说明

图1为本发明编队无人机的典型运行环境示意图；

图2为本发明编队无人机的典型运行环境的简化抽象示意图；

图3为本发明无人机的移动目标跟踪引力受力分析图；

图4为本发明无人机的移动障碍规避斥力受力分析图；

图5为本发明编队无人机移动目标跟踪与规避算法的MATLAB程序流程图；

图6为MATLAB仿真t＝0s时编队无人机与移动目标和障碍初始位置图；

图7为MATLAB仿真t＝1.9s无人机和空间移动目标位置图；

图8为MATLAB仿真t＝16.2s无人机追踪与规避轨迹图；

图9为MATLAB仿真编队无人机关于时间的队形保持误差曲线图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

1)首先建立三维空间惯性坐标系O_g，并将三维空间内移动目标和障碍进行简化和抽象。在进行简化和抽象时分别将空间内地面移动的障碍抽象为圆柱形和半球形，将空中的移动障碍抽象为球形。

如图1所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法典型的运行环境，包括地面防空设施1、高山2、空中移动目标3以及空中移动威胁4，编队无人机5等。在进行编队无人机对移动目标追踪与规避时，要求编队无人机绕开所有的动态障碍，同时保持期望队形完成对移动目标的追踪。

如图2所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法典型运行环境的简化和抽象示意图。在进行简化和和抽象时首先建立力三维惯性坐标系O_g，同时根据地面和空中不同物体的特点，将它们分别进行如下简化和抽象，其中地面防空设施1抽象为半球形；空中移动威胁4抽象为球形；建筑2抽象为圆柱形。

在建立移动目标的跟踪与规避函数之前，首先做如下设定：

a)将编队中各无人机作为质点；

b)已知编队中各无人机的当前位置为q_c，当前速度为v_c；

c)已知空间内移动目标和移动障碍物的当前位置分别为q_g,q_o，当前速度分别为v_g,v_o。

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

21)通过引入移动目标的速度量，建立关于移动目标位置和速度的三维移动目标跟踪函数。首先假设当前无人机的位置为q_c＝[x,y,z]，当前速度为v_c＝[v_x,v_y,v_z]，当前目标点位置为q_g＝[x_g,y_g,z_g]，目标点移动速度为v_g＝[v_g,v_g,v_g]，因此，定义三维目标追踪函数如公式(1)所示:

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

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

31)将移动目标跟踪函数分别对相应的位置和速度求负梯度，得到对应的跟踪引力。

因此，由公式(1)可以看出三维目标跟踪函数是一个关于位置q和速度v相关的函数，定义移动目标点对无人机产生的虚拟引力F(q,v)_a等于目标追踪函数U(q,v)_a分别对位置和速度v求负梯度。如公式(2)所示:

式中:

分别为公式(3),(4)所示:

将公式(1)代入公式(2)可得编队无人机受到的虚拟力表达式如公式(5)所示:

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

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

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

如图3所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法无人机的移动目标跟踪引力受力分析图。其中,F_x,F_y,F_z分别表示F(q)_a+F(v)_a移动目标在x,y,z三个方向上的分力；v_x,v_y,v_z分别表示v_g移动目标在x,y,z三个方向上的分速度。

22)同理，通过引入空间内移动障碍的速度量，建立关于移动障碍位置和速度的三维移动障碍规避函数。首先，假设当前无人机的位置为q_c＝[x,y,z]，对应的当前速度为v_c＝[v_x,v_y,v_z]，空间内移动障碍物i的当前位置为q_oi＝[x_oi,y_oi,z_oi]。因此，定义无人机与移动障碍i的相对速度如公式(8)所示：

式中:e_co表示在x,y平面内由无人机指向移动障碍i的单位向量。

因此，结合公式(8)定义无人机对移动障碍物i三维规避函数如公式(9)所示：

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

32)将移动障碍规避函数分别对相应的位置和速度求负梯度，得到对应的规避斥力。

因此，由公式(9)看出三维规避函数同样是一个与位置q和速度v相关的函数。因此，定义移动障碍i产生的虚拟斥力

等于规避函数

分别对位置q和速度v求负梯度。如公式(10)所示:

由公式(8)可得v_co对q和v的梯度如公式(11),(12)所示：

将公式(8),(9)代入公式(10)可得移动障碍i产生的虚拟斥力表达式如公式(13)所示:

式中:

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

式中:

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

如图4所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法无人机的移动障碍规避斥力受力分析图。其中:F_xr,F_yr,F_zr分别表示F(q)_r+F(v)_r移动目标在x,y,z三个方向上的分力，v_xo,v_yo,v_zo分别表示v_oi移动目标在x,y,z三个方向上的分速度。

4)求解三维空间内无人机所受虚拟合外力，并将无人机的运动学问题简化为以虚拟合外力为输入的正向动力学问题，并建立三维空间内无人机运动学方程。

无人机所受的虚拟合外力F_T等于将无人机所受到空间内所有虚拟跟踪引力F(q,v)_a和规避斥力

的线性叠加。如公式(18)所示：

在将无人机的运动学问题简化为以虚拟合外力F_T为输入的正向动力学问题，同时建立三维空间内无人机运动学方程如公式(19)所示：

其中，

分别为三维空间内的位置、速度、加速度向量，分别如下：

m表示无人机权重矩阵，K_p是与编队中无人机性能相关的比例因子。

5)在无人机进行编队控制方面，将空间内对移动目标跟踪和规避的编队无人机看作为受队形约束的约束动力学系统。根据约束动力学理论，填入拉格朗日乘子建立编队无人机约束动力学方程如公式(20)所示：

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

式中:q表示编队中各个无人机的当前位置矩阵；

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

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

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

如图5所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法的MATLAB程序流程图。具体步骤如下：

(1)初始化编队中各个无人机的初始位置和速度，障碍物初始位置和速度，移动目标点的初始位置和速度；

(2)利用公式(1),(2),(9),(10)分别求出空间内移动目标和各个移动障碍队无人机产生的跟踪引力和规避斥力；

(3)利用公式(18)求解出编队中各个无人机所受到的虚拟合外力合力；

(4)利用公式(19)-(22)分别建立无人机运动学模型和编队无人机约束动力学模型；

(5)利用MATLAB每0.05步长求解得出编队中各个无人机相对于移动目标和障碍的下一个位置点、方向和角度；

通过判断无人机编队与移动目标之间的欧拉距离是否满足设定跟踪距离来判断编队无人机是否对移动目标进行有效跟踪。如果是，则输出编队无人机飞行轨迹，移动目标飞行轨迹，移动障碍移动轨迹，同时输出队形保持误差曲线，程序跳转至步骤6)，否则程序跳转到步骤2)；

(6)解算结束。

如图6所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法的MATLAB仿真t＝0s时编队无人机与移动目标和障碍初始位置图。

如图7所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法的MATLAB仿真t＝1.9s无人机和空间移动目标位置图。可以看出在t＝1.9s时，移动障碍A沿y轴方向垂直运动拦截编队无人机，移动目标朝向固定障碍B移动，编队无人机首先加速绕过障碍A然后加速向目标点移动。

如图8所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法的MATLAB仿真t＝16.2s无人机追踪与规避轨迹图。在t＝16.2s时，移动目标朝向并绕过障碍，编队无人机成功规避障碍并完成对移动目标的追踪。

如图9所示，本发明公开的一种三维空间内编队无人机对移动目标追踪与规避的算法的MATLAB仿真编队无人机关于时间的队形保持误差曲线图。仿真结果可以看出在编队无人机对移动目标追踪的在整个过程中队形均方差均趋近于0，说明在整个过程中编队无人机可以完成多移动目标的追踪同时保持期望队形。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

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)所示:

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

Images