CN108521670B - 面向多机紧密编队飞行基于uwb通信与定位方法及一体化系统 - Google Patents

Abstract

本发明公开了一种面向多机紧密编队飞行基于UWB通信与定位方法及一体化系统，该方法为：在“长机‑僚机”编队中，长机按照设定的航线飞行，僚机去自主“跟随”长机完成编队；在长机上安装GPS模块和UWB模块以完成定位、飞行控制模块以完成飞行控制、机间通信设备以完成机站通信和机间通信，在僚机上安装处理器实现编队控制算法；在编队的过程中，由GPS模块来引导僚机接近长机，当机间的相对距离达到UWB的有效工作距离之内，就引入UWB的数据来实现紧密编队。该系统用来实施上述方法。本发明具有原理简单、易实现、可靠性好等优点。

Description

面向多机紧密编队飞行基于UWB通信与定位方法及一体化 系统

技术领域

本发明主要涉及到无人机领域，特指一种面向多机紧密编队飞行基于UWB通信与定位方法及一体化系统。

背景技术

多无人机(UAVs)编队可以协同完成任务，提高单次作战任务的成功概率，因而引起各国对多机编队飞行的研究热潮。而UAV紧密编队具有重量轻、展弦比大、气动性能好以及结构强度高的特点，更是目前研究的热点。相比单UAV飞行，多UAV紧密编队更加复杂，需要突破一系列技术难点。其中，机间相对定位是紧密编队的关键问题。

目前无人机广泛采用GPS定位技术，使得无人机的定位精度控制在10米左右，而密集编队需要达到分米级、甚至厘米级的精度。如果进一步提高精度，就要采用差分GPS、雷达、视觉传感器等技术，这将增大无人机系统的复杂程度，并且增加成本，有悖于发展无人机集群的初衷。

由于无人机是在一定区域内紧密编队，所以可以考虑无线定位技术。常用的无线定位技术包括红外线、蓝牙、Wifi、ZigBee、RFID等，都难以满足紧密编队的需求。红外线只适合短距离传播，而且容易被光源干扰，在定位精度上有很大局限性。蓝牙和Wifi基于接收信号强度(RSSI)实现定位，均适用于小范围定位，受噪声干扰较大，定位误差不稳定。ZigBee也基于RSSI定位，受干扰影响大，定位精度较低(约3米)。而RFID技术需要布设大量基站，整体成本较高，且系统稳定性差，定位误差较大(约20米)。

超宽带(UWB)信号采用持续时间极短的窄脉冲，对时间、空间的分辨力都很强。不考虑其他误差时，其定位精度可以达到厘米级。与传统的窄带系统相比，具有穿透力强、功耗低、抗多径效果好、安全性高、系统复杂度低、能提供精确定位精度等优点。有从业者人员的研究表明，UWB在对静态目标定位时的精度甚至可以达到2厘米，而对动态目标定位的精度也可以控制在30厘米内。因此，UWB技术应用于UAV相对定位是十分可行的。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种原理简单、易实现、可靠性好的面向多机紧密编队飞行基于UWB通信与定位方法及一体化系统。

为解决上述技术问题，本发明采用以下技术方案：

一种面向多机紧密编队飞行基于UWB通信与定位方法，在“长机-僚机”编队中，长机按照设定的航线飞行，僚机去自主“跟随”长机完成编队；在长机上安装GPS模块和UWB模块以完成定位、飞行控制模块以完成飞行控制、机间通信设备以完成机站通信和机间通信，在僚机上安装处理器实现编队控制算法；在编队的过程中，由GPS模块来引导僚机接近长机，当机间的相对距离达到UWB的有效工作距离之内，就引入UWB的数据来实现紧密编队。

作为本发明方法的进一步改进：在编队控制过程中，GPS模块用来引导僚机接近长机，处理器用来检测是否有稳定的UWB定位数据。

作为本发明方法的进一步改进：当UWB模块能在数据速率850KHz的模式下稳定工作时，利用低频的UWB定位来控制UAV编队；随着距离的靠近，当UWB模块在数据速率6.8MHz的模式下稳定工作时，就利用高频的UWB定位来控制UAV编队；一旦飞机距离增大，UWB模块无法稳定定位，及时切换回GPS定位，保证能在全过程中得到长机与僚机的相对位置关系。

作为本发明方法的进一步改进：所述GPS模块根据长机和僚机的GPS数据，经处理和计算，得到机间的相对距离和方位；所述GPS模块的数据首先要经过滤波，排除粗大的位置误差并减小噪声干扰，然后把经度纬度高度信息转换到空间坐标系中，以用来求解空间位置关系。

作为本发明方法的进一步改进：在数据滤波时，结合递推平均滤波法和限幅滤波法，将输入的数据看作一个队列，按照先进先出原则使队列的长度为固定值；将队列内数据求取均值后，剔除偏离均值最大的数据点，不断重复这样的处理，直到偏离均值的最大偏差在阈值之内，实现了数据滤波。

本发明进一步公开了一种面向多机紧密编队飞行基于UWB通信与定位的一体化系统，包括GPS数据处理模块、UWB数据处理模块、数据同步/融合处理模块及控制驱动模块，组成一个闭环回路；所述GPS数据处理模块、UWB数据处理模块、数据同步/融合处理模块为用来实现相对定位的核心模块；所述GPS数据处理模块在飞行引导阶段和定位方式切换时发挥作用，所述UWB数据处理模块用来在近距离紧密编队时发挥作用。

作为本发明系统的进一步改进：在编队的过程中，由GPS数据处理模块来引导僚机接近长机，当机间的相对距离达到UWB数据处理模块的有效工作距离之内，就引入UWB数据处理模块的数据来实现紧密编队。

作为本发明系统的进一步改进：所述GPS数据处理模块用来根据长机和僚机的GPS数据，经处理和计算，得到机间的相对距离和方位；所述GPS数据处理模块的数据首先要经过滤波，排除粗大的位置误差并减小噪声干扰，然后把经度纬度高度信息转换到空间坐标系中，以用来求解空间位置关系。

与现有技术相比，本发明的优点在于：

提出将UWB技术应用于UAV相对定位，并考虑将UWB定位与通信相结合，在紧密编队中，融合UAV的状态信息，构成UWB通信与定位一体化系统，获得UAV间高精度的相对位置信息。

附图说明

图1是本发明在应用实例中的原理示意图。

图2是本发明在具体应用实例中的拓扑结构示意图。

图3是本发明在具体应用实例中的工作流程示意图。

图4是本发明在具体应用实例中编队控制进程示意图。

图5是本发明在具体应用实例中编队控制数据处理流程图.

图6是本发明在具体应用实例中上位机系统结构图。

图7是本发明在具体应用实例中GPS数据处理模块的结构示意图。

图8是本发明在具体应用实例中数据滤波流程示意图。

图9是本发明在具体应用实例中GPS数据处理流程图。

图10是本发明在具体应用实例中UWB数据处理模块的结构示意图。

图11是本发明在具体应用实例中UWB模块数据处理流程图。

图12是本发明在具体应用实例中融合处理模块结构图。

图13是本发明在具体应用实例中融合处理模块数据处理流程图。

图14是本发明在具体应用实例中建立的坐标系示意图。

图15是本发明在具体应用实例中通信数据包的结构示意图。

图16是本发明在具体应用实例中长机与僚机的示意图。

图17是本发明在具体应用实例中室内定位时X轴坐标对比Y轴坐标对比示意图。

图18是本发明在具体应用实例中室内定位时定位路径对比定位误差的对比示意图。

图19是本发明在具体应用实例中室外定位结果比较的示意图。

图20是本发明在具体应用实例中飞行试验结果的示意图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

如图1-图6所示，本发明的面向多机紧密编队飞行基于UWB通信与定位方法，在“长机-僚机”编队中，长机按照设定的航线飞行，由僚机去自主“跟随”长机完成编队；在长机上安装GPS和UWB节点以完成定位、飞行控制模块以完成飞行控制、机间通信设备以完成机站通信和机间通信，在僚机上安装处理器模块实现编队控制算法。在编队的过程中，由GPS模块来引导僚机接近长机，当机间的相对距离达到UWB的有效工作距离之内，就引入UWB的数据来实现紧密编队。

本发明将UWB技术应用于UAV相对定位，并考虑将UWB通信与定位相结合，在“长机-僚机”方式紧密编队中，获得长机UAV与僚机UAV间的相对位置、姿态关系，并通过长机UAV的位姿信息求得僚机UAV的绝对位置、姿态信息。

在多UAV紧密编队飞行时，UAV间的气动干扰将直接影响并改变在队形中不同位置上的UAV所受到的力和力矩，通过进行合理控制，将能够使这种气动干扰转换为有利于机群编队飞行的动力，即减少僚机UAV的飞行阻力，达到降低能量消耗的效果。紧密编队的实现，需要依赖于编队飞行的控制策略，目前的控制策略主要有：基于行为方式的编队控制、“长机-僚机”方式的编队控制、基于虚拟结构方式的编队控制等。在这些控制策略中，获取UAV间的相对位置、姿态信息，是非常基本和关键的一个环节。

在编队控制过程中，GPS模块用来引导僚机接近长机，处理器检测是否有稳定的UWB定位数据。当UWB模块可以在数据速率850KHz的模式下稳定工作时，就利用低频的UWB定位来控制UAV编队。随着距离的靠近，当UWB模块可以在数据速率6.8MHz的模式下稳定工作时，就利用高频的UWB定位来控制UAV编队。一旦飞机距离增大，UWB模块无法稳定定位，需要及时切换回GPS定位，保证能在全过程中得到长机与僚机的相对位置关系。

在实现编队过程中，上位机对GPS数据和UWB数据的处理、判断、融合是至关重要的，需要闭环的系统控制来实现。

本发明的面向多机紧密编队飞行基于UWB通信与定位的一体化系统，包括GPS数据处理模块、UWB数据处理模块、数据同步/融合处理模块及控制驱动模块，组成一个闭环回路。GPS数据处理模块、UWB数据处理模块、数据同步/融合处理模块是实现相对定位的核心模块。其中GPS数据处理模块主要在飞行引导阶段和定位方式切换时发挥作用，而UWB数据处理模块主要在近距离紧密编队时发挥作用。

因为UWB模块的有效工作距离有限，所以在编队的过程中，需要由GPS模块来引导僚机接近长机，当机间的相对距离达到UWB的有效工作距离之内，就可以适时引入UWB的数据来实现紧密编队。

需要说明的是，UWB不同工作模式下的性能是不同的。以数据速率为850KHz和6.8MHz两种模式为例，当数据速率较低时，其工作距离较远，但是定位精度较低；当数据速率较高时，定位精度较高，但工作距离较近。当UAV间距较远时，对机间相对定位的精度要求不是很苛刻，而UAV间的距离越近，越需要更高精度、更高频率的定位数据，保证机间距离控制在一个稳定的数值范围内。

在具体应用实例中，参见图7-图9所示，GPS数据处理模块的作用就是根据长机和僚机的GPS数据，经处理和计算，得到机间的相对距离和方位。GPS的数据首先要经过滤波，排除粗大的位置误差并减小噪声干扰，然后把经度纬度高度等信息转换到空间坐标系中，以用来求解空间位置关系。

在数据滤波时，常用的滤波方法有限幅滤波法、中值滤波法、算术平均滤波法、递推平均滤波法等等。本实施例中，结合递推平均滤波法和限幅滤波法，将输入的数据看作一个队列，按照先进先出原则使队列的长度为固定值。将队列内数据求取均值后，剔除偏离均值最大的数据点，不断重复这样的处理，直到偏离均值的最大偏差在阈值之内，实现了数据滤波。这种方法的滤波效果较好，延时也在接受的范围内，不但可以应用于GPS数据滤波，也可以应用于UWB数据滤波。

本实施例中，所选择的编队飞行算法，主要是根据长机和僚机在同一投影面上的投影点间的距离和方位角进行飞行控制，所以在这里只要求出长机与僚机的相对距离distance和水平面上的方位角angle_n即可。

长机和僚机都选取同一个空间点作为参考点，根据GPS坐标求出它们相对参考点的距离D_i和方位α_i。这样就建立起一个坐标系，选取参考点为坐标原点，X轴由坐标原点指向正北方向，Y轴由坐标原点指向正东方向。所以UAV坐标：

X_i＝D_i×cosα_i；

Y_i＝D_i×sinα_i；i＝1,2

根据坐标可以算出机间的相对位置关系：

参见图10和图11，在具体应用实例中，UWB数据处理模块跟GPS数据处理模块的作用类似，也是根据测量信息求解UAV间的相对位置关系。不过，要达到厘米级的定位精度，需要对UWB数据做更复杂的处理。一方面，要对UWB测距数据进行滤波，剔除粗大误差；另一方面，利用无人机自身的飞行状态信息，对机间的相对位置关系做预测，估计无人机位置变化，对相对定位结果做反馈验证。UWB测距数据若是与估计值较为接近，那就可以信任测距信息，否则，就认为测距数据受到较大噪声干扰，估计值更加可信。然后根据相对距离信息，建立方程组并求解。求解过程中会出现奇异解，该系统中加入高度计作为判断，排除奇异值，得到唯一的空间坐标点。根据空间坐标可以计算长机和僚机的相对距离和方位，完成相对定位。

为了便于计算分析，建立如图14的坐标系。原点取在长机质心上，X轴为长机纵轴机头正向，Y轴为右机翼正向，Z轴方向由右手螺旋定则确定，并朝机身下方方向。

假设Anchor坐标分别为P_i(x_i,y_i,z_i)，i＝1,2,3，待求的Tag节点坐标为P(x_t,y_t,z_t)，节点P_i到P点的距离为r_i，则：

(x_i-x_t)²+(y_i-y_t)²+(z_i-z_t)²＝r_i ²，i＝1,2,3

其中，z_t即长机与僚机的高度差，则方程可以改写为：

(x_i-x_t)²+(y_i-y_t)²＝r_i ²-(z_i-z_t)²，i＝1,2,3

式中，x_i、y_i、z_i(i＝1,2,3)均为已知量。利用最小二乘法，可以由方程组得到矩阵形式：

H·x＝b；

其中：

则(x_t,y_t)的最小方差解为：

为了使UWB定位的结果更好地适应编队控制算法，需要把机体坐标系中的坐标转换到机载地理坐标系中。机载地理坐标系，即通常所讲的NED(North East Down)坐标系，原点取在载机质心上，X轴为地理正北方向，E为地球自转切向(正东)，D为载机到地平面的垂线，并指向下。

若β为机体的偏航角，ε为机体的俯仰角，γ为机体的滚转角，则机体坐标系到机载地理坐标系的转换矩阵为：

所以，P点在机载地理坐标系中的坐标(x,y,z)为：

在求得P(x,y,z)后，可以求出机间的相对位置关系：

参见图12和图13所示，紧密编队时，GPS定位和UWB定位需要很好的融合，才能实现机间的精准定位和编队控制。尤其是在GPS定位与UWB定位切换的时候，需要根据当前UAV的飞行状态，判断采用哪种定位方式更适合。融合处理模块就实现了这一功能，通过同步GPS数据和UWB数据，引入UAV状态信息进行数据对比，确保在定位方式切换时，不会造成相对定位结果的突变，使无人机在编队过程中平稳飞行。

在数据融合时，首先要判断的就是UWB定位是否稳定，若是UWB模块无法正常工作，那就直接输出GPS的定位结果。当UWB模块测距结果稳定时，比较GPS定位结果和UWB定位结果，若是二者结果接近，那就认为UWB定位是准确的；若是二者相差较大，那就需要根据UAV的飞行速度、航迹角等信息进行判断，哪个定位结果更加可信。

本发明当中，采用机间信息通信机制，实现UAV间的相对定位，还需要进行机间通信，传输UAV状态数据包。建立机间通信链路，需要遵循通信协议，并构建相应的数据包。

MAVLink(Micro Air Vehicle Link)协议是在串口通讯基础上的一种更高层的开源通讯协议，是目前UAV与地面站以及UAV之间最常用的通信协议。本实施例中同样采用MAVlink协议，但为确保UAV间的通信链路畅通，减少机间通信的失真和迟滞，需要简化通信数据包。通信数据包结构如图15所示。

利用只有47字节的数据包，将长机的位置、姿态、运动状态等信息发送给僚机，占用信道容量较小，可以适应高频通信的需求。

本发明在一个具体应用实例中，为实现基于UWB的通信与定位一体化系统，硬件平台选用DWM1000作为定位模块。该模块基于DecaWave公司开发的DW1000芯片，是兼容IEEE802.14.4-2011协议的超宽带无线收发模块。

参见图16，将三个DWM1000定位模块固定在长机上，作为Anchor节点；一个Tag节点固定在僚机上，与长机的三个Anchor节点分别进行测距，根据Tag节点与三个Anchor节点的距离，解算出Tag节点的相对位置，即僚机相对长机的位置。

(一)室内测试；

室内测试实验的目的有两方面，一是测试UWB模块的测距性能，包括零值偏差、有效测距精度等；二是检验定位算法的运算结果是否正确，定位的精度是否满足定位需求。

本发明选择VICON系统为定位结果提供参照。VICON是英国OML公司生产的光学动作捕捉系统，它的定位精度可以达到毫米级。在VICON系统中，利用maker球测出每个UWB模块的坐标；将Anchor节点建立的坐标系与VICON坐标系重合，用UWB模块进行定位，求得Tag节点的坐标，并与VICON测定的坐标进行比较，其结果如图17和图18所示。

从比较的结果可以看出，在测距误差和噪声干扰的情况下，UWB定位的精度约为20cm，但在发生转弯等DOP值较差的情况下，定位精度可能增大到约40cm。总体而言，此定位精度可以满足UAV定位的要求。

(二)室外测试；

在完成室内测试的基础上，进行室外测试的目的有两个。一是确定UWB模块的有效工作距离，二是与GPS定位结果进行比较，验证UWB定位是否可行。

测试过程中，先将无人机静置一段时间，然后改变无人机位置。这样可以便于观察UWB与GPS的定位结果，哪个更符合实际情况，二者的定位性能有何差异。期间可以不断增大机间距离，直到UWB模块无法工作，确定UWB的有效工作距离。综合考虑UWB定位的定位精度和频率性能，选择数据速率为850KHz的工作模式进行试验。

参见图19，可以看出，UWB模块的有效工作距离约为87.9m，可以满足多UAV紧密编队的设计目标。在静止状态下，UWB定位精度明显高于GPS；在移动过程中，GPS和UWB的定位结果相差不大，虽暂无法确定二者间的精度性能，但可以确认，UWB完全可以满足机间定位的要求。

(三)飞行试验；

在完成测试实验的基础上，认为UWB定位可以应用于无人机紧密编队。把UWB模块安装在无人机上，测距数据和无人机状态信息经串口发送至处理器，实现GPS定位和UWB定位的融合。经过仿真实验和半实物仿真确认系统工作正常后，开展飞行试验，试验的结果如图20所示。

飞行实验过程有准备阶段、起飞阶段、引导阶段、编队阶段、降落阶段。在准备阶段，UWB模块正常，定位结果与GPS定位结果相近；在起飞阶段，无人机之间的距离迅速增大，当机间距离超过UWB有效工作范围，仅由GPS对无人机进行定位；在引导阶段，两机之间的距离逐渐接近，UWB模块恢复定位；在编队阶段，根据UWB定位结果进行编队控制；在降落阶段，由于飞机之间的高度差很大，所以UWB模块几乎不能测距，而当两机都降落后，UWB又恢复定位。

虽然定位过程中UWB模块和GPS模块都有定位误差，但是实验结果足以证明，UWB技术应用于UAV定位是完全可行的。

结果分析：从实验结果可以看出，本发明的UWB通信与定位一体化系统，可以完成无人机紧密编队中的相对定位，并且相比于GPS等定位手段，具有精度高、成本低等优点。在实际飞行中，UWB的有效工作距离为约80m，定位精度可以达到分米级。

综上所述，本发明面向多UAV紧密编队，提出将UWB技术应用于UAV相对定位，并考虑将UWB定位与通信相结合，在紧密编队中，融合UAV的状态信息，构成UWB通信与定位一体化系统，获得UAV间高精度的相对位置信息。经过试验验证和实际飞行，该系统可以满足紧密编队的需求。本次试验采用“长机-僚机”编队方式，用两架无人机来完成的验证，而不同Tag节点与Anchor节点的定位数据很容易识别，本系统可以推广应用到多UAV紧密编队的情形。若研究更加优化的定位算法、更加准确的滤波技术、更加智能的控制策略，该系统将更加完善。

本发明的UWB通信与定位一体化系统，是将UWB技术应用于固定翼无人机定位的一次探索和验证，是多UAV紧密编队定位的一次创新。并且，该系统实现了GPS定位技术与相对定位技术的融合，既可以满足广域条件下的无人机定位，又提高了局域内无人机定位的精度，使得多UAV紧密编队更加稳定。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (6)

1.一种面向多机紧密编队飞行基于UWB通信与定位方法，其特征在于，在“长机-僚机”编队中，长机按照设定的航线飞行，僚机去自主“跟随”长机完成编队；在长机上安装GPS模块和UWB模块以完成定位、飞行控制模块以完成飞行控制、机间通信设备以完成机站通信和机间通信，在僚机上安装处理器实现编队控制算法；在编队的过程中，GPS模块用来引导僚机接近长机，处理器用来检测是否有稳定的UWB定位数据, 当UWB模块能在数据速率850KHz的模式下稳定工作时，利用低频的UWB定位来控制UAV编队；随着距离的靠近，当UWB模块在数据速率6.8MHz的模式下稳定工作时，就利用高频的UWB定位来控制UAV编队；一旦飞机距离增大，UWB模块无法稳定定位，切换回GPS定位，保证能在全过程中得到长机与僚机的相对位置关系。

2.根据权利要求1所述的面向多机紧密编队飞行基于UWB通信与定位方法，其特征在于，所述GPS模块根据长机和僚机的GPS数据，经处理和计算，得到机间的相对距离和方位；所述GPS模块的数据首先要经过滤波，以排除粗大的位置误差并减小噪声干扰，然后把经度纬度高度信息转换到空间坐标系中，以用来求解空间位置关系。

3.根据权利要求2所述的面向多机紧密编队飞行基于UWB通信与定位方法，其特征在于，在数据滤波时，结合递推平均滤波法和限幅滤波法，将输入的数据看作一个队列，按照先进先出原则使队列的长度为固定值；将队列内数据求取均值后，剔除偏离均值最大的数据点，不断重复这样的处理，直到偏离均值的最大偏差在阈值之内，实现了数据滤波。

4.一种面向多机紧密编队飞行基于UWB通信与定位的一体化系统，其特征在于，包括GPS数据处理模块、UWB数据处理模块、数据同步/融合处理模块及控制驱动模块，组成一个闭环回路；所述GPS数据处理模块、UWB数据处理模块、数据同步/融合处理模块为用来实现相对定位的核心模块；所述GPS数据处理模块用来根据长机和僚机的GPS数据，经处理和计算，得到机间的相对距离和方位；所述UWB数据处理模块用来根据测量信息求解UAV间的相对位置关系。

5.根据权利要求4所述的面向多机紧密编队飞行基于UWB通信与定位的一体化系统，其特征在于，在编队的过程中，由GPS数据处理模块来引导僚机接近长机，当机间的相对距离达到UWB数据处理模块的有效工作距离之内，就引入UWB数据处理模块的数据来实现紧密编队。

6.根据权利要求5所述的面向多机紧密编队飞行基于UWB通信与定位的一体化系统，其特征在于，所述GPS数据处理模块的数据首先要经过滤波，以排除粗大的位置误差并减小噪声干扰，然后把经度纬度高度信息转换到空间坐标系中，以用来求解空间位置关系。

Images