CN108459612B - 基于人工势场法的无人机编队控制方法及装置 - Google Patents

基于人工势场法的无人机编队控制方法及装置 Download PDF

Info

Publication number
CN108459612B
CN108459612B CN201710092084.XA CN201710092084A CN108459612B CN 108459612 B CN108459612 B CN 108459612B CN 201710092084 A CN201710092084 A CN 201710092084A CN 108459612 B CN108459612 B CN 108459612B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
formation
coordinate system
determining
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
Application number
CN201710092084.XA
Other languages
English (en)
Other versions
CN108459612A (zh
Inventor
张学军
聂尊礼
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN201710092084.XA priority Critical patent/CN108459612B/zh
Publication of CN108459612A publication Critical patent/CN108459612A/zh
Application granted granted Critical
Publication of CN108459612B publication Critical patent/CN108459612B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

本发明涉及一种基于人工势场法的无人机编队控制方法及装置，通过将编队坐标系转换到全局NED坐标系，确定编队中每个无人机在全局NED坐标系中的理想位置；根据无人机的预定目标位置，无人机的理想位置，确定无人机所受到的预定目标位置对其施加的引力；根据无人机的速度矢量、与无人机相对应的障碍物的速度矢量，确定无人机所受到的障碍物对其施加的斥力；根据无人机受到的引力与无人机受到的所有障碍物对其施加的斥力，确定无人机所受到的合力；根据无人机受到的合力，无人机及其周围僚机的飞行状态信息，确定无人机的运动趋势，以使控制器根据无人机的运动模型对无人机编队进行飞行控制。

Description

基于人工势场法的无人机编队控制方法及装置
技术领域
本发明涉及无人机技术领域，尤其涉及一种基于人工势场法的无人机编队控制方法及装置。
背景技术
随着航空技术的发展，无人机在民用和军用领域的应用越来越普遍。
无人机是在不搭载驾驶员的情况下，具有一定作业功能的飞行器，其在民用领域应用广泛，诸如农业植保，电力巡检，地理测绘和航拍航摄等。然而，不断复杂的应用场景以及无人机的集成化、小型化发展趋势，使得单一无人机受限于体积、能耗的制约，其在日益复杂的应用环境下工作能力较为有限。为了补偿单一无人机的局限性，多无人机编队的协同作业成为未来无人机应用的一大热点研究方向。
在多机编队协同中，各机通过相互配合提高任务完成质量，整个编队具有更高的任务执行能力和任务执行成功率。其中，若要使得一组无人机沿着预定航迹要求，保持一定队形飞行，则需要对编队进行控制。目前主要有三种控制策略。1)领航跟随法；领航者按照规划好的航迹飞行，跟随者参考编队中与领航者的相对位置飞行。领航跟随法的局限性在于，领航者没有跟随者的位置误差反馈，跟随者的位置误差迭代增加或者跟随者受到较大扰动时，无人机编队易失效；此外，领航者自身的失效故障也会导致整个编队的失效。2)虚拟领航法；虚拟领航法将编队看成刚体的虚拟结构，每架无人机是这个虚拟结构上位置相对固定的一点，编队移动时无人机跟踪其所对应的固定点。3)行为法；行为控制法由一系列不可再分的行为组成，每个行为具有各自的目标。基于行为控制的系统主要就是设计各种基本行为，基本行为包括搜索可能的避撞目标、编队保持、行为协调机制、行为仲裁方案等。该方法具有实时反馈的特点，是一种分布式的控制结构，不存在单点无人机失败导致整队无人机失败的问题，但是该控制策略难以系统化的进行数学分析和相应的稳定性分析。此外，对于编队的控制形式主要有集中式控制和分布式控制两种。对于集中式编队，无人机通过通信链路将各自的位置和意图信息发送到控制中心，由控制中心计算每架无人机在下一个时间周期的运行策略并通过通信链路发给该无人机执行。控制中心能够获取到编队中所有无人机的信息，进行集中优化调控。该方法计算量大，对无人机编队的控制中心的计算性能有较高要求。弊端是存在控制中心失效情况，系统稳健性较差。对于分布式编队，没有统一的控制中心进行策略规划，每架无人机将自己的位置、速度等信息直接与编队中的其他无人机进行交互，基于分布式控制策略计算自身的控制策略。编队中每架无人机都被认为是平等的，任何一架的失控都不会影响到其他无人机的运行，整个编队具有较强的稳健性和灵活性。此外，具体到对无人机飞行路径规划和多无人机的协同控制算法，常用的有人工势场法(Artificial Potential Field，简称“APF”)，其假设无人机的移动轨迹是在一个虚拟势场中受力移动的结果，该无人机受到飞行目标点处的引力，这种力使无人机向该目标点处移动；该无人机还受到障碍物的斥力，斥力将无人机推离障碍物或威胁源(例如，僚机对本机来说可以看作障碍物，斥力使得本机与其周边的僚机保持在一定距离范围内)。无人机通过引力和斥力的合力作用到达目标点，且具有规避障碍物的能力。但是传统的人工势场是全局的静态规划方法，无人机与障碍物之间的安全距离为固定值，这种对避障问题的简单化考虑，易造成避障过程中存在严重的无人机在安全距离边缘抖动情况，严重影响编队飞行控制。
发明内容
本发明提供一种基于人工势场法的无人机编队控制方法及装置，基于人工势场法，根据采集获取到的无人机与周围僚机的速度矢量信息，综合考虑无人机与障碍物的速度大小及速度方向，并计算得到斥力，使得无人机所受到的斥力与引力合成的合力更加准确。这种全局的动态规划方法，有效地避免了无人机与障碍物安全距离边缘处的抖动问题，且该方法较好地满足了编队飞行的实时动态要求，使得编队中的各机具有较好的一致性性能。
本发明提供一种基于人工势场法的无人机编队控制方法，包括：
将编队坐标系转换到全局NED坐标系，确定编队中每个无人机在所述全局NED坐标系中的理想位置；
根据无人机的预定目标位置，无人机的所述理想位置，确定所述无人机所受到的所述预定目标位置对其施加的引力；
根据无人机的速度矢量、与所述无人机相对应的障碍物的速度矢量，确定所述无人机所受到的所述障碍物对其施加的斥力；
根据所述无人机受到的引力与所述无人机受到的所有障碍物对其施加的斥力，确定所述无人机所受到的合力；
根据所述无人机受到的所述合力，所述无人机及其周围僚机的飞行状态信息，确定所述无人机的运动趋势，以使控制器根据所述无人机的运动模型对无人机编队进行飞行控制。
本发明还提供一种基于人工势场法的无人机编队控制装置，包括：
转换模块，用于将编队坐标系转换到全局NED坐标系；
第一确定模块，用于确定编队中每个无人机在所述全局NED坐标系中的理想位置；
第二确定模块，用于根据无人机的预定目标位置，无人机的所述理想位置，确定所述无人机所受到的所述预定目标位置对其施加的引力；
第三确定模块，用于根据无人机的速度矢量、与所述无人机相对应的障碍物的速度矢量，确定所述无人机所受到的所述障碍物对其施加的斥力；
第四确定模块，用于根据所述无人机受到的引力与所述无人机受到的所有障碍物对其施加的斥力，确定所述无人机所受到的合力；
第五确定模块，用于根据所述无人机受到的所述合力，所述无人机及其周围僚机的飞行状态信息，确定所述无人机的运动趋势，以使控制器根据所述无人机的运动模型对无人机编队进行飞行控制。
本发明的基于人工势场法的无人机编队控制方法及装置，通过将编队坐标系转换到全局NED坐标系，确定编队中每个无人机在全局NED坐标系中的理想位置；根据无人机的预定目标位置，无人机的理想位置，确定无人机所受到的预定目标位置对其施加的引力；根据无人机的速度矢量、与无人机相对应的障碍物的速度矢量，确定无人机所受到的障碍物对其施加的斥力；根据无人机受到的引力与无人机受到的所有障碍物对其施加的斥力，确定无人机所受到的合力；根据无人机受到的合力，无人机及其周围僚机的飞行状态信息，确定无人机的运动趋势，以使控制器根据无人机的运动模型对无人机编队进行飞行控制。从而，通过综合考虑无人机与障碍物的速度大小及速度方向，得到更加符合实际情况的斥力，进而使得无人机所受到的斥力与引力合成的合力更加准确。这种全局的动态规划方法，有效地避免了无人机与障碍物安全距离边缘处的抖动问题，且该方法较好地满足了编队飞行的实时动态要求，使得编队中的各机具有较好的一致性性能。
附图说明
图1为本发明一示例性实施例示出的基于人工势场法的无人机编队控制方法的流程示意图；
图2为图1所示实施例的无人机编队动作控制的流程图；
图3为本发明另一示例性实施例示出的基于人工势场法的无人机编队控制方法的流程示意图；
图4为图3所示实施例的无人机编队坐标系示意图；
图5为图3所示实施例的无人机全局NED坐标系示意图；
图6为图3所示实施例的无人机与障碍物间安全距离的示意图；
图7为图3所示实施例的无人机与障碍物间斥力的示意图；
图8为图3所示实施例的无人机与障碍物间斥力分量的示意图；
图9为本发明一示例性实施例示出的基于人工势场法的无人机编队控制装置的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
图1为本发明一示例性实施例示出的基于人工势场法的无人机编队控制方法的流程示意图，首先，介绍一下现有人工势场法(Artificial PotentialField，简称“APF”)的概况，人工势场法被广泛的用于机器人路径规划和多机器人协同控制上。最早是上世纪80年代由Khatibt提出的用于静态全局的路径规划算法，它通过模拟出一个吸引力场和斥力场来完成目标的路径规划过程。吸引力场负责牵引机器人到达目标点，斥力场是由环境中的障碍物生成，当机器人处于障碍物的一定范围内时，斥力场负责排斥机器人远离障碍物以达到安全避障的作用。最终通过吸引力和斥力的合力作用使机器人到达目标点，并能够规避障碍物。具有物理意义明确，计算量小等优点。在无人机编队控制中，无人机需要保持一定的空间几何构型，也就是保持一定的相对距离，并以一致的速度沿着预定的航迹运行。在这个过程中，假设无人机通过他们之间的数据通信链路直接进行通信，交换各自的实时位置和速度信息，以用于僚机的机动动作决策。同时，根据安全需要，在生成编队或者改变编队队形的时候，每一架无人机需要根据周围的无人机或者障碍物的信息，自动生成无冲突的飞行控制策略，保持每一架无人机的相对安全距离和编队构型。但是人工势场法存在着局部最优与边界震荡等固有问题。许多学者针对这些问题做了许多工作。Sfeir通过修改斥力函数，添加了一个乘数因子，解决局部最优问题。添加了垂直于目标连线位置的力来避免震荡。ZHU针对无人机航迹规划问题，建立了比较完善的物理限制条件，并将安全距离的概念引入到避障中，但并没有考虑无人机速度与安全距离的关系。传统的人工势场是全局的静态规划方法，Tingbin在引力函数中加入速度因子使其适应动态目标的跟踪。Paul和Yang等将APF应用于多机器人编队运行中，并针对局部最优和震荡问题进行了探讨，但是，针对多机器人之间的安全距离和避障等问题考虑比较简单，容易在避障过程中存在严重的抖动情况。本发明采用基于背景技术中所提及的行为法的编队控制策略，采用APF用于三维空间的无人机编队运行与避障控制中。该方法是一种分布式编队方法，没有集中式控制中的中心节点的控制，无人机直接与周围僚机进行信息交换，考虑包括定位与运行误差在内的因素，将传统的人工势场法进行修改，以适应编队的实时动态的要求，使得编队队伍中的各机具有较好的一致性性能。针对障碍物和无人机之间的冲突避障，设立与相对速度有关的安全距离，并针对避撞力进行了改进，有效避免了人工势场固有的局部最优与震荡问题。且通过仿真可以验证该算法应用于多无人机的编队与避障的有效性。
如图1所示，本实施例示出的基于人工势场法的无人机编队控制方法，包括：
步骤101、将编队坐标系转换到全局NED坐标系，确定编队中每个无人机在该全局NED坐标系中的理想位置。
具体的，前面背景技术部分介绍过编队控制的三种策略，其中，本发明采用的是基于行为法的编队策略。在领航跟随法、虚拟领航法中，各机是以某一个体或者抽象的个体为参考点进行跟踪飞行，在行为法中，没有这样可以供无人机自身参考的信息源，所有的个体在队伍中都是平等的，每个个体无人机直接从本机的传感器以及队伍中的僚机获取信息。这些信息可以是本机以及僚机的位置及速度信息，还可以是其他利于控制编队飞行队形的飞行状态信息。这些状态信息需要以统一的标准坐标系进行衡量，该统一的标准坐标系为全局NED(North East Down，简称“NED”)坐标系，从而确定出编队中每架无人机其相对于该全局NED坐标系的理想位置点。
步骤102、根据无人机的预定目标位置，无人机的该理想位置，确定无人机所受到的预定目标位置对其施加的引力。
具体的，根据编队要求，无人机从初始位置开始运行，并形成预定的空间队形结构。采用APF的引力场牵引无人机到达队形中的预定目标位置。引力可以分为位置引力和速度引力。位置引力主要与无人机所在位置有关，基于无人机的理想位置计算得出位置引力，并由该位置引力负责牵引无人机到达编队中的指定位置。相应的速度引力与无人机此刻速度状态有关，负责改变各机的速度状态，使得整个队形保持一致的飞行速度。
步骤103、根据无人机的速度矢量、与无人机相对应的障碍物的速度矢量，确定无人机所受到的障碍物对其施加的斥力。
具体的，斥力主要是为了保障各机处于相对安全的间隔距离之外。主要有针对编队生成、编队变换和编队保持过程中，保障各无人机相对安全间隔和针对于周围障碍物保持安全距离，从而达到避障作用。基于无人机的速度矢量，以及根据各个无人机其所对应的障碍物自身的速度矢量，确定无人机受到的斥力。对编队中的无人机来说，每架无人机周边的僚机可能会成为该无人机的障碍物，尤其当两架或多架无人机偏离预期轨迹，而相互靠近的时候，其次，空中的其他环境因素都可能会构成障碍物。因此，通常来说，对一架无人机来说，其可能受到多个障碍物对其施加的斥力，则在计算斥力的时候，需要针对该无人机周围的障碍物，一个个的计算每个障碍物对其的斥力，在后续计算合力的时候，再将该无人机受到的所有斥力进行求和操作。
步骤104、根据无人机受到的引力与无人机受到的所有障碍物对其施加的斥力，确定无人机所受到的合力。
具体的，可以将吸引力(以Fatt表示)作用于保持无人机相对一致的空间队形和速度，斥力(以Frep表示)同样用于无人机编队针对空间障碍物的避障，更重要的是，可以将斥力场用于编队生成和变换过程中，无人机与无人机之间的安全间隔保持。最终，由通过合力(以Ftotal表示)Ftotal＝Fatt+Frep作用于每一架无人机上，作为编队意图信息，最后由无人机飞行控制系统生成控制信号来实现对无人机的编队动作控制。无人机的编队动作控制流程图如图2所示。机载传感器通过通信模块采集获取本机无人机以及僚机无人机的状态信息，然后采用人工势场法，根据采集得到的状态信息进行计算，得到无人机所受到的势场力(引力和斥力)，进而计算得到合力，并通过飞行控制器发送基于合力的控制信号给无人机，对其进行编队飞行轨迹的控制。
步骤105、根据无人机受到的合力，无人机及其周围僚机的飞行状态信息，确定无人机的运动趋势，以使控制器根据无人机的运动模型对无人机编队进行飞行控制。
具体的，如图2所示的无人机编队动作控制的流程图，飞行控制器通常可以根据人工势场法得到的合力，以及机载传感器获取得到的无人机及其周围僚机的飞行状态信息，控制器根据无人机的运动模型，通过控制信号对无人机的运动趋势，如加速度矢量，进行控制，以实现对编队中各个无人机飞行轨迹的精确控制。
本实施例的基于人工势场法的无人机编队控制方法，通过将编队坐标系转换到全局NED坐标系，确定编队中每个无人机在全局NED坐标系中的理想位置；根据无人机的预定目标位置，无人机的理想位置，确定无人机所受到的预定目标位置对其施加的引力；根据无人机的速度矢量、与无人机相对应的障碍物的速度矢量，确定无人机所受到的障碍物对其施加的斥力；根据无人机受到的引力与无人机受到的所有障碍物对其施加的斥力，确定无人机所受到的合力；根据无人机受到的合力，无人机及其周围僚机的飞行状态信息，确定无人机的运动趋势，以使控制器根据无人机的运动模型对无人机编队进行飞行控制。从而，通过综合考虑无人机与障碍物的速度大小及速度方向，得到更加符合实际情况的斥力，进而使得无人机所受到的斥力与引力合成的合力更加准确。这种全局的动态规划方法，有效地避免了无人机与障碍物安全距离边缘处的抖动问题，且该方法较好地满足了编队飞行的实时动态要求，使得编队中的各机具有较好的一致性性能。
图3为本发明另一示例性实施例示出的基于人工势场法的无人机编队控制方法的流程示意图，进一步的，在上述实施例的基础上，本实施的基于人工势场法的无人机编队控制方法，具体包括：
步骤201、定义编队坐标系为笛卡尔直角坐标系(OfXfYfZf)，定义全局NED坐标系为(OgXgYgZg)；以各个无人机的中心位置点po作为各个无人机在编队坐标系中的坐标原点Of，坐标轴OfYf指向编队中所有无人机的平均速度vo的方向，坐标轴OfXf平行于全局NED坐标系的水平面XgOgYg，坐标轴OfZf垂直于编队坐标系的水平面XfOfYf。
步骤202、确定平均速度vo在NED坐标系中的俯仰角
Figure BDA0001229192560000091
确定平均速度vo在全局NED坐标系的水平面XgOgYg的投影与坐标轴OgXg的夹角θ；根据俯仰角
Figure BDA0001229192560000092
夹角θ，确定将编队坐标系转换到全局NED坐标系的转换矩阵C。
其中，
Figure BDA0001229192560000093
步骤203、根据转换矩阵C，确定编队中每个无人机在全局NED坐标系中的理想位置
Figure BDA0001229192560000094
Figure BDA0001229192560000095
式中，
Figure BDA0001229192560000096
为无人机i在所述编队坐标系中的位置坐标，i≥2，po(t)为编队中各个无人机的中心位置坐标。
具体的，编队飞行的行为法控制策略中，由于没有可供无人机自身参考的信息源，所有的个体在队伍中都是平等的，每个个体无人机直接从本机的传感器以及队伍中的僚机获取信息，则编队的队形状态以各机的即时状态均值可以作为参考点。该参考点的即时状态均值为
Figure BDA0001229192560000097
其中，
Figure BDA0001229192560000098
N为编队中无人机的数量；xi(t)为无人机i的即时状态；po为各无人机的中心位置点；vo为各机的即时速度均值。
定义编队坐标系(OfXfYfZf)，该坐标系是一个笛卡尔直角坐标系。如图4所示。坐标原点为Of，是各无人机的中心位置点po。坐标轴OfYf指向编队中所有无人机的平均速度vo的方向。坐标轴OfXf平行于NED坐标系中的水平面XgOgYg，坐标轴OfZf垂直于编队坐标系水平面XfOfYf。如图5所示，
Figure BDA0001229192560000099
是vo在NED坐标系中的俯仰角，θ是vo在水平面XgOgYg的投影于坐标轴OgXg的夹角。在这个编队参考坐标系中，无人机的编队由各机的理想位置定义，表示为
Figure BDA00012291925600000910
为了计算无人机在NED坐标中的理想位置，以便飞行控制器进行编队控制，需要通过转换矩阵将编队坐标从编队参考系中转换到全局NED坐标系中。该转换矩阵为
Figure BDA0001229192560000101
其中，RotaZ表示Z轴的旋转角度；RotaX表示X轴的旋转角度。通过转换矩阵可以得到编队理想位置在全局NED坐标系中的坐标：
Figure BDA0001229192560000102
因此，NED坐标系中各机实时目标编队位置为
Figure BDA0001229192560000103
N为编队中无人机的数量值。
步骤204、获取编队整体所需要到达的目标点的位置坐标pg，获取各个无人机的中心位置坐标po(t)，获取无人机i的理想位置坐标
Figure BDA0001229192560000104
获取无人机i的实时位置坐标pi(t)；根据pg、po(t)、
Figure BDA0001229192560000105
pi(t)，确定无人机i受到的位置引力分量Fp。
其中，
Figure BDA0001229192560000106
式中，Kg为第一位置权重因子、Kp为第二位置权重因子。pg是编队整体所需要到达的目标点。这是一个全局目标点位置，可以是在任务执行之前提前指定的，为整个编队已知的前提条件，也可以是在编队运行过程中，沿预定航迹运行的航迹点。
步骤205、获取无人机i的理想速度
Figure BDA0001229192560000107
获取无人机i的实时速度vi(t)；根据
Figure BDA0001229192560000108
vi(t)，确定无人机i受到的速度引力分量Fv。
其中，
Figure BDA0001229192560000109
式中，Kv为第一速度权重因子。
步骤206、根据位置引力分量Fp、速度引力分量Fv，确定无人机i所受到的预定目标位置对其施加的引力
Figure BDA00012291925600001010
该引力为
Figure BDA00012291925600001011
步骤207、获取无人机i的速度矢量
Figure BDA00012291925600001012
获取无人机i对应的障碍物j的速度矢量
Figure BDA00012291925600001013
根据
Figure BDA00012291925600001014
确定垂直于
Figure BDA00012291925600001015
的垂直斥力分量Frep⊥和平行于
Figure BDA00012291925600001016
的平行斥力分量Frep//。
其中，
Figure BDA0001229192560000111
Figure BDA0001229192560000112
式中，rsafe为无人机与障碍物之间的安全距离；
Figure BDA0001229192560000113
Figure BDA0001229192560000114
为无人机与障碍物之间的最小安全距离；
Ksafe为第二速度权重因子；
δ为无人机i的速度矢量
Figure BDA0001229192560000115
与障碍物和无人机i位置连线之间的夹角；
vi为无人机i的速度矢量的大小；
vj为障碍物j的速度矢量的大小；
Figure BDA0001229192560000116
为无人机i的位置坐标pi与障碍物j的位置坐标pj之间的距离；
K为斥力权重因子；
步骤208、根据垂直斥力分量Frep⊥和平行斥力分量Frep//，确定无人机所受到的障碍物对其施加的斥力
Figure BDA0001229192560000117
Figure BDA0001229192560000118
式中，α为垂直斥力权重因子；β为平行斥力权重因子。
具体的，安全距离rsafe主要是为了实现障碍物和无人机之间的冲突避让。当无人机与目标障碍物的距离小于安全距离时，引入斥力引导无人机远离障碍目标。在现有技术中安全距离通常被定义为一个固定值或者只是与速度绝对值大小有关的值。然而，无人机总是具有最大速度和最大加速度的限制，当飞行速度过快，加速度值不足以使得无人机实现安全避障。如果将速度设置成只与速度大小有关，即使在高速运行时，也存在相对靠近但速度较小的情况。所以，本实施例中，将安全距离设定为与速度的大小和方向都有关的值。
如图6所示，可以将空间中不规则的障碍物进行膨胀，使其抽象的包含在一个最小球体内，其圆心位置为O，半径为r。无人机的最小安全距离为
Figure BDA0001229192560000121
在t时刻，无人机对于该空间障碍物的安全距离为
Figure BDA0001229192560000122
其中，δ为无人机速度方向与障碍物和无人机位置连线的夹角。针对无人机与无人机之间的安全距离，综合考虑相关无人机的速度状态：
Figure BDA0001229192560000123
其中，δ为无人机的相对速度方向与无人机位置连线的夹角，如图7所示，可以看出，当无人机朝向障碍物或者两架无人机不断靠近时，相对速度越大，安全距离也越大。当无人机的距离小于安全距离时，就会引入斥力Frep，使得无人机远离障碍物或者使两架无人机保持在安全距离之外。由于无人机的最大加速度的物理限制，当处于较高速度运行时，需要更长的安全距离来完成减速避障的动作。
在传统的人工势场法中，斥力Frep(t)仅与个体和障碍物的相对位置状态有关。如传统的斥力定义为：
Figure BDA0001229192560000131
其中，
Figure BDA0001229192560000132
为无人机与障碍物的距离，当其小于安全距离时，斥力作用于无人机进行避障。该斥力只与个体障碍物的位置信息有关，且总是反向沿着个体与障碍物的中心位置连线，如图7所示。假定目标障碍物为环境中的静态障碍物，虚线是最小安全距离范围。Frep(t)是避障的斥力。当夹角δ比较小的时候，斥力的主要分量作用于减小速度的大小，但是对速度的方向改变较小。由于传统斥力只与无人机和障碍物的距离有关。虽然斥力能够减小无人机的速度，但是由于速度的方向改变较小，很容易造成无人机在避障界限的边缘来回反复。极端情况下，当斥力与和引力作用在相对位置连线时，无人机的速度只会产生大小的改变，方向依然朝着障碍物，最终导致避障失败。所以，本发明对传统的斥力函数进行修改，将无人机的速度大小作为影响斥力的因素之一，同时使斥力能够改变速度方向，而不仅仅是速度的大小。设
Figure BDA0001229192560000133
是障碍物j对无人机i产生的斥力。参考图8，修改之后的斥力函数如下：
Figure BDA0001229192560000134
Figure BDA0001229192560000135
Figure BDA0001229192560000136
其中，δ为无人机速度方向与障碍物和无人机位置连线的夹角，Frep与位置和相对速度方向都有关，α和β分别为权重因子。通过调节α和β来设定斥力对于速度的大小和方向的改变权重。这样可以使无人机在面对障碍物时避免因速度方向改变不足而导致避障失败的问题。在式中，
Figure BDA0001229192560000141
表示本机与障碍物的相对速度在其位置方向上的分量，可以理解为本机与障碍相互靠近的速度。当靠近速度越大，斥力也越大。
Figure BDA0001229192560000142
表示避障过程中障碍物的速度。在编队生成或者编队变换过程中，对每架无人机来说，障碍物既可以是空间中静态的障碍物，即
Figure BDA0001229192560000143
为0；也可以是队伍中其他的无人机，即
Figure BDA0001229192560000144
不为0。
假设某一时刻，无人机所能感知到周围的需要避障的目标为N个，那么就有总的避障力
Figure BDA0001229192560000145
进一步地，对于无人机编队中的局部最优问题，由于无人机在运行过程中，是受到合力
Figure BDA0001229192560000146
作用的。但是当合力方向是沿着无人机与障碍物之间位置连线时，且速度方向与位置连线的夹角较小时，无人机将会在该连线方向陷入死循环(deadlock)。为了解决该问题，引入随机变换角θ，使其作用于引力Fatt上，得到该与随机变换角θ有关的力Frand，
Figure BDA0001229192560000147
Figure BDA0001229192560000148
是一个单位向量，并且垂直于当前速度v方向，K为权重因子。
进一步地，对于无人机所处的复杂环境影响，合力的计算还可以考虑加入无人机所受到的阻尼力。由于无人机是在力场下受合力的作用运行的，但是，当无人机朝着目标位置运行时，由合力产生的加速度不断作用于无人机本身，导致无人机在靠近目标位置时依然有可能保持一个较大的速度，从而飞跃目标点，由于吸引力总是指向目标点的，最终可能造成无人机在目标点周围来回震荡，所以有必要在总的合力的基础上添加一个沿着速度方向反向的阻尼力Fdamp，Fdamp＝-K·vi，vi为无人机i的速度，K为权重因子。
步骤209、根据无人机受到的引力与无人机受到的所有障碍物对其施加的斥力，确定无人机所受到的合力。
具体的，根据该时刻下各机的状态信息以及周围环境的信息，可以在势场中获得一个总的合力
Figure BDA0001229192560000151
该合力包含了此时无人机i运行的意图信息，包括加速度的大小以及加速度的方向。考虑无人机的最大飞行速度和最大加速度的限制，可以将合力限制在一定的范围内，同时保留该合力的方向，以及该合力内各个分量的相对配比大小。
Figure BDA0001229192560000152
其中，Fmax(t)表示当前时刻下最大加速度
Figure BDA0001229192560000153
的大小，其为无人机机动性能所限制的最大加速度Fmax，
Figure BDA0001229192560000154
步骤210、获取无人机i以及编队中无人机i周边僚机的飞行状态信息yi(t)；根据获取到的无人机i的位置坐标pi(t)、无人机i的速度vi(t)，确定无人机i在t时刻的运行状态xi(t)。
具体的，飞行状态信息至少包括：无人机i的位置与速度信息、无人机i周边僚机的位置与速度信息；
Figure BDA0001229192560000155
步骤211、根据xi(t)，yi(t)，得到无人机的运动模型，确定其运动趋势，以使控制器根据运动模型对无人机编队进行飞行控制。
运动模型为：
Figure BDA0001229192560000161
其中，
Figure BDA0001229192560000162
ΔT为所述飞行状态信息的采样间隔时间，I为单位矩阵；C为单位矩阵；
加速度
Figure BDA0001229192560000163
反映了t时刻无人机i的运动趋势；其中，m为无人机i的质量；其中，F(t)为所述无人机i受到的所有障碍物对其施加的斥力
Figure BDA0001229192560000164
与所述无人机i受到的引力
Figure BDA0001229192560000165
之和，N为无人机i所能感知到的周围需要避障的障碍物的个数；
处理噪声Sw为无人机飞行过程受到的内部及外部环境干扰的衡量参数；
测量噪声Sv为传感器采集所述飞行状态信息的偏差参数。
具体的，该运动模型的应用场景可以为四旋翼飞行器模型。由于四旋翼的机械结构简单，运动灵活性好，在民用领域得到了很大的发展。将每一架无人机都看作是一个处于三维空间的质点，由笛卡尔NED坐标系描述。对每一架无人机i，xi(t)表示t时刻的其运行状态。
Figure BDA0001229192560000166
其中pi(t)＝[pix,piy,piz]T表示在t时刻，无人机i其空间所处位置；vi(t)＝[vix,viy,viz]T，表示在参考坐标系中三个参考方向的速度大小。运动方程表示为：
Figure BDA0001229192560000167
Figure BDA0001229192560000168
Figure BDA0001229192560000169
其中，ΔT是系统的采样间隔，
Figure BDA00012291925600001610
是t时刻由的加速度，表明此时无人机的运动意图、运动趋势。yi(t)表示无人机通过传感器获取到的当前的系统状态信息。
在编队队伍中，各机通过机载的传感器设备，获取到自身当前的运动状态，并发送给各机。实际上，通过根据传感器的性能，获取的到的信息总是有偏差的。将其称作测量噪声，以Sv表示，是一个零均值的多维高斯正态分布。同时，根据上述的运动学方程，xi(t+1)＝Axi(t)+Bai(t)，由于无人机在t时刻根据势场法所计算出的此刻的控制变量ai(t)，并以此执行机动动作，无人机的运行总会收到环境因素的干扰，比如风等因素，并且由于图2中的飞行控制器内部存在的偏差和测量设备的偏差，飞行控制器所获取的加速度也一定会存在偏差，将其称作处理噪声，以Sw一个零均值的多维高斯正态分布对自身以及僚机的状态信息进行估计，各机需要对获取的信息进行卡尔曼滤波。
进一步地，在无人机运行过程中，最大飞行速度是一种重要的需要考虑的物理限制因素。在整过编队飞行过程中，任意一架无人机都不能超过其最大飞行速度。定义||Vmax||为最大速度。
vi(t)≤||Vmax||
在某个采样时刻，通过APF产生对于无人机的牵引力F，从而产生该时刻下的加速度a(t)＝{ax(t),ay(t),az(t)}，分别代表在该坐标系下三个方向上的加速度。根据牛顿运动学定律，ΔT·F为采样间隔时间内的动量变化，由最大速度的限制可得
Figure BDA0001229192560000171
图9为本发明一示例性实施例示出的基于人工势场法的无人机编队控制装置的结构示意图，如图9所示，该无人机编队控制装置包括：
转换模块1，用于将编队坐标系转换到全局NED坐标系。第一确定模块2，用于确定编队中每个无人机在全局NED坐标系中的理想位置。第二确定模块3，用于根据无人机的预定目标位置，无人机的理想位置，确定无人机所受到的预定目标位置对其施加的引力。第三确定模块4，用于根据无人机的速度矢量、与无人机相对应的障碍物的速度矢量，确定无人机所受到的障碍物对其施加的斥力。第四确定模块5，用于根据无人机受到的引力与无人机受到的所有障碍物对其施加的斥力，确定无人机所受到的合力。第五确定模块6，用于根据无人机受到的合力，无人机及其周围僚机的飞行状态信息，确定无人机的运动趋势，以使控制器根据无人机的运动模型对无人机编队进行飞行控制。
本实施例的装置，可用于执行图1所述基于人工势场法的无人机编队控制方法的实施例的技术方案，其实现原理和技术效果类似，此处不再赘述。
进一步地，在上述实施例的基础上，该基于人工势场法的无人机编队控制装置中的转换模块1，具体用于定义编队坐标系为笛卡尔直角坐标系(OfXfYfZf)，定义全局NED坐标系为(OgXgYgZg)；以各个无人机的中心位置点po作为各个无人机在编队坐标系中的坐标原点Of，坐标轴OfXf指向编队中所有无人机的平均速度vo的方向，坐标轴OfXf平行于全局NED坐标系的水平面XgOgYg，坐标轴OfZf垂直于编队坐标系的水平面XfOfYf；确定平均速度vo在NED坐标系中的俯仰角
Figure BDA0001229192560000181
确定平均速度vo在全局NED坐标系的水平面XgOgYg的投影与坐标轴OgXg的夹角θ；根据俯仰角
Figure BDA0001229192560000182
夹角θ，确定将编队坐标系转换到全局NED坐标系的转换矩阵C，
Figure BDA0001229192560000183
相应的，第一确定模块2，具体用于根据转换矩阵C，确定编队中每个无人机在全局NED坐标系中的理想位置
Figure BDA0001229192560000184
其中，
Figure BDA0001229192560000185
为无人机i在编队坐标系中的位置坐标，i≥2，po(t)为编队中各个无人机的中心位置坐标。
进一步地，第二确定模块3，具体用于获取编队整体所需要到达的目标点的位置坐标pg，获取各个无人机的中心位置坐标po(t)，获取无人机i的理想位置坐标
Figure BDA0001229192560000191
获取无人机i的实时位置坐标pi(t)；根据pg、po(t)、
Figure BDA0001229192560000192
pi(t)，确定无人机i受到的位置引力分量Fp；
Figure BDA0001229192560000193
其中，Kg为第一位置权重因子、Kp为第二位置权重因子；获取无人机i的理想速度
Figure BDA0001229192560000194
获取无人机i的实时速度vi(t)；根据
Figure BDA0001229192560000195
vi(t)，确定无人机i受到的速度引力分量Fv；
Figure BDA0001229192560000196
其中，Kv为第一速度权重因子；根据位置引力分量Fp、速度引力分量Fv，确定无人机i所受到的预定目标位置对其施加的引力
Figure BDA0001229192560000197
进一步地，第三确定模块4，具体用于获取无人机i的速度矢量
Figure BDA0001229192560000198
获取无人机i对应的障碍物j的速度矢量
Figure BDA0001229192560000199
根据
Figure BDA00012291925600001910
确定垂直于
Figure BDA00012291925600001911
的垂直斥力分量Frep⊥和平行于
Figure BDA00012291925600001912
的平行斥力分量Frep//；
Figure BDA00012291925600001913
Figure BDA00012291925600001914
其中，rsafe为无人机与障碍物之间的安全距离；
Figure BDA00012291925600001915
式中，
Figure BDA00012291925600001916
为无人机与障碍物之间的最小安全距离；Ksafe为第二速度权重因子；δ为无人机i的速度矢量
Figure BDA00012291925600001917
与，障碍物和无人机i位置连线，之间的夹角；vi为无人机i的速度矢量的大小、vj为障碍物j的速度矢量的大小；
Figure BDA0001229192560000201
为无人机i的位置坐标pi与障碍物j的位置坐标pj之间的距离；K为斥力权重因子；根据垂直斥力分量Frep⊥和平行斥力分量Frep//，确定无人机所受到的障碍物对其施加的斥力
Figure BDA0001229192560000202
其中，α为垂直斥力权重因子；β为平行斥力权重因子。
进一步地，第五确定模块6，具体用于获取无人机i以及编队中无人机i周边僚机的飞行状态信息yi(t)；飞行状态信息至少包括：无人机i的位置与速度信息；无人机i周边僚机的位置与速度信息；根据获取到的无人机i的位置坐标pi(t)、无人机i的速度vi(t)，确定无人机i在t时刻的运行状态xi(t)，
Figure BDA0001229192560000203
根据xi(t)，yi(t)，确定无人机的运动模型：
Figure BDA0001229192560000204
其中，
Figure BDA0001229192560000205
ΔT为飞行状态信息的采样间隔时间，I为单位矩阵；C为单位矩阵；其中，无人机i的运动趋势包括：ai(t)；
Figure BDA0001229192560000206
为t时刻无人机i的加速度，m为无人机i的质量；F(t)为无人机i受到的所有障碍物对其施加的斥力
Figure BDA0001229192560000207
与无人机i受到的引力
Figure BDA0001229192560000208
之和，N为无人机i所能感知到的周围需要避障的障碍物的个数；处理噪声Sw为无人机飞行过程受到的内部及外部环境干扰的衡量参数；测量噪声Sv为传感器采集飞行状态信息的偏差参数；根据确定的每架无人机的加速度信息，控制器基于无人机的运动模型对无人机编队进行飞行控制。
本实施例的装置，可用于执行图3所述基于人工势场法的无人机编队控制方法的实施例的技术方案，其实现原理和技术效果类似，此处不再赘述。
本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (8)

1.一种基于人工势场法的无人机编队控制方法，其特征在于，包括：
将编队坐标系转换到全局NED坐标系，确定编队中每个无人机在所述全局NED坐标系中的理想位置；
根据无人机的预定目标位置，无人机的所述理想位置，确定所述无人机所受到的所述预定目标位置对其施加的引力；
根据无人机的速度矢量、与所述无人机相对应的障碍物的速度矢量，确定所述无人机所受到的所述障碍物对其施加的斥力；
所述根据无人机的速度矢量、与所述无人机相对应的障碍物的速度矢量，确定所述无人机所受到的所述障碍物对其施加的斥力，包括：
获取无人机i的速度矢量
Figure FDA0002751347200000011
获取所述无人机i对应的障碍物j的速度矢量
Figure FDA0002751347200000012
根据所述
Figure FDA0002751347200000013
所述
Figure FDA0002751347200000014
确定垂直于所述
Figure FDA0002751347200000015
的垂直斥力分量Frep⊥和平行于所述
Figure FDA0002751347200000016
的平行斥力分量Frep//；
Figure FDA0002751347200000017
Figure FDA0002751347200000018
其中，rsafe为无人机与障碍物之间的安全距离；
Figure FDA0002751347200000019
式中，
Figure FDA0002751347200000021
为无人机与障碍物之间的最小安全距离；Ksafe为第二速度权重因子；δ为无人机i的速度矢量
Figure FDA0002751347200000022
与障碍物和无人机i位置连线，之间的夹角；vi为无人机i的速度矢量的大小、vj为障碍物j的速度矢量的大小；
Figure FDA0002751347200000023
为无人机i的位置坐标pi与障碍物j的位置坐标pj之间的距离；K为斥力权重因子；
根据所述垂直斥力分量Frep⊥和所述平行斥力分量Frep//，确定所述无人机所受到的所述障碍物对其施加的斥力
Figure FDA0002751347200000024
其中，α为垂直斥力权重因子；β为平行斥力权重因子；
根据所述无人机受到的引力与所述无人机受到的所有障碍物对其施加的斥力，确定所述无人机所受到的合力；
根据所述无人机受到的所述合力，所述无人机及其周围僚机的飞行状态信息，确定所述无人机的运动趋势，以使控制器根据所述无人机的运动模型对无人机编队进行飞行控制。
2.根据权利要求1所述的方法，其特征在于，所述将编队坐标系转换到全局NED坐标系，包括：
定义所述编队坐标系为笛卡尔直角坐标系(OfXfYfZf)，定义所述全局NED坐标系为(OgXgYgZg)；
以各个无人机的中心位置点po作为各个无人机在所述编队坐标系中的坐标原点Of，坐标轴OfYf指向编队中所有无人机的平均速度vo的方向，坐标轴OfXf平行于所述全局NED坐标系的水平面XgOgYg，坐标轴OfZf垂直于所述编队坐标系的水平面XfOfYf；
确定所述平均速度vo在NED坐标系中的俯仰角
Figure FDA0002751347200000025
确定所述平均速度vo在所述全局NED坐标系的水平面XgOgYg的投影与坐标轴OgXg的夹角θ；
根据所述俯仰角
Figure FDA0002751347200000031
所述夹角θ，确定将编队坐标系转换到全局NED坐标系的转换矩阵C，
Figure FDA0002751347200000032
相应的，所述确定编队中每个无人机在所述全局NED坐标系中的理想位置，包括：
根据所述转换矩阵C，确定编队中每个无人机在所述全局NED坐标系中的理想位置
Figure FDA0002751347200000033
Figure FDA0002751347200000034
其中，
Figure FDA0002751347200000035
为无人机i在所述编队坐标系中的位置坐标，i≥2，po(t)为编队中各个无人机的中心位置坐标。
3.根据权利要求2所述的方法，其特征在于，所述根据无人机的预定目标位置，无人机的所述理想位置，确定所述无人机所受到的所述预定目标位置对其施加的引力，包括：
获取编队整体所需要到达的目标点的位置坐标pg，获取各个无人机的中心位置坐标po(t)，获取无人机i的理想位置坐标
Figure FDA0002751347200000036
获取无人机i的实时位置坐标pi(t)；
根据所述pg、所述po(t)、所述
Figure FDA0002751347200000037
所述pi(t)，确定无人机i受到的位置引力分量Fp；
Figure FDA0002751347200000038
其中，Kg为第一位置权重因子、Kp为第二位置权重因子；
获取无人机i的理想速度
Figure FDA0002751347200000039
获取无人机i的实时速度vi(t)；
根据所述
Figure FDA00027513472000000310
所述vi(t)，确定无人机i受到的速度引力分量Fv；
Figure FDA00027513472000000311
其中，Kv为第一速度权重因子；
根据所述位置引力分量Fp、所述速度引力分量Fv，确定所述无人机i所受到的所述预定目标位置对其施加的引力
Figure FDA00027513472000000312
Figure FDA00027513472000000313
4.根据权利要求3所述的方法，其特征在于，所述根据所述无人机受到的所述合力，所述无人机及其周围僚机的飞行状态信息，确定所述无人机的运动趋势，以使控制器根据所述无人机的运动模型对无人机编队进行飞行控制包括：
获取无人机i以及编队中无人机i周边僚机的飞行状态信息yi(t)；所述飞行状态信息至少包括：无人机i的位置与速度信息；无人机i周边僚机的位置与速度信息；
根据获取到的所述无人机i的实时位置坐标pi(t)、所述无人机i的速度vi(t)，确定无人机i在t时刻的运行状态xi(t)，
Figure FDA0002751347200000041
根据所述xi(t)，所述yi(t)，得到所述无人机的运动模型：
Figure FDA0002751347200000042
其中，
Figure FDA0002751347200000043
ΔT为所述飞行状态信息的采样间隔时间，I为单位矩阵；C为转换矩阵；
其中，无人机i的运动趋势包括：ai(t)；
Figure FDA0002751347200000044
为t时刻无人机i的加速度，m为无人机i的质量；F(t)为所述无人机i受到的所有障碍物对其施加的斥力
Figure FDA0002751347200000045
与所述无人机i受到的引力
Figure FDA0002751347200000046
之和，N为无人机i所能感知到的周围需要避障的障碍物的个数；
处理噪声Sw为无人机飞行过程受到的内部及外部环境干扰的衡量参数；
测量噪声Sv为传感器采集所述飞行状态信息的偏差参数；
根据确定的每架无人机的加速度信息，控制器基于无人机的运动模型对无人机编队进行飞行控制。
5.一种基于人工势场法的无人机编队控制装置，其特征在于，包括：
转换模块，用于将编队坐标系转换到全局NED坐标系；
第一确定模块，用于确定编队中每个无人机在所述全局NED坐标系中的理想位置；
第二确定模块，用于根据无人机的预定目标位置，无人机的所述理想位置，确定所述无人机所受到的所述预定目标位置对其施加的引力；
第三确定模块，用于根据无人机的速度矢量、与所述无人机相对应的障碍物的速度矢量，确定所述无人机所受到的所述障碍物对其施加的斥力；
所述第三确定模块，具体用于获取无人机i的速度矢量
Figure FDA0002751347200000051
获取所述无人机i对应的障碍物j的速度矢量
Figure FDA0002751347200000052
根据所述
Figure FDA0002751347200000053
所述
Figure FDA0002751347200000054
确定垂直于所述
Figure FDA0002751347200000055
的垂直斥力分量Frep⊥和平行于所述
Figure FDA0002751347200000056
的平行斥力分量Frep//；
Figure FDA0002751347200000057
Figure FDA0002751347200000058
其中，rsafe为无人机与障碍物之间的安全距离；
Figure FDA0002751347200000059
式中，
Figure FDA00027513472000000510
为无人机与障碍物之间的最小安全距离；Ksafe为第二速度权重因子；δ为无人机i的速度矢量
Figure FDA00027513472000000511
与障碍物和无人机i位置连线之间的夹角；vi为无人机i的速度矢量的大小、vj为障碍物j的速度矢量的大小；
Figure FDA0002751347200000061
为无人机i的位置坐标pi与障碍物j的位置坐标pj之间的距离；K为斥力权重因子；根据所述垂直斥力分量Frep⊥和所述平行斥力分量Frep//，确定所述无人机所受到的所述障碍物对其施加的斥力
Figure FDA0002751347200000062
其中，α为垂直斥力权重因子；β为平行斥力权重因子；
第四确定模块，用于根据所述无人机受到的引力与所述无人机受到的所有障碍物对其施加的斥力，确定所述无人机所受到的合力；
第五确定模块，用于根据所述无人机受到的所述合力，所述无人机及其周围僚机的飞行状态信息，确定所述无人机的运动趋势，以使控制器根据所述无人机的运动模型对无人机编队进行飞行控制。
6.根据权利要求5所述的装置，其特征在于，
所述转换模块，具体用于定义所述编队坐标系为笛卡尔直角坐标系(OfXfYfZf)，定义所述全局NED坐标系为(OgXgYgZg)；以各个无人机的中心位置点po作为各个无人机在所述编队坐标系中的坐标原点Of，坐标轴OfYf指向编队中所有无人机的平均速度vo的方向，坐标轴OfXf平行于所述全局NED坐标系的水平面XgOgYg，坐标轴OfZf垂直于所述编队坐标系的水平面XfOfYf；确定所述平均速度vo在NED坐标系中的俯仰角
Figure FDA0002751347200000063
确定所述平均速度vo在所述全局NED坐标系的水平面XgOgYg的投影与坐标轴OgXg的夹角θ；根据所述俯仰角
Figure FDA0002751347200000064
所述夹角θ，确定将编队坐标系转换到全局NED坐标系的转换矩阵C，
Figure FDA0002751347200000065
相应的，所述第一确定模块，具体用于根据所述转换矩阵C，确定编队中每个无人机在所述全局NED坐标系中的理想位置
Figure FDA0002751347200000066
Figure FDA0002751347200000067
其中，
Figure FDA0002751347200000068
为无人机i在所述编队坐标系中的位置坐标，i≥2，po(t)为编队中各个无人机的中心位置坐标。
7.根据权利要求6所述的装置，其特征在于，
所述第二确定模块，具体用于获取编队整体所需要到达的目标点的位置坐标pg，获取各个无人机的中心位置坐标po(t)，获取无人机i的理想位置坐标
Figure FDA0002751347200000071
获取无人机i的实时位置坐标pi(t)；根据所述pg、所述po(t)、所述
Figure FDA0002751347200000072
所述pi(t)，确定无人机i受到的位置引力分量Fp；
Figure FDA0002751347200000073
其中，Kg为第一位置权重因子、Kp为第二位置权重因子；获取无人机i的理想速度
Figure FDA0002751347200000074
获取无人机i的实时速度vi(t)；根据所述
Figure FDA0002751347200000075
所述vi(t)，确定无人机i受到的速度引力分量Fv；
Figure FDA0002751347200000076
其中，Kv为第一速度权重因子；根据所述位置引力分量Fp、所述速度引力分量Fv，确定所述无人机i所受到的所述预定目标位置对其施加的引力
Figure FDA0002751347200000077
Figure FDA0002751347200000078
8.根据权利要求7所述的装置，其特征在于，
所述第五确定模块，具体用于获取无人机i以及编队中无人机i周边僚机的飞行状态信息yi(t)；所述飞行状态信息至少包括：无人机i的位置与速度信息；无人机i周边僚机的位置与速度信息；根据获取到的所述无人机i的实时位置坐标pi(t)、所述无人机i的速度vi(t)，确定无人机i在t时刻的运行状态xi(t)，
Figure FDA0002751347200000079
根据所述xi(t)，所述yi(t)，得到所述无人机的运动模型：
Figure FDA00027513472000000712
其中，
Figure FDA00027513472000000710
ΔT为所述飞行状态信息的采样间隔时间，I为单位矩阵；C为转换矩阵；其中，无人机i的运动趋势包括：ai(t)；
Figure FDA00027513472000000711
为t时刻无人机i的加速度，m为无人机i的质量；F(t)为所述无人机i受到的所有障碍物对其施加的斥力
Figure FDA0002751347200000081
与所述无人机i受到的引力
Figure FDA0002751347200000082
之和，N为无人机i所能感知到的周围需要避障的障碍物的个数；处理噪声Sw为无人机飞行过程受到的内部及外部环境干扰的衡量参数；测量噪声Sv为传感器采集所述飞行状态信息的偏差参数；根据确定的每架无人机的加速度信息，控制器基于无人机的运动模型对无人机编队进行飞行控制。
CN201710092084.XA 2017-02-21 2017-02-21 基于人工势场法的无人机编队控制方法及装置 Active CN108459612B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710092084.XA CN108459612B (zh) 2017-02-21 2017-02-21 基于人工势场法的无人机编队控制方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710092084.XA CN108459612B (zh) 2017-02-21 2017-02-21 基于人工势场法的无人机编队控制方法及装置

Publications (2)

Publication Number Publication Date
CN108459612A CN108459612A (zh) 2018-08-28
CN108459612B true CN108459612B (zh) 2021-03-23

Family

ID=63228900

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710092084.XA Active CN108459612B (zh) 2017-02-21 2017-02-21 基于人工势场法的无人机编队控制方法及装置

Country Status (1)

Country Link
CN (1) CN108459612B (zh)

Families Citing this family (49)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109445459B (zh) * 2018-10-25 2019-10-15 中国人民解放军战略支援部队航天工程大学 无人机集群编队控制方法及其装置
CN109542115B (zh) * 2018-10-29 2021-08-31 中国航空无线电电子研究所 一种多无人机编队飞行避障控制策略制定方法
CN109240341A (zh) * 2018-12-04 2019-01-18 中国航空无线电电子研究所 一种基于消息交互的无人机密集编队控制方法
CN109683628B (zh) * 2018-12-26 2022-01-25 哈尔滨工程大学 一种基于有限时间分布式速度观测器的航天器相对位置控制方法
CN109460064B (zh) * 2019-01-03 2019-11-15 中国人民解放军战略支援部队航天工程大学 基于虚拟势场函数的无人机集群区域覆盖方法及其装置
CN109870906B (zh) * 2019-02-25 2020-06-12 北京航空航天大学 一种基于bbo优化人工势场的高速旋翼飞行器路径规划方法
CN109839956B (zh) * 2019-03-04 2020-08-07 北京邮电大学 一种无人机的路径规划方法及装置
CN110096073A (zh) * 2019-04-18 2019-08-06 北京航空航天大学 仿信鸽智能行为的超大规模无人机集群控制系统及方法
CN112015198A (zh) * 2019-05-29 2020-12-01 北京京东尚科信息技术有限公司 一种无人机编队同步环绕的控制方法及系统
CN110134142B (zh) * 2019-05-29 2020-07-07 北京理工大学 一种基于旋转排斥场的旋翼无人机领航跟随编队控制方法
CN110320929B (zh) * 2019-06-04 2022-03-29 杭州电子科技大学 一种自主式无人机群目标跟随避障方法
CN110162096A (zh) * 2019-06-21 2019-08-23 南京邮电大学 基于人工势场法的无人机编队队形形成与保持方法
CN110377055A (zh) * 2019-08-14 2019-10-25 西南石油大学 基于改进型人工势场法的无人机三维编队方法
CN110488866B (zh) * 2019-08-20 2022-08-05 西南石油大学 一种基于梯度函数的无人机编队避障方法
CN110488867B (zh) * 2019-08-28 2021-11-23 中国人民解放军国防科技大学 一种基于改进虚拟力场的无人机集群护航行为生成方法
CN110488845A (zh) * 2019-09-18 2019-11-22 中国人民解放军火箭军工程大学 一种障碍物遮挡下多智能体自抗扰时变编队跟踪与避撞控制方法
CN110703751B (zh) * 2019-10-14 2022-09-30 东南大学 一种多移动机器人系统半自主编队及避障控制方法
CN110764531B (zh) * 2019-11-12 2023-01-03 西北工业大学 基于激光雷达与人工势场法的无人机编队飞行避障方法
CN110989656A (zh) * 2019-11-13 2020-04-10 中国电子科技集团公司第二十研究所 一种基于改进人工势场法的冲突解脱方法
CN111290429B (zh) * 2019-12-16 2023-04-21 南京信息工程大学 一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法
CN110986959B (zh) * 2019-12-31 2021-10-29 深圳市优必选科技股份有限公司 一种双足机器人及其导航定位方法和装置
CN111258332B (zh) * 2020-05-07 2020-08-07 北京航空航天大学 一种无人机群编队方法
CN112180954B (zh) * 2020-07-28 2021-12-24 北京理工大学 一种基于人工势场的无人机避障方法
CN112214034B (zh) * 2020-09-29 2022-04-08 南京航空航天大学 一种基于ZigBee的四旋翼飞行器编队飞行控制方法
CN112560241B (zh) * 2020-12-04 2022-08-16 北京航空航天大学 通讯不确定下的多智能体控制器和安全半径分离设计方法
CN112731962B (zh) * 2020-12-13 2022-12-06 西北工业大学 一种多无人系统协同编队控制方法
CN112783210B (zh) * 2021-01-04 2022-03-25 中国人民解放军国防科技大学 无人机集群控制系统的多目标控制参数调优方法
CN112678208B (zh) * 2021-01-20 2021-09-07 哈尔滨工业大学 基于包含动态规避和阻尼特性人工势场法的卫星集群控制方法
CN112965497A (zh) * 2021-02-23 2021-06-15 武汉理工大学 一种船舶编队控制方法、装置及存储介质
CN112947501B (zh) * 2021-03-04 2022-06-28 河海大学 一种基于改进人工势场法和状态切换的多auv混合编队方法
CN113268076B (zh) * 2021-03-06 2022-10-04 南京航空航天大学 一种多无人机集群编队协同控制算法
CN112947571A (zh) * 2021-03-11 2021-06-11 天津大学 一种人工势场和虚拟领航者的无人机编队形成和保持方法
DE102021203990A1 (de) 2021-04-21 2022-10-27 Rheinisch-Westfälische Technische Hochschule (Rwth) Aachen Verfahren und Vorrichtung zur Bestimmung einer kontinuierlichen Trajektorie eines Fahrzeuges, sowie deren Verwendung
CN113341956B (zh) * 2021-05-20 2022-10-28 西安交通大学 基于改进人工势场法的多智能体主从式编队控制方法
CN113485446B (zh) * 2021-08-12 2023-09-26 北京航空航天大学 一种无人飞艇编队飞行控制方法、系统及存储介质
CN113608555B (zh) * 2021-10-08 2021-12-21 广东博创佳禾科技有限公司 多无人机协同控制方法和装置
CN113867412B (zh) * 2021-11-19 2023-05-05 中国工程物理研究院电子工程研究所 一种基于虚拟领航的多无人机航迹规划方法
CN114063621B (zh) * 2021-11-22 2023-10-24 燕山大学 一种轮式机器人编队跟踪与避障控制方法
CN114200471B (zh) * 2021-12-07 2022-08-23 杭州电子科技大学信息工程学院 基于无人机的森林火源检测系统、方法、存储介质、设备
CN113961020B (zh) * 2021-12-22 2022-04-08 普宙科技(深圳)有限公司 一种无人机三维空间运动控制方法及系统
CN114296482B (zh) * 2021-12-27 2023-11-28 北京理工大学 一种基于自变增益策略的无人机集群避障方法
CN114355974B (zh) * 2021-12-30 2023-08-22 重庆擎羚科技有限公司 基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法
CN114510068B (zh) * 2022-02-24 2024-08-02 北京航空航天大学 一种基于信息融合的多无人机协同态势感知方法及系统
CN114859945A (zh) * 2022-05-26 2022-08-05 厦门大学 一种基于人工势场法的水下编队控制方法、系统及介质
CN114779827B (zh) * 2022-06-21 2022-09-06 四川腾盾科技有限公司 一种基于异构无人机编队虚拟势场协同避障拓扑控制方法
CN114779828B (zh) * 2022-06-22 2022-09-16 四川腾盾科技有限公司 基于异构队形基准点无人机集群拓扑控制与智能防撞方法
CN115145275B (zh) * 2022-06-24 2024-04-30 中国安全生产科学研究院 基于改进人工势场法的多机器人编队避障控制方法
CN115167528B (zh) * 2022-09-05 2022-12-02 北京航空航天大学 一种基于人工势场法的空间协同制导方法及装置
CN117369523A (zh) * 2023-12-05 2024-01-09 中国人民解放军国防科技大学 多无人机编队避障控制方法、装置、设备和介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102096415A (zh) * 2010-12-31 2011-06-15 重庆邮电大学 基于Ad-Hoc网络和leader-follower算法的多机器人编队方法
CN103365299A (zh) * 2013-08-02 2013-10-23 中国科学院自动化研究所 一种无人机的避障方法及其装置
US9436187B2 (en) * 2015-01-15 2016-09-06 The United States Of America As Represented By The Secretary Of The Navy System and method for decentralized, multi-agent unmanned vehicle navigation and formation control
CN106125760A (zh) * 2016-07-25 2016-11-16 零度智控（北京）智能科技有限公司 无人机编队路径自动规划方法及装置
CN107168329A (zh) * 2017-06-20 2017-09-15 南京长峰航天电子科技有限公司 基于跟随领航者法的航速航向协同控制编队航行控制方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101286071B (zh) * 2008-04-24 2010-11-10 北京航空航天大学 基于微粒群优化和遗传算法的多无人机三维编队重构方法
CN102541070A (zh) * 2012-01-20 2012-07-04 哈尔滨工业大学 一种卫星编队飞行地面试验系统的碰撞规避方法
US8463463B1 (en) * 2012-02-27 2013-06-11 Ge Aviation Systems Llc Methods for formation flying aided by a relative navigation system
CN102591358B (zh) * 2012-03-12 2015-07-08 北京航空航天大学 一种多无人机的动态编队控制方法
CN102749847A (zh) * 2012-06-26 2012-10-24 清华大学 多无人机协同着陆方法
CN103901887B (zh) * 2014-03-04 2017-05-24 重庆邮电大学 一种基于改进粒子群算法的多移动机器人编队控制方法
CN105718997B (zh) * 2016-01-15 2019-02-15 中国民航管理干部学院 基于人工势场法与蚁群算法的混合多飞行器冲突解脱方法
CN105629974B (zh) * 2016-02-04 2018-12-04 重庆大学 一种基于改进型人工势场法的机器人路径规划方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102096415A (zh) * 2010-12-31 2011-06-15 重庆邮电大学 基于Ad-Hoc网络和leader-follower算法的多机器人编队方法
CN103365299A (zh) * 2013-08-02 2013-10-23 中国科学院自动化研究所 一种无人机的避障方法及其装置
US9436187B2 (en) * 2015-01-15 2016-09-06 The United States Of America As Represented By The Secretary Of The Navy System and method for decentralized, multi-agent unmanned vehicle navigation and formation control
CN106125760A (zh) * 2016-07-25 2016-11-16 零度智控（北京）智能科技有限公司 无人机编队路径自动规划方法及装置
CN107168329A (zh) * 2017-06-20 2017-09-15 南京长峰航天电子科技有限公司 基于跟随领航者法的航速航向协同控制编队航行控制方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Local and asymmetrical potential field approach to leader tracking problem in rigid formations of fixed-wing UAVs;Cezary Kownacki;《Aerospace Science and Technology》;20170608;第465-474页 *
UAV Formation Flight based on Artificial Potential Force in 3D Environment;Nie Zunli 等;《CCDC》;20170530;第5465-5470页 *
基于改进人工势场的无人机编队防碰撞控制方法;朱旭 等;《哈尔滨工程大学学报》;20170415;第962-969页 *
多AUV主从式编队及避障控制方法;丁国华;《高技术通讯》;20140515;第538-544页 *

Also Published As

Publication number Publication date
CN108459612A (zh) 2018-08-28

Similar Documents

Publication Publication Date Title
CN108459612B (zh) 基于人工势场法的无人机编队控制方法及装置
Zhang et al. Multi-UAV formation control based on a novel back-stepping approach
CN108549407B (zh) 一种多无人机协同编队避障的控制算法
Liu et al. Mission-oriented miniature fixed-wing UAV swarms: A multilayered and distributed architecture
Ghamry et al. Formation control of multiple quadrotors based on leader-follower method
CN108958289A (zh) 基于相对速度障碍的集群无人机避碰方法
Kaminer et al. Coordinated path following for time-critical missions of multiple UAVs via L1 adaptive output feedback controllers
Zhu et al. Model of collaborative UAV swarm toward coordination and control mechanisms study
CN111596684A (zh) 固定翼无人机密集编队与防撞避障半实物仿真系统及方法
CN114138002A (zh) 分布式多无人机编队群集行为动态障碍物避障控制方法
CN117170410B (zh) 用于无人机编队飞行的控制方法及相关产品
Walid et al. Modeling and control of a quadrotor UAV
Oliveira et al. Three dimensional moving path following for fixed-wing unmanned aerial vehicles
Choutri et al. Quadrotors UAVs swarming control under Leader-Followers formation
CN107678442B (zh) 一种基于双模型下的四旋翼自主着船控制方法
Lin et al. Cooperative formation and obstacle avoidance algorithm for multi-uav system in 3d environment
Muslimov et al. Consensus-based cooperative circular formation control strategy for multi-UAV system
Bodi et al. Reinforcement learning based UAV formation control in GPS-denied environment
Tan et al. Tracking of ground mobile targets by quadrotor unmanned aerial vehicles
Zunli et al. UAV formation flight based on artificial potential force in 3D environment
Wang et al. Least global position information based control of fixed-wing UAVs formation flight: Flight tests and experimental validation
Velagić et al. Design of LQR controller for 3D trajectory tracking of octocopter unmanned aerial vehicle
Wang et al. Formation flight of unmanned rotorcraft based on robust and perfect tracking approach
Whitzer et al. In-flight formation control for a team of fixed-wing aerial vehicles
Davidi et al. Formation flight using multiple integral backstepping controllers

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