超宽带测距辅助的无人机近距离相对导航方法

熊 骏，熊 智，于永军，许建新，王云舒

（1. 南京航空航天大学 自动化学院，南京 210016；2. 南京理工大学 机械工程学院，南京 210094）

目前，无人机技术受到军事、民用等各个应用领域的关注，其中无人机相对导航技术逐渐成为一个研究热点。在无人机编队飞行或联合任务执行等应用场景下，为保证准确的协同任务执行效果，精确的相对导航信息是必不可少的信息源。

相对导航的测量传感器主要包括激光雷达（LiDAR）、卫星导航、视觉测量、超宽带（UWB）、结构光等[1-6]，其中基于GPS/INS的相对导航方法具有成本低、数据输出频率高、系统较为可靠等优势，得到了广泛的应用和研究。然而由于卫星导航容易受到干扰和遮蔽等特殊情况的影响，当可用卫星数目较少时，仅依赖 GPS/INS的相对导航系统性能会受到一定程度的削弱[7-9]。

为了获得更好的相对导航效果，国内外学者进行了针对性的研究，其中一种主流方案是采用相对测量手段对相对导航进行组合与提升。Wang等人采用了VisNav/INS/DGPS的组合方案，通过分布式滤波架构进行信息融合，其仿真结果精度高于仅采用 VisNav/INS或INS/DGPS的相对导航方案[10]。Daniel B. Wilson等人采用了红外摄像头和主动式红外LED标记的组合，对仅依赖GPS/INS的编队飞行进行了辅助，实验结果显示其相对位置估计的均方根误差（RMSE）在水平方向为1.2m，垂直方向为0.44m[11]。Jason N. Gross等人采用了DGPS/INS/UWB组合进行相对导航，当差分GPS效果较差时，使用UWB相对量测辅助整周模糊度的修复，仿真结果表明UWB辅助下的相对导航系统具有更高的精度和稳定性[12]，然而没有引入相对速度估计，且没有针对UWB的实际使用情况进行模型优化。

本文给出了一种超宽带测距辅助的无人机近距离相对导航方案，建立了无人机之间双差、双差变化率、UWB测距以及基于双机紧组合定位信息做差的量测方程，并且设计了相对导航状态方程，最后针对不同的相对导航配置和卫星可用情况进行了仿真和分析。

1 无人机相对导航方案

进行相对导航的无人机均配备惯性测量单元、卫星导航接收机、UWB传感器以实现高精度相对定位。本方案中，为获得更好的单机定位效果，每架无人机均采用了基于伪距和伪距率的GPS/INS紧组合导航系统。飞行过程中，机载UWB进行无人机之间的相对测量，获取相对距离和速度信息。同时，机载惯性导航测量数据以及卫星导航接收机数据通过数据链传输到每架无人机的机载计算机，进而完成相对导航信息融合，其相对导航方案如图1所示。

图1 无人机近距离相对导航方案Fig.1 Schematic of close relative navigation

2 超宽带测距辅助的相对导航方法

2.1 相对导航量测方程

2.1.1 相对差分量测方程

以两架无人机和两个卫星的情况为例，无人机A的卫星接收机与卫星S1之间的伪距可表示为[13]：

图2 无人机相对差分原理Fig.2 Schematic of relative differential pseudorange

考虑到无人机是近距离伴飞，基线长度相对卫星高度很小，卫星S1到无人机A和无人机B的方向余弦矢量差值很小，则有：

同理，无人机A和无人机B的卫星接收机与卫星S2之间的站际单差可以表示为：

同时，为了满足相对速度估计，引入双差变化率：

2.1.2 UWB量测方程

为了减少不同无人机搭载的UWB的晶振偏差，本文采用了对称双边双向测距（SDS-TWR，Symmetric Double-Sided Two-Way Ranging）算法[14]，可以有效减少由于不同节点晶振偏差大导致的测量误差，其测距原理如图3所示。

图3 对称双边双向测距示意图Fig.3 Schematic of symmetric double-sided two-way ranging

在SDS-TWR中，B接收到A的响应信号后，等待一段处理时间再返回另一个测距信息。A节点接收信号后记录信号接收时间，得到双向测距结果。

在该模式下，测量得到的信号传输时间（TOF，Time of Flight）为：

由于空中障碍物较少，且无人机体型不大，因此可以忽略 UWB的非视距误差（NLOS，Non-line of Sight）。同时，考虑到不同节点晶振差异引起的时钟差异，UWB的测量结果可以表示为：

在本方案内，进行相对导航的无人机均搭载UWB传感器，用于实现无人机之间相对位置和速度进行实时测量，其量测方程分别为：

2.1.3 双机定位数据做差量测模型

每架无人机均采用 GPS/INS紧组合进行绝对定位，根据无人机自身输出的导航信息，对两架无人机A、B紧组合输出的位置和速度进行做差，可得到基于双机定位数据做差的相对位置和相对速度量测方程：

2.2 相对导航状态方程

超宽带测距辅助的相对导航方法采用扩展卡尔曼滤波器（EKF）作为信息融合手段，滤波器采用两种量测来增强无人机之间的相对差分定位，一是所有无人机的绝对定位数据做差结果二是UWB对基线长度的测量结果

构造状态方程如式(17)所示：

2.3 基于UWB/相对差分/双机定位数据做差的观测方程

由于UWB的相对测量信息为非线性形式，因此对于EKF滤波器构建如下观测方程：

关于UWB测量信息，以初始状态为起始点进行泰勒展开得到：

因此，基于2.1节推导的观测模型和上述线性化过程可构建如式(21)(22)所示的观测向量与观测噪声向量：

根据相对差分(式(7)(8))、UWB 测量(式(12)(13))、双机定位数据做差(式(14)(15))的模型，可定义如式(23)所示的观测矩阵：

构建量测方程(23)后，结合状态方程(17)，采用EKF算法解算状态变量，在此不赘述。

3 仿真分析

3.1 仿真初始条件

无人机A按照如图4所示的轨迹飞行，无人机B对A进行跟随飞行，单机均采用GPS/INS紧组合定位，飞行过程中进行实时相对导航，飞行时长1 h。

表1列出了无人机所搭载的设备仿真参数。两架无人机均配备惯性测量单元、卫星导航接收机、UWB传感器，伪距、伪距率、UWB测量噪声均考虑为高斯噪声。

图4 无人机A飞行轨迹Fig.4 Trajectory of UAV A

表1 传感器配置与仿真参数设置Tab.1 Sensor configuration and simulation parameters

3.2 仿真结果及分析

图5以地心地固系（ECEF，Earth-Centered，Earth-Fixed）下X、Y、Z三个方向的相对距离作为对比项，对比了飞行过程中的相对距离真值和本文方法解算得到的相对距离，可用卫星数为 8。具体的相对定位精度如表2的UWB/相对差分部分所示。

图5 解算值和真值相对轨迹对比Fig.5 Comparison of relative trajectories

为考察不同传感器配置情况下的相对导航性能，图6～8对比了三种配置下的相对导航结果，设定可用导航卫星数目为8：

1） UWB+相对差分：本文提出的方法，采用UWB、相对差分和双机定位数据做差值作为量测量；

2）相对差分：传统相对差分技术，仅采用伪距双差数据做相对状态估计；

3）UWB：采用本文算法框架，仅使用 UWB和双机定位数据做差值作为量测量，无相对差分数据。

以X、Y、Z（ECEF系）三个方向的相对导航均方根误差值（RMSE）作为对比项，UWB+相对差分的相对导航精度最高，其鲁棒性最佳；相对差分的相对导航精度次之；仅采用UWB作为额外量测信息的相对导航性能最差。具体对比数据如表2所示。

表2 相对导航结果对比Tab.2 Comparison of relative navigation

图7 Y方向相对导航误差对比Fig.7 Comparison of relative navigation errors in Y direction

图8 Z方向相对导航误差对比Fig.8 Comparison of relative navigation errors in Z direction

为考察可用卫星数较少时，本文算法相对于传统相对差分技术的提升效果，图9～11对比了两种方法在较少可用星情况下的相对导航结果。当可见星仅有 4颗时，UWB辅助的相对导航精度明显高于传统相对差分技术，在X、Y、Z三个方向（ECEF系）的相对导航精度分别提升了3.2、13.062、2.01倍。具体数据如表3所示。

表3 相对导航结果对比（4可见星）Tab.3 Comparison of relative navigation

图9 X方向相对导航误差对比（4可见星）Fig.9 Comparison of relative navigation errors in X direction (4 visible satellites)

图10 Y方向相对导航误差对比（4可见星）Fig.10 Comparison of relative navigation errors in Y direction (4 visible satellites)

图11 Z方向相对导航误差对比（4可见星）Fig.11 Comparison of relative navigation errors in Z direction (4 visible satellites)

4 结 论

本文以无人机之间的相对导航为背景，研究了一种UWB辅助的近距离相对导航方案，给出了无人机之间双差、双差变化率、UWB、双机定位信息做差的量测方程，设计了相对导航状态方程，并针对不同的卫星可用情况和不同的组合配置情况进行了仿真。仿真结果表明，该方案可以有效提升仅依赖相对差分的相对导航效果，特别是在可见卫星数量较少时，相对导航精度提升明显，适用于编队飞行等对无人机相对导航精度和鲁棒性有较高要求的应用场景。