CN106873621B - 一种基于拉格朗日方程的无人机编队队形控制算法 - Google Patents
一种基于拉格朗日方程的无人机编队队形控制算法 Download PDFInfo
- Publication number
- CN106873621B CN106873621B CN201710105504.3A CN201710105504A CN106873621B CN 106873621 B CN106873621 B CN 106873621B CN 201710105504 A CN201710105504 A CN 201710105504A CN 106873621 B CN106873621 B CN 106873621B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- formation
- aerial vehicle
- equation
- constraint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 157
- 238000000034 method Methods 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims description 9
- 239000003550 marker Substances 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 claims description 2
- 230000005484 gravity Effects 0.000 claims description 2
- 238000005381 potential energy Methods 0.000 abstract description 4
- 238000005755 formation reaction Methods 0.000 description 107
- 238000011160 research Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 3
- 230000007123 defense Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 229910003460 diamond Inorganic materials 0.000 description 1
- 239000010432 diamond Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于拉格朗日方程的无人机编队队形控制算法,在编队无人机路径规划方面采用改进的人工势能场;在队形保持方面将编队无人机运动看作一个约束动力学***。在建立约束动力学***时,首先利用多个无人机相对于惯性坐标系的空间几何关系,根据需要的编队形状建立空间队形约束方程,然后引入拉格朗日乘子建立含有队形约束的编队无人机的约束动力学方程组,最后结合鲍姆加特稳定性采用直接消除拉格朗日乘子方法并求解拉格朗日乘子。通过约束方程建立多无人机在空间的几何关系,对含有约束条件的编队运动学方程进行求解得出编队无人机在保持期望队形情况下的飞行轨迹。在解决无人机编队飞行时队形保持和变换有较强的灵活性和有效性。
Description
技术领域
本发明涉及一种无人机控制方法,具体是一种基于拉格朗日方程的无人机编队队形控制算法。
背景技术
随着无人机技术的快速发展,无人机在军事、灾害救援等方面都起到了不可替代的作用,但是随着任务环境越来越复杂,单无人机执行任务的缺点也迅速体现出来。比如:采用单一无人机执行任务时侦测范围较窄;在复杂地形环境下,采用单机完成任务相对比较困难,甚至难以完成等等。反之采用多机协同编队飞行则能够很好的解决单机暴露的很多问题,同时可以提高***的鲁棒性和可靠性。因此,无人机编队的概念一经提出就迅速成为当今国内外无人机研究的一个重要热点问题。因此对无人机编队的研究具有重要的现实意义和研究价值,特别是对无人机编队的路径规划和队形控制等方面的研究。
发明内容
本发明要解决的技术问题是提供一种基于拉格朗日方程的无人机编队队形控制算法,该方法能够很好地解决编队无人机的路径规划问题和飞行过程中队形的保持问题。
为实现上述发明目的,本发明的一种基于拉格朗日方程的无人机编队队形控制算法,在编队无人机路径规划方面采用改进的人工势能场;在队形保持方面将编队无人机运动看作一个约束动力学***。在建立约束动力学***时,首先利用多个无人机相对于惯性坐标系的空间几何关系,根据需要的编队形状建立空间队形约束方程;其次然后引入拉格朗日乘子建立含有队形约束的编队无人机的约束动力学方程组,最后结合鲍姆加特稳定性采用直接消除拉格朗日乘子方法并求解拉格朗日乘子。具体如下:
一种基于拉格朗日方程的无人机编队队形控制算法,其特征在于,包括以下步骤:
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)求解整个编队无人机的合力。对于整个编队无人机来说所受到的合力等于对编队中所有无人机所受的合力进行求和。
如图3所示:为对三架无人机进行菱形编队。本发明以三架无人机编队来说明编队约束方程与编队无人机数量之间的关系,具体步骤如下:
(1)首先,本发明定义队形的概念为编队中无人机之间的相对位置保持不变。
(2)然后,将编队无人机飞行时的队形作为刚体队形看待得到编队无人机个数与约束方程的关系为:
M=2N-3 (6)
其中,M表示约束方程个数,N表示参与编队的无人机个数。
(3)最后得到约束方程为如下方程:
g=g(q,t)=0 (7)
是一个与无人机位置相关的函数,根据要求决定是否和时间相关;如果要求整个编队无人机飞行过程中要求队形不改变时,约束方程与时间不相关,否则约束方程与时间相关。
(4)以三架无人机为例进行编队时约束方程如下:
其中,Dij为无人机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)引入拉格朗日乘子,建立含有队形约束的无人机编队约束动力学方程组:
g=g(q,t)=0 (10)
其中,q为2n维的编队无人机位置矩阵;λ为拉格朗日乘子;M为2n*2n编队无人机的权重矩阵;F(q,v,t,FTotal)为2n维的外力矩阵;为无人机当前速度;t为时间;FTotal为合外力向量,g为队形约束方程;为队形约束函数的雅可比矩阵;
5)采用直接消除拉格朗日乘子方法,并结合鲍姆加特稳定性对编队无人机约束动力学方程组进行求解,得到拉格朗日乘子方程:
其中,α、δ为比例因子;
6)通过判断所述无人机编队受到的合力是否为零,来判断编队无人机是否运行到目标区域:如果所述无人机编队受到的合力为零,则输出所述无人机编队飞行轨迹,同时输出误差曲线,解算结束;如果所述无人机编队受到的合力不为零,则返回步骤2)。
3.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法,其特征在于,所述步骤2)中求解所述斥力的方程为:
其中,ρob(q)表示无人机于障碍物最小影响范围边界的最短距离;Ks为常值系数;Dlimit障碍物最小影响范围;Eqo为编队无人机与障碍物之间的单位矢量。
6.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法,其特征在于,建立含有队形约束的无人机编队动力学方程时,首先定义编队无人机的队形的为编队中无人机之间的相对位置保持不变;然后把编队无人机的队形作为刚体队形看待,得到无人机个数与约束方程个数的关系为:
M=2N-3 (6)
其中M表示约束方程个数,N表示参与编队的无人机个数;
最后,编队无人机的约束方程为g=g(q,t) (7)。
7.根据权利要求1所述的基于拉格朗日方程的无人机编队队形控制算法,其特征在于,所述步骤1)中将地面标志物抽象为圆形障碍物。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710105504.3A CN106873621B (zh) | 2017-02-26 | 2017-02-26 | 一种基于拉格朗日方程的无人机编队队形控制算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710105504.3A CN106873621B (zh) | 2017-02-26 | 2017-02-26 | 一种基于拉格朗日方程的无人机编队队形控制算法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106873621A CN106873621A (zh) | 2017-06-20 |
CN106873621B true CN106873621B (zh) | 2020-02-21 |
Family
ID=59169346
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710105504.3A Active CN106873621B (zh) | 2017-02-26 | 2017-02-26 | 一种基于拉格朗日方程的无人机编队队形控制算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106873621B (zh) |
Families Citing this family (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107329492A (zh) * | 2017-08-18 | 2017-11-07 | 上海顺砾智能科技有限公司 | 一种无人机集群的编队控制方法 |
CN108563237B (zh) * | 2018-03-30 | 2021-03-23 | 北京润科通用技术有限公司 | 一种协同避障方法及装置 |
CN108563240B (zh) * | 2018-07-26 | 2020-09-29 | 电子科技大学 | 一种大规模微型无人机的编队及控制方法 |
CN109032186B (zh) * | 2018-09-30 | 2021-04-02 | 西安科技大学 | 一种无人机群协同退出圆形轨迹的控制方法 |
CN110007688B (zh) * | 2019-04-25 | 2021-06-01 | 西安电子科技大学 | 一种基于强化学习的无人机集群分布式编队方法 |
CN110926479B (zh) * | 2019-12-20 | 2023-04-28 | 杜明利 | 自动生成室内三维导航地图模型的方法和*** |
CN111571586B (zh) * | 2020-05-07 | 2020-12-11 | 江苏原力数字科技股份有限公司 | 一种反向动力学自由度的动态约束方法 |
CN111596690B (zh) * | 2020-06-15 | 2021-06-08 | 湖南大学 | 一种无线速度测量的四旋翼飞行机器人机动编队控制方法 |
CN111522362B (zh) * | 2020-06-15 | 2021-05-07 | 湖南大学 | 一种无线速度反馈的四旋翼无人机分布式编队控制方法 |
CN113220023B (zh) * | 2021-04-28 | 2022-10-14 | 中国科学院重庆绿色智能技术研究院 | 一种高精度无人机实时路径规划方法 |
CN113506440B (zh) * | 2021-09-08 | 2021-11-30 | 四川国蓝中天环境科技集团有限公司 | 一种拉格朗日坐标系下多源数据融合的交通状态估计方法 |
CN113885554B (zh) * | 2021-09-13 | 2023-07-25 | 汕头大学 | 一种无人机集群的分布式围捕控制方法及*** |
CN113848988B (zh) * | 2021-11-05 | 2022-04-01 | 南京航空航天大学 | 适用于大规模无人机的网格化编队方法 |
CN114637323A (zh) * | 2022-03-01 | 2022-06-17 | 浙江大学 | 面向无人机编队保持的航路规划方法 |
PL442932A1 (pl) * | 2022-11-24 | 2024-05-27 | Uniwersytet Rzeszowski | Sposób inteligentnego harmonogramowania zadań lotniczych dla bezzałogowych statków powietrznych |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5001646A (en) * | 1988-12-19 | 1991-03-19 | Mcdonnell Douglas Corporation | Automated helicopter flight control system |
EP1244946B1 (en) * | 1999-12-21 | 2004-06-23 | Lockheed Martin Corporation | Spatial avoidance method and apparatus |
EP2629166B1 (en) * | 2012-02-17 | 2016-08-17 | The Boeing Company | An unmanned aerial vehicle harvesting energy in updraft |
CN102591358B (zh) * | 2012-03-12 | 2015-07-08 | 北京航空航天大学 | 一种多无人机的动态编队控制方法 |
CN103744290B (zh) * | 2013-12-30 | 2016-02-24 | 合肥工业大学 | 一种多无人机编队分层目标分配方法 |
US10289122B2 (en) * | 2014-10-27 | 2019-05-14 | Sikorsky Aircraft Corporation | Communication link accessibility aware navigation |
CN104656663B (zh) * | 2015-02-15 | 2017-12-01 | 西北工业大学 | 一种基于视觉的无人机编队感知与规避方法 |
CN105974939B (zh) * | 2016-07-25 | 2018-12-28 | 天津远度科技有限公司 | 无人机编队阵型自动生成方法及装置 |
CN106125760B (zh) * | 2016-07-25 | 2019-05-31 | 天津远度科技有限公司 | 无人机编队路径自动规划方法及装置 |
CN106200679B (zh) * | 2016-09-21 | 2019-01-29 | 中国人民解放军国防科学技术大学 | 基于多模态自然交互的单操作员多无人机混合主动控制方法 |
-
2017
- 2017-02-26 CN CN201710105504.3A patent/CN106873621B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN106873621A (zh) | 2017-06-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106873621B (zh) | 一种基于拉格朗日方程的无人机编队队形控制算法 | |
CN107015571B (zh) | 一种编队无人机追踪与规避移动目标的算法 | |
Price et al. | Deep neural network-based cooperative visual tracking through multiple micro aerial vehicles | |
Shen et al. | Vision-based state estimation for autonomous rotorcraft MAVs in complex environments | |
Tisdale et al. | Autonomous UAV path planning and estimation | |
García Carrillo et al. | Combining stereo vision and inertial navigation system for a quad-rotor UAV | |
US20180088597A1 (en) | Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav) | |
Deng et al. | Indoor multi-camera-based testbed for 3-D tracking and control of UAVs | |
Sabatini et al. | Low-cost sensors data fusion for small size unmanned aerial vehicles navigation and guidance | |
Stegagno et al. | Relative localization and identification in a heterogeneous multi-robot system | |
Ma et al. | Adaptive vision-based guidance law with guaranteed performance bounds | |
Zhang et al. | Agile formation control of drone flocking enhanced with active vision-based relative localization | |
Zhang et al. | Leader–follower interactive potential for target enclosing of perception-limited UAV groups | |
Bryson et al. | Architectures for cooperative airborne simultaneous localisation and mapping | |
CN110823223A (zh) | 一种无人机群的路径规划方法及装置 | |
Xiang et al. | UAV based target tracking and recognition | |
Park et al. | Indoor path planning for an unmanned aerial vehicle via curriculum learning | |
Agarwal et al. | Evaluation of a commercially available autonomous visual inertial odometry solution for indoor navigation | |
CN114138022A (zh) | 一种基于精英鸽群智能的无人机集群分布式编队控制方法 | |
Garvey et al. | An autonomous unmanned aerial vehicle system for sensing and tracking | |
Chen et al. | Real-time tracking a ground moving target in complex indoor and outdoor environments with UAV | |
Owen et al. | Moving ground target tracking in urban terrain using air/ground vehicles | |
Sabatini et al. | RPAS navigation and guidance systems based on GNSS and other low-cost sensors | |
Kumar et al. | Development of Efficient Swarm Intelligence Algorithm for Simulating Two-Dimensional Orthomosaic for Terrain Mapping Using Cooperative Unmanned Aerial Vehicles | |
Ross et al. | Stochastic real-time optimal control for bearing-only trajectory planning |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |