技术领域
[0001] 本
发明是一种仿信鸽智能行为的超大规模无人机集群控制系统及方法，属于无人机控制技术领域。
背景技术
[0002] 多无人机自主协同控制就是以多无人机协同系统为研究对象，在高度非结构化、不确定性的环境中，无需或最少人工干预，以集中或分布的方式选择和协同从而获得比独立设计单架无人机的控制
算法更有效的工作能
力。
[0003] 协同系统主要有以下几个方面的特点：1、信息的高度共享；2、任务的高度整合；3、资源的高度优化。具有这样特点的系统，将使得无人机集群具有更高的可靠性和安全性，并且更有可能在极端条件下完成复杂的任务。编队飞行作为协同系统的一部分，是无人机的前沿发展方向之一，具有广阔的前景。
[0004] 多无人机自主协同控制包括两大类方面的研究，一类是基于分层递阶方法的多无人机协同控制，另一类是基于自组织方法的多无人机协同控制。
[0005] 其中分层递阶方法在国内外已得到广泛的使用。具体来讲基于分层递阶方法集群编队控制主要有领航跟随法、虚拟领航法和行为控制法。领航跟随法中第一架无人机作为领航者，其他无人机作为跟随者，这种方法的局限性在于编队中的领航者没有跟随者的
位置误差反馈，当跟随者受到较大扰动、领航者的控制失效或故障将导致整个编队无法维持；虚拟领航法是指将编队队形看成一个刚体的虚拟结构，每架无人机是这个虚拟结构上相对位置固定的一点，当编队移动时，无人机跟随刚体对应固定点的运动即可。这种方法简化了任务的描述和分配，有较高的编队控制
精度，但难以进行容错处理，需要大量的通信，集中式控制，鲁棒性较差；行为控制法由一系列单
原子行为组成，每个行为具有各自的目标或者任务，这种设计基于行为的系统主要任务时设计各种基本行为。系统的柔性较好，可以适应动态加入新无人机的情况，且没有单点失败的结构；但该系统难以对系统进行数学分析和相应的
稳定性分析，比其他方法更复杂。
[0006] 尽管分层递阶结构的分解策略能够有效降低多无人机自主协同控制问题的求解难度和复杂性，但却面临着“主问题分解子问题建模-子问题求解-子问题协调”一系列复杂的求解流程。这种自顶向下的解决方案无疑意味着响应时间较长，因此基于自组织方法采用自底向上的解决方案，更加强调个体对环境的动态反应，以及多个个体之间基于规则的行为协调。可以认为，这类协调方法是一种分布式协调方法。多无人机自组织方法最初起源于对昆虫群落的行为研究，并逐渐发展形成了群集智能理论。这类方法通过模拟蜂群、蚁群、
鸟群、鱼群等
生物群体的行为实现多无人机分布式自组织控制，采用自底向上的数据驱动和建模策略，将简单对象构成大集合，通过简单
智能主体的聚集协同来实现全局的智能行为，具有计算简单、鲁棒性好等特点。
[0007] 从生物系统行为中获得灵感，是解决无人机编队飞行关键技术的有效途径。与无人机集群相似，很多飞行动物已经进化出了和他们的同伴协同飞行的能力。例如，大雁利用其他雁在“V”形成时产生的上冲气流，大大减少了个体体力的损失，飞行距离比同样
能量的单独飞行的候鸟多70％。但是在集群控制方面，现有仿大雁无人机编队仅设置一架长机控制所有僚机，长机传递和接受的信息过多，时间复杂度过大。鸟在个体和群体之间可以进行拓扑交互，这使得巨大的鸟群能够迅速转向而不会发生碰撞。但是，现有鸟群控制机制中头机数量较多，人工控制的复杂度增加，且每个头机控制的僚机数量较多，信息交互难度加大。
[0008] 鸽子作为一种非常常见的鸟类，以其惊人的导航能力，出色的飞行机制而备受关注。通过分析鸽子经过障碍树的飞行数据，有人提出了鸽子响应多个近距离障碍物的飞行路径调整方法。通过对多组鸽子的飞行行为记录进行分析，我们对鸽群决策进行了深入研究。在现有的Vicsek模型和其他的改进模型的
基础上，我们对鸽群飞行时的等级行为进行了建模，运用人工势场法描述了不同级别鸽子之间的影响，其优点在于具有较完善的拓扑结构，信息交互便捷，时间复杂度低，人工控制简单。在此基础上，设计了四旋翼
飞行器的
控制器。
发明内容
[0009] 本发明提出了一种仿信鸽智能行为的超大规模无人机集群控制系统及方法，其目的是对多架无人机信息交互进行规划设计及模拟信鸽智能行为的编队构建，同时实现了无人机集群编队保持，提高了无人机系统整体的抗干扰性和实时性，提供了一种在复杂多变的环境下的具有适应性的一种仿信鸽智能行为的超大规模无人机集群控制系统及方法。该发明可应用于自主大规模快速军事侦察、三维场景重建、工业勘察等领域。
[0010] 本发明针对多无人机自主编队控制问题，从自然生物群体-鸽群行为机制展开研究。并在此基础上，设计了一种仿信鸽智能行为的超大规模无人机集群控制系统及方法。
[0011] 一种仿信鸽智能行为的超大规模无人机集群控制系统，所述的无人机为四旋翼；所述的控制系统至少包括设置在每一架无人机上的飞行控制板、数据通信和处理模
块、接收机；所述的四旋翼飞行控制包括
姿态控制、速度控制、位置控制、状态估计器、多
传感器数据融合和故障保护，主要由飞行控制板上的处理器实现。该飞行控制板上设置有开源的
飞行控制系统。
[0012] 所述飞行控制板上的飞行控制系统如图2所示，其主要包括四个部分：鸽群编队模型、控制指令解算器、无人机模型、和状态转换器。
[0013] (1)鸽群编队模型具体为：利用
有向图描述鸽群中存在的拓扑结构对鸽群层级结构进行建模得到的模型、在已有群集模型基础上运用人工势场法对每只鸽子与其受控上级鸽子之间的领导作用进行建模得到的模型、以及各鸽子动力学模型，由此得到鸽群编队模型。其输入为来自状态转换器的各鸽子的位置和速度，输出为各鸽子的控制输入量。
[0014] (2)控制指令解算器：控制指令解算器的输入为各鸽子的控制输入量即总体输入量，输出为各无人机的
俯仰、
滚转和垂直方向的控制输入。
[0015] 其中，各鸽子的控制输入量ui由三部分组成，该三部分即为对应的无人机控制输入，可采用鸽群编队模型中的以下公式(4)～(8)分别对三部分进行求解；公式如下所示：
[0017]
[0018] 垂直方向，场函数：
[0019]
[0020] 速度一致由下式描述：
[0021]
[0022]
[0023] 其中，kv为速度反馈系数，kh为高度反馈系数，XYij代表X或Y方向上鸽子i与受控上级j的距离。
[0024] 则鸽子i的控制量ui可表示为：
[0025] ui＝αi+βi+γi (8)
[0026] (3)无人机模型：使用无人机的配有俯仰、滚转、
偏航和垂直方向
加速度4个通道的
自动驾驶仪,在编队中航向
角不产生作用，得到无人机模型：
[0027]
[0028] 其中(xi,yi,zi)为无人机i的位置，ψicθicλic为自动驾驶仪3个回路上的控制输入，g为
重力加速度，这里取9.8m/s2。
[0029] 该无人机模型的输入为控制指令解算器输出的三个方面的控制输入，输出为各无人机的姿态和位置。
[0030] (4)状态转换器：其输入为各无人机的姿态和位置，输出为各无人机的位置和速度。
[0031] 所述的数据通信和处理模块，用于实现四旋翼共享速度和位置信息，地面站监控整个群的状态。该速度和位置信息通过Xbee模块以Mavlink消息的形式传输，Mavlink消息定义了参数传输的规则。
[0032] 本发明的控制系统进一步包括有地面站，地面站是四旋翼无人机集群的指挥控制中心。通过Xbee模块采集各四旋翼无人机的运动数据和传感器参数，进行实时监测。当四旋翼无人机运动时，它们的轨迹被记录下来并显示在地图上。地面站可以向所有或其中一个四旋翼无人机发送解
锁、
起飞、切换模式、着陆和锁定等命令。
[0033] 一种仿信鸽智能行为的超大规模无人机集群控制方法，实现具体步骤如下：
[0034] 步骤一：地面站给出编队指令、目标位置和队形参数
[0035] 本发明的实现过程中使用mission planner地面站，为使无人机集群在任务开始之初能够做出编队形成和编队保持，并根据目标位置规划出飞行轨迹。因此首先给出编队指令、目标位置和队形参数。其中，编队指令包括无人机参数和无人机群层级参数。无人机参数又包括最大加速度ax,y,zMAX和最大飞行速度vx,y,zMAX,ZMAX是无人机所能到达的最大高度，这些参数均为保证飞行过程中的安全性能所设置；无人机群层级参数包括人工势场增益系数kp和ke，速度反馈系数kv和高度反馈系数kh。目标位置的给出仅需在地面站上输入坐标位置，或在地面站上直接点击确定目标位置。而队形参数则是为了使无人机集群保持一定的队形而确定的，例如保持空间正四面体形，则需要给出顶角和边长的参数。
[0036] 这些参数的确定必须满足对应的安全性条件，无人机参数的设定是为了确保在飞行过程中发生事故，不会引起较大的破坏和损失；队形参数则需要保证各飞机之间的最小安全距离，从而使得各飞机之间不会有相互之间的
气动影响引起飞行性能的偏差，以及给各无人机之间冗余的信息处理时间。无人机群层级参数的设定则是通过不断的仿真实验所确定出来的能够达到最佳实验性能的参数。
[0037] 步骤二：各无人机发布自己的位置信息
[0038] 为进一步形成编队，各无人机需发布自己的位置信息，位置信息包括无人机的位置(xi,yi,zi)和速度vi。第一次各无人机发布自己位置信息之后需切换至Guided模式并进行解锁和起飞，如为从步骤六返回，则无需进行切换模式、解锁和起飞操作，直接进入步骤三。
[0039] 此步骤的便利之处在于：仅需输入无人机的位置和速度，无需其他附余的参数，因而在实际飞行过程中，无人机之间的通讯以及地面站与无人机之间的通讯都将得到相应的简化。这将大大降低通讯的难度，并对时间复杂度进行了优化。这样的位置信息，也充分体现了在鸽群机制中每只鸽子只需接收局部的信息来得到全局意识的特性。
[0040] 步骤三：鸽群层级拓扑结构、单只鸽子动力学模型的建立，求得僚机的受控上级集合Ni
[0041] 鸽群行为机制中,不同个体间存在严格等级关系.头鸽处于绝对领导地位,其余跟随鸽下层服从上层,但无法影响上层。即:下层鸽子行为不仅受头鸽影响,也受其他上层鸽子的影响,而往往来自于临近上级的影响更为直接迅速。
[0042] 因此，我们规定，头鸽的等级为一级，它是自由的，除头鸽外，每一级由2只鸽子组成。二级的两只鸽子仅受头鸽领导，但它们的相对位置不固定。在实验中，我们
指定了它们相对于头鸽的位置。其余鸽子均受三只上级鸽子的领导，根据几何原理,与空间内不共线三点的确定距离可唯一确定一点，因此，它们具有固定的相对位置。由此建立的鸽群层级有向图如图3所示。
[0043] 综上,令Ni为对鸽子i存在领导作用的上级鸽子集合,鸽群中的领导关系,可用Ni表示为
[0044]
[0045] 设n只鸽子在3维欧式空间内无障碍飞行，则他们可被视作质点，根据
牛顿第二定律，每只鸽子的动力学模型如下：
[0046]
[0047] vi∈R3为鸽子i的速度矢量，mi为鸽子的
质量，ui∈R3为鸽子的控制输入，ki>0为阻力系数。
[0048] 步骤四：建立鸽群层级机制，根据受控上级位置信息，得下一时刻僚机理想控制量[0049] 根据鸽群编队模型中的受控上级集合，鸽群中上级对下级的领导作用,使下级在避免与上级发生碰撞(即防撞)的基础上,尝试与其保持接近(即聚集),同时试图与其速度匹配(即速度一致)。
[0050] 本方法使用人工势场法来描述防撞和聚集两个机制。
[0051] 对于水平方向，场函数：
[0052]
[0053] Uatt是引力场，Ureq是斥力场，
[0054] 分别代表鸽子i与受控上级j之间的真实距离和期望距离,kp和ke为人工势场增益系数。
[0055] 质点在场中某一方向上受力大小为势场在该方向上的偏导数，因此每只鸽子在场中的受力为：
[0056]
[0057] 垂直方向，场函数：
[0058]
[0059] 速度一致由下式描述：
[0060]
[0061]
[0062] 其中，kv为速度反馈系数，kh为高度反馈系数，XYij代表X或Y方向上鸽子i与受控上级j的距离。
[0063] 鸽子i的控制量ui可表示为：
[0064] ui＝αi+βi+γi (8)
[0065] 根据上述式子，各无人机的控制输入ψicθicλic可以被计算出。
[0066] 步骤五：对无人机建模，解算僚机自动驾驶仪控制输入
[0067] 本发明使用无人机的配有俯仰、滚转、偏航和垂直方向加速度4个通道的自动驾驶仪,在编队中航向角不产生作用，则无人机可以简化为如下模型：
[0068]
[0069] 其中(xi,yi,zi)为无人机i的位置，ψicθicλic为自动驾驶仪3个回路上的控制输入，g为重力加速度，这里取9.8m/s2。
[0070] 考虑无人机的实用性约束：
[0071]
[0072] axMAX,ayMAX,azMAX分别是三个
自由度上的最大过载。
[0073] 自驾仪控制输入可由(2)和(10)解算出。
[0074] 步骤六：返回步骤二，直到任务终止
[0075] 鸽群中，一旦确定了目的地，头鸽会飞在整个群体的前面，其他鸽子会寻找自己的上级。每一只鸽通过视觉观察集群中其他个体的位置和速度，以及与自己的距离来做出决策。对于无人机来说，个体通过网络广播自己的位置和速度信息，然后根据自己的等级寻找上级，按照上述鸽群机制和队形参数输出姿态控制量,返回步骤二，直到任务终止。
[0076] 本发明提出了一种仿信鸽智能行为的超大规模无人机集群控制系统及方法，为无人机集群在复杂多变的环境下执行任务提供了一种具有较强鲁棒性的编队系统和方法。本方法联系自然界中鸽群的行为方式建立了鸽群层级拓扑结构和单只鸽子的动力学模型，依据鸽群中的领导模式构建出了受控上级集合。在人工势场法的基础上，建立了鸽群层级机制。并根据受控上级位置信息，得到了下一时刻僚机理想控制量。对无人机建模，解算僚机自动驾驶仪控制输入，进而更新无人机集群的位置。本方法对多架无人机进行信息交互规划设计，增强了单架无人机系统所需提高的监视范围、角度、工作效率、抗干扰性；通过模拟实现鸽群的层级结构，使得无人机集群相对于一般的自顶而下的分层递阶式方法具有较高的鲁棒性能且响应时间较短，除此之外与本方法与其他自组织式方法相比具有独一无二的拓扑结构和编队构成。本方法提高了无人机对于复杂动态任务环境下适应性。
附图说明
[0077] 图1仿信鸽智能行为的超大规模集群系统
框架[0079] 图3鸽群层级有向图
[0080] 图4集群系统实现流程示意图
[0081] 图5三维轨迹图
[0082] 图6二维轨迹图
[0083] 图7google地图下的飞行轨迹
[0084] 图1中标号及符号说明如下：
[0085] Mavlink 一种通用的飞行控制器通讯协议；
[0086] Xbee 一种IEEE 802.15.4标准的无线通讯模块；
[0087] S-BUS 用于遥控输入的
舵机控制总线
[0088] 图2中标号及符号说明如下：
[0089] ui 鸽子的控制输入；
[0090] ψicθicλic 无人机的控制输入(分别为俯仰角、滚转角和偏航角)；
[0091] ψi,θi,λi,xi,yi,zi 无人机的位置状态(分别为俯仰角、滚转角、偏航角、x坐标、y坐标和z坐标)；
[0092] vi,Xi 无人机的速度和位置
具体实施方式
[0093] 下面通过一个具体的无人机集群实例来验证本发明所提出方法的有效性。实验计算机配置为i5-5200U处理器，2.20GHz主频，4G内存，
软件环境为R2016a版本的Matlab。
[0094] 一种仿信鸽智能行为的超大规模无人机集群控制系统的框架如图1所示。所述的无人机为四旋翼；所述的控制系统至少包括设置在每一架无人机上的飞行控制板、数据通信和处理模块、接收机；所述的四旋翼飞行控制包括姿态控制、速度控制、位置控制、状态估计器、多
传感器数据融合和故障保护，主要由飞行控制板上的处理器实现。该飞行控制板上设置有开源的飞行控制系统Ardupilot。它支持多种飞行模式(稳定，游荡，制导)。测试中采用程序命令控
制模式(制导)，任务管理层通过调用姿态控制
接口，实现了基于鸽群行为机制的编队。
[0095] 所述的飞行控制系统如图2所示，其主要包括四个部分：鸽群编队模型、控制指令解算器、无人机模型、和状态转换器。
[0096] (1)鸽群编队模型具体为：利用有向图描述鸽群中存在的拓扑结构对鸽群层级结构进行建模得到的模型、在已有群集模型基础上运用人工势场法对每只鸽子与其受控上级鸽子之间的领导作用进行建模得到的模型、以及各鸽子动力学模型，由此得到鸽群编队模型。其输入为来自状态转换器的各鸽子的位置和速度，输出为各鸽子的控制输入量。
[0097] 针对鸽群飞行机制中特有的层级行为，分析了无人机编队飞行中与其相仿的仿生机理，并从两个方面进行了建模：一方面，利用有向图描述鸽群中存在的拓扑结构，对鸽群层级结构进行建模；另一方面，在已有群集模型(Vicsek模型)的基础上，运用人工势场法对每只鸽子与其受控上级鸽子之间的领导作用进行建模。在此基础上，附加上对各鸽子动力学模型的建立，由此得到鸽群编队模型。鸽群编队模型基于稳定性、层次等级和信息交互三个层面进行设计，并且其具有使鸽群聚集、鸽群内各鸽子之间防撞和速度一致性的功能。其输入为来自状态转换器的各鸽子的位置和速度，输出为各鸽子的控制输入量。
[0098] (2)控制指令解算器：控制指令解算器的输入为各鸽子的控制输入量(即总体输入量)，输出为各无人机的俯仰、滚转和垂直方向的控制输入。各鸽子的控制输入量ui由三部分组成，该三部分即为对应的无人机控制输入，可采用鸽群编队模型中的以下公式(4)～(8)分别对三部分进行求解。公式如下所示：
[0099] 水平方向，场函数：
[0100]
[0101] 垂直方向，场函数：
[0102]
[0103] 速度一致由下式描述：
[0104]
[0105]
[0106] 其中，kv为速度反馈系数，kh为高度反馈系数，XYij代表X或Y方向上鸽子i与受控上级j的距离。
[0107] 则鸽子i的控制量ui可表示为：
[0108] ui＝αi+βi+γi (8)
[0109] (3)无人机模型：使用无人机的配有俯仰、滚转、偏航和垂直方向加速度4个通道的自动驾驶仪,在编队中航向角不产生作用，得到无人机模型：
[0110]
[0111] 其中(xi,yi，zi)为无人机i的位置，ψicθicλic为自动驾驶仪3个回路上的控制输入，g为重力加速度，这里取9.8m/s2。
[0112] 该无人机模型的输入为控制指令解算器输出的三个方面的控制输入，输出为各无人机的姿态和位置。
[0113] (4)状态转换器：其输入为各无人机的姿态和位置，输出为各无人机(即各鸽子)的位置和速度。
[0114] 所述的数据通信和处理模块，对四旋翼的形成至关重要，因为四旋翼必须共享速度和位置信息，地面站需要监控整个群的状态。该速度和位置信息通过Xbee模块以Mavlink消息的形式传输，Mavlink消息定义了参数传输的规则。
[0115] 在所述的飞行控制系统Ardupilot中，任务管理作为一个线程循环，
频率为20Hz。它负责任务的选择，任务的启动和停止，向其他四旋翼飞行器发送位置和速度信息，接收和存储其他飞行器的数据。基于鸽群机制的群体形成是管理任务之一。除了完成编队任务外，任务管理还可以通过地面站加载和切换其他任务。
[0116] 进一步的，本发明的控制系统进一步包括有地面站，地面站是四旋翼无人机集群的指挥控制中心。通过Xbee模块采集各四旋翼无人机的运动数据和传感器参数，进行实时监测。当四旋翼无人机运动时，它们的轨迹被记录下来并显示在地图上。地面站可以向所有或其中一个四旋翼无人机发送解锁、起飞、切换模式、着陆和锁定等命令。
[0117] 本发明实例的一种仿信鸽智能行为的超大规模无人机集群控制方法，具体实现步骤如下：
[0118] 步骤一：地面站给出编队指令、目标位置和队形参数
[0119] 本发明的实现过程中使用mission planner地面站，为使无人机集群系统在任务开始之初能够做出编队形成和编队保持，并根据目标位置规划出飞行轨迹。因此首先给出编队指令、目标位置和队形参数。其中，编队指令包括无人机参数和无人机群层级参数。无人机参数又包括最大加速度ax,y,zMAX和最大飞行速度vx,y,zMAX，h是无人机所能到达的最大高度，这些参数均为保证飞行过程中的安全性能所设置；无人机群层级参数包括人工势场增益系数kp和k，速度反馈系数kv和高度反馈系数kh。目标位置的给出仅需在地面站上输入坐标位置，或在地面站上直接点击确定目标位置。而队形参数则是为了使无人机集群保持一定的队形而确定的，例如保持空间正四面体形，则需要给出顶角和边长的参数。
[0120] 这些参数的确定必须满足对应的安全性条件，无人机参数的设定是为了确保在飞行过程中发生事故，不会引起较大的破坏和损失；队形参数则需要保证各飞机之间的最小安全距离，从而使得各飞机之间不会有相互之间的气动影响引起飞行性能的偏差，以及给各无人机之间冗余的信息处理时间。无人机群层级参数的设定则是通过不断的仿真实验所确定出来的能够达到最佳实验性能的参数。
[0121] 在本次具体实例过程中，确定无人机集群总架数5，让五架无人机作为顶角为60度边长为5米的四面体的四个
顶点及其中心。并且给定以下参数的初始值：kp＝4,kv＝2,ke＝1.2,kh＝2,m＝1,ax,y,zMAX＝2,vx,y,zMAX＝3,h＝8。
[0122] 步骤二：各无人机发布自己的位置信息
[0123] 为进一步形成编队，各无人机需发布自己的位置信息，位置信息包括无人机的位置(xi,yi,zi)和速度vi。第一次各无人机发布自己位置信息之后需切换至Guided模式并进行解锁和起飞，如为从步骤六返回，则无需进行切换模式、解锁和起飞操作，直接进入步骤三。
[0124] 此步骤的便利之处在于：仅需输入无人机的位置和速度，无需其他附余的参数，因而在实际飞行过程中，无人机之间的通讯以及地面站与无人机之间的通讯都将得到相应的简化。这将大大降低通讯的难度，并对时间复杂度进行了优化。这样的位置信息，也充分体现了在鸽群机制中每只鸽子只需接收局部的信息来得到全局意识的特性。
[0125] 在本次具体实例过程中，速度参数设置为0.0，四架无人机的位置参数分别设置为：(0,0,0)(2,0,0)(4,0,0)(6,0,0)(8,0,0)。
[0126] 步骤三：鸽群层级拓扑结构、单只鸽子动力学模型的建立，求得僚机的受控上级集合Ni
[0127] 鸽群行为机制中,不同个体间存在严格等级关系.头鸽处于绝对领导地位,其余跟随鸽下层服从上层,但无法影响上层.；即:下层鸽子行为不仅受头鸽影响,也受其他上层鸽子的影响,而往往来自于临近上级的影响更为直接迅速。
[0128] 因此，我们规定，头鸽的等级为一级，它是自由的，除头鸽外，每一级由2只鸽子组成。二级的两只鸽子仅受头鸽领导，但它们的相对位置不固定。在实验中，我们指定了它们相对于头鸽的位置。其余鸽子均受三只上级鸽子的领导，根据几何原理,与空间内不共线三点的确定距离可唯一确定一点，因此，它们具有固定的相对位置。
[0129] 综上,令Ni为对鸽子i存在领导作用的上级鸽子集合,鸽群中的领导关系,可用Ni表示为
[0130]
[0131] 设n只鸽子在3维欧式空间内无障碍飞行，则他们可被视作质点，根据牛顿第二定律，每只鸽子的动力学模型如下：
[0132]
[0133] vi∈R3为鸽子i的速度矢量，mi为鸽子的质量，ui∈R3为鸽子的控制输入，ki>0为阻力系数。
[0134] 步骤四：建立鸽群层级机制，根据受控上级位置信息，得下一时刻僚机理想控制量[0135] 根据鸽群编队模型中的受控上级集合，鸽群中上级对下级的领导作用,使下级在避免与上级发生碰撞(即防撞)的基础上,尝试与其保持接近(即聚集),同时试图与其速度匹配(即速度一致)。
[0136] 本发明使用人工势场法来描述防撞和聚集两个机制。
[0137] 对于水平方向，场函数：
[0138]
[0139] Uatt是引力场，Ureq是斥力场
[0140] 分别代表鸽子i与受控上级j之间的真实距离和期望距离,kp和ke为人工势场增益系数。
[0141] 质点在场中某一方向上受力大小为势场在该方向上的偏导数，因此每只鸽子在场中的受力为：
[0142]
[0143] 垂直方向，场函数：
[0144]
[0145] 速度一致由下式描述：
[0146]
[0147]
[0148] 其中，kv为速度反馈系数，kh为高度反馈系数，XYij代表X或Y方向上鸽子i与受控上级j的距离。
[0149] 鸽子i的控制量ui可表示为：
[0150] ui＝αi+βi+γi (8)
[0151] 根据上述式子，各无人驾驶飞机的控制输入ψicθicλic可以被计算出。
[0152] 步骤五：对无人机建模，解算僚机自动驾驶仪控制输入
[0153] 本文使用无人机的配有俯仰、滚转、偏航和垂直方向加速度4个通道的自动驾驶仪,在编队中航向角不产生作用，则无人机可以简化为如下模型：
[0154]
[0155] 其中(xi,yi,zi)为无人机i的位置，ψicθicλic为自动驾驶仪3个回路上的控制输入，g为重力加速度，这里取9.8m/s2。
[0156] 考虑无人机的实用性约束：
[0157]
[0158] axMAX,ayMAX,az AX分别是三个自由度上的最大过载。
[0159] 自驾仪控制输入可由(2)和(10)解算出。
[0160] 步骤六：返回步骤二，直到任务终止
[0161] 鸽群中，一旦确定了目的地，头鸽会飞在整个群体的前面，其他鸽子会寻找自己的上级。每一只鸽子通过视觉观察集群中其他个体的位置和速度，以及与自己的距离来做出决策。对于无人机来说，个体通过网络广播自己的位置和速度信息，然后根据自己的等级寻找上级，按照上述鸽群机制和队形参数输出姿态控制量，返回步骤二，直到任务终止。
[0162] 实例验证结果图如图5-7所示，三张图分别是三维、二维和google地图中呈现出的轨迹图，飞行轨迹充分体现了防撞、聚集的特性，充分体现了仿信鸽智能行为的超大规模集群系统及方法的可行性。
