CN106873621A - 一种基于拉格朗日方程的无人机编队队形控制算法 - Google Patents

Abstract

本发明公开了一种基于拉格朗日方程的无人机编队队形控制算法，在编队无人机路径规划方面采用改进的人工势能场；在队形保持方面将编队无人机运动看作一个约束动力学系统。在建立约束动力学系统时，首先利用多个无人机相对于惯性坐标系的空间几何关系，根据需要的编队形状建立空间队形约束方程，然后引入拉格朗日乘子建立含有队形约束的编队无人机的约束动力学方程组，最后结合鲍姆加特稳定性采用直接消除拉格朗日乘子方法并求解拉格朗日乘子。通过约束方程建立多无人机在空间的几何关系，对含有约束条件的编队运动学方程进行求解得出编队无人机在保持期望队形情况下的飞行轨迹。在解决无人机编队飞行时队形保持和变换有较强的灵活性和有效性。

Description

一种基于拉格朗日方程的无人机编队队形控制算法

技术领域

本发明涉及一种无人机控制方法，具体是一种基于拉格朗日方程的无人机编队队形控制算法。

背景技术

随着无人机技术的快速发展，无人机在军事、灾害救援等方面都起到了不可替代的作用，但是随着任务环境越来越复杂，单无人机执行任务的缺点也迅速体现出来。比如：采用单一无人机执行任务时侦测范围较窄；在复杂地形环境下，采用单机完成任务相对比较困难，甚至难以完成等等。反之采用多机协同编队飞行则能够很好的解决单机暴露的很多问题，同时可以提高系统的鲁棒性和可靠性。因此，无人机编队的概念一经提出就迅速成为当今国内外无人机研究的一个重要热点问题。因此对无人机编队的研究具有重要的现实意义和研究价值，特别是对无人机编队的路径规划和队形控制等方面的研究。

发明内容

本发明要解决的技术问题是提供一种基于拉格朗日方程的无人机编队队形控制算法，该方法能够很好地解决编队无人机的路径规划问题和飞行过程中队形的保持问题。

为实现上述发明目的，本发明的一种基于拉格朗日方程的无人机编队队形控制算法，在编队无人机路径规划方面采用改进的人工势能场；在队形保持方面将编队无人机运动看作一个约束动力学系统。在建立约束动力学系统时，首先利用多个无人机相对于惯性坐标系的空间几何关系，根据需要的编队形状建立空间队形约束方程；其次然后引入拉格朗日乘子建立含有队形约束的编队无人机的约束动力学方程组，最后结合鲍姆加特稳定性采用直接消除拉格朗日乘子方法并求解拉格朗日乘子。具体如下：

一种基于拉格朗日方程的无人机编队队形控制算法，其特征在于，包括以下步骤：

1)对无人机编队的运行环境进行简化和抽象，包括将无人机编队飞行三维惯性坐标系简化到二维惯性平面坐标系，将地面标志物抽象为障碍物；为了尽可能的避免局部极小值的存在以及陷阱区域的存在，该方法将障碍物抽象为圆形。

2)对所述无人机编队进行受力分析，包括求解在当前位置时目标区域对所述无人机编队中各个无人机产生的引力，以及所述障碍物对所述无人机编队产生的斥力；

3)求解所述无人机编队中各个无人机受到的合力，以及整个所述无人机编队收受到的合力；

4)引入朗格朗日乘子，建立含有队形约束的无人机编队的运动学方程。首先将编队无人机在整个势场中的运动学作为正向动力学问题，其中障碍物产生的斥力与目标空域产生的引力的合力作为为输入。同时将编队无人机的运动作为受队形约束的约束动力学系统，系统的输入为编队无人机受到的合力，系统的输出为编队无人机的位置轨迹。

最后，含有队形约束的编队无人机的运动学方程组为：

g＝g(q,t)＝0 (10)

其中，q为2n维的编队无人机位置矩阵；λ为拉格朗日乘子；M为2n*2n编队无人机的权重矩阵；F(q,v,t,F_Total)为2n维的外力矩阵；为无人机当前速度；t为时间；F_Total为合外力向量，为队形约束方程；为队形约束函数的雅可比矩阵；

5)采用直接消除拉格朗日乘子方法，并结合鲍姆加特稳定性求解编队无人机的运动学方程组中的朗格朗日乘子。首先将编队无人机的运动学方程利用状态空间表示如下：

g＝g(q,t)＝0 (12)

其中，为队形约束函数的雅可比矩阵；λ为拉格朗日乘子。

然后，由方程(12)得

利用鲍姆加特稳定性得：

结合方程(9)得：

因此，可得拉格朗日乘子的表达式为：

其中,α、δ为比例因子。

6)通过判断所述无人机编队受到的合力是否为零，来判断编队无人机是否运行到目标区域：如果所述无人机编队受到的合力为零，则输出所述无人机编队飞行轨迹，同时输出误差曲线，解算结束；如果所述无人机编队受到的合力不为零，则返回步骤2)。

进一步的，所述步骤2)中求解所述引力的方程为：

其中：(ρ_g)＝||ρ_q-ρ_goal||为无人机与目标点空域之间的欧氏距离；ε为正的比例因子；m是大于0的常数；E_qg表示编队无人机与目标区域之间的单位矢量；v为无人机的速度；K_p与无人机性能相关的比例因子。

进一步的，所述步骤2)中求解所述斥力的方程为：

进一步的，所述步骤3)中求解所述各个无人机的合力的公式为：

进一步的，所述步骤3)中求解所述整个无人机编队的合力的公式为：

进一步的，建立含有队形约束的无人机编队动力学方程时，首先定义编队无人机的队形的为编队中无人机之间的相对位置保持不变；然后把编队无人机的队形作为刚体队形看待，得到无人机个数与约束方程个数的关系为：

M＝2N-3 (6)

其中M表示约束方程个数，N表示参与编队的无人机个数；

最后，编队无人机的约束方程为g＝g(q,t) (7)。

进一步的，所述步骤1)中将地面标志物抽象为圆形障碍物。

本发明的一种基于拉格朗日方程的无人机编队队形控制算法，该控制方法基于改进人工势能场和拉格朗日方程，主要应用于编队无人机的路径规划和编队队形保持。该方法能够很好的解决编队无人机的路径规划问题和飞行过程中对性的保持问题。仿真实验表明在解决无人机编队飞行时队形保持和变换该方法具有较强的灵活性和有效性。

附图说明

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

图2为本发明无人机编队在当前位置时的受力分析图；

图3为以三架无人机菱形编队示意图；；

图4为本发明基于拉格朗日方程的无人机编队队形控制算法流程图。

具体实施方式

下面结合附图，对本发明提出的一种基于拉格朗日方程的无人机编队队形控制算法进行详细说明。

如图1所示，本发明公开的一种基于改进人工势能场和拉格朗日方程的无人机编队控制算法典型的运行环境，包括地面雷达1、防空导弹2、地面军事基地3以及障碍物4、编队无人机5、目标区域6等。其中，R1表示抽象的障碍物半径，R2表示障碍物的影响范围。这里要求编队无人机能够以编队的形式绕开所有的障碍物顺利的到达目标区域。在进行简化的时候本发明将编队无人的运动简化到抽象的平面内运动，并要求编队无人机绕开所有的障碍物顺利到达目标区域。

在进行简化和抽象时具体步骤如下：

(1)建立三维惯性坐标系S1；

(2)建立二维惯性平面坐标系S2；

(3)对地面雷达1、防空导弹2、地面军事基地3以及高山4等抽象为圆形障碍物，且半径大小R1(黑色区域半径)为障碍物的中心到其边界的最大距离，并建立障碍物的影响范围R2(外圈半径)；

如图2所示：对运行环境进行抽象和简化完成之后要对编队无人机进行受力分析，具体步骤如下：

(1)编队无人机在运行环境中目标区域始终对编队无人机产生引力作用，只有编队无人机进入障碍物的影响范围时编队无人机收到相应的障碍物产生的斥力作用。

将经典引力势函数改进为：

其中：(ρ_g)＝||ρ_q-ρ_goal||为无人机与目标点空域之间的欧氏距离；ε为正的比例因子；m是大于0的常数；E_qg表示编队无人机与目标区域之间的单位矢量；v为无人机的速度；K_p与无人机性能相关的比例因子。

然后设置斥力函数的改进条件为a:所有的障碍物产生的斥力函数必须连续；b:所有的障碍都都有一个大于障碍物半径的最小影响范围。将斥力函数改进为如下函数：

其中：ρ_ob(q)表示无人机于障碍物最小影响范围边界的最短距离；K_s为常值系数；D_limit障碍物最小影响范围；E_qo为编队无人机与障碍物之间的单位矢量。

得到每个无人所受的引力为：

每个无人所受的斥力为：

(2)求解编队中单个无人机的合力。对于单个无人机来说，所受的合力等于目标区域产生的引力和障碍物产生斥力的合力。

(3)求解整个编队无人机的合力。对于整个编队无人机来说所受到的合力等于对编队中所有无人机所受的合力进行求和。

(4)如图2所示，其中F_Total＝F，另外，编队无人机在没有接近R2时，因为F_rep＝0，所以F_Total＝F_attr＝F。

如图3所示：为对三架无人机进行菱形编队。本发明以三架无人机编队来说明编队约束方程与编队无人机数量之间的关系，具体步骤如下：

(1)首先，本发明定义队形的概念为编队中无人机之间的相对位置保持不变。

(2)然后，将编队无人机飞行时的队形作为刚体队形看待得到编队无人机个数与约束方程的关系为：

M＝2N-3 (6)

其中，M表示约束方程个数，N表示参与编队的无人机个数。

(3)最后得到约束方程为如下方程：

g＝g(q,t)＝0 (7)

是一个与无人机位置相关的函数，根据要求决定是否和时间相关；如果要求整个编队无人机飞行过程中要求队形不改变时，约束方程与时间不相关，否则约束方程与时间相关。

(4)以三架无人机为例进行编队时约束方程如下：

其中，D_ij为无人机i，j之间的期望距离。

如图4所示：为求解该算法的程序流程图，具体步骤如下：

(1)首先给定编队中各个无人机的初始位置，初始速度；

(2)利用公式(2),(3)分别求出在当前位置时目标区域对编队中各个无人机产生的引力，以及障碍物对编队无人机产生的斥力；

(3)利用公式(4)求解编队中各个无人机受到的合力，同时利用公式(27)求解整个无人机编队受到的合力；

(4)利用MATLAB每0.1步长求解出编队中各个无人机的下一个位置点、方向和角度；

(5)通过判断无人机编队受到的合力是否为零，来判断编队无人机是否运行到目标区域。如果是，则输出编队无人机飞行轨迹，同时输出误差曲线，程序跳转至步骤6)，否则程序跳转到步骤2)；

(6)解算结束。

基于对本发明优选实施方式的描述，应该清楚，由所附的权利要求书所限定的本发明并不仅仅局限于上面说明书中所阐述的特定细节，未脱离本发明宗旨或范围的对本发明的许多显而易见的改变同样可能达到本发明的目的。

Claims (7)

1.一种基于拉格朗日方程的无人机编队队形控制算法，其特征在于，包括以下步骤：

1)对无人机编队的运行环境进行简化和抽象，包括将无人机编队飞行三维惯性坐标系简化到二维惯性平面坐标系，将地面标志物抽象为障碍物；

2)对所述无人机编队进行受力分析，包括求解在当前位置时目标区域对所述无人机编队中各个无人机产生的引力，以及所述障碍物对所述无人机编队产生的斥力；

3)求解所述无人机编队中各个无人机受到的合力，以及整个所述无人机编队收受到的合力；

4)引入拉格朗日乘子,建立含有队形约束的无人机编队约束动力学方程组：

$\stackrel{\·}{q}=v---\left(8\right)$

$M\stackrel{\·\·}{q}=F(q,v,t,F_{Total})-J{\left(q\right)}^T\mathrm{\λ}---\left(9\right)$

g＝g(q,t)＝0 (10)

其中，q为2n维的编队无人机位置矩阵；λ为拉格朗日乘子；M为2n*2n编队无人机的权重矩阵；F(q,v,t,F_Total)为2n维的外力矩阵；为无人机当前速度；t为时间；F_Total为合外力向量，g为队形约束方程；为队形约束函数的雅可比矩阵；

5)采用直接消除拉格朗日乘子方法，并结合鲍姆加特稳定性对编队无人机约束动力学方程组进行求解,得到拉格朗日乘子方程:

$\mathrm{\λ}={\[J\left(q\right)M^{-1}{J}^T\left(q\right)\]}^{-1}\[J\left(q\right)M^{-1}f(q,v,t,F_{Total})+J\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+\frac{{\∂}^{2}g}{\∂t^2}++\mathrm{\α}\stackrel{\·}{g}+\mathrm{\δ}g\]---\left(17\right)$

其中,α、δ为比例因子；

6)通过判断所述无人机编队受到的合力是否为零，来判断编队无人机是否运行到目标区域：如果所述无人机编队受到的合力为零，则输出所述无人机编队飞行轨迹，同时输出误差曲线，解算结束；如果所述无人机编队受到的合力不为零，则返回步骤2)。

2.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法，其特征在于，所述步骤2)中求解所述引力的方程为：

$U{\left(q\right)}_{attr}=\frac{1}{2}\mathrm{\ϵ}{\left({\mathrm{\ρ}}_g\right)}^m+K_p{\mathrm{vE}}_{qg}---\left(1\right).$

其中：(ρ_g)＝||ρ_q-ρ_goal||为无人机与目标点空域之间的欧氏距离；ε为正的比例因子；m是大于0的常数；E_qg表示编队无人机与目标区域之间的单位矢量；v为无人机的速度；K_p与无人机性能相关的比例因子。

3.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法，其特征在于，所述步骤2)中求解所述斥力的方程为：

$U_{Oi}^{repO}=\left\{\begin{array}{cc}(\frac{K_O}{{\mathrm{\&rho;}}_{Ob}\left(q\right)}-\frac{K_O}{D_{\lim it}})E_{qo}& 0<{\mathrm{if\&rho;}}_{Ob}\left(q\right)<D_{\lim it}\\ K_s& {\mathrm{if\&rho;}}_{Ob}\left(q\right)\&le;0\\ 0& Others\end{array}\right.---\left(2\right).$

4.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法，其特征在于，所述步骤3)中求解所述各个无人机的合力的公式为：

$F_i=\frac{\&part;({\&Sigma;}_{i=1}^{i=n}{U}_i^{rep}+U_{attr})}{\&part;q}---\left(4\right).$

5.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法，其特征在于，所述步骤3)中求解所述整个无人机编队的合力的公式为：

$F_{Total}={\mathrm{\&Sigma;}}_{i=1}^{i=n}{F}_i---\left(5\right).$

6.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法，其特征在于，建立含有队形约束的无人机编队动力学方程时，首先定义编队无人机的队形的为编队中无人机之间的相对位置保持不变；然后把编队无人机的队形作为刚体队形看待，得到无人机个数与约束方程个数的关系为：

M＝2N-3 (6)

其中M表示约束方程个数，N表示参与编队的无人机个数；

最后，编队无人机的约束方程为g＝g(q,t) (7)。

7.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法，其特征在于，所述步骤1)中将地面标志物抽象为圆形障碍物。