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

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

An algorithm for formation control of UAVs based on Lagrange equation

The invention discloses a UAV formation control algorithm based on Lagrange equation, in the formation of UAV path planning using improved artificial potential field; in keeping the formation of UAV formation movement as a constrained dynamical system. In the establishment of constrained dynamical system, firstly several UAV relative to the space geometrical relationship between the inertial coordinate system, the establishment of space formation constraint equations according to the formation shape need, then introduce the Lagrange multiplier set containing constraints of UAV formation formation constraint dynamical equations, finally Baumgartner stability by direct elimination method of Lagrange multiplier and the solution of Lagrange multiplier. The geometric relationship between UAVs in space is established by constraint equations, and the formation kinematics equations of constrained formation are solved, and the flight trajectory of the formation UAV under the condition of expected formation is obtained. Formation and maintenance of UAV formation flight have strong flexibility and effectiveness.

【技术实现步骤摘要】
一种基于拉格朗日方程的无人机编队队形控制算法
本专利技术涉及一种无人机控制方法，具体是一种基于拉格朗日方程的无人机编队队形控制算法。
技术介绍
随着无人机技术的快速发展，无人机在军事、灾害救援等方面都起到了不可替代的作用，但是随着任务环境越来越复杂，单无人机执行任务的缺点也迅速体现出来。比如：采用单一无人机执行任务时侦测范围较窄；在复杂地形环境下，采用单机完成任务相对比较困难，甚至难以完成等等。反之采用多机协同编队飞行则能够很好的解决单机暴露的很多问题，同时可以提高系统的鲁棒性和可靠性。因此，无人机编队的概念一经提出就迅速成为当今国内外无人机研究的一个重要热点问题。因此对无人机编队的研究具有重要的现实意义和研究价值，特别是对无人机编队的路径规划和队形控制等方面的研究。
技术实现思路
本专利技术要解决的技术问题是提供一种基于拉格朗日方程的无人机编队队形控制算法，该方法能够很好地解决编队无人机的路径规划问题和飞行过程中队形的保持问题。为实现上述专利技术目的，本专利技术的一种基于拉格朗日方程的无人机编队队形控制算法，在编队无人机路径规划方面采用改进的人工势能场；在队形保持方面将编队无人机运动看作一个约束动力学系统。在建立约束动力学系统时，首先利用多个无人机相对于惯性坐标系的空间几何关系，根据需要的编队形状建立空间队形约束方程；其次然后引入拉格朗日乘子建立含有队形约束的编队无人机的约束动力学方程组，最后结合鲍姆加特稳定性采用直接消除拉格朗日乘子方法并求解拉格朗日乘子。具体如下：一种基于拉格朗日方程的无人机编队队形控制算法，其特征在于，包括以下步骤：1)对无人机编队的运行环境进行简化和抽象，包括将无人机编队飞行三维惯性坐标系简化到二维惯性平面坐标系，将地面标志物抽象为障碍物；为了尽可能的避免局部极小值的存在以及陷阱区域的存在，该方法将障碍物抽象为圆形。2)对所述无人机编队进行受力分析，包括求解在当前位置时目标区域对所述无人机编队中各个无人机产生的引力，以及所述障碍物对所述无人机编队产生的斥力；3)求解所述无人机编队中各个无人机受到的合力，以及整个所述无人机编队收受到的合力；4)引入朗格朗日乘子，建立含有队形约束的无人机编队的运动学方程。首先将编队无人机在整个势场中的运动学作为正向动力学问题，其中障碍物产生的斥力与目标空域产生的引力的合力作为为输入。同时将编队无人机的运动作为受队形约束的约束动力学系统，系统的输入为编队无人机受到的合力，系统的输出为编队无人机的位置轨迹。最后，含有队形约束的编队无人机的运动学方程组为：g＝g(q,t)＝0(10)其中，q为2n维的编队无人机位置矩阵；λ为拉格朗日乘子；M为2n*2n编队无人机的权重矩阵；F(q,v,t,FTotal)为2n维的外力矩阵；为无人机当前速度；t为时间；FTotal为合外力向量，为队形约束方程；为队形约束函数的雅可比矩阵；5)采用直接消除拉格朗日乘子方法，并结合鲍姆加特稳定性求解编队无人机的运动学方程组中的朗格朗日乘子。首先将编队无人机的运动学方程利用状态空间表示如下：g＝g(q,t)＝0(12)其中，为队形约束函数的雅可比矩阵；λ为拉格朗日乘子。然后，由方程(12)得利用鲍姆加特稳定性得：结合方程(9)得：因此，可得拉格朗日乘子的表达式为：其中,α、δ为比例因子。6)通过判断所述无人机编队受到的合力是否为零，来判断编队无人机是否运行到目标区域：如果所述无人机编队受到的合力为零，则输出所述无人机编队飞行轨迹，同时输出误差曲线，解算结束；如果所述无人机编队受到的合力不为零，则返回步骤2)。进一步的，所述步骤2)中求解所述引力的方程为：其中：(ρg)＝||ρq-ρgoal||为无人机与目标点空域之间的欧氏距离；ε为正的比例因子；m是大于0的常数；Eqg表示编队无人机与目标区域之间的单位矢量；v为无人机的速度；Kp与无人机性能相关的比例因子。进一步的，所述步骤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的常数；Eqg表示编队无人机与目标区域之间的单位矢量；v为无人机的速度；Kp与无人机性能相关的比例因子。然后设置斥力函数的改进条件为a:所有的障碍物产生的斥力函数必须连续；b:所有的障碍都都有一个大于障碍物半径的最小影响范围。将斥力函数改进为如下函数：其中：ρob(q)表示无人机于障碍物最小影响范围边界的最短距离；Ks为常值系数；Dlimit障碍物最小影响范围；Eqo为编队无人机与障碍物之间的单位矢量。得到每个无人所受的引力为：每个无人所受的斥力为：(2)求解编队中单个无人机的合力。对于单个无人机来说，所受的合力等于目标区域产生的引力和障碍物产生斥力的合力。(3)求解整个编队无人本文档来自技高网...

【技术保护点】
一种基于拉格朗日方程的无人机编队队形控制算法，其特征在于，包括以下步骤：1)对无人机编队的运行环境进行简化和抽象，包括将无人机编队飞行三维惯性坐标系简化到二维惯性平面坐标系，将地面标志物抽象为障碍物；2)对所述无人机编队进行受力分析，包括求解在当前位置时目标区域对所述无人机编队中各个无人机产生的引力，以及所述障碍物对所述无人机编队产生的斥力；3)求解所述无人机编队中各个无人机受到的合力，以及整个所述无人机编队收受到的合力；4)引入拉格朗日乘子,建立含有队形约束的无人机编队约束动力学方程组：

【技术特征摘要】
1.一种基于拉格朗日方程的无人机编队队形控制算法，其特征在于，包括以下步骤：1)对无人机编队的运行环境进行简化和抽象，包括将无人机编队飞行三维惯性坐标系简化到二维惯性平面坐标系，将地面标志物抽象为障碍物；2)对所述无人机编队进行受力分析，包括求解在当前位置时目标区域对所述无人机编队中各个无人机产生的引力，以及所述障碍物对所述无人机编队产生的斥力；3)求解所述无人机编队中各个无人机受到的合力，以及整个所述无人机编队收受到的合力；4)引入拉格朗日乘子,建立含有队形约束的无人机编队约束动力学方程组：g＝g(q,t)＝0(10)其中，q为2n维的编队无人机位置矩阵；λ为拉格朗日乘子；M为2n*2n编队无人机的权重矩阵；F(q,v,t,FTotal)为2n维的外力矩阵；为无人机当前速度；t为时间；FTotal为合外力向量，g为队形约束方程；为队形约束函数的雅可比矩阵；5)采用直接消除拉格朗日乘子方法，并结合鲍姆加特稳定性对编队无人机约束动力学方程组进行求解,得到拉格朗日乘子方程:其中,α、δ为比例因子；6)通过判断所述无人机编队受到的合力是否为零，来判断编队无人机是否运行到目标区域：如果所述无人机编队受到的合力为零，则输出所述无人机编队飞行轨迹，同时输出误差曲线，解算结束；如果所述无人机编队受到的合力不为零，则返回步骤2)。2.根据权利要求1所述的基...

【专利技术属性】
技术研发人员：陈志明，牛康，李磊，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32