惯性信息辅助的源信息不同步的数据链相对定位方法

严 飞，傅金琳，张崇猛，庞志超

（1.海装北京局驻天津地区某军事代表室，天津 300131；2.中国船舶航海保障技术实验室，天津 300131；3.天津航海仪器研究所，天津 300131）

无人机协同完成任务能够缩短任务执行时间，提升任务完成质量，在军民领域都得到广泛应用[1-3]。而无人机协同完成任务的一个前提为无人机能够感知其在编队中的相对位置矢量。由于无人机的体积限制，难以装配高精度的惯性导航设备，一般采用卫星导航来提供导航信息。然而，卫星导航容易受到电磁环境干扰，使得无人机定位能力下降，甚至中断，影响无人机编队的执行任务能力[4,5]。为了解决此问题，有学者提出了视觉相对定位技术[6,7]及超宽带测距技术[8],但是当无人机间距离较远时，这些技术难以满足应用需求。因此学者们提出了数据链相对定位技术。数据链相比卫星导航信号强度大大提升，且抗干扰能力大幅度提升，成为未来无人机协同导航的重要手段之一。学者对数据链相对定位技术开展了相关研究。文献[9]对数据链用于智能导弹协同定位进行了分析，文献[10]分析了数据链相对导航性能，并进行了仿真验证；文献[11]对惯导/数据链动态相对定位进行了研究。上述文献的研究中都假设待相对定位无人机接收到不同源的相对测距信息是同时发出的，然而实际的数据链系统是一个时分系统，即给编队成员分配特定的时隙，成员只能在指定时隙发送信息，也就是说待相对定位的无人机接收到的不同源的相对测距信息是不同步的。本文针对这个问题，提出了一种惯性信息辅助的源信息不同步的数据链相对定位方法。

1 数据链工作模式

数据链通信使用时分多址工作方式，它将时间轴划分为一个个定长的时元，每个时元划分为等长的时帧，每个时帧再划分为等长的时隙。每个数据链成员在系统规定的标准时隙内发送有关自己的位置坐标、时间质量等级等广播信息（PPLI）用于全网信息资源共享，在不发送信号的其他时隙，则接收来自网中其他成员PPLI 消息。数据链网络中一个台站负责控制网的工作，该台站为网内所有用户规定公共系统时隙并确定时隙的使用顺序。网络中的基准成员在每个时隙内发送时间校准报文，其余成员首先随机选择一个时间间隔，在这个时间间隔内扫描该校准报文，通过时间校准报文实现粗时间同步，借助发送往返计时报文实现时钟精同步。在完成系统内时钟同步后，需进行相对定位的成员通过测量PPLI 消息的到达时间，即可推算出其与发送该PPLI 消息成员之间的距离。由于PPLI 消息是分时发送的，所有需进行相对定位成员所测量的与不同成员之间的距离也是不同时的。

2 数据链相对定位原理

在新加入成员完成精同步后，通过三个及以上源成员精确的位置坐标可实现新加入成员的精确定位。新加入成员根据对三个源成员的PPLI 消息的到达时间（TOA）测量得到的数据和这些报文中所包含的发送源成员位置来计算自己的三维位置数据，即通过PPLI 报文的到达时间可以推算出新加入成员与源成员之间的距离，组成距离方程组来计算出新加入成员的坐标。已知三个源成员的坐标分别为，新加入成员的坐标为（xu,yu,zu），建立如下方程组，

式中，c为无线电传播的速度；t1、t2、t3为通过PPLI 报文计算的信息传输时间。

解上述方程组即可得到新加入成员的坐标位置。上述假定三个源PPLI 消息是同时发出的，如果是分时发出，需要对收集的PPLI 信息进行同步处理。

3 基于卡尔曼滤波的相对定位方案

采用卡尔曼滤波算法来实现源信息不同步数据链相对定位。

3.1 状态模型

采用新加入成员的三维位置误差δx、δy、δz，三维速度误差δvx、δvy、δvz作为状态量，则状态量X表示为

从而可得相对定位的状态模型为:

式中，

nk为状态噪声；T为卡尔曼滤波周期。

3.2 量测模型

数据链相对定位中新加入成员与源成员之间的相对测距模型为:

式中，ρc为新加入成员到源成员的几何距离；xs、ys、zs表示源成员的三维坐标；ρ0表示新加入成员到源成员的伪距；TTOA为新加入成员观测到的相对于其本身的到达时间；bs为源成员的残余时间偏差；b为新加入成员的时间偏差；n表示测量噪声和。

由于源成员的时间偏差在发送PPLI 消息时已经非常小，因此残余的时间偏差bs非常小，并且这部分时间偏差难以估算出来，因此将此部分时间偏差合并到测量噪声中。由于相对定位之前，先完成时间同步，因此新加入成员的实际偏差在此也不考虑，从而将相对测距模型更新为:

令

则有

通过数据链相对测距获得相对第j个源成员的几何距离重写为:

进一步化简为

从而可得相对定位的量测模型为:

由于到达信息时间不同步，量测矩阵H计算需要考虑时间的不同步性。在数据链中，假设以Tcs 为周期进行时隙分配，即每个用户PPLI 消息的发送周期为Tcs。假定选择三个已实现导航的成员作为源，以第三个源成员的到达时间作为滤波时间，第一、第二个源成员比第三个源成员PPLI 消息到达时间分别早T1和T2。 假设第k时刻，新加入成员的坐标为xuk、yuk、zuk，速度为vxk、vyk、vzk，则

从而得到观测矩阵Hk为

3.3 惯性信息辅助相对定位方案

虽然对新加入成员的速度进行了估计，由于估计是对运动状态的预测，与新加入成员的实际运动存在差距，因此通过引入惯导速度辅助，在不同源信息进行同步处理时，采用惯导速度进行递推，能够有效改善数据链相对定位精度。

惯导信息辅助数据链相对定位与无辅助的相对定位不同在于观测矩阵H的计算。在惯导信息辅助数据链相对定位方案中，观测矩阵的计算公式为:

式中，vIx、vIy、vIz为惯导给出了三维速度；Tj为第j个源节点PPLI 消息到达时间；TN为最后一个源节点N PPLI 消息到达时间。

3.4 基于卡尔曼滤波源信息不同步数据链相对定位算法

基于卡尔曼滤波的源信息不同步数据链相对定位算法是根据前一时刻对状态的估计值和当前时刻的观测值来获得当前时刻的滤波值，因此，整个环路滤波过程可以分为状态预测和状态更新两部分。

1）预测

一步状态预测均方误差Pk,k-1:

式中，Qe为系统噪声方差矩阵。

2）更新

滤波增益Kk:

状态估计方程:

状态估计均方差:

4 源信息不同步数据链相对定位仿真分析

采用本文提出的源信息不同步数据链相对定位方法和没有进行同步处理方法，分别就新加入成员静止和匀速运动两种状态进行仿真分析。假设选取3 个已实现导航用户作为源节点，三个源节点匀速运动。测距周期为3 s，每秒新加入成员与三个源节点之一完成一次测距。

1）静态场景仿真分析

静态场景中，三个源节点匀速运动，新加入节点静止。图1为采用本文提出的信息不同步数据相对定位算法得到的新加入节点计算三维位置与实际位置的三维均方差。图2为未对三个源节点信息同步处理，直接计算的新加入节点三维位置与实际位置的三维方差。

图1 静态场景，信息同步处理后定位误差Fig.1 Static scene,positioning error after information synchronization

图2 静态场景，信息未同步定位误差Fig.2 Static scene,positioning error before information synchronization

从图1可知，采用本文所提出方法，在100 次滤波周期后，新加入节点三维位置均方差小于10-9m。从图2可知，未做同步处理方法，在700 次滤波周期后，新加入节点三维位置方差依然大于600 m。可见，本文所提出的信息不同步数据链相对定位方法能够有效实现数据链相对定位。

2）匀速运动场景仿真分析

匀速运动场景中，三个源节点匀速运动，新加入节点匀速。图3为采用所提出的信息不同步数据相对定位算法得到的新加入节点计算三维位置与实际位置的三维均方差。图4为未对三个源节点信息同步处理，直接计算的新加入节点三维位置与实际位置的三维均方差。

图3 匀速场景，信息同步处理后定位误差Fig.3 Uniform scene,positioning error after information synchronization

图4 匀速场景，信息未同步定位误差Fig.4 Uniform scene,positioning error before information synchronization

从图3可知，采用提出方法，在100次滤波周期后，新加入节点三维位置均方差小于10-8m。从图4可知，未做同步处理方法，在700 次滤波周期后，新加入节点三维位置方差依然大于800 m。可见，本文提出的信息不同步数据链相对定位方法对运动载体同样有效。

3）有误差无辅助仿真分析

上述条件为源位置无误差，相对测距无误差的理想情况，这里在仿真分析存在误差情况下，提出算法的性能。仿真中，假设三个源节点的三维位置误差为10 m，相对测距误差为30 m。

图5 有误差情况，相对定位三维误差Fig.5 Relative positioning 3D error in case of error

从图5可知，加入上述误差后，提出算法得到的数据链相对定位三维误差最大近160 m，最小在10 m左右，统计得到的三维位置误差为54.2 m，满足多数情况下无人机集群导航信息保障需求。

4）有误差惯导信息辅助仿真分析

仿真中，源节点位置误差和测距误差与无辅助相同，惯导速度误差为0.5 m/s。图6为惯导速度辅助数据相对定位得到的新加入节点的三维位置误差。

图6 惯导信息辅助相对定位三维误差Fig.6 INS information aided relative positioning 3D error

从图6可知，引入惯导速度辅助后，数据链相对定位三维误差最大在30 m 左右，最小不足1 m，统计得到的三维位置均方误差为9.8 m。可见，采用惯导速度辅助能够有效改善数据链相对定位精度。

5 结 论

无人机集群完成任务成为发展趋势。本文针对无人机集群中导航定位信息需求保障，以及现有导航手段存在的不足，提出了采用数据链相对定位方式。并针对数据链的时分多址特征，提出了一种惯性信息辅助的源信息不同步的数据链相对定位算法，推导了该算法的数学模型，分别在匀速运动场景下，对比分析了提出算法和未做同步处理算法、未辅助算法的定位精度，表明所提出的相对定位方法能够有效实现信息不同步的数据链相对定位，为无人机集群提供保精度、可靠的导航信息。