CN109613931A - 基于生物社会力的异构无人机集群目标跟踪系统及方法 - Google Patents

Abstract

本发明公开基于生物社会力的异构无人机集群目标跟踪系统及方法，该目标跟踪系统包括集群硬件系统架构、集群软件系统架构、集群通信系统架构、集群飞行管理和控制系统。该方法步骤如下：一、无人机状态建模；二、一致性协议的设计；三、基于相位差一致算法的无人机控制指令设计；四、基于非线性导引的轨迹跟踪；五、基于生物社会力的异构无人机集群系统描述；六、基于生物社会力的异构无人机集群目标跟踪。本发明的优点在于：实现异构无人机集群的目标跟踪；提高控制无人机逐渐收敛到期望路径的速度。整个目标跟踪系统系统完整，功能完善，可以实现对不同任务场景目标的验证。

Description

基于生物社会力的异构无人机集群目标跟踪系统及方法

技术领域

本发明涉及一种基于生物社会力的异构无人机集群目标跟踪系统及方法，属于无人机导航与控制领域。

背景技术

无人机因其低成本、高机动性、部署灵活和“零人员伤亡”等特点，被广泛应用于军事和民用等诸多领域。而单个无人机在执行军事监视和侦察、大型灾难场景搜救等任务时，因任务区域广阔、环境复杂多变、传感器感知能力有限和单节点故障等不利因素，面临着任务失败的风险，因此，这也促进了“以无人机平台为中心”的传统研究和应用模式向“以网络为中心”的多个无人机协同模式的转变，即：通过整合多个无人机的环境感知能力、共享其计算处理能力来扩展无人机集群的任务执行能力，提高任务执行成功的概率。

随着无人机的智能化水平不断提高，无人机的控制方式逐步从程序控制向人机共融交互控制甚至是全自主控制方向发展，其任务区域逐渐从侦察/监视的安全空域转向打击、对抗等复杂空域，其应用样式也逐步从单平台向多平台协同方向发展。典型的多无人机的协同控制的研究主要包括多个无人机协同侦察、协同搜索、协同目标跟踪、协同定位和编队控制等问题。其中，以多个无人机广域搜索和打击任务为主要目的自治编队混合主动控制为代表，具有代表性的系统为美空军的低成本自主攻击系统，该系统利用多架低成本、机动能力不强的无人机协同，可以加大传感器的覆盖范围，增强系统的跟踪能力。

同时，一些高校和研究机构也针对目标跟踪及路径跟踪算法问题进行了深入研究，现有的路径跟踪方法分为基于几何计算的方法和基于控制理论的方法两大类。基于几何计算的方法如：追踪法、视线法，以及二者相结合的方法，都需要在参考路径上选择合适的虚拟目标点，然后再控制无人机追逐虚拟目标点，最终实现对期望路径的跟踪。这类方法容易理解，计算简单，容易投入实际应用，但是由于依赖于虚拟距离的选取，因此，在跟踪变曲率曲线路径时存在较大的跟踪误差；而利用控制理论研究路径跟踪问题，可以方便的考虑稳定性和鲁棒性等性能指标，最常见的基于比例-积分-微分(PID)控制的路径跟踪方法的跟踪性能差于非线性导航方法。此外，一些非线性控制理论也被用于研究路径跟踪问题，如线性二次型调节器、模型预测控制和自适应控制等。虽然这些方法可以方便的考虑系统稳定性和鲁棒性等问题，但是相对于基于几何计算的方法，这些方法更加抽象，更依赖对象的数学模型，因此较少投入实际应用。而非线性导引控制方法与追踪法、视线法以及两者相结合的方法类似，采取了虚拟目标点的概念，在期望路径上选取虚拟目标点之后，通过设计非线性的侧向加速度，控制无人机逐渐收敛到期望路径，减小跟踪误差。

发明内容

本发明提供了一种基于生物社会力的异构无人机集群目标跟踪系统及方法，旨在通过合理配置无人机集群在目标周围特定方位上分布，实现对于目标的全方位、立体化监测。利用于生物社会力设计控制率，实现异构无人机集群系统对目标的跟踪，从而解决单无人机目标跟踪存在的机动性差、目标易跟丢以及传感器探测范围有限等问题。

本发明针对异构无人机集群目标跟踪问题及路径跟踪算法，搭建了无人机集群目标跟踪系统，具体如下：

本发明的基于生物社会力的异构无人机集群目标跟踪系统，系统结构框图如图1所示，该目标跟踪系统架构由4部分组成，分别为：集群硬件系统架构、集群软件系统架构、集群通信系统架构、集群飞行管理和控制系统。

A集群硬件系统架构：主要由四旋翼机体、动力系统、顶层任务计算机、底层飞行控制器、感知环境信息的传感器、负责监控的地面站以及通信设备构成。硬件系统组成及各组件之间的关系如图2所示，实线表示组件之间的动力流关系，虚线表示组件之间的数据流关系。

无人机的机架选用猛禽四旋翼机架，轴距650mm，其动力系统由四组电机、电子调速器和桨叶组成，电机为T-motor电机，电子调速器型号为T-motor 40A，桨叶为17*5.8碳钎维桨。这套动力可以满足当前选型的任务计算机、飞行控制器、无线模块和电源模块的重量要求，且有一定的余量，可方便后续对于载荷的扩展。整个系统采用型号为4000mAh航模理电池供电，由于机载的各设备对于电压和电流的需求各不相同，采用分电板和稳压模块分别给飞行控制器和动力系统、任务计算机供电，通信设备和传感器由与其连接的控制板进行供电。

集群硬件系统中的控制器为两层的架构，即顶层任务计算机和底层飞行控制器，所述的顶层任务计算机为安装了Linux操作系统的单板计算机，作为任务处理器，安装机器人操作系统(Robot Operating System,ROS)，在ROS环境中进行多无人机协调算法和通信程序的编写；所述的底层飞行控制器为Pixhawk开源飞行控制器，作为自动驾驶仪负责无人机的本体控制，实现如自主起飞、着陆、悬停、跟踪航路点功能。机载的各种传感器，负责感知环境信息，为无人机的控制系统提供反馈，包括三轴加速度计、微型陀螺仪、磁力计、GPS，为无人机提供位置和姿态等信息。

顶层任务计算机作为多机协调控制的大脑，是实现多机协调控制的核心模块，可加载单机自主控制及多级协同控制程序，选择时需要综合考虑运算能力、功耗、重量、外设等多种因素，而选用的Odroid-XU4作为任务计算机，计算能力能够满足机载任务的计算需求，且重量轻，功耗小，开发板上设有3个USB接口，方便连接飞行控制器和外接无线设备。选用磊科1200M大功率双频USB接收器为无线通信设备，将其设置为Ad Hoc模式。在无人机集群控制中，需要为每架无人机上装配一台任务计算机，通过无线通信设备与无线自组织网络配合，可以实现多机的分布式控制。任务计算机上运动Ubuntu系统，配置ROS环境，任务机上通信、协同控制等程序均以ROS节点的形式运行。

飞行控制器Pixhawk负责无人机的本体控制，为无人机提供自动驾驶仪的功能，需要具有较高的可靠性和较强运算能力，是实现无人机自主控制的核心。在飞行控制器板上集成了微处理器、各种传感器和丰富的外设接口，可实现灵活可靠的飞行控制，并支持根据需求进行二次开发。该微处理器芯片具有更强大的运算能力和更高的主循环频率。此外，控制板上的传感器为双余度设计，可以有效减少意外的噪声，提供更加准确的姿态、速度、高度等信息，保障可靠和稳定飞行。

地面站和数据通信设备负责无人机与地面工作人员之间、无人机之间的通信交流。通过地面站，操作人员可以实时监控无人机的当前位置、速度和飞行姿态，可以在紧急情况下发出控制指令，实现飞手的手动控制或迫降。通过无人机间的通信协议实现无人机分布式集群控制。

集群硬件系统中的地面站和数据的通讯设备有三套通信模块，一套由无线数据传输模块组成，负责在紧急情况下接管无人机；另外一套由无线自组织网络(Wireless AdHoc Network，WANET)组成，负责多无人机之间的状态、指令的发送和接收；还有一套为遥控器WFT-OSII和WFR09S接收机，用于手动飞行、紧急状态下的干预。

B集群软件系统架构：飞行控制器负责底层的姿态控制，任务管理计算机作为自主性载荷，实现顶层的任务规划和协调任务，整个软件系统架构如图3所示。自主载荷的软件部分，全部运行在ROS的框架之下，各部分通过ROS消息进行内部通信，任务计算机-飞行控制器通信节点作为飞行控制器和ROS通信的接口，自组织网络通信节点负责无人机与无人机之间以及无人机与地面站之间的通信。ROS内部的通信一般通过发布和订阅话题，也可以调用服务，区别是话题的订阅和发布者只负责消息的发送和接收，而服务模式下包括请求消息和响应消息。

自主性载荷主要以ROS节点形式存在，包括以下主要功能模块：

(1)任务计算机-飞行控制器通信模块：是飞行控制器和任务计算机的接口，利用MAVlink数据传输协议以设定频率从串口读取无人机的位置、姿态等信息，并以ROS话题消息形式发布出去；同时将顶层任务计算机计算得出的控制指令回传给飞行控制器。

(2)自组织网络通信模块：集群中不同无人机之间信息共享的接口，使得无人机能够利用UDP数据传输控制协议与其他无人机分享自身的位置、姿态等状态信息，并接受其他无人机的信息，同时可以作为通信中继将地面监控站的指令发送给自主性载荷。

(3)集群状态整合模块：对于无人机通过串口得到的自身信息以及通过无线网络收到的其他无人机的信息所存在得丢包、时延等情况，进行校正、同步，并对各无人机的信息进行整合后提供给无人机进行下一步的规划和控制；

(4)集群飞行管理模块：该节点负责监控无人机的状态，具体包括：飞行控制器心跳包、GPS数据、无线通信网络、集群飞行准备是否完成，以判定是否能够进入编队飞行状态。

(5)集群飞行控制模块：根据集群中无人机的位置、速度等信息，利用加载到任务计算机上的基于生物群集行为的无人机集群飞行控制策略，计算出供飞行控制器执行的指令，以实现特定的集群飞行功能。

C集群通信系统架构：多无人机系统的通信模块由机内通信和机间通信两部分构成，集群通信系统架构如图4所示。机内通信一方面的目的是将飞行器当前的位置、速度、姿态等信息传输到任务计算机，并将任务计算机的计算出的协同指令信息回传给飞行控制器，另一方面是在任务计算机内部通过ROS框架实现线程间的便捷通信，以实现内部各模块之间的信息交互。机间通信是实现多机协同的必要条件，通过机间通信可以获取到其他无人机当前的位置、速度、姿态等信息，从而实现整个编队之间的信息贡献，。

(1)机内通信：多无人机系统的机内通信包括从飞行控制器到任务计算机的通信和任务计算机内部的节点之间的通信。在无人机进行飞行的过程中，Pixhawk飞行控制器上配备的加速度计、陀螺、磁力计和GPS设备能够实时无人机的位置、姿态、速度等飞行信息。任务计算机通过串口与飞行控制器进行连接，飞行控制器上滤波之后的传感器信息以及控制器当前的状态信息，利用MAVLink数据传输协议以5Hz频率发送给任务计算机。MAVlink协议是在串口通信协议基础上开发的一种用于无人飞行器的开源通信协议，该协议针对飞行器中常用的数据制定了发送、接收和校验的规则，以消息库的形式提供给开发者使用，可用于飞行器和地面站之间、飞行器之间以及飞行器内部的数据交换。

任务计算机将收到飞行控制器的数据按照MAVlink规则进行解析，随后将解析后的数据发送给任务计算机上的控制模块，同时将数据通过机间通信网络传输给其他无人机使用。ROS提供了一种底层进程间的信息传递机制，在ROS框架下，节点通过话题的发布/订阅实现与其他节点的联系。在使用的过程中，节点只将需要发送的消息发送到指定的话题，若另外一个节点对此话题进行了监听，则会在话题发布后自动进入相应的回调函数进行处理。话题的发布是一种一对多的通信方式，即一个话题可以被多个订阅者订阅，假设协同控制节点和机间通信节点均对“无人机当前位置”这一话题进行了订阅，则在“无人机当前位置”话题发布后，协同控制节点和机间通信节点均会收到话题信息且自动进入相应处理程序。

(2)机间通信：多机系统的机间通信采用无线自组织网络，该网络与基础式架构网络不同，是一种非中心式的网络，具有自组织、分布式等特点。分布式是说整个网络中不存在中心节点，所有的节点地位是对等的，这种的结构使得网络在某个节点出现故障时不会对整个网络造成影响，使得整体网络具有较强的抗毁性能。自组织是指网络的构建不依赖于任何预设的基础设施，如路由器、信号中心等，只需参与通信的各个节点便可自动地形成独立的自组织网络，自组织网络的成员节点可以进行移动，且所有的节点均可以随时加入或者离开网络。在多无人平台系统搭建的过程中，考虑多机协同系统通信对分布式、自组织等需求，多机的状态的通信网络架构采用Ad Hoc自组织网络。

将每架无人机视作无线自组织网络中的一个通信节点，将每架无人机的任务计算机上的网络设备设置为Ad Hoc网络架构，将网络IP地址与无人机编号一一映射，采用802.11n无线通信协议标准，利用UDP协议实现集群内部各个无人机之间的通信。每架无人机按照5Hz的频率发布自身飞行相关信息，包括GPS位置、高度、速度和姿态，按照2Hz的频率发布飞行模式、心跳包等无无人机当前状态的信息，同时接收来自其他无人机的信息，并对接收到的数据进行整合、处理，然后输入到编队控制模块，来自不同无人机之间的数据对齐操作如图5所示。试验过程中，所有的消息按照UDP传输控制协议以广播的形式发布，即每架无人机向通信范围内所有的个体发送自身的飞行和安全状态信息。

D集群飞行管理和控制系统：无人机的控制采用双层的架构，顶层的任务计算机由群集运动模型计算出的控制指令，底层为无人机的自动驾驶仪，这种双层的架构可以实现不同层之间故障的隔离，增强无人机的安全性。基于生物社会力的协同运动控制器(飞行控制器Pixhawk)将速度、航向角和高度指令发送给自动驾驶仪，从而实现多机之间的协同。对于无人机集群系统而言，通过任务计算机利用邻居个体和自身的状态信息计算出控制指令，发送给自动驾驶仪，实现多机的协同控制。任务计算机上的功能分为两大部分，分别为群集飞行管理和基于生物社会力的协调控制器，其中群集飞行管理包括集群的状态监控，集群的任务管理和规划，以及不同控制模式之间的切换功能，而基于生物社会力的协调控制器，主要计算出无人机的控制指令，发送给自动驾驶仪，以实现集群的协调运动(如图6所示)。

群集管理模块包含状态监控、任务规划、任务管理和模式切换功能。状态监控部分负责监控集群系统当前的状态，如飞行控制器心跳包、GPS数据、无线通信网络、以及其他无人机当前的状态，以判定无人机能否进入编队状态。在进行模式切换的控制时，采用有限状态机的形式，分为未就绪未激活、己就绪未激活、未就绪己激活和己就绪己激活四种状态，其中无人机的初始状态为未就绪且未激活，未就绪己激活为不期望进入的状态，状态机的输入为各无人机当前的状态和激活控制信号。各状态之间的状态转换逻辑如图7所示，该有限状态机可以保证集群飞行模式在准备完毕且收到触发信号时才能够被激活，保证不会进入还未准备完毕但相应的飞行模式己经被激活的状态。

基于生物社会力的异构无人机集群目标跟踪方法，其中的协同跟踪框架如图8所示，实现步骤如下：

步骤一：无人机状态建模

假设无人机具有底层控制器，具备高度保持能力，并能够跟踪期望姿态、期望速度和期望转弯速率。此时，内环控制器的状态量速度、航向角和高度变为外环控制器的输入，无人机的12阶状态方程可以简化为6阶状态方程，表示如下：

其中，表示无人机的水平位置变化率，表示高度变化率，表示垂直方向速度变化率，ψ表示无人机的航向，表示无人机的航向变化率，V_c、ψ_c和h_c为自动驾驶仪的输入指令，分别为速度指令、航向指令和高度指令,V表示无人机当前的速度，h_i表示无人机当前的高度。τψ、τ_v、τ_h、分别为与自动驾驶仪三个通道有关的时间常数。

步骤二：一致性协议的设计

在集群协同目标跟踪的过程中，为了保持实现对目标的稳定跟踪，主要对水平通道进行控制。在高度通道，假设无人机需要维持到特定的高度，无人机的高度控制采用如下的一致性协议：

其中，c_h为高度协同系数，为垂直方向速度协同系数，N_i为无人机i的邻居集合，分别表示第i架无人机的垂直方向速度，h_j、分别表示第i架无人机邻居集合中的任何一架无人机的高度和垂直方向速度。如果多架飞机构成的网络为连通图，公式(2)所示的控制协议保证无人机高度实现一致性，将公式(2)代入无人机的运动学模型(1)中，可以得到自动驾驶仪的高度控制指令如下：

步骤三：基于相位差一致算法的无人机控制指令设计

将无人机相对目标的方位称为相位θ_i，i＝1,2,...,N，在相对于目标的距离不变的情况下，无人机相对于目标的速度可用来表征相位的变化，集群相位图如图9所示。记无人机的相位差为△θ_i，当i＝1,2,...,N-1时，△θ_i＝θ_i+1-θ_i，当i＝N时，△θ_N＝θ₁-θ_N，对于无人机相位差的控制可以通过调节无人机相对于目标的速度实现。设个体i期望的相位差为无人机在目标周围均匀分布，意味着所有无人机的期望的相位差相等，同样的，当i＝1,2,...,N-1时，当i＝N时，所有无人机的相位θ＝[θ₁,θ₂,...,θ_N]^T，相位差△θ＝[△θ₁,△θ₂,...,△θ_N]^T，期望的相位期望的相位差设计无人机的控制指令形式如下：

其中，k_θ为正的常数，V₀为期望的速度，R为跟踪的距离，θ_i为无人机i相对于目标的方位角，< >为将(θ_i+1-θ_i)转化为[0,2π)的操作。

对该公式(4)所示的控制协议，无人机仅需要知道与相邻无人机的相位差△θ_i和△θ_i-1即可进行计算。根据相位的角速率可以计算出期望的速度指令而当个体的相位差相等时，有此时V_i＝V₀。

步骤四：基于非线性导引的轨迹跟踪

在非线性导引控制中，首先在期望路径上选择与无人机当前位置距离为(以下称前视距离)L₁、且无人机与该点的连线与期望运动方向夹角小于90度的点，作为虚拟目标点P_v，如图10所示。该操作的具体实现可以通过以无人机所在位置为圆心，L₁为半径作圆，从该圆与期望轨迹的交点中选取出虚拟目标点P_v。然后，计算无人机的法向加速度指令a_cmd，该加速度方向垂直于无人机运动速度V方向，大小为：

|a_cmd|＝2V²sinη/L₁ (5)

其中，V为无人机当前的速度大小，η为无人机速度方向与视线方向的夹角。当无人机和虚拟目标点P_v的连线位于速度向量的右侧时，加速度指令方向垂直于速度方向指向右侧，反之，加速度指令方向垂直于速度方向指向左侧。当无人机与期望路径的最小距离大于L₁时，即以无人机所在位置为圆心，L₁为半径的圆与期望路径无交点时，设置无人机的期望运动方向指向无人机在期望路径上的投影点。根据非线性导引得到法向加速度之后，可以计算出相应的偏航角指令ψ_c，发送给航向自动驾驶仪。

对于非线性导引控制器，当所跟踪的期望路径为直线时，导引律近似于比例微分控制器。如图11所示，视线方向和速度方向的夹角η＝η₁+η₂，其中η₂为视线和所跟踪曲线的夹角，η₂＝d/L₁，其中d为无人机距离其在期望路径上的投影点的距离，η₁为速度方向和所跟踪直线的夹角，其中为无人机与其在期望路径上投影点距离的导数。所以，非线性导引控制指令此时该控制器比例微分控制器，控制器的参数为无人机当前的速度大小V和前视距离L₁，对应的二阶系统的阻尼系数为常值0.707，时间常数为L₁/V，在无人机速度大小保持不变时，前视距离L₁越小，响应时间越快。假设无人机所需跟踪路径包含的最高频率分量对应的波长为L_p，则无人机能够准确跟踪期望路径的条件为前视距离参数L≤L_p/4.4，该条件可用以选择非线性导引控制的参数L。

无人机进行目标跟踪时的期望轨迹为以目标为中心的圆周运动。在进行多机协同跟踪的过程中，首先根据相位差控制计算出无人机i的期望速度V_i，而后根据期望速度V_i利用式(2)所示的非线性导引控制计算出加速度指令，进而得出偏航角速度指令。由于上述过程计算得出的期望速度以及偏航角速度均为相对目标而言。因此，在实际的飞行过程中，需要利用目标的速度对上述过程计算出的速度和偏航角速率进行校正，计算出无人机实际的速度和偏航角指令，发送给自动驾驶仪。

步骤五：基于生物社会力的异构无人机集群系统描述

在某些任务场景中，单架无人机不足以完成预定的任务，需要多架无人机甚至是无人机集群配合工作，如单架无人机所携带的激光照射量不足以摧毁目标，需要多架同能时对目标进行照射以满足功率要求或者交替工作以满足对目标照射时长的要求。针对这一问题，该部分对部分信息个体下的无人机集群目标跟踪问题进行研究，集群中携带光电设备的无人机能够获取到目标的位置和速度等信息，剩余的无人机携带激光武器等其他装备。按获取信息情况对集群中的无人机进行分类，将能够获取到目标信息的无人机称为信息个体，剩余的无人机为非信息个体，则异构无人机集群中的信息个体根据非线性导引方法计算出目标跟踪指令，非信息个体受到生物社会力的作用。

考虑异构无人机集群系统由信息个体和非信息个体组成，信息个体能够获取到所需跟踪目标的信息，而非信息个体在群集中其他个体的影响下进行运动，系统方程如下：

其中，为个体i的位置，为个体i的速度，为个体受到的控制力的作用。

步骤六：基于生物社会力的异构无人机集群目标跟踪

在进行目标跟踪的过程中，群集中的信息个体能够根据目标当前的位置和速度利用非线性导引方法计算出虚拟目标点来进行跟踪，假设该虚拟目标的运动方程如下，

其中，q_L为虚拟目标的位置，P_L为虚拟目标的速度，f_L为虚拟目标的加速度。

设计个体的控制输入由三部分组成，α为通过远距离吸引-近距排斥作用控制个体位置的位置协同项，β为实现个体速度一致的速度协同项，这两项共同构成了个体所受到的社会力，γ为对虚拟目标进行跟踪的控制项，个体所受的控制力形式如下：

其中，N_i为个体i的交互邻居集合，ψ(||q_i-q_j||)为产生吸引-排斥作用的势函数，a_ij(t)为邻接矩阵中的元素，c₁和c₂为正的增益，h_i为信息个体的示性函数，如果个体i为信息个体则h_i＝i，否则h_i＝0。

在上述控制力作用中，只有在个体为信息个体时，才受到γ项对虚拟目标进行跟踪的作用力，而非信息个体只受到社会力的作用。对于非信息个体，采用基于注意力机制的群集模型来确定交互邻居，根据周围个体对局部有序度的贡献计算其显著度值，而后采用阈值响应模型改变自身的状态，并在不同的状态下采取不同的运动策略。

群集系统方程可写成如下矩阵形式：

其中，位置向量速度向量控制力向量

对于上述的群集系统，考虑如下的系统能量函数：

其中，U_i(q)为个体i的势能，E_i(p_i)为个体i相对于虚拟目标的动能，分别表示如下:

本发明提出了基于生物社会力的异构无人机集群目标跟踪系统及方法。该方法和系统的优势主要体现在两个方面：一方面，借助生物社会力设计控制协议，利用信息个体和非信息个体的交互，实现异构无人机集群的目标跟踪；此外，结合非线性导引控制方法，通过在期望路径上选取虚拟目标点，设计非线性的侧向加速度，提高控制无人机逐渐收敛到期望路径的速度。另一方面，整个目标跟踪系统包括集群硬件系统架构、集群软件系统架构、集群通信系统架构、集群飞行管理与控制系统，系统完整，功能完善，可以实现对不同任务场景目标的验证。

附图说明

图1集群系统结构框图

图2集群硬件系统架构

图3集群软件系统架构

图4集群通信系统架构

图5集群通信数据同步操作

图6集群控制系统架构

图7模式切换逻辑

图8集群协同目标跟踪框架

图9集群相位图

图10非线性导引控制示意图

图11直线路径跟踪示意图

图12异构集群协同目标跟踪1

图13异构集群协同目标跟踪2

θ_i——无人机i相对于目标的方位；

V——无人机当前的速度大小；

η——无人机速度方向与视线方向的夹角；

P_v——虚拟目标点；

L₁——半径的圆与期望路径交点；

a_cmd——非线性导引得到法向加速度；

ψ_c——偏航角指令；

η₁——速度方向和所跟踪直线的夹角

η₂——视线和所跟踪曲线的夹角；

d——无人机与其在期望路径上投影点距离；

h——无人机高度；

θ_d——期望姿态；

p_T——目标的位置；

v_T——目标的速度；

ψ——无人机的航向；

R_standoff——无人机间距离；

T——无人机升力；

具体实施方式

下面通过三个具体实例来验证本发明所提出的系统及其方法的有效性，该系统及方法具体步骤如下：

步骤一：无人机状态建模

假设无人机具有底层控制器，具备高度保持能力，并能够跟踪期望姿态、期望速度和期望转弯速率。此时，内环控制器的状态量速度、航向角和高度变为外环控制器的输入，无人机的12阶状态方程可以简化为6阶状态方程，表示如下：

以3个无人机集群(每个集群有15架无人机)对初始位置为[0,0]m，速度5m/sm，运动轨迹为圆弧(由半径分别为150m,150m和80m3段首尾相切构成)的目标进行协同跟踪的场景进行仿真。

步骤二：一致性协议的设计

在集群协同目标跟踪的过程中，为了保持实现对目标的稳定跟踪，主要对水平通道进行控制。在高度通道，假设无人机需要维持到特定的高度，无人机的高度控制采用如下的一致性协议：

其中，c_h为高度协同系数，c_h为垂直方向速度协同系数，N_i为无人机i的邻居集合。如果多架飞机构成的网络为连通图，控制协议(2)保证无人机高度实现一致性，将控制协议(2)代入无人机的运动学模型(1)中，可以得到自动驾驶仪的高度控制指令如下：

步骤三：基于相位差一致算法的无人机控制指令设计

每个无人机集群中无人机相对目标的方位称为相位θ_i，i＝1,2,...,15，在相对于目标的距离不变的情况下，无人机相对于目标的速度可用来表征相位的变化。记无人机的相位差为△θ_i，当i＝1,2,...,14时，△θ_i＝θ_i+1-θ_i，当i＝15时，△θ₁₅＝θ₁-θ₁₅，对于无人机相位差的控制可以通过调节无人机相对于目标的速度实现。设个体i期望的相位差为无人机在目标周围均匀分布，意味着所有无人机的期望的相位差相等，同样的，当i＝1,2,...,14时，当i＝15时，所有无人机的相位θ＝[θ₁,θ₂,θ₃]^T，相位差△θ＝[△θ₁,△θ₂,...,△θ₁₅]^T，期望的相位期望的相位差设计无人机的控制指令形式如下：

其中，k_θ为正的常数，V₀为期望的速度，R为跟踪的距离，θ_i为无人机i相对于目标的方位角，< >为将(θ_i+1-θ_i)转化为[0,2π)的操作。

对该控制协议，无人机仅需要知道与相邻无人机的相位差△θ_i和△θ_i-1即可进行计算。根据相位的角速率可以计算出期望的速度指令而当个体的相位差相等时，有此时V_i＝V₀。

步骤四：基于非线性导引的轨迹跟踪

在非线性导引控制中，首先在期望路径上选择与无人机当前位置距离为L₁＝200m，且无人机与该点的连线与期望运动方向夹角小于90度的点，作为虚拟目标点P_v。以无人机所在位置为圆心，L₁＝200m为半径作圆，从该圆与期望轨迹的交点中选取出虚拟目标点P_v。然后，计算无人机的法向加速度指令a_cmd，该加速度方向垂直于无人机运动速度V方向，大小为：

|a_cmd|＝2V²sinη/200 (16)

其中，V为无人机当前的速度大小，η为无人机速度方向与视线方向的夹角。当无人机和虚拟目标点P_v的连线位于速度向量的右侧时，加速度指令方向垂直于速度方向指向右侧，反之，加速度指令方向垂直于速度方向指向左侧。当无人机与期望路径的最小距离大于200m时，即以无人机所在位置为圆心，半径为200m的圆与期望路径无交点时，设置无人机的期望运动方向指向无人机在期望路径上的投影点。根据非线性导引得到法向加速度之后，可以计算出相应的偏航角指令ψ_c，发送给航向自动驾驶仪。

对于非线性导引控制器，当所跟踪的期望路径为直线时，导引律近似于比例微分控制器。视线方向和速度方向的夹角η＝η₁+η₂，其中η₂为视线和所跟踪曲线的夹角，η₂＝d/200，其中d为无人机距离其在期望路径上的投影点的距离，η₁为速度方向和所跟踪直线的夹角，其中为无人机与其在期望路径上投影点距离的导数。所以，非线性导引控制指令此时该控制器比例一微分控制器，控制器的参数为无人机的速度大小V和前视距离L₁，对应的二阶系统的阻尼系数为常值0.707，时间常数为200/V，在无人机速度大小保持不变时，前视距离L₁越小，响应时间越快。假设无人机所需跟踪路径包含的最高频率分量对应的波长为L_p，则无人机能够准确跟踪期望路径的条件为前视距离参数L≤L_p/4.4，该条件可用以选择非线性导引控制的参数L。

无人机进行目标跟踪时的期望轨迹为以目标为中心的圆周运动。在进行多机协同跟踪的过程中，首先根据相位差控制计算出无人机i的期望速度V_i，而后根据期望速度V_i利用式(2)所示的非线性导引控制计算出加速度指令，进而得出偏航角速度指令。由于上述过程计算得出的期望速度以及偏航角速度均为相对目标而言。因此，在实际的飞行过程中，需要利用目标的速度对上述过程计算出的速度和偏航角速率进行校正，计算出无人机实际的速度和偏航角指令，发送给自动驾驶仪。

步骤五：基于生物社会力的异构无人机集群系统描述

集群中携带光电设备的无人机能够获取到目标的位置和速度等信息，剩余的无人机携带激光武器等其他装备。按获取信息情况对集群中的无人机进行分类，每个无人机集群中信息个体10架，非信息个体5架，则异构无人机集群中的信息个体根据非线性导引方法计算出目标跟踪指令，非信息个体受到生物社会力的作用，集群系统的系统方程如下：

其中，为个体i的位置，为个体i的速度，为个体受到的控制力的作用。

步骤六：基于生物社会力的异构无人机集群目标跟踪

在进行目标跟踪的过程中，群集中的信息个体能够根据目标当前的位置和速度利用非线性导引方法计算出虚拟目标点来进行跟踪，假设该虚拟目标的运动方程如下，

其中，q_L为虚拟目标的位置，P_L为虚拟目标的速度，f_L为虚拟目标的加速度。

设计个体的控制输入由三部分组成，α为通过远距离吸引-近距排斥作用控制个体位置的位置协同项，β为实现个体速度一致的速度协同项，这两项共同构成了个体所受到的社会力，γ为对虚拟目标进行跟踪的控制项，个体所受的控制力形式如下：

其中，N_i为个体i的交互邻居集合，ψ(||q_i-q_j||)为产生吸引-排斥作用的势函数，a_ij(t)为邻接矩阵中的元素，c₁和c₂为正的增益，h_i为信息个体的示性函数，如果个体i为信息个体则h_i＝i，否则h_i＝0。

在上述控制力作用中，只有在个体为信息个体时，才受到γ项对虚拟目标进行跟踪的作用力，而非信息个体只受到社会力的作用。对于非信息个体，采用基于注意力机制的群集模型来确定交互邻居，根据周围个体对局部有序度的贡献计算其显著度值，而后采用阈值响应模型改变自身的状态，并在不同的状态下采取不同的运动策略。

群集系统方程可写成如下矩阵形式：

其中，位置向量速度向量控制力向量

对于上述的群集系统，考虑如下的系统能量函数：

其中，U_i(q)为个体i的势能，E_i(p_i)为个体i相对于虚拟目标的动能，分别表示如下:

基于生物社会力的异构无人机集群协同目标跟踪仿真中，由每个集群的几何中心构成的三维运动轨迹如图12(a)所示，几何中心对于地面目标的跟踪误差变化曲线如图12(b)所示。图13(a)和(b)为集群2中的信息个体根据目标跟踪控制算法计算出的期望轨迹和速度，以及集群中每架无人机的轨迹和速度)。可以看出，在部分信息个体的导引下，无人机集群能够实现对期望指令的跟踪。图13(c)为每架无人机以及集群的中心相对于运动目标的跟踪误差，集群的几何中心对于地面目标的跟踪精度最终收敛到l0m以内，且集群中所有无人机均在期望的对峙跟踪距离的附近，图13(d)所示为无人机最终的位置速度以及交互关系的示意图。

Claims (5)

1.一种基于生物社会力的异构无人机集群目标跟踪系统，其特征在于：该目标跟踪系统4部分组成，分别为：集群硬件系统架构、集群软件系统架构、集群通信系统架构、集群飞行管理和控制系统；

A集群硬件系统架构：主要由四旋翼机体、动力系统、顶层任务计算机、底层飞行控制器、感知环境信息的传感器、负责监控的地面站以及通信设备构成；

四旋翼机体选用猛禽四旋翼机架，轴距650mm，其动力系统由四组电机、电子调速器和桨叶组成；整个系统采用型号为4000mAh航模理电池供电，采用分电板和稳压模块分别给飞行控制器和动力系统、任务计算机供电，通信设备和传感器由与其连接的控制板进行供电；

集群硬件系统中的控制器为两层的架构，即顶层任务计算机和底层飞行控制器，所述的顶层任务计算机为安装了Linux操作系统的单板计算机，作为任务处理器，安装机器人操作系统ROS，在ROS环境中进行多无人机协调算法和通信程序的编写；所述的底层飞行控制器为Pixhawk开源飞行控制器，作为自动驾驶仪负责无人机的本体控制；机载的各种传感器，负责感知环境信息，为无人机的控制系统提供反馈，包括三轴加速度计、微型陀螺仪、磁力计、GPS，为无人机提供位置和姿态信息；

顶层任务计算机作为多机协调控制的大脑，可加载单机自主控制及多级协同控制程序，在无人机集群控制中，要为每架无人机上装配一台任务计算机，通过无线通信设备与无线自组织网络配合，实现多机的分布式控制；任务计算机上运动Ubuntu系统，配置ROS环境，任务机上通信、协同控制程序均以ROS节点的形式运行；

飞行控制器Pixhawk负责无人机的本体控制，为无人机提供自动驾驶仪的功能；在飞行控制器板上集成了微处理器、传感器和外设接口；

地面站和数据通信设备负责无人机与地面工作人员之间、无人机之间的通信交流；共有三套通信模块，一套由无线数据传输模块组成，负责在紧急情况下接管无人机；另外一套由无线自组织网络组成，负责多无人机之间的状态、指令的发送和接收；还有一套为遥控器WFT-OSII和WFR09S接收机，用于手动飞行、紧急状态下的干预；

B集群软件系统架构：飞行控制器负责底层的姿态控制，任务管理计算机作为自主性载荷，实现顶层的任务规划和协调任务；自主载荷的软件部分，全部运行在ROS的框架之下，各部分通过ROS消息进行内部通信，任务计算机-飞行控制器通信节点作为飞行控制器和ROS通信的接口，自组织网络通信节点负责无人机与无人机之间以及无人机与地面站之间的通信；

C集群通信系统架构：多无人机系统的通信模块由机内通信和机间通信两部分构成；机内通信一方面将飞行器当前的位置、速度、姿态信息传输到任务计算机，并将任务计算机的计算出的协同指令信息回传给飞行控制器，另一方面是在任务计算机内部通过ROS框架实现线程间的便捷通信，实现内部各模块之间的信息交互；机间通信实现多机协同，通过机间通信获取到其他无人机当前的位置、速度、姿态信息，从而实现整个编队之间的信息贡献；

D集群飞行管理和控制系统：无人机的控制采用双层的架构，顶层的任务计算机由群集运动模型计算出的控制指令，底层为无人机的自动驾驶仪；顶层任务计算机上的功能分为两大部分，分别为群集飞行管理和控制系统即基于生物社会力的协调控制器；集群飞行管理包括集群的状态监控，集群的任务管理和规划，以及不同控制模式之间的切换功能，而基于生物社会力的协调控制器，主要计算出无人机的控制指令，发送给自动驾驶仪，以实现集群的协调运动；基于生物社会力的协同运动控制器即飞行控制器Pixhawk将速度、航向角和高度指令发送给自动驾驶仪，从而实现多机之间的协同；对于无人机集群系统而言，通过任务计算机利用邻居个体和自身的状态信息计算出控制指令，发送给自动驾驶仪，实现多机的协同控制。

2.根据权利要求1所述的一种基于生物社会力的异构无人机集群目标跟踪系统，其特征在于：集群软件系统架构所述的自主性载荷主要以ROS节点形式存在，包括以下主要功能模块：

(1)任务计算机-飞行控制器通信模块：是飞行控制器和任务计算机的接口，利用MAVlink数据传输协议以设定频率从串口读取无人机的位置、姿态信息，并以ROS话题消息形式发布出去；同时将顶层任务计算机计算得出的控制指令回传给飞行控制器；

(2)自组织网络通信模块：集群中不同无人机之间信息共享的接口，使得无人机能够利用UDP数据传输控制协议与其他无人机分享自身的位置、姿态状态信息，并接受其他无人机的信息，同时可以作为通信中继将地面监控站的指令发送给自主性载荷；

(3)集群状态整合模块：对于无人机通过串口得到的自身信息以及通过无线网络收到的其他无人机的信息所存在的丢包、时延，进行校正、同步，并对各无人机的信息进行整合后提供给无人机进行下一步的规划和控制；

(4)集群飞行管理模块：负责监控无人机的状态，具体包括：飞行控制器心跳包、GPS数据、无线通信网络、集群飞行准备是否完成，以判定是否能够进入编队飞行状态；

(5)集群飞行控制模块：根据集群中无人机的位置、速度信息，利用加载到任务计算机上的基于生物群集行为的无人机集群飞行控制策略，计算出供飞行控制器执行的指令，以实现特定的集群飞行功能。

3.根据权利要求1所述的一种基于生物社会力的异构无人机集群目标跟踪系统，其特征在于：集群通信系统架构所述的机内通信，具体过程如下：机内通信包括从飞行控制器到任务计算机的通信和任务计算机内部的节点之间的通信；在无人机进行飞行的过程中，Pixhawk飞行控制器上配备的加速度计、陀螺、磁力计和GPS设备能够实时无人机的位置、姿态、速度飞行信息；任务计算机通过串口与飞行控制器进行连接，飞行控制器上滤波之后的传感器信息以及控制器当前的状态信息，利用MAVLink数据传输协议以5Hz频率发送给任务计算机；

任务计算机将收到飞行控制器的数据按照MAVlink规则进行解析，随后将解析后的数据发送给任务计算机上的控制模块，同时将数据通过机间通信网络传输给其他无人机使用；ROS提供了一种底层进程间的信息传递机制，在ROS框架下，节点通过话题的发布/订阅实现与其他节点的联系；在使用的过程中，节点只将需要发送的消息发送到指定的话题，若另外一个节点对此话题进行了监听，则会在话题发布后自动进入相应的回调函数进行处理；话题的发布是一种一对多的通信方式，即一个话题可以被多个订阅者订阅，假设协同控制节点和机间通信节点均对“无人机当前位置”这一话题进行了订阅，则在“无人机当前位置”话题发布后，协同控制节点和机间通信节点均会收到话题信息且自动进入相应处理程序。

4.根据权利要求1所述的一种基于生物社会力的异构无人机集群目标跟踪系统，其特征在于：集群通信系统架构所述的机间通信，具体过程如下：机间通信采用无线自组织网络，多机的状态的通信网络架构采用Ad Hoc自组织网络；

将每架无人机视作无线自组织网络中的一个通信节点，将每架无人机的任务计算机上的网络设备设置为Ad Hoc网络架构，将网络IP地址与无人机编号一一映射，采用802.11n无线通信协议标准，利用UDP协议实现集群内部各个无人机之间的通信；每架无人机按照5Hz的频率发布自身飞行相关信息，按照2Hz的频率发布无人机当前状态的信息，同时接收来自其他无人机的信息，并对接收到的数据进行整合、处理，然后输入到编队控制模块，所有的消息按照UDP传输控制协议以广播的形式发布，即每架无人机向通信范围内所有的个体发送自身的飞行和安全状态信息。

5.一种基于生物社会力的异构无人机集群目标跟踪方法，具体步骤如下：

步骤一：无人机状态建模

假设无人机具有底层控制器，具备高度保持能力，并能够跟踪期望姿态、期望速度和期望转弯速率；此时，内环控制器的状态量速度、航向角和高度变为外环控制器的输入，无人机的12阶状态方程可以简化为6阶状态方程，表示如下：

其中，表示无人机的水平位置变化率，表示高度变化率，表示垂直方向速度变化率，ψ表示无人机的航向，表示无人机的航向变化率，V_c、ψ_c和h_c为自动驾驶仪的输入指令，分别为速度指令、航向指令和高度指令，V表示无人机当前的速度，h_i表示无人机当前的高度；τ_ψ、τ_v、τ_h、分别为与自动驾驶仪三个通道有关的时间常数；

步骤二：一致性协议的设计

在集群协同目标跟踪的过程中，为了保持实现对目标的稳定跟踪，主要对水平通道进行控制；在高度通道，假设无人机需要维持到特定的高度，无人机的高度控制采用如下的一致性协议：

其中，c_h为高度协同系数，为垂直方向速度协同系数，N_i为无人机i的邻居集合，分别表示第i架无人机的垂直方向速度，h_j，分别表示第i架无人机邻居集合中的任何一架无人机的高度和垂直方向速度；如果多架飞机构成的网络为连通图，公式(2)所示的控制协议保证无人机高度实现一致性，将公式(2)代入无人机的运动学模型(1)中，可以得到自动驾驶仪的高度控制指令如下：

步骤三：基于相位差一致算法的无人机控制指令设计

将无人机相对目标的方位称为相位θ_i，i＝1,2,...,N，在相对于目标的距离不变的情况下，无人机相对于目标的速度可用来表征相位的变化；记无人机的相位差为△θ_i，当i＝1,2,...,N-1时，△θ_i＝θ_i+1-θ_i，当i＝N时，△θ_N＝θ₁-θ_N，对于无人机相位差的控制可以通过调节无人机相对于目标的速度实现；设个体i期望的相位差为无人机在目标周围均匀分布，意味着所有无人机的期望的相位差相等，同样的，当i＝1,2,...,N-1时，当i＝N时，所有无人机的相位θ＝[θ₁,θ₂,...,θ_N]^T，相位差△θ＝[△θ₁,△θ₂,...,△θ_N]^T，期望的相位期望的相位差设计无人机的控制指令形式如下：

其中，k_θ为正的常数，V₀为期望的速度，R为跟踪的距离，θ_i为无人机i相对于目标的方位角，<>为将(θ_i+1-θ_i)转化为[0,2π)的操作；

对该公式(4)所示的控制协议，无人机仅需要知道与相邻无人机的相位差△θ_i和△θ_i-1即可进行计算；根据相位的角速率可以计算出期望的速度指令而当个体的相位差相等时，有此时V_i＝V₀；

步骤四：基于非线性导引的轨迹跟踪

在非线性导引控制中，首先在期望路径上选择与无人机当前位置距离为L₁，以下称前视距离、且无人机与该点的连线与期望运动方向夹角小于90度的点，作为虚拟目标点P_v；该操作的具体实现可以通过以无人机所在位置为圆心，L₁为半径作圆，从该圆与期望轨迹的交点中选取出虚拟目标点P_v；然后，计算无人机的法向加速度指令a_cmd，该加速度方向垂直于无人机运动速度V方向，大小为：

|a_cmd|＝2V²sinη/L₁ (5)

其中，V为无人机当前的速度大小，η为无人机速度方向与视线方向的夹角；当无人机和虚拟目标点P_v的连线位于速度向量的右侧时，加速度指令方向垂直于速度方向指向右侧，反之，加速度指令方向垂直于速度方向指向左侧；当无人机与期望路径的最小距离大于L₁时，即以无人机所在位置为圆心，L₁为半径的圆与期望路径无交点时，设置无人机的期望运动方向指向无人机在期望路径上的投影点；根据非线性导引得到法向加速度之后，可以计算出相应的偏航角指令ψ_c，发送给航向自动驾驶仪；

对于非线性导引控制器，当所跟踪的期望路径为直线时，导引律近似于比例微分控制器；视线方向和速度方向的夹角η＝η₁+η₂，其中η₂为视线和所跟踪曲线的夹角，η₂＝d/L₁，其中d为无人机距离其在期望路径上的投影点的距离，η₁为速度方向和所跟踪直线的夹角，其中为无人机与其在期望路径上投影点距离的导数；所以，非线性导引控制指令此时该控制器比例微分控制器，控制器的参数为无人机当前的速度大小V和前视距离L₁，对应的二阶系统的阻尼系数为常值0.707，时间常数为L₁/V，在无人机速度大小保持不变时，前视距离L₁越小，响应时间越快；假设无人机所需跟踪路径包含的最高频率分量对应的波长为L_p，则无人机能够准确跟踪期望路径的条件为前视距离参数L≤L_p/4.4，该条件可用以选择非线性导引控制的参数L；

无人机进行目标跟踪时的期望轨迹为以目标为中心的圆周运动；在进行多机协同跟踪的过程中，首先根据相位差控制计算出无人机i的期望速度V_i，而后根据期望速度V_i利用式(2)所示的非线性导引控制计算出加速度指令，进而得出偏航角速度指令；由于上述过程计算得出的期望速度以及偏航角速度均为相对目标而言；因此，在实际的飞行过程中，需要利用目标的速度对上述过程计算出的速度和偏航角速率进行校正，计算出无人机实际的速度和偏航角指令，发送给自动驾驶仪；

步骤五：基于生物社会力的异构无人机集群系统描述

集群中携带光电设备的无人机能够获取到目标的位置和速度信息，剩余的无人机携带激光武器其他装备；按获取信息情况对集群中的无人机进行分类，将能够获取到目标信息的无人机称为信息个体，剩余的无人机为非信息个体，则异构无人机集群中的信息个体根据非线性导引方法计算出目标跟踪指令，非信息个体受到生物社会力的作用；

考虑异构无人机集群系统由信息个体和非信息个体组成，信息个体能够获取到所需跟踪目标的信息，而非信息个体在群集中其他个体的影响下进行运动，系统方程如下：

其中，i＝1,2,...,N,为个体i的位置，为个体i的速度，为个体受到的控制力的作用；

步骤六：基于生物社会力的异构无人机集群目标跟踪

在进行目标跟踪的过程中，群集中的信息个体能够根据目标当前的位置和速度利用非线性导引方法计算出虚拟目标点来进行跟踪，假设该虚拟目标的运动方程如下：

其中，q_L为虚拟目标的位置，P_L为虚拟目标的速度，f_L为虚拟目标的加速度；

设计个体的控制输入由三部分组成，α为通过远距离吸引-近距排斥作用控制个体位置的位置协同项，β为实现个体速度一致的速度协同项，这两项共同构成了个体所受到的社会力，γ为对虚拟目标进行跟踪的控制项，个体所受的控制力形式如下：

其中，N_i为个体i的交互邻居集合，ψ(||q_i-q_j||)为产生吸引-排斥作用的势函数，a_ij(t)为邻接矩阵中的元素，c₁和c₂为正的增益，h_i为信息个体的示性函数，如果个体i为信息个体则h_i＝i，否则h_i＝0；

在上述控制力作用中，只有在个体为信息个体时，才受到γ项对虚拟目标进行跟踪的作用力，而非信息个体只受到社会力的作用；对于非信息个体，采用基于注意力机制的群集模型来确定交互邻居，根据周围个体对局部有序度的贡献计算其显著度值，而后采用阈值响应模型改变自身的状态，并在不同的状态下采取不同的运动策略；

群集系统方程可写成如下矩阵形式：

其中，位置向量速度向量控制力向量

对于上述的群集系统，考虑如下的系统能量函数：

其中，U_i(q)为个体i的势能，E_i(p_i)为个体i相对于虚拟目标的动能，分别表示如下：