室内多无人机编队飞行仿真验证平台及其实现方法与流程

本发明涉及多无人机编队控制技术领域、无线通信领域、嵌入式技术领域，具体来说，是一套由室内定位系统、多无人机、ZigBee无线传输以及地面站组成的多无人机编队飞行仿真验证平台。

背景技术：

无人机技术经过几十年的发展已经相对成熟,在军事和民用中均发挥了独特的作用。在很多条件恶劣，现场环境复杂导致人员难以抵达的情况下,无人机凭借其自身优点给我们提供了一个更加合适的选择。然而为了适应未来的挑战,除了提高单机的功能和效用外,还需要考虑如何基于现有的技术手段,发展更加有效的无人机管理和组织模式。多无人机编队飞行是近年来提出的无人机合作化发展方向中的一个重要概念。多无人机编队飞行,即多架无人机依据任务要求而进行的某种队形排列和任务分配的组织模式,它既包括编队飞行的队形产生、保持和变化,也包括飞行任务的规划和组织。多无人机编队飞行是无人机发展的一个重要趋势,拥有十分广阔的应用前景。

因为单无人机搭载的设备有限,所以要完成某些比较复杂的任务时,就必须出动多次。而编队作业的无人机组可以分散搭载不同设备,将一个复杂的任务拆分为多个简单的任务,分配给编队中的不同无人机,使任务能够一次完成,大大提升执行任务的效率。多无人机携带不同的装备,还可以通过协作完成单架无人机不能完成的任务,如高精度定位、多角度成像及战区通信中继等。而且在无人机执行任务过程中,难免由于各种意外而造成损失。当无人机因突发情况而不得不脱离任务时,对于单无人机来说,就意味着任务失败,而对于编队飞行的无人机来说,只是任务的完成程度受到了影响,我们可以在编队中编入备用的无人机以保证整个编队能够继续执行任务。这种可靠性和冗余度在复杂多变的任务中(特别是战场中)更为重要,这是多无人机编队飞行的一个突出的特点和优势。然而目前现有的多无人机控制方法还存在很多难题，大多数研究仅限于理论仿真，而真正采取多无人机开展的验证仍然比较少，因而搭建一套多无人机编队飞行仿真验证平台已成为理论研究进一步发展的迫切需求。

技术实现要素：

为克服现有技术的不足，本发明旨在提出一套能够支持多无人机编队控制从理论分析到实际飞行的仿真验证平台。

本发明在实际编队飞行中可实时获取飞行参数，用以改进理论算法，进一步促进理论研究的发展。

本发明在仿真验证的基础上可同时作为多无人机编队飞行演示平台，更直观的显示多无人机编队控制效果。

本发明采用的技术方案是，室内多无人机编队飞行仿真验证实现方法，步骤是，首先利用室内定位系统经过标定构建出室内xyz三维坐标系，利用高速红外摄像头依据贴装在各无人机上的标记点，实时抓拍并进行预处理，借助交换机传给地面计算机；利用地面计算机对摄像头传来的数据进行处理，实时地解算出每个无人机的当前位置，并经过打包通过TCP/IP协议的方式传送给地面站；采用借助ROS的进程通信机制的地面站，其中，每个进程以节点方式运行，采取订阅、发布会话的途径接收定位系统传来的位置数据，同时在地面站运行相关决策算法计算出各无人机下一时刻的期望位置，并通过ZigBee无线通信传输出去；无人机通讯网络采取分布式、自组网的形式进行搭建，没有长机僚机之分，每个无人机均可直接与地面站通信，同时彼此间也可进行数据交互，实现了数据的组网传输；各无人机接收地面站传来的数据，经过解包处理提取当前位置与期望位置信息，并运行机载控制算法，驱动无人机运动到设定的期望位置，如此循环往复，最终实现多无人机编队飞行的仿真验证。

利用地面站提供多无人机参考轨迹、显示多无人机当前状态与通信功能，地面站同时需要实时运行相关的编队控制算法。

通过ZigBee传输出去的具体步骤是，首先将地面站上Xbee模块设置为协调器模块，其他Xbee模块均设为路由模块，协调器模块通过RS-232串口与地面站相连，路由模块同样借助串口与每个无人机上的控制器有线相连，操作人员在地面站只需编写相应的控制指令，然后通过查询各模块的网络地址，组建相应的数据帧，便可借助协调器发送到目标飞行器，同时各飞行器也可以回传信息，检验发送是否成功，是否存在丢包、堵塞问题；而且各路由模块均可广播信息，用以告知自己的姿态、位置等信息，避免编队飞行中发生碰撞。

采用借助ROS的进程通信机制的地面站具体是，地面站设置有接收定位系统数据模块、显示各无人机状态信息模块、编队轨迹生成模块和给每个无人机发送反馈位置与参考轨迹模块，各个模块分别以节点的形式运行于ROS环境中，相当于操作系统中各个进程的运行方式；各个节点之间通过ROS提供的“发布/订阅”的方式进行数据交互。

室内多无人机编队飞行仿真验证平台，包括，无人机、机载传感器、地面计算机、地面站、室内红外摄像头；

无人机的姿态信息通过机载传感器获得；

地面计算机用于：标定构建出室内xyz三维坐标系；红外摄像头依据贴装在各无人机上的标记点，实时抓拍并进行预处理，借助交换机传给地面计算机，地面计算机对红外摄像头传来的数据进行处理，实时地解算出每个无人机的当前位置，并发送给地面站；

地面站运行在Linux环境下，借助ROS的进程通信机制，每个进程以节点方式运行，采取订阅、发布会话的途径接收地面计算机传来的每个无人机位置数据，同时运行相关决策算法计算出各无人机下一时刻的期望位置，并通过ZigBee无线通信传输出去；

无人机通讯网络采取分布式、自组网的形式进行搭建，每个无人机均可直接与地面站通信，同时彼此间也可进行数据交互；各无人机接收地面站传来的数据，经过解包处理提取当前位置与期望位置信息，并运行机载控制算法，驱动无人机运动到设定的期望位置。

无人机采用分环控制的结构形式，内环接收机载传感器进行姿态控制，外环为位置控制，均采用机载PID控制器实现。

本发明的特点及有益效果是：

本发明对于多无人机编队控制领域的算法研究具有十分重要的意义。本发明稳定可靠，可直接作为多无人机编队飞行研究的仿真验证平台，通过多无人机位置反馈环节、机载控制器、ZigBee无线通信机制、地面站等的设计，解决编队协同控制、编队避障、视觉跟踪以及轨迹优化等问题，推动多无人机编队控制理论的发展。其飞行过程不仅可以为多无人机编队算法提供数据支持，进一步改进算法，还可以直接充当演示效果。总而言之，该平台可以很好的为理论研究服务，避免不必要的经费投入，具有很高的实用价值。

附图说明：

附图1室内多无人机编队飞行仿真验证平台总体结构图。

附图2室内定位系统效果图。

附图3室内定位系统原理图。

附图4多无人机编队控制结构图。

附图5无人机单机控制结构图。

附图6 ZigBee通信网络结构图。

附图7 ZigBee组网发送数据流程图。

附图8地面站功能结构图。

附图9地面站程序流程图。

具体实施方式

多无人机和地面站间的无线通信是实现整个系统的关键一环，肩负着无人机编队和地面站间的信息交换。随着无线通信技术的快速发展，无人机通讯网络也得到了极大的提升。可是，当前无人机通讯数据链仍然存在结构简易、传输速率低、抗干扰能力有限等问题。在当代电子战背景下，无人机通讯网络要求拥有大容量的数据收发，极强性的抗干扰性。因此，改善通讯传输速率，加强抗干扰能力依然是无人机通讯网络系统亟待解决的研究课题。ZigBee是以IEEE802.15.4为标准的局域网通信协议，具有简易、自组网、价廉、低功耗等特性，广泛应用于自动控制领域。基于ZigBee是一个自组网的无线通讯网络，传输距离最远能达到几公里，而且可以随意扩展，因此我们选取ZigBee来进行无人机通讯网络的开发。

ROS(Robot Operating System)是一个机器人软件平台，它能为异质计算机集群提供类似操作系统的功能，例如硬件抽象、底层设备控制、常用功能实现、进程间消息以及数据包管理等。ROS基于图状架构，不同节点的进程可以彼此接受、发布各种信息(例如传感，控制，状态，规划等)。我们主要借助它的进程通信功能，通过TCP/IP网络协议，接受定位系统传来的数据并实时发送给目标飞行器，最终实现了地面站与多无人机间数据的实时交互。

本发明就是通过基于室内定位系统，并融合ZigBee无线通信技术、嵌入式开发技术发明了一种多无人机编队飞行仿真验证平台。该平台可以很直观的进行相关理论算法验证(例如编队协同控制算法、编队避障算法、视觉跟踪算法以及轨迹优化算法等)，促进理论研究的更好更快发展。

要设计多无人机编队控制系统，首先需要确定该系统由哪几个部分组成。一般来说，一套完整的控制闭环需要有被控对象、传感器、控制器。就多无人机编队控制系统来说，需要有多架四旋翼无人机，需要有能够得到每个无人机状态信息(姿态信息和位置信息)的传感器，需要有地面控制站(可简称为地面站)。地面控制站和无人机编队之间需要有无线通信机制。四旋翼无人机的实现可以通过直接购买成品无人机，或者自行设计，购买零部件进行组装，本平台采取购买零部件组装的方式。各无人机的姿态信息通过机载传感器获得，位置信息由室内定位系统提供。室内定位系统由摄像头、交换机、地面计算机组成，摄像头负责捕捉拍照，交换机将摄像头采集来的数据传送到地面计算机，即定位系统上位机，地面计算机运行配套软件通过对数据进行解算，便可得出各无人机的当前位置。多无人机与地面站之间的无线通信机制则采取ZigBee分布式自组网的方式。总之，整个系统闭环由室内定位系统、多无人机、ZigBee无线传输以及地面站组成。

室内多无人机编队飞行仿真验证平台工作原理为：首先室内定位系统经过标定构建出室内xyz三维坐标系，8个高速红外摄像头依据贴装在各无人机上的标记点，实时抓拍并进行预处理，借助交换机传给地面计算机，即定位系统的上位机。地面计算机运行定位系统配套软件对摄像头传来的数据进行处理，实时地解算出每个无人机的当前位置，并经过打包通过TCP/IP协议的方式传送给地面站；地面站运行在Linux环境下，借助ROS的进程通信机制，每个进程以节点方式运行，采取订阅、发布会话的途径接收定位系统传来的位置数据，同时运行编队控制算法，可实时对编队队形进行切换并计算出各无人机下一时刻的期望位置，然后通过ZigBee无线通信传输出去；无人机通讯网络采取分布式、自组网的形式进行搭建，没有长机僚机之分，每个无人机均可直接与地面站通信，同时彼此间也可进行数据交互，实现了数据的组网传输；各无人机接收地面站传来的数据，经过解包处理提取当前位置与期望位置信息，并运行机载控制算法，驱动无人机运动到设定的期望位置，如此循环往复，最终实现多无人机编队飞行的仿真验证。其中，地面站提供多无人机参考轨迹、显示多无人机当前状态与通信功能。

此项发明对于多无人机编队控制领域的算法研究具有十分重要的意义。该项发明稳定可靠，可直接作为多无人机编队飞行研究的仿真验证平台，解决编队协同控制、编队避障、视觉跟踪以及轨迹优化等问题，推动多无人机编队控制理论的发展。其飞行过程不仅可以为多无人机编队算法提供数据支持，进一步改进算法，还可以直接充当演示效果。总而言之，该平台可以很好的为理论研究服务，避免不必要的经费投入，具有很高的实用价值。

本发明主要具有以下功能及特点：

(1)为实现多无人机位置反馈环节，本平台采用了基于运动捕捉系统的室内定位技术。系统使用之前首先需要进行标定操作，构造出室内环境中的xyz三维坐标系。在被测物体上安装红外反射标记点，红外摄像头便可高速捕捉反射标记点，实时输出捕捉信息。同时，需要一台计算机运行相关配套软件，对采集来的数据进行处理，最终准确地解算出被测物体的三维坐标数据与三轴姿态数据，进而可以通过以太网，以TCP/IP协议格式发送给所需要的计算机。

(2)机载控制器是无人机的核心部件，肩负着每个无人机上的所有运算任务，包括数据采集与滤波，实时控制、无线通讯等。不少研究机构设计了基于单片机、ARM或者DSP架构的机载控制器。但大多数机载控制器是封闭的，无法嵌入自定义的控制算法，不利于理论方法的研究。本平台选取了Pixhawk作为小型四旋翼无人机的机载控制器，提供软件接口，所有源代码均开源，用户可以自行开发所有功能，如需更改，只需将自己的算法以代码形式嵌入机载控制器即可，方便快捷，极大地缩短了研发周期。

(3)多无人机编队通信网络选取实时性最好的ZigBee无线通信机制。无线模块为美国Digi公司生产的Xbee Pro S2B。首先将一个Xbee模块设置为协调器模式，其他模块设置均设为路由模式，协调器模块通过RS-232串口与地面站相连，路由模块同样借助串口与每个无人机上的控制器有线相连，操作人员在地面站只需编写相应的控制指令，然后通过查询各模块的网络地址，组建相应的数据帧，便可借助协调器发送到目标飞行器，同时各飞行器也可以回传信息，检验发送是否成功，是否存在丢包、堵塞等问题；而且各路由模块均可广播信息，用以告知自己的姿态、位置等信息，避免编队飞行中发生碰撞。

(4)地面站作为完整闭环控制系统中不可或缺的一部分，承担着相关数据信息的接收、处理、显示、发送等任务，同时需要运行编队控制算法，实时对编队队形、编队轨迹进行控制。为了能够快速地实现地面站的功能，平台选择了基于ROS(机器人操作系统)开发环境进行相关的开发工作。ROS系统广泛应用于机器人领域，提供了类似桌面操作系统的功能，如进程间通信机制、硬件抽象描述、底层驱动程序管理、程序间的消息传递、程序发行包管理等，同时支持多种编程语言。地面站的开发，在硬件上需要一台通用计算机，在该计算机上安装Linux操作系统，因为ROS系统只能在类Unix环境下运行。目前地面站实现的4个子功能分别是“接收定位系统数据”、“显示各无人机状态信息”、“编队轨迹生成”和“给每个无人机发送反馈位置与参考轨迹”。各个子功能分别以节点(Node)的形式运行于ROS环境中，相当于操作系统中各个进程的运行方式。各个节点之间通过ROS提供的“发布/订阅”的方式进行数据交互。

地面计算机其实就是定位系统的上位机，定位系统由摄像头、交换机、上位机组成，摄像头进行捕捉拍照，交换机将八个摄像头采集来的数据传到定位系统上位机(地面计算机)，上位机运行配套软件进行解算，得出标记点的位置数据(即无人机的当前位置)，然后通过tcp/ip协议传给地面站；地面站运行编队控制算法，确定每架无人机的期望位置，再通过zigbee将每架无人机的当前位置、期望位置发送出去。

地面站运行编队控制算法，可实时对编队队形进行切换并最终计算出编队里每架无人机的位置，通过ZigBee发给每架无人机；无人机端只运行机载控制算法，驱动自身运动到设定的期望位置；

结合附图对本发明作进一步详述。

参见图1，室内多无人机编队飞行仿真验证平台总体结构图。系统主要由室内定位系统、多无人机、ZigBee无线传输以及地面站组成，可实现多无人机编队协同控制、编队避障、视觉跟踪以及轨迹优化等算法的仿真验证。

参见图2，四旋翼无人机要能够跟踪期望轨迹，控制回路就需要有位置反馈，就需要有能够提供无人机位置信息的机制。考虑到本平台设计的多无人机编队飞行控制系统处于室内环境中，因此无法使用全球定位系统(GPS)为无人机提供位置信息，而是采用了基于运动捕捉系统的室内定位技术。本文所使用的室内定位系统，是由美国NaturalPoint公司生产的Optitrack系列产品。该设备主要8个高速红外摄像头、多个红外反射标记点、交换机、标定工具以及相关软件组成，一般情况下，系统输出的三维位置精度能够达到毫米级。图中，被测物体为6架小型四旋翼无人机，定位系统通过识别每架无人机上贴装的红外标记点便可实时地输出各无人机的位置信息与姿态信息。

参见图3，理论上说，对于空间中的一个点，只要它能同时为两部摄像机所见，则根据同一时刻两部摄像机所拍摄的图像和摄像机参数，可以确定这一时刻该点在空间中的位置。当摄像机以足够高的速率连续拍摄时，从图像序列中就可以得到该点的运动轨迹。本平台所选取的定位系统采用8个高速红外摄像头，每个摄像头具有由62个LED灯组成的环形阵列，可以辐射出波长为850nm的红外射线。工作时只需要与红外反射标记点配套使用，便能够快速捕捉反射标记点在三维空间中的位置。同时，该摄像头还具有千兆以太网接口，能够实时地输出捕捉信息。该设备安装于室内环境中，使用之前需要进行标定操作，构造出室内环境中的xyz三维坐标系。然后在被测物体上安装红外反射标记点，8个红外摄像头高速捕捉反射标记点，在同一时刻拍下被测物体(安装标记点)的图像并完成预处理工作，预处理的图像通过交换机(10个以太网接口)传给地面计算机，即定位系统上位机。地面计算机运行相关配套软件，对接收到的8张预处理过的图像实时进行相关运算，最终准确地解算出被测物体的三维坐标数据与三轴姿态数据，进而以NatNet协议打包定位数据，并通过以太网以TCP/IP协议格式发送给其他需要使用这些数据的计算机。

参见图4，多无人机编队飞行需要地面站给各无人机发送期望位置与位置反馈。期望位置通过编队控制算法生成，位置反馈通过定位系统采集，然后均借助ZigBee无线通信机制发送给目标无人机，驱动各无人机运动到设定的期望位置，实现多无人机的编队飞行验证，如此循环往复，最终构成多无人机编队控制的系统闭环。

参见图5，针对四旋翼无人机单机的控制问题，采用分环控制的结构形式，内环为姿态控制，外环为位置控制，均采用PID控制算法实现。从软件实现的角度看，本文将姿态控制器和位置控制器实现为两个独立运行的进程，利用机载控制器Nuttx系统提供的uORB进程间通信机制，将位置控制器的输出传递给姿态控制器作为输入值。姿态控制器和位置控制器的软件实现在写法上基本相同。

参见图6，Xbee模块有透传(AT模式)与API俩种工作形式。各无人机位置反馈采用透传模式，当模块工作在透传形式，一根串口线仿佛就可以将它代替。DIN针脚接收到的全部数据可以借助射频形式发送，与此同时DOUT针脚则送入全部接收到的数据，非常简单。地面站与每个无人机单独形成一个通信链路，这样可以很快的将定位系统采集来的位置信息发送给目标飞行器，保证多无人机编队飞行过程中位置反馈环节的实时性，但透传模式只支持点对点进行传输，无法实现组网通信。各无人机期望位置采取API模式，API操作是另一种传输的操作方式，支持自组网，可实现点对多点的数据交互，地面站运行一个Xbee模块，各无人机依次搭载一个Xbee模块，他们之间通过寻址可任意传输数据，同时地面站也可以通过广播将期望轨迹发送到每个无人机，最终实现整个无人机通讯网络的搭建。

参见图7，ZigBee组网协议给每个模块指定了两种地址：16位网络地址和64位地址。16位网络地址：当一个节点连接网络后，协调器会自动将一个16位网络地址分配给它。64位网络地址：每个节点在出厂时包含一个唯一的64位网络地址，这个地址是永久的，可以用它在全世界找出一个特定的节点。模块在没有进行数据传输时，工作在于空闲状态。当模块接收到数据，首先会退出空闲状态并开始发送数据。哪个路由可以接收到数据由数据的目标地址决定。工作之前，模块必须先保证16位网络地址与目标路由已经连接。假如网络地址还未知道，模块要搜寻周边存在的网络地址；假如路由未知道，模块同样也会开展相应的搜寻。一旦确定模块就立刻将数据放到发送缓冲区，进而发送到目标模块，否则丢弃。

参见图8，地面控制站的开发，在硬件上需要一台通用计算机，在该计算机上安装Linux操作系统，因为ROS系统只能在类Unix环境下运行。我们选用开发文档最丰富的Ubuntu系统，同时Ubuntu系统也是ROS官方支持的版本。在Linux系统下的terminal终端窗口中通过命令行的形式完成ROS的下载与安装，并进行地面站主体功能的开发。目前确定了需要实现的5个子功能分别是“接收定位系统数据”、“显示每个无人机状态信息”、“轨迹生成”、“接收每个无人机状态信息”、“给各无人机发送反馈位置与参考轨迹”。各个子功能分别以节点(Node)的形式运行于ROS环境中，彼此通过“发布/订阅”的方式进行数据交互。

参见图9，在ROS中完成地面站主控制程序的开发，主控制程序首先需要订阅并读取定位系统发来的无人机位置数据(大于100Hz)，再通过ZigBee无线通信机制地将定位数据反馈给各无人机。然而无人机只有位置反馈信息还不能够正常飞行，还需要地面站端给出无人机的期望位置，无人机上的位置控制器迫使无人机跟踪给定的期望位置。在软件实现上，采用多线程的编程技术。订阅并读取每个无人机当前位置信息，进而再通过ZigBee无线通信实时的发送给目标无人机，这个过程实现为一个线程。读取各无人机的期望位置，并通过ZigBee无线通信发送给目标无人机，这个过程实现为另一个线程。这两个线程独立运行，但当两个线程需要访问共享资源时，比如两个进程都想往串口写数据(发生冲突)，这时就涉及到线程间的同步问题，本平台采用互斥锁来处理此问题。