CN110413006B - 一种基于5g通信的无人机编队组网系统及方法 - Google Patents

Abstract

本发明公开了一种基于5G通信的无人机编队组网系统及方法，属于多无人机协同编队技术，组网系统包括无人机及安装在无人机上的无人机飞控系统、RTK‑北斗/GPS定位模块、IMU惯导模块、5G通信模块，以及地面控制系统和地面5G基站。包括以下步骤：S10：无人机上的5G通信模块接受地面控制系统发出的指令；S20：无人机自适应组网，寻找空闲节点；S30：单机间交换飞控、定位等数据；S40：发送消息至地面控制端并进行集群控制。采用了5G通信方式，提高了传输速率和传输机制的可靠性；采用了自定义通信格式，可以提高通信效率和增强保密性；采用RTK‑北斗/GPS定位模块，提高无人机的定位精度；采用自适应组网方式，提高机间通信效率，降低掉包概率。

Description

一种基于5G通信的无人机编队组网系统及方法

技术领域

本发明涉及无人机编队组网的通信技术领域，具体涉及一种基于5G通信的无人机编队组网系统及方法。

技术背景

由于无人机具有在体积和重量上的高度灵活性、成本低以及可代替人类在恶劣环境下完成任务等方面的优势，因此在不同领域的应用越来越广泛。无人机在地质勘探、防灾、减灾、侦察、作战等多种领域展示出了巨大的价值。随着无人机技术的日趋成熟，无人机应用趋势逐渐由单机执行任务转为多机协同执行任务，朝着集群化的方向快速发展，采用无人机集群技术可以最直接有效的提高多无人机工作能力和效率，而无人机集群首先要解决组网通信和编队控制这两个关键技术点。未来集群协同无人机必将有巨大的应用空间。无线自组织通信网络具有无中心、不依靠基础通信设施、地位对等、可任意部署的优点，在集群飞行控制中有非常重要的应用价值。由于受环境影响和无人机自身移动等因素的影响，无人机集群的网络拓扑结构随时可能发生变化，为了形成集群编队，避免无人机相互碰撞，各无人机之间需要实时通信，获取相互位置信息，同时无人机也需要将三维坐标数据等及时有效的回传，便于控制中心实时掌控每架无人机的状态。因此如何以较低的网络开销保证各无人机之间相互通信，就成了无人机集群要解决的首要问题。

发明内容

针对现有技术的不足，本发明提供了一种基于5G通信的无人机编队组网系统及方法的技术方案，通过5G通信网络的低时延技术，保证了无人机集群编队通信网络的实时数据传输。

一种基于5G通信的无人机编队组网系统，包括无人机，还包括安装在无人机上的无人机飞控系统、RTK-北斗/GPS定位模块、IMU惯导模块、5G通信模块，以及地面控制系统和地面5G基站；

RTK-北斗/GPS定位模块，用于获取无人机集群中每架飞机的室外定位数据；

IMU惯导模块用于获取无人机集群中每架飞机的姿态角度和各个方向的运动加速度；从而获取每架无人机的实时飞行姿态；

5G通信模块，通过5G通信协议和频段进行无人机机间的信息通信以及无人机集群和地面控制系统的信息通信；获取地面控制系统通过地面5G基站发出的指令和数据；

无人机飞控系统，与RTK-北斗/GPS定位模块、IMU惯导模块、5G通信模块连接，在总线上对各模块进行调度使用和数据融合等工作；

地面控制系统规划无人机运动轨迹，通过地面5G基站发布控制指令和期望航迹，可以与任意无人机的数据交互。

进一步的，所述的IMU惯导模块包括陀螺仪、加速度计和磁力计，用四元数软件结算姿态，具体步骤包括：

1)利用欧拉角描述一次平面旋转，并得到一个表示旋转的方向余弦矩阵；

2)定义四元数，根据四元数方向余弦矩阵和欧拉角的转换关系，得到最终的欧拉角值；

在一个坐标旋转α角后得到新的坐标系，在空间中的一个矢量在坐标系中的投影可以用矩阵形式表示：

得到表示旋转的方向余弦矩阵后，先定义一个四元数：

方向余弦矩阵用四元数表示则为：

所以在软件解算中，叉积是陀螺仪测量的姿态和加速计测量的姿态在向量上的叉乘，可以用向量叉积表示；叉积向量仍旧是位于机体坐标上的，而陀螺仪误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，所以先用叉积误差来做PI修正陀螺仪零偏：

exInt＝exInt+ex*Ki；

eyInt＝eyInt+ey*Ki；

ezInt＝ezInt+ez*Ki；

gx＝gx+Kp*ex+exInt；

gy＝gy+Kp*ey+eyInt；

gz＝gz+Kp*ez+ezInt；

其中Ki表示陀螺仪收敛速率的积分增益，Kp表示加速度计收敛速率的比例增益gxyz表示陀螺仪校正，四元数微分方程：

q₀＝q₀+(-q₁*gx-q₂*gy-q₃*gz)*halfT；

q₁＝q₁+(q₀*gx+q₂*gz-q₃*gy)*halfT；

q₂＝q₂+(q₀*gy-q₁*gz+q₃*gx)*halfT；

q₃＝q₃+(q₀*gz+q₁*gy-q₂*gx)*halfT；

其中halfT是测量周期的一半；

最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：

Yaw＝atan2(2*q₁*q₂+2*q₀*q₃,-2*q₂*q₂-2*q₃*q₃+1)*57.3；

pitch＝asin(-2*q₁*q₃+2*q₀*q₂)*57.3；

roll＝atan2(2*q₂*q₃+2*q₀*q₁,-2*q₁*q₁-2*q₂*q₂+1)*57.3；。

进一步的，所述的5G通讯模块的通讯方式为：利用5G通信的非正交传输技术，支持多个终端在同一时间共同使用无线资源，在mMTC型应用场景中，通过地面5G基站对于多架无人机进行实时的，少量控制信令的信息传递。

进一步的，所述的无人机飞控系统将对所采集到的每帧数据进行处理，具体地，先进行CRC和校验取出有效数据再对数据进行提取使用，所用的通信协议为：0x41,0x88,0,0xca,0xde,’W’,’A’,’V’,’E’,0x21,0,0,0,0,0,0,0,0,0,0,0,0

由以下字段组成：

字节0/1：帧控制(0x8841表示使用16位寻址的数据帧)；

字节2：序列号，每个新帧递增；

字节3/4:PAN ID(0XDECA)；

字节5/6：目的地地址；

字节7/8：源地址；

字节9：功能代码(特定值，用于指示它在测距过程中的信息)；

其余字节特定于每条消息，如下所示：

响应消息：

字节10：活动代码，用于切换无人机的飞行状态，0x00：无人机做定点起飞；0x01：无人机做队列变换；0x02，告诉发起程序继续进行位置交换；

字节11->13：活动参数，包括pitch、roll、yaw角度值、身份ID；

字节14->17：响应消息接收时间戳；

字节18->20：最终消息传输时间戳。

进一步的，地面控制系统内置地图界面，可完成地图尺度的缩放，用于显示无人机各项数据，并设有输入text，通过软件可以手动输入无人机需要达到的地图坐标，通过5G通信来控制集群的坐标移动；地面控制系统可通过异步串口的通信方式与与电脑进行搭建；地面控制系统采用无人机MAC寻址的方式并通过5G通信模块发布控制指令和期望航迹，与无人机的进行数据交互；地面控制系统数据控制链路基于mavlink通信协议对飞行状态、姿态、位置和指令等数据打包，可采用单播、广播、组播和中继的通信模式与无人机通信，同时地面控制系统为每一架无人机分配一个MAC地址用于通信。

一种的基于5G通信的无人机编队组网方法，无人机上设有无人机飞控系统、RTK-北斗/GPS定位模块、IMU惯导模块、5G通信模块，地面设有地面控制系统和地面5G基站；组网方法包括以下步骤：

S10：无人机上的5G通信模块接受地面控制系统发出的指令；

S20：无人机自适应组网，寻找空闲节点；

S30：单机间交换飞控、定位等数据；

S40：发送消息至地面控制系统并进行集群控制。

进一步的，无人机编队组网网络包括多个子网络，子网络由网关，主控节点，成员节点组成，无人机编队组网网络中的所有节点均是独立的、对等的、可替代的，网络中的任意节点的失效，不会影响整个网络的通信效率，所以每架无人机更改机间通信的协议，让每架无人机都能得到与相邻无人机的飞行状态和位置。

进一步的，所述步骤S20中，无人机自适应组网进行自组网任务的分配，确定子网络中的网关，主控节点和成员节点，具体如下：

5G通信的无人机集群组网系统采用分层分布式的网络结构，集群数据链的MAC层协议采用CSMA和异步跳频机制为基础的SPMA协议，根据忙闲程度来决定是否发送或接受信息(即通过任务分配为导向选择休眠机制、接入机制、收发机制)，在实际飞行中，假设每架无人机到达网络的过程速率为u，则总的到达速率为Qu，每架飞机接入时隙数为Ms，每个时隙总的到达时间为Qu/Ms，且有一架飞机到达即为接入成功，所以一个时隙上飞机接入成功的概率Pm为：

其中Tm指节点接入时长；

将首先成功接入网络的单机作为主控节点，在多个子网络中，路由选择只是由网络中的小部分节点来完成；主控节点在路由链路中广播通信协议，最先收到信息并做出回复的4到5架飞机作为成员节点，如果成员节点收到多个主控节点发出的通信协议信息，则将自己设置为网关节点，如果出现信息传递错误，飞机处于重传状态等于多次处于重传状态的飞机累计和，可以得到单架飞机处于重传状态次数Nr为：

其中r是重传次数，q为节点需要重新接入的概率；

在自网络中，一个成员节点分配一个相应的时隙，当一个节点分配的时隙到达时塔才开始工作；在分配的时隙中完成相应的任务时会消耗相应的时间，传输时延Td的长度为：

其中

为用户每次重传均需等待平均传输周期，r为重传次数，q为节点需重新接入的概率；

在两个子网络需要相互通信时，主控节点会把所有成员节点的数据打包，通过网关发送到其他几个子网络中。

进一步的，所述步骤S20中，无人机自适应组网进行自组网任务的调度，具体如下：自组网络任务调度基于多维列表调度，任务调度按照一个任务链路中的优先级来运行，每个主控节点首先获得时隙后进行任务的执行，处于链路位置后面的成员节点根据之后分配到的时隙一次进行任务执行，链路位置后面的任务需要前面的任务完成后才能开始；

首先，选择第一个准备好的节点，通常为主控节点即优先级较高的节点，完成初始化的时间为t，连接改变的数量为d，初始化时间集合FT＝[0]，主控节点完成任务的时间f＝min(f_t)，完成一个节点的任务后需要进行资源的更新，R＝R+d_FG*H，R为初始化集群剩余资源的集合，

H为自定义配系矩阵；d_FG表示对应数量下所需完成的任务。

更新完资源后，第二步判断下面的成员节点能完成任务；

如果链路下一层的节点对应的模体需求矢量与配系矩阵的乘积大于集群剩余资源的集合，即h_new*H＞R则认定其没有收到上一层节点完成任务后的信息，则继续返回完成上一任务。

若小于剩余资源集合，则下一节点继续执行任务，并且已执行完的节点数不断累加，即now＝now+h_new，剩余资源集合不断减少，即R＝R-h_new*H，至此完成两层任务的时间为f+S_new，S_new为上两层传递时消耗掉的时间，最终完成所有任务的时间是FT与f+S_new的并集，即FT＝FT∪{f+S_new}；

通过轮询使用时隙完成所有成员节点的任务，并计算出总的通信时长。

进一步的，无人机组网任务中，采用基于分布式人工势场算法防止无人机碰撞，具体如下：

每架无人机周围的势场分成3层，斥力球(r1)，引力球(r2)，临界球(r3)；当同一组中的任意两架无人机之间的距离过近时，即d≤2r₁时，无人机沿着相切的方向向相反方向移动，以减小碰撞坠机的危险性；

如果当同一组中的任意两架无人机之间的距离过远，即d≥2r₂时，他们会在两点相连的直线上向相近的方向移动，保持机间的正常距离；

如果在同一组中的任意两架无人机之间的距离2r₁≤d≤2r₂时，在这个情况中既没有斥力也没有引力，可以在此情况中维持，可以达到机间距离的平衡；

如果当无人机间的距离越发接近，需要控制他们之间的相互斥力更加明显，从而控制两架距离过近的飞机原理，现在设m为飞行区域中的一点，两架无人机之间的距离为n，v为斥力增益，则第i架无人机的斥力势场函数

为

斥力

是由斥力势场函数负梯度所得，可以表示为：

相同的，如果两架飞机距离过远，就会触发引力因子，在d≥2r₂时，引力势场会产生一个相互吸引的力，将两架飞机在向相的方向上互相靠近；如果设m为飞行区域中的一点，两架无人机之间的距离为n，w为斥力增益，则第j架无人机的斥力势场函数

为

为介于引力层半径至作用层半径之间的距离；

相对的，相应的引力

等于

至此，每架飞机所受到的合力等于其受到的所有斥力加上所有引力，即

本发明的有益效果：

1.采用了5G通信方式，提高了传输速率和传输机制的可靠性；

2.采用了自定义通信格式，可以提高通信效率和增强保密性；

3.采用RTK-北斗/GPS定位模块，提高无人机的定位精度；

4.采用自适应组网方式，提高机间通信效率，降低掉包概率。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面对实施例中所需要的附图作简单的介绍。

图1为本申请实施例提供的5G通信的无人机编队组网系统方法的流程示意图；

图2为申请实施例提供的无人机编队组网系统的系统框图；

图3为实施例中5G组网网络的结构示意图；

其中：1-无人机飞控系统；2-通信接口；3-RTK-北斗/GPS定位模块；4-IMU惯导模块；5-5G通信模块；6-地面5G基站；7-地面控制系统；8-显示模块。

具体实施方式

本发明实施例提供的5G通信的无人机编队组网方法，是利用5G通信的高时效性和高传输率对无人机集群中的机间进行可靠的传递与交互飞控和定位等信息，并将消息和指令通过地面控制系统和地面5G基站来对集群进行实时的交互和控制。另外，本实施例还提供了一种基于上述方法的5G通信的无人机编队组网系统。

以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。

实施例1

一种基于5G通信的无人机编队组网系统，如图2所示，包括安装在无人机上的无人机飞控系统1、RTK-北斗/GPS定位模块3、IMU惯导模块4、通信接口2、5G通信模块5、显示模块8，以及地面的地面控制系统7和地面5G基站6，所述无人机飞控系统1由处理器构成。

RTK-北斗/GPS定位模块3，用于获取无人机集群中每架飞机的室外定位数据；

IMU惯导模块4包括陀螺仪、加速度计和磁力计，用于获取无人机集群中每架无人机的姿态角度和各个方向的运动加速度；

IMU惯导模块，用于获取每架无人机的实时飞行姿态，其具体方式包括：利用硬件DMP解算姿态，或者用四元数软件结算姿态得到飞行状态；

利用四元数软件结算姿态的基本步骤有：

1)利用欧拉角描述一次平面旋转，并得到一个表示旋转的方向余弦矩阵。

2)定义四元数，根据四元数方向余弦矩阵和欧拉角的转换关系，得到最终的欧拉角值。

在一个坐标旋转α角后得到新的坐标系，在空间中的一个矢量在坐标系中的投影可以用矩阵形式表示：

得到表示旋转的方向余弦矩阵后，先定义一个四元数：

方向余弦矩阵用四元数表示则为：

所以在软件解算中，叉积是陀螺仪测量的姿态和加速计测量的姿态在向量上的叉乘，可以用向量叉积表示。叉积向量仍旧是位于机体坐标上的，而陀螺仪误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，所以先用叉积误差来做PI修正陀螺仪零偏：

exInt＝exInt+ex*Ki；

eyInt＝eyInt+ey*Ki；

ezInt＝ezInt+ez*Ki；

gx＝gx+Kp*ex+exInt；

gy＝gy+Kp*ey+eyInt；

gz＝gz+Kp*ez+ezInt；

其中Ki表示陀螺仪收敛速率的积分增益，Kp表示加速度计收敛速率的比例增益gxyz表示陀螺仪校正，四元数微分方程：

q₀＝q₀+(-q₁*gx-q₂*gy-q₃*gz)*halfT；

q₁＝q₁+(q₀*gx+q₂*gz-q₃*gy)*halfT；

q₂＝q₂+(q₀*gy-q₁*gz+q₃*gx)*halfT；

q₃＝q₃+(q₀*gz+q₁*gy-q₂*gx)*halfT；

其中halfT是测量周期的一半。

最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：

Yaw＝atan2(2*q₁*q₂+2*q₀*q₃,-2*q₂*q₂-2*q₃*q₃+1)*57.3；

pitch＝asin(-2*q₁*q₃+2*q₀*q₂)*57.3；

roll＝atan2(2*q₂*q₃+2*q₀*q₁,-2*q₁*q₁-2*q₂*q₂+1)*57.3；

就此得到每架无人机实时的飞行状况，结合图2中的RTK-北斗/GPS定位模块3得到的高精确度的定位数据，共同得到集群需要传递的信息。

DMP解算姿态的方法为通过IIC接口直接获取IMU惯导模块4的寄存器中的四元数。具体地，IMU惯导模块4自带一个时钟int，时钟频率可以在寄存器中设置，在目标时钟脚上用软件设置外部中断，外部中断设置为高电平触发，在时钟高电平到来时，触发外部中断，在中断中读取DMP硬件解算四元数。最后根据四元数方向余弦阵和欧拉角的转换关系，转化成欧拉角。表面上看DMP硬件解算角度更加方便而且不需要像软件解算那样需要自己设四元数，并通过算法实现，节省处理器的处理时间。但是DMP硬件解算存在一定的缺点，因为硬件解算在寄存器的频率设置中最大为200hz，也就是说5ms更新一次数据，但是软件解算可以把频率提高到1000hz，1ms一次，在无人机这种高速运行的控制类机器时，高刷新率是非常重要的。

通讯接口，用于无人机飞控系统1与RTK-北斗/GPS定位模块3、IMU惯导模块4、5G通信模块5、显示模块8的通信；IMU惯导模块4和处理器之间采用的是IIC的通信接口，RTK-北斗/GPS定位模块3和处理器之间采用的是UART的通信接口；5G通讯模块和处理器之间采用的是UART通讯接口。

5G通信模块5，是空中传递数据帧用的，数据在两个5G通信模块之间进行对接。通过5G通信协议和频段进行无人机机间的信息通信以及无人机集群和地面控制系统7的信息通信；

图2的5G通信模块5，将用于获取地面控制系统7通过地面5G基站6发出的指令和数据，其具体方式包括：可以利用5G通信的非正交传输技术，支持多个终端在同一时间共同使用无线资源，在mMTC型应用场景中，通过地面5G基站6对于多架无人机进行实时的，少量控制信令的信息传递。

无人机飞控系统1，利用处理器在总线上对各模块进行调度使用和数据融合等工作，与RTK-北斗/GPS定位模块3相连接，获取无人机的飞行信息包含位置数据、姿态数据和状态数据位置等；无人机飞控系统1与5G通信模块5连接，用于和附近相邻的无人机开展数据交互，并实时传递相关信息地面5G基站6；

显示模块8，用于显示飞控及定位结果。

图2无人机飞控系统1将对所采集到的每帧数据进行处理，具体地，先进行CRC和校验取出有效数据再对数据进行提取使用。

自定通信协议为0x41,0x88,0,0xca,0xde,’W’,’A’,’V’,’E’,0x21,0,0,0,0,0,0,0,0,0,0,0,0

这些帧的前10个字节是常见的，由以下字段组成：

字节0/1：帧控制(0x8841表示使用16位寻址的数据帧)。

字节2：序列号，每个新帧递增。

字节3/4:PAN ID(0XDECA)。

字节5/6：目的地地址。

字节7/8：源地址。

字节9：功能代码(特定值，用于指示它在测距过程中的信息)。

其余字节特定于每条消息，如下所示：

响应消息：

字节10：活动代码，用于切换无人机的飞行状态，0x00：无人机做定点起飞；0x01：无人机做队列变换；0x02，告诉发起程序继续进行位置交换。

字节11->13：活动参数，包括pitch、roll、yaw角度值、身份ID。

字节14->17：响应消息接收时间戳。

字节18->20：最终消息传输时间戳。

地面5G基站6将收到的无人机相关数据通过5G地面通信接口传输到地面控制系统7。

地面控制系统7为编队控制软件，通过异步串口的通信方式与笔记本电脑进行搭建，并且具有轨迹规划功能。内置地图界面，可完成地图尺度的缩放，用于显示无人机各项数据，并设有输入text，通过软件可以手动输入无人机需要达到的地图坐标，通过5G通信来控制集群的坐标移动。地面控制系统7采用无人机MAC寻址的方式并通过5G通信模块5发布控制指令和期望航迹，可以与任意无人机的数据交互；地面控制系统7数据控制链路基于mavlink通信协议对飞行状态、姿态、位置和指令等数据打包，可采用单播、广播、组播和中继的通信模式与无人机通信。同时地面控制系统7为每一架无人机分配一个MAC地址用于通信。

实施例2

一种利用实施例1中5G通信的无人机编队组网系统的组网方法，如图1所示，包括以下步骤：

S10：无人机上的5G通信模块5接受地面控制系统7发出的指令；

S20：无人机自适应组网，寻找空闲节点；

S30：单机间交换飞控、定位等数据；包括RTK-北斗/GPS定位模块3、IMU惯导模块4数据。

S40：发送消息至地面控制系统7并进行集群控制。

由于在5G通信条件下，高传输性的mMTC环境中，无人机集群网络中的所有节点钧是独立的、对等的、可替代的，网络中的任意节点的失效，不会影响整个网络的通信效率，所以每架飞机需要更改机间通信的协议，让每架飞机都能得到与相邻飞机的飞行状态和位置，所以在5G通信的无人机集群控制中需要多个由网关，主控节点，成员节点组成的子网络。

所述S20中，无人机自适应组网的具体方法包括：

S201：自组网任务分配和调度；

S202：自组网防碰撞算法的实现。

所述S201：自组网络任务分配和调度，具体包括：

1)确定子网络中的网关，主控节点和成员节点

5G通信的无人机集群组网系统采用分层分布式的网络结构，集群数据链的MAC层协议采用CSMA和异步跳频机制为基础的SPMA协议，根据忙闲程度来决定是否发送或接受信息(即通过任务分配为导向选择休眠机制、接入机制、收发机制)，在实际飞行中，假设每架无人机到达网络的过程速率为u，则总的到达速率为Qu，每架飞机接入时隙数为Ms，每个时隙总的到达时间为Qu/Ms，且有一架飞机到达即为接入成功，所以一个时隙上飞机接入成功的概率Pm为：

其中Tm指节点接入时长。

如图3所示，将首先成功接入网络的单机作为主控节点，在多个子网络中，路由选择只是由网络中的小部分节点来完成。主控节点在路由链路中广播通信协议，最先收到信息并做出回复的4到5架飞机作为成员节点，如果成员节点收到多个主控节点发出的通信协议信息，则将自己设置为网关节点，如果出现信息传递错误，飞机处于重传状态等于多次处于重传状态的无人机累计和，可以得到单架无人机处于重传状态次数Nr为：

其中r是重传次数，q为节点需要重新接入的概率。

在自网络中，一个成员节点分配一个相应的时隙，当一个节点分配的时隙到达时塔才开始工作。在分配的时隙中完成相应的任务时会消耗相应的时间，传输时延Td的长度为：

其中

为用户每次重传均需等待平均传输周期，r为重传次数，q为节点需重新接入的概率。

在两个子网络需要相互通信时，主控节点会把所有成员节点的数据打包，通过网关发送到其他几个子网络中。

2)自组网络任务调度

自组网络任务调度基于多维列表调度，任务调度按照一个任务链路中的优先级来运行，每个主控节点首先获得时隙后进行任务的执行，处于链路位置后面的成员节点根据之后分配到的时隙一次进行任务执行，链路位置后面的任务需要前面的任务完成后才能开始。

首先，选择第一个准备好的节点，通常为主控节点即优先级较高的节点，完成初始化的时间为t，连接改变的数量为d，初始化时间集合FT＝[0]，主控节点完成任务的时间f＝min(f_t)，完成一个节点的任务后需要进行资源的更新，R＝R+d_FG*H，R为初始化集群剩余资源的集合，

H为自定义配系矩阵。其中d_FG表示对应数量下所需完成的任务。

更新完资源后，第二步判断下面的成员节点能完成任务。

如果链路下一层的节点对应的模体需求矢量与配系矩阵的乘积大于集群剩余资源的集合，即h_new*H＞R则认定其没有收到上一层节点完成任务后的信息，则继续返回完成上一任务。

若小于剩余资源集合，则下一节点继续执行任务，并且已执行完的节点数不断累加，即now＝now+h_new，剩余资源集合不断减少，即R＝R-h_new*H，至此完成两层任务的时间为f+S_new，S_new为上两层传递时消耗掉的时间，最终完成所有任务的时间是FT与f+S_new的并集，即FT＝FT∪{f+S_new}。

通过轮询使用时隙完成所有成员节点的任务，并计算出总的通信时长。

S202：自组网络的防碰撞算法的实现，具体如下：

在无人机的集群组网任务中，实时性方面要求比较高，而且分配结果必须具有可操作性，所以本方法采用基于分布式人工势场算法为基础的任务调度算法来满足UAV集群组网任务。

每架无人机周围的势场可以分成3层，斥力球(r1)，引力球(r2)，临界球(r3)。当同一组中的任意两架无人机之间的距离过近时，即d≤2r₁时，无人机会沿着相切的方向向相反方向移动，以减小碰撞坠机的危险性。

如果当同一组中的任意两家无人机之间的距离过远，即d≥2r₂时，无人机会在两点相连的直线上向相近的方向移动，保持机间的正常距离。

如果在同一组中的任意两架无人机之间的距离2r₁≤d≤2r₂时，在这个情况中既没有斥力也没有引力，可以在此情况中维持，可以达到机间距离的平衡。

如果当无人机间的距离越发接近，需要控制无人机之间的相互斥力更加明显，从而控制两架距离过近的无人机原理，现在设m为飞行区域中的一点，两架无人机之间的距离为n的v为斥力增益，则第i架无人机的斥力势场函数

为

斥力

是由斥力势场函数负梯度所得，可以表示为：

相同的，如果两架飞机距离过远，就会触发引力因子，在d≥2r₂时，引力势场会产生一个相互吸引的力，将两架飞机在向相的方向上互相靠近。如果设m为飞行区域中的一点，两架无人机之间的距离为n，w为斥力增益，则第j架无人机的斥力势场函数

为

为介于引力层半径至作用层半径之间的距离。

相对的，相应的引力

等于

至此，每架飞机所受到的合力等于其受到的所有斥力加上所有引力，即

另外该系统还提供了通讯接口和显示模块8，用于传输数据以及显示飞行状况结果，飞行状况结果包含：六轴数据、GPRS定位数据、合力大小等等，可用于后期大数据分析。

Claims (5)

1.一种基于5G通信的无人机编队组网方法，包括无人机，安装在无人机上的无人机飞控系统(1)、RTK-北斗/GPS定位模块(3)、IMU惯导模块(4)、5G通信模块(5)，以及地面控制系统(7)和地面5G基站(6)；

RTK-北斗/GPS定位模块(3)，用于获取无人机集群中每架飞机的室外定位数据；

IMU惯导模块(4)用于获取无人机集群中每架飞机的姿态角度和各个方向的运动加速度；从而获取每架无人机的实时飞行姿态；

5G通信模块(5)，通过5G通信协议和频段进行无人机机间的信息通信以及无人机集群和地面控制系统(7)的信息通信；获取地面控制系统(7)通过地面5G基站(6)发出的指令和数据；

无人机飞控系统(1)，与RTK-北斗/GPS定位模块(3)、IMU惯导模块(4)、5G通信模块(5)连接，在总线上对各模块进行调度使用和数据融合工作；

地面控制系统(7)规划无人机运动轨迹，通过地面5G基站(6)发布控制指令和期望航迹，可以与任意无人机的数据交互；

其特征在于无人机编队组网网络包括多个子网络，子网络由网关，主控节点，成员节点组成，无人机编队组网网络中的所有节点均是独立的、对等的、可替代的，网络中的任意节点的失效，不会影响整个网络的通信效率，所以每架无人机更改机间通信的协议，让每架无人机都能得到与相邻无人机的飞行状态和位置，组网方法包括以下步骤：

S10：无人机上的5G通信模块(5)接受地面控制系统(7)发出的指令；

S20：无人机自适应组网，寻找空闲节点；无人机自适应组网进行自组网任务的分配，确定子网络中的网关，主控节点和成员节点，具体如下：

5G通信的无人机集群组网系统采用分层分布式的网络结构，集群数据链的MAC层协议采用CSMA和异步跳频机制为基础的SPMA协议，根据忙闲程度来决定是否发送或接受信息(即通过任务分配为导向选择休眠机制、接入机制、收发机制)，在实际飞行中，假设每架无人机到达网络的过程速率为u，则总的到达速率为Qu，每架飞机接入时隙数为Ms，每个时隙总的到达时间为Qu/Ms，且有一架飞机到达即为接入成功，所以一个时隙上飞机接入成功的概率Pm为：

其中Tm指节点接入时长；

将首先成功接入网络的单机作为主控节点，在多个子网络中，路由选择只是由网络中的小部分节点来完成；主控节点在路由链路中广播通信协议，最先收到信息并做出回复的4到5架飞机作为成员节点，如果成员节点收到多个主控节点发出的通信协议信息，则将自己设置为网关节点，如果出现信息传递错误，飞机处于重传状态等于多次处于重传状态的飞机累计和，可以得到单架飞机处于重传状态次数Nr为：

其中r是重传次数，q为节点需要重新接入的概率；

在自网络中，一个成员节点分配一个相应的时隙，当一个节点分配的时隙到达时才开始工作；在分配的时隙中完成相应的任务时会消耗相应的时间，传输时延Td的长度为：

T为用户每次重传均需等待平均传输周期，r为重传次数，q为节点需重新接入的概率；

在两个子网络需要相互通信时，主控节点会把所有成员节点的数据打包，通过网关发送到其他几个子网络中；

无人机自适应组网进行自组网任务的调度，具体如下：自组网络任务调度基于多维列表调度，任务调度按照一个任务链路中的优先级来运行，每个主控节点首先获得时隙后进行任务的执行，处于链路位置后面的成员节点根据之后分配到的时隙一次进行任务执行，链路位置后面的任务需要前面的任务完成后才能开始；

首先，选择第一个准备好的节点，通常为主控节点即优先级较高的节点，完成初始化的时间为t，连接改变的数量为d，初始化时间集合FT＝[0]，主控节点完成任务的时间f＝min(f_t)，完成一个节点的任务后需要进行资源的更新，R＝R+d_FG*H，R为初始化集群剩余资源的集合，

H为自定义配系矩阵；d_FG表示对应数量下所需完成的任务；

更新完资源后，第二步判断下面的成员节点能完成任务；

如果链路下一层的节点对应的模体需求矢量与配系矩阵的乘积大于集群剩余资源的集合，即h_new*H＞R则认定其没有收到上一层节点完成任务后的信息，则继续返回完成上一任务；

若小于剩余资源集合，则下一节点继续执行任务，并且已执行完的节点数不断累加，即now＝now+h_new，剩余资源集合不断减少，即R＝R-h_new*H，至此完成两层任务的时间为f+S_new，S_new为上两层传递时消耗掉的时间，最终完成所有任务的时间是FT与f+S_new的并集，即FT＝FT∪{f+S_new}；

通过轮询使用时隙完成所有成员节点的任务，并计算出总的通信时长；

无人机组网任务中，采用基于分布式人工势场算法防止无人机碰撞，具体如下：

每架无人机周围的势场分成3层，斥力球(r1)，引力球(r2)，临界球(r3)；当同一组中的任意两架无人机之间的距离过近时，即d≤2r₁时，无人机沿着相切的方向向相反方向移动，以减小碰撞坠机的危险性；

如果当同一组中的任意两架无人机之间的距离过远，即d≥2r₂时，他们会在两点相连的直线上向相近的方向移动，保持机间的正常距离；

如果在同一组中的任意两架无人机之间的距离2r₁≤d≤2r₂时，在这个情况中既没有斥力也没有引力，可以在此情况中维持，可以达到机间距离的平衡；

如果当无人机间的距离越发接近，需要控制他们之间的相互斥力更加明显，从而控制两架距离过近的飞机原理，现在设m为飞行区域中的一点，两架无人机之间的距离为n，v为斥力增益，则第i架无人机的斥力势场函数

为

斥力

是由斥力势场函数负梯度所得，可以表示为：

相同的，如果两架飞机距离过远，就会触发引力因子，在d≥2r₂时，引力势场会产生一个相互吸引的力，将两架飞机在向相的方向上互相靠近；如果设m为飞行区域中的一点，两架无人机之间的距离为n，w为斥力增益，则第j架无人机的引力势场函数

为

为介于引力层半径至作用层半径之间的距离；

相对的，相应的引力

等于

至此，每架飞机所受到的合力等于其受到的所有斥力加上所有引力，即

S30：单机间交换飞控、定位数据；

S40：发送消息至地面控制系统(7)并进行集群控制。

2.根据权利要求1所述的一种基于5G通信的无人机编队组网方法，其特征在于所述的IMU惯导模块(4)包括陀螺仪、加速度计和磁力计，用四元数软件结算姿态，具体步骤包括：

1)利用欧拉角描述一次平面旋转，并得到一个表示旋转的方向余弦矩阵；

2)定义四元数，根据四元数方向余弦矩阵和欧拉角的转换关系，得到最终的欧拉角值；

在一个坐标旋转α角后得到新的坐标系，在空间中的一个矢量在坐标系中的投影可以用矩阵形式表示：

得到表示旋转的方向余弦矩阵后，先定义一个四元数：

方向余弦矩阵用四元数表示则为：

所以在软件解算中，叉积是陀螺仪测量的姿态和加速计测量的姿态在向量上的叉乘，可以用向量叉积表示；叉积向量仍旧是位于机体坐标上的，而陀螺仪误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，所以先用叉积误差来做PI修正陀螺仪零偏：

exInt＝exInt+ex*Ki；

eyInt＝eyInt+ey*Ki；

ezInt＝ezInt+ez*Ki；

gx＝gx+Kp*ex+exInt；

gy＝gy+Kp*ey+eyInt；

gz＝gz+Kp*ez+ezInt；

其中Ki表示陀螺仪收敛速率的积分增益，Kp表示加速度计收敛速率的比例增益gxyz表示陀螺仪校正，四元数微分方程：

q₀＝q₀+(-q₁*gx-q₂*gy-q₃*gz)*halfT；

q₁＝q₁+(q₀*gx+q₂*gz-q₃*gy)*halfT；

q₂＝q₂+(q₀*gy-q₁*gz+q₃*gx)*halfT；

q₃＝q₃+(q₀*gz+q₁*gy-q₂*gx)*halfT；

其中halfT是测量周期的一半；

最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：

Yaw＝atan2(2*q₁*q₂+2*q₀*q₃,-2*q₂*q₂-2*q₃*q₃+1)*57.3；

pitch＝asin(-2*q₁*q₃+2*q₀*q₂)*57.3；

roll＝atan2(2*q₂*q₃+2*q₀*q₁,-2*q₁*q₁-2*q₂*q₂+1)*57.3。

3.根据权利要求1所述的一种基于5G通信的无人机编队组网方法，其特征在于所述的5G通讯模块的通讯方式为：利用5G通信的非正交传输技术，支持多个终端在同一时间共同使用无线资源，在mMTC型应用场景中，通过地面5G基站(6)对于多架无人机进行实时的，少量控制信令的信息传递。

4.根据权利要求1所述的一种基于5G通信的无人机编队组网方法，其特征在于所述的无人机飞控系统(1)将对所采集到的每帧数据进行处理，具体地，先进行CRC和校验取出有效数据再对数据进行提取使用，所用的通信协议为：

0x41,0x88,0,0xca,0xde,’W’,’A’,’V’,’E’,0x21,0,0,0,0,0,0,0,0,0,0,0,0

由以下字段组成：

字节0/1：帧控制(0x8841表示使用16位寻址的数据帧)；

字节2：序列号，每个新帧递增；

字节3/4:PAN ID(0XDECA)；

字节5/6：目的地地址；

字节7/8：源地址；

字节9：功能代码(特定值，用于指示它在测距过程中的信息)；

其余字节特定于每条消息，如下所示：

响应消息：

字节10：活动代码，用于切换无人机的飞行状态，0x00：无人机做定点起飞；0x01：无人机做队列变换；0x02，告诉发起程序继续进行位置交换；

字节11->13：活动参数，包括pitch、roll、yaw角度值、身份ID；

字节14->17：响应消息接收时间戳；

字节18->20：最终消息传输时间戳。

5.根据权利要求1所述的一种基于5G通信的无人机编队组网方法，其特征在于地面控制系统(7)内置地图界面，可完成地图尺度的缩放，用于显示无人机各项数据，并设有输入text，通过软件可以手动输入无人机需要达到的地图坐标，通过5G通信来控制集群的坐标移动；地面控制系统(7)可通过异步串口的通信方式与电脑进行搭建；地面控制系统(7)采用无人机MAC寻址的方式并通过5G通信模块(5)发布控制指令和期望航迹，与无人机的进行数据交互；地面控制系统(7)数据控制链路基于mavlink通信协议对飞行状态、姿态、位置和指令数据打包，可采用单播、广播、组播和中继的通信模式与无人机通信，同时地面控制系统(7)为每一架无人机分配一个MAC地址用于通信。

Images