基于人工势场法的无人机编队控制方法及装置与流程

本发明涉及无人机技术领域，尤其涉及一种基于人工势场法的无人机编队控制方法及装置。

背景技术：

随着航空技术的发展，无人机在民用和军用领域的应用越来越普遍。

无人机是在不搭载驾驶员的情况下，具有一定作业功能的飞行器，其在民用领域应用广泛，诸如农业植保，电力巡检，地理测绘和航拍航摄等。然而，不断复杂的应用场景以及无人机的集成化、小型化发展趋势，使得单一无人机受限于体积、能耗的制约，其在日益复杂的应用环境下工作能力较为有限。为了补偿单一无人机的局限性，多无人机编队的协同作业成为未来无人机应用的一大热点研究方向。

在多机编队协同中，各机通过相互配合提高任务完成质量，整个编队具有更高的任务执行能力和任务执行成功率。其中，若要使得一组无人机沿着预定航迹要求，保持一定队形飞行，则需要对编队进行控制。目前主要有三种控制策略。1)领航跟随法；领航者按照规划好的航迹飞行，跟随者参考编队中与领航者的相对位置飞行。领航跟随法的局限性在于，领航者没有跟随者的位置误差反馈，跟随者的位置误差迭代增加或者跟随者受到较大扰动时，无人机编队易失效；此外，领航者自身的失效故障也会导致整个编队的失效。2)虚拟领航法；虚拟领航法将编队看成刚体的虚拟结构，每架无人机是这个虚拟结构上位置相对固定的一点，编队移动时无人机跟踪其所对应的固定点。3)行为法；行为控制法由一系列不可再分的行为组成，每个行为具有各自的目标。基于行为控制的系统主要就是设计各种基本行为，基本行为包括搜索可能的避撞目标、编队保持、行为协调机制、行为仲裁方案等。该方法具有实时反馈的特点，是一种分布式的控制结构，不存在单点无人机失败导致整队无人机失败的问题，但是该控制策略难以系统化的进行数学分析和相应的稳定性分析。此外，对于编队的控制形式主要有集中式控制和分布式控制两种。对于集中式编队，无人机通过通信链路将各自的位置和意图信息发送到控制中心，由控制中心计算每架无人机在下一个时间周期的运行策略并通过通信链路发给该无人机执行。控制中心能够获取到编队中所有无人机的信息，进行集中优化调控。该方法计算量大，对无人机编队的控制中心的计算性能有较高要求。弊端是存在控制中心失效情况，系统稳健性较差。对于分布式编队，没有统一的控制中心进行策略规划，每架无人机将自己的位置、速度等信息直接与编队中的其他无人机进行交互，基于分布式控制策略计算自身的控制策略。编队中每架无人机都被认为是平等的，任何一架的失控都不会影响到其他无人机的运行，整个编队具有较强的稳健性和灵活性。此外，具体到对无人机飞行路径规划和多无人机的协同控制算法，常用的有人工势场法(artificialpotentialfield，简称“apf”)，其假设无人机的移动轨迹是在一个虚拟势场中受力移动的结果，该无人机受到飞行目标点处的引力，这种力使无人机向该目标点处移动；该无人机还受到障碍物的斥力，斥力将无人机推离障碍物或威胁源(例如，僚机对本机来说可以看作障碍物，斥力使得本机与其周边的僚机保持在一定距离范围内)。无人机通过引力和斥力的合力作用到达目标点，且具有规避障碍物的能力。但是传统的人工势场是全局的静态规划方法，无人机与障碍物之间的安全距离为固定值，这种对避障问题的简单化考虑，易造成避障过程中存在严重的无人机在安全距离边缘抖动情况，严重影响编队飞行控制。

技术实现要素：

本发明提供一种基于人工势场法的无人机编队控制方法及装置，基于人工势场法，根据采集获取到的无人机与周围僚机的速度矢量信息，综合考虑无人机与障碍物的速度大小及速度方向，并计算得到斥力，使得无人机所受到的斥力与引力合成的合力更加准确。这种全局的动态规划方法，有效地避免了无人机与障碍物安全距离边缘处的抖动问题，且该方法较好地满足了编队飞行的实时动态要求，使得编队中的各机具有较好的一致性性能。

本发明提供一种基于人工势场法的无人机编队控制方法，包括：

将编队坐标系转换到全局ned坐标系，确定编队中每个无人机在所述全局ned坐标系中的理想位置；

根据无人机的预定目标位置，无人机的所述理想位置，确定所述无人机所受到的所述预定目标位置对其施加的引力；

根据无人机的速度矢量、与所述无人机相对应的障碍物的速度矢量，确定所述无人机所受到的所述障碍物对其施加的斥力；

根据所述无人机受到的引力与所述无人机受到的所有障碍物对其施加的斥力，确定所述无人机所受到的合力；

根据所述无人机受到的所述合力，所述无人机及其周围僚机的飞行状态信息，确定所述无人机的运动趋势，以使控制器根据所述无人机的运动模型对无人机编队进行飞行控制。

本发明还提供一种基于人工势场法的无人机编队控制装置，包括：

转换模块，用于将编队坐标系转换到全局ned坐标系；

第一确定模块，用于确定编队中每个无人机在所述全局ned坐标系中的理想位置；

第二确定模块，用于根据无人机的预定目标位置，无人机的所述理想位置，确定所述无人机所受到的所述预定目标位置对其施加的引力；

第三确定模块，用于根据无人机的速度矢量、与所述无人机相对应的障碍物的速度矢量，确定所述无人机所受到的所述障碍物对其施加的斥力；

第四确定模块，用于根据所述无人机受到的引力与所述无人机受到的所有障碍物对其施加的斥力，确定所述无人机所受到的合力；

第五确定模块，用于根据所述无人机受到的所述合力，所述无人机及其周围僚机的飞行状态信息，确定所述无人机的运动趋势，以使控制器根据所述无人机的运动模型对无人机编队进行飞行控制。

本发明的基于人工势场法的无人机编队控制方法及装置，通过将编队坐标系转换到全局ned坐标系，确定编队中每个无人机在全局ned坐标系中的理想位置；根据无人机的预定目标位置，无人机的理想位置，确定无人机所受到的预定目标位置对其施加的引力；根据无人机的速度矢量、与无人机相对应的障碍物的速度矢量，确定无人机所受到的障碍物对其施加的斥力；根据无人机受到的引力与无人机受到的所有障碍物对其施加的斥力，确定无人机所受到的合力；根据无人机受到的合力，无人机及其周围僚机的飞行状态信息，确定无人机的运动趋势，以使控制器根据无人机的运动模型对无人机编队进行飞行控制。从而，通过综合考虑无人机与障碍物的速度大小及速度方向，得到更加符合实际情况的斥力，进而使得无人机所受到的斥力与引力合成的合力更加准确。这种全局的动态规划方法，有效地避免了无人机与障碍物安全距离边缘处的抖动问题，且该方法较好地满足了编队飞行的实时动态要求，使得编队中的各机具有较好的一致性性能。

附图说明

图1为本发明一示例性实施例示出的基于人工势场法的无人机编队控制方法的流程示意图；

图2为图1所示实施例的无人机编队动作控制的流程图；

图3为本发明另一示例性实施例示出的基于人工势场法的无人机编队控制方法的流程示意图；

图4为图3所示实施例的无人机编队坐标系示意图；

图5为图3所示实施例的无人机全局ned坐标系示意图；

图6为图3所示实施例的无人机与障碍物间安全距离的示意图；

图7为图3所示实施例的无人机与障碍物间斥力的示意图；

图8为图3所示实施例的无人机与障碍物间斥力分量的示意图；

图9为本发明一示例性实施例示出的基于人工势场法的无人机编队控制装置的结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明一示例性实施例示出的基于人工势场法的无人机编队控制方法的流程示意图，首先，介绍一下现有人工势场法(artificialpotentialfield，简称“apf”)的概况，人工势场法被广泛的用于机器人路径规划和多机器人协同控制上。最早是上世纪80年代由khatibt提出的用于静态全局的路径规划算法，它通过模拟出一个吸引力场和斥力场来完成目标的路径规划过程。吸引力场负责牵引机器人到达目标点，斥力场是由环境中的障碍物生成，当机器人处于障碍物的一定范围内时，斥力场负责排斥机器人远离障碍物以达到安全避障的作用。最终通过吸引力和斥力的合力作用使机器人到达目标点，并能够规避障碍物。具有物理意义明确，计算量小等优点。在无人机编队控制中，无人机需要保持一定的空间几何构型，也就是保持一定的相对距离，并以一致的速度沿着预定的航迹运行。在这个过程中，假设无人机通过他们之间的数据通信链路直接进行通信，交换各自的实时位置和速度信息，以用于僚机的机动动作决策。同时，根据安全需要，在生成编队或者改变编队队形的时候，每一架无人机需要根据周围的无人机或者障碍物的信息，自动生成无冲突的飞行控制策略，保持每一架无人机的相对安全距离和编队构型。但是人工势场法存在着局部最优与边界震荡等固有问题。许多学者针对这些问题做了许多工作。sfeir通过修改斥力函数，添加了一个乘数因子，解决局部最优问题。添加了垂直于目标连线位置的力来避免震荡。zhu针对无人机航迹规划问题，建立了比较完善的物理限制条件，并将安全距离的概念引入到避障中，但并没有考虑无人机速度与安全距离的关系。传统的人工势场是全局的静态规划方法，tingbin在引力函数中加入速度因子使其适应动态目标的跟踪。paul和yang等将apf应用于多机器人编队运行中，并针对局部最优和震荡问题进行了探讨，但是，针对多机器人之间的安全距离和避障等问题考虑比较简单，容易在避障过程中存在严重的抖动情况。本发明采用基于背景技术中所提及的行为法的编队控制策略，采用apf用于三维空间的无人机编队运行与避障控制中。该方法是一种分布式编队方法，没有集中式控制中的中心节点的控制，无人机直接与周围僚机进行信息交换，考虑包括定位与运行误差在内的因素，将传统的人工势场法进行修改，以适应编队的实时动态的要求，使得编队队伍中的各机具有较好的一致性性能。针对障碍物和无人机之间的冲突避障，设立与相对速度有关的安全距离，并针对避撞力进行了改进，有效避免了人工势场固有的局部最优与震荡问题。且通过仿真可以验证该算法应用于多无人机的编队与避障的有效性。

如图1所示，本实施例示出的基于人工势场法的无人机编队控制方法，包括：

步骤101、将编队坐标系转换到全局ned坐标系，确定编队中每个无人机在该全局ned坐标系中的理想位置。

具体的，前面背景技术部分介绍过编队控制的三种策略，其中，本发明采用的是基于行为法的编队策略。在领航跟随法、虚拟领航法中，各机是以某一个体或者抽象的个体为参考点进行跟踪飞行，在行为法中，没有这样可以供无人机自身参考的信息源，所有的个体在队伍中都是平等的，每个个体无人机直接从本机的传感器以及队伍中的僚机获取信息。这些信息可以是本机以及僚机的位置及速度信息，还可以是其他利于控制编队飞行队形的飞行状态信息。这些状态信息需要以统一的标准坐标系进行衡量，该统一的标准坐标系为全局ned(northeastdown，简称“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坐标系中的俯仰角确定平均速度vo在全局ned坐标系的水平面xgogyg的投影与坐标轴ogxg的夹角θ；根据俯仰角夹角θ，确定将编队坐标系转换到全局ned坐标系的转换矩阵c。

其中，

步骤203、根据转换矩阵c，确定编队中每个无人机在全局ned坐标系中的理想位置

式中，为无人机i在所述编队坐标系中的位置坐标，i≥2，po(t)为编队中各个无人机的中心位置坐标。

具体的，编队飞行的行为法控制策略中，由于没有可供无人机自身参考的信息源，所有的个体在队伍中都是平等的，每个个体无人机直接从本机的传感器以及队伍中的僚机获取信息，则编队的队形状态以各机的即时状态均值可以作为参考点。该参考点的即时状态均值为其中，n为编队中无人机的数量；xi(t)为无人机i的即时状态；po为各无人机的中心位置点；vo为各机的即时速度均值。

定义编队坐标系(ofxfyfzf)，该坐标系是一个笛卡尔直角坐标系。如图4所示。坐标原点为of，是各无人机的中心位置点po。坐标轴ofyf指向编队中所有无人机的平均速度vo的方向。坐标轴ofxf平行于ned坐标系中的水平面xgogyg，坐标轴ofzf垂直于编队坐标系水平面xfofyf。如图5所示，是vo在ned坐标系中的俯仰角，θ是vo在水平面xgogyg的投影于坐标轴ogxg的夹角。在这个编队参考坐标系中，无人机的编队由各机的理想位置定义，表示为

为了计算无人机在ned坐标中的理想位置，以便飞行控制器进行编队控制，需要通过转换矩阵将编队坐标从编队参考系中转换到全局ned坐标系中。该转换矩阵为

其中，rotaz表示z轴的旋转角度；rotax表示x轴的旋转角度。通过转换矩阵可以得到编队理想位置在全局ned坐标系中的坐标：因此，ned坐标系中各机实时目标编队位置为n为编队中无人机的数量值。

步骤204、获取编队整体所需要到达的目标点的位置坐标pg，获取各个无人机的中心位置坐标po(t)，获取无人机i的理想位置坐标获取无人机i的实时位置坐标pi(t)；根据pg、po(t)、pi(t)，确定无人机i受到的位置引力分量fp。

其中，式中，kg为第一位置权重因子、kp为第二位置权重因子。pg是编队整体所需要到达的目标点。这是一个全局目标点位置，可以是在任务执行之前提前指定的，为整个编队已知的前提条件，也可以是在编队运行过程中，沿预定航迹运行的航迹点。

步骤205、获取无人机i的理想速度获取无人机i的实时速度vi(t)；根据vi(t)，确定无人机i受到的速度引力分量fv。

其中，式中，kv为第一速度权重因子。

步骤206、根据位置引力分量fp、速度引力分量fv，确定无人机i所受到的预定目标位置对其施加的引力

该引力为

步骤207、获取无人机i的速度矢量获取无人机i对应的障碍物j的速度矢量根据确定垂直于的垂直斥力分量frep⊥和平行于的平行斥力分量frep//。

其中，

式中，rsafe为无人机与障碍物之间的安全距离；

为无人机与障碍物之间的最小安全距离；

ksafe为第二速度权重因子；

δ为无人机i的速度矢量与障碍物和无人机i位置连线之间的夹角；

vi为无人机i的速度矢量的大小；

vj为障碍物j的速度矢量的大小；

为无人机i的位置坐标pi与障碍物j的位置坐标pj之间的距离；

k为斥力权重因子；

步骤208、根据垂直斥力分量frep⊥和平行斥力分量frep//，确定无人机所受到的障碍物对其施加的斥力

式中，α为垂直斥力权重因子；β为平行斥力权重因子。

具体的，安全距离rsafe主要是为了实现障碍物和无人机之间的冲突避让。当无人机与目标障碍物的距离小于安全距离时，引入斥力引导无人机远离障碍目标。在现有技术中安全距离通常被定义为一个固定值或者只是与速度绝对值大小有关的值。然而，无人机总是具有最大速度和最大加速度的限制，当飞行速度过快，加速度值不足以使得无人机实现安全避障。如果将速度设置成只与速度大小有关，即使在高速运行时，也存在相对靠近但速度较小的情况。所以，本实施例中，将安全距离设定为与速度的大小和方向都有关的值。

如图6所示，可以将空间中不规则的障碍物进行膨胀，使其抽象的包含在一个最小球体内，其圆心位置为o，半径为r。无人机的最小安全距离为在t时刻，无人机对于该空间障碍物的安全距离为

其中，δ为无人机速度方向与障碍物和无人机位置连线的夹角。针对无人机与无人机之间的安全距离，综合考虑相关无人机的速度状态：

其中，δ为无人机的相对速度方向与无人机位置连线的夹角，如图7所示，可以看出，当无人机朝向障碍物或者两架无人机不断靠近时，相对速度越大，安全距离也越大。当无人机的距离小于安全距离时，就会引入斥力frep，使得无人机远离障碍物或者使两架无人机保持在安全距离之外。由于无人机的最大加速度的物理限制，当处于较高速度运行时，需要更长的安全距离来完成减速避障的动作。

在传统的人工势场法中，斥力frep(t)仅与个体和障碍物的相对位置状态有关。如传统的斥力定义为：

其中，为无人机与障碍物的距离，当其小于安全距离时，斥力作用于无人机进行避障。该斥力只与个体障碍物的位置信息有关，且总是反向沿着个体与障碍物的中心位置连线，如图7所示。假定目标障碍物为环境中的静态障碍物，虚线是最小安全距离范围。frep(t)是避障的斥力。当夹角δ比较小的时候，斥力的主要分量作用于减小速度的大小，但是对速度的方向改变较小。由于传统斥力只与无人机和障碍物的距离有关。虽然斥力能够减小无人机的速度，但是由于速度的方向改变较小，很容易造成无人机在避障界限的边缘来回反复。极端情况下，当斥力与和引力作用在相对位置连线时，无人机的速度只会产生大小的改变，方向依然朝着障碍物，最终导致避障失败。所以，本发明对传统的斥力函数进行修改，将无人机的速度大小作为影响斥力的因素之一，同时使斥力能够改变速度方向，而不仅仅是速度的大小。设是障碍物j对无人机i产生的斥力。参考图8，修改之后的斥力函数如下：

其中，δ为无人机速度方向与障碍物和无人机位置连线的夹角，frep与位置和相对速度方向都有关，α和β分别为权重因子。通过调节α和β来设定斥力对于速度的大小和方向的改变权重。这样可以使无人机在面对障碍物时避免因速度方向改变不足而导致避障失败的问题。在式中，表示本机与障碍物的相对速度在其位置方向上的分量，可以理解为本机与障碍相互靠近的速度。当靠近速度越大，斥力也越大。表示避障过程中障碍物的速度。在编队生成或者编队变换过程中，对每架无人机来说，障碍物既可以是空间中静态的障碍物，即为0；也可以是队伍中其他的无人机，即不为0。

假设某一时刻，无人机所能感知到周围的需要避障的目标为n个，那么就有总的避障力

进一步地，对于无人机编队中的局部最优问题，由于无人机在运行过程中，是受到合力作用的。但是当合力方向是沿着无人机与障碍物之间位置连线时，且速度方向与位置连线的夹角较小时，无人机将会在该连线方向陷入死循环(deadlock)。为了解决该问题，引入随机变换角θ，使其作用于引力fatt上，得到该与随机变换角θ有关的力frand，

是一个单位向量，并且垂直于当前速度v方向，k为权重因子。

进一步地，对于无人机所处的复杂环境影响，合力的计算还可以考虑加入无人机所受到的阻尼力。由于无人机是在力场下受合力的作用运行的，但是，当无人机朝着目标位置运行时，由合力产生的加速度不断作用于无人机本身，导致无人机在靠近目标位置时依然有可能保持一个较大的速度，从而飞跃目标点，由于吸引力总是指向目标点的，最终可能造成无人机在目标点周围来回震荡，所以有必要在总的合力的基础上添加一个沿着速度方向反向的阻尼力fdamp，fdamp＝-k·vi，vi为无人机i的速度，k为权重因子。

步骤209、根据无人机受到的引力与无人机受到的所有障碍物对其施加的斥力，确定无人机所受到的合力。

具体的，根据该时刻下各机的状态信息以及周围环境的信息，可以在势场中获得一个总的合力

该合力包含了此时无人机i运行的意图信息，包括加速度的大小以及加速度的方向。考虑无人机的最大飞行速度和最大加速度的限制，可以将合力限制在一定的范围内，同时保留该合力的方向，以及该合力内各个分量的相对配比大小。

其中，fmax(t)表示当前时刻下最大加速度的大小，其为无人机机动性能所限制的最大加速度fmax，

步骤210、获取无人机i以及编队中无人机i周边僚机的飞行状态信息yi(t)；根据获取到的无人机i的位置坐标pi(t)、无人机i的速度vi(t)，确定无人机i在t时刻的运行状态xi(t)。

具体的，飞行状态信息至少包括：无人机i的位置与速度信息、无人机i周边僚机的位置与速度信息；

步骤211、根据xi(t)，yi(t)，得到无人机的运动模型，确定其运动趋势，以使控制器根据运动模型对无人机编队进行飞行控制。

运动模型为：其中，

δt为所述飞行状态信息的采样间隔时间，i为单位矩阵；c为单位矩阵；

加速度反映了t时刻无人机i的运动趋势；其中，m为无人机i的质量；其中，f(t)为所述无人机i受到的所有障碍物对其施加的斥力与所述无人机i受到的引力之和，n为无人机i所能感知到的周围需要避障的障碍物的个数；

处理噪声sw为无人机飞行过程受到的内部及外部环境干扰的衡量参数；

测量噪声sv为传感器采集所述飞行状态信息的偏差参数。

具体的，该运动模型的应用场景可以为四旋翼飞行器模型。由于四旋翼的机械结构简单，运动灵活性好，在民用领域得到了很大的发展。将每一架无人机都看作是一个处于三维空间的质点，由笛卡尔ned坐标系描述。对每一架无人机i，xi(t)表示t时刻的其运行状态。其中pi(t)＝[pix,piy,piz]^t表示在t时刻，无人机i其空间所处位置；vi(t)＝[vix,viy,viz]^t，表示在参考坐标系中三个参考方向的速度大小。运动方程表示为：

其中，δt是系统的采样间隔，是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为采样间隔时间内的动量变化，由最大速度的限制可得

图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坐标系中的俯仰角确定平均速度vo在全局ned坐标系的水平面xgogyg的投影与坐标轴ogxg的夹角θ；根据俯仰角夹角θ，确定将编队坐标系转换到全局ned坐标系的转换矩阵c，

相应的，第一确定模块2，具体用于根据转换矩阵c，确定编队中每个无人机在全局ned坐标系中的理想位置其中，为无人机i在编队坐标系中的位置坐标，i≥2，po(t)为编队中各个无人机的中心位置坐标。

进一步地，第二确定模块3，具体用于获取编队整体所需要到达的目标点的位置坐标pg，获取各个无人机的中心位置坐标po(t)，获取无人机i的理想位置坐标获取无人机i的实时位置坐标pi(t)；根据pg、po(t)、pi(t)，确定无人机i受到的位置引力分量fp；其中，kg为第一位置权重因子、kp为第二位置权重因子；获取无人机i的理想速度获取无人机i的实时速度vi(t)；根据vi(t)，确定无人机i受到的速度引力分量fv；其中，kv为第一速度权重因子；根据位置引力分量fp、速度引力分量fv，确定无人机i所受到的预定目标位置对其施加的引力

进一步地，第三确定模块4，具体用于获取无人机i的速度矢量获取无人机i对应的障碍物j的速度矢量根据确定垂直于的垂直斥力分量frep⊥和平行于的平行斥力分量frep//；

其中，rsafe为无人机与障碍物之间的安全距离；

式中，为无人机与障碍物之间的最小安全距离；ksafe为第二速度权重因子；δ为无人机i的速度矢量与，障碍物和无人机i位置连线，之间的夹角；vi为无人机i的速度矢量的大小、vj为障碍物j的速度矢量的大小；为无人机i的位置坐标pi与障碍物j的位置坐标pj之间的距离；k为斥力权重因子；根据垂直斥力分量frep⊥和平行斥力分量frep//，确定无人机所受到的障碍物对其施加的斥力其中，α为垂直斥力权重因子；β为平行斥力权重因子。

进一步地，第五确定模块6，具体用于获取无人机i以及编队中无人机i周边僚机的飞行状态信息yi(t)；飞行状态信息至少包括：无人机i的位置与速度信息；无人机i周边僚机的位置与速度信息；根据获取到的无人机i的位置坐标pi(t)、无人机i的速度vi(t)，确定无人机i在t时刻的运行状态xi(t)，根据xi(t)，yi(t)，确定无人机的运动模型：

其中，δt为飞行状态信息的采样间隔时间，i为单位矩阵；c为单位矩阵；其中，无人机i的运动趋势包括：ai(t)；为t时刻无人机i的加速度，m为无人机i的质量；f(t)为无人机i受到的所有障碍物对其施加的斥力与无人机i受到的引力之和，n为无人机i所能感知到的周围需要避障的障碍物的个数；处理噪声sw为无人机飞行过程受到的内部及外部环境干扰的衡量参数；测量噪声sv为传感器采集飞行状态信息的偏差参数；根据确定的每架无人机的加速度信息，控制器基于无人机的运动模型对无人机编队进行飞行控制。

本实施例的装置，可用于执行图3所述基于人工势场法的无人机编队控制方法的实施例的技术方案，其实现原理和技术效果类似，此处不再赘述。

本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储介质包括：rom、ram、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。