基于5G信号异步时钟误差补偿的多无人机协同定位方法

黄 郑, 王红星, 郇希岳, 张 欣

(1.江苏方天电力技术有限公司,南京 211000; 2.南京航空航天大学,南京 211000)

0 引言

近年来,随着无人机技术的快速发展,其在军事领域也得到了广泛应用,如远程侦察[1]、战场测绘[2]、边境巡逻[3]等。考虑到单架无人机载荷小、抗毁伤能力差、执行复杂任务成功率低的缺点,多无人机协作分工可显著提高战场复杂环境下任务执行的效率和成功率[4-5]。导航信息作为无人机飞行控制系统的输入,是多无人机协同完成任务的前提[6]。在卫星信号可用的条件下,惯性导航系统(INS)与全球卫星导航系统(GNSS)的组合广泛应用于无人机编队,可以为多无人机提供高精度的定位服务[7-8]。

然而,在战场复杂电磁环境下,卫星接收机易受外界干扰或故障。当卫星接收机发生故障时,由于惯导系统的误差会随时间累积,因此无法满足长时间的精确导航需求[9]。相对测距/测角信息作为一种可靠的外源信息,能够在卫星受扰或不可用时对多无人机的协同定位进行辅助,因此得到了广泛关注。传统的测距/测角信息一般是由数据链系统提供的,采用到达时间(TOA)/到达角(AOA)方式可获得无人机之间的相对信息[10]。但是,数据链系统普遍存在着体积大以及数据传输速率低的缺点[11],难以实现高精度的协同定位。相比数据链系统,5G信号是一种具有超高速率、超低时延以及超高密度覆盖优势的无线电通讯技术[12],能够提供连续、精确的相对测距/测角信息。目前,5G信号已在无人机的远程控制中得到了有效应用,而在多无人机协同导航领域却鲜有应用。

针对基于测距/测角信息的协同定位方法,研究者们已开展了一些工作。文献[13]提出了一种基于相对测距模式的无人机编队协同定位方法,利用几何图形变换估计各节点的惯导系统定位误差,能够有效提升编队的定位精度;文献[14]提出了一种基于自适应卡尔曼滤波的无人机编队协同定位方法,在无人机时钟节点同步的基础上,首先通过GNSS获取3架主机的位姿信息,并利用从机与主机之间的相对距离信息进行量测,可有效提高从机的定位精度;文献[15]提出了一种基于相对测距/测角信息辅助的双机协同定位方法,利用主机与目标间的相对距离、角度作为量测信息,通过最小二乘牛顿迭代法可以实现对目标的高精度定位。然而,上述方法均假设无人机节点间的时钟完全同步,而没有考虑机间异步时钟误差对定位精度的影响。

为此,本文结合多无人机在卫星接收机不可用时的高精度协同定位需求,提出了一种基于5G信号异步时钟误差补偿的多无人机协同定位方法;构建了基于5G信号异步时钟误差的相对测距误差模型;在此基础上,利用无人机间的相对测距/测角信息以及气压高度计辅助导航信息,构建了多无人机协同定位位置观测方程;最后,通过设计迭代扩展卡尔曼滤波器实现对从机惯导系统误差以及异步时钟误差的联合最优估计,提高了卫星接收机故障条件下从机的绝对定位精度。

1 基于5G信号异步时钟误差的相对测距模型

协同飞行时,无人机间的飞行距离相对较短,因此可采用点波束等方式提高数据收发效率。5G相对测距信号具有较强的抗干扰能力,能够在复杂环境下为多无人机协同定位提供可靠的观测信息。目前,基于无线电的测距方法众多,按其工作原理可分为脉冲测距、相位测距和频率测距[16]。其中,载波相位测距是一种常用的测距方法[17],其测距原理为

ρ=(k+φ/2π)c/f

(1)

式中:k为整周模糊度;φ为相位延迟;f为载波频率;c为无线电传播速度。

式(1)的误差形式可以表示为

(2)

式(2)表明5G信号测距误差主要受相位测量误差Δφ、整周模糊度误差Δk、测距信号传播速度误差Δc、振荡频率误差Δf因素影响。结合文献[17-21]的分析可知,测距误差主要受相位误差影响。由于相位延迟与时钟误差具有相关性,因此在进行相位延迟测量时,无人机节点间5G终端接收机的时钟不一致会导致异步时钟误差。式(2)可以表示为

Δρ=cΔts+Δem+εφ

(3)

式中:cΔts为无人机节点间5G通讯异步时钟所引起的误差;Δem为多路径误差;εφ为零值随机噪声。上述误差中的多路径误差与无人机执行任务环境有关,本文将其与随机误差综合考虑为高斯类型白噪声ε0,可表示为

Δρ=cΔts+ε0。

(4)

2 基于异步时钟误差补偿的多无人机协同定位算法设计

本文针对“一主多从”无人机构型的协同定位方法开展研究,其中主机配备高精度导航系统,并通过5G信号与从机进行相对测距、测角量测,如图1所示。

图1 协同定位场景

由于各从机配置的导航传感器相同,因此本章以“一主一从”2架无人机为例,对提出的基于5G信号异步时钟误差补偿的多无人机协同定位方法进行设计。

本文在选择系统状态量时,除了包含从机惯导系统误差以外,还将主机和从机节点之间的异步时钟误差作为系统状态量进行联合最优估计。为此,迭代扩展卡尔曼滤波器的系统状态量为

(5)

基于从机惯导系统误差微分方程构建其状态递推模型,可以表示为

Xk+1=Φk+1,kXk+ΓkWk

(6)

式中:Xk为系统k时刻的状态矢量;Φk+1,k为系统从k时刻到k+1时刻的状态转移矩阵;Γk为系统噪声矩阵;Wk为系统白噪声随机误差矩阵[22]。

通过机载5G通讯系统,从机可实时获得与主机之间的三维相对测距/测角信息。主从机节点的相对测距、测角空间几何关系如图2所示。

图2 主从机节点相对测距、测角模型

图2中:(xM,yM,zM)为主机节点在地心地固(Earth-Centered,Earth-Fixed,ECEF)坐标系下的三维位置;(xF,yF,zF)为从机节点在ECEF坐标下的三维位置;r为主从无人机节点间的三维距离;dx,dy,dz分别为r在x轴、y轴、z轴上的投影;相对方向角α为r在xoy平面上的投影与x轴正向的夹角;相对仰角β为r与自身在xoy平面上投影的夹角。

由图2可知,机载5G通讯系统在t时刻获得的从机与主机之间的三维相对距离、方位角、仰角信息可表示为

(7)

根据主机节点与从机节点的三维空间结构关系,从机节点位置可表示为

(8)

(9)

(10)

因此,基于迭代扩展卡尔曼滤波器架构,系统量测方程可表示为

(11)

3 仿真验证与分析

为了验证算法的可行性,本文以“一主二从”方式开展了协同定位算法的仿真分析。仿真过程中以1架无人机作为主机节点,飞行在卫星信号可用区域,搭载惯导系统、卫星接收机及5G通讯装置,通过INS/GNSS松组合得到位置信息;另外2架为因卫星接收机发生故障需要定位的从机节点,搭载低精度惯导系统及5G通讯装置。在协同定位过程中,主机节点通过5G通讯系统实时将自身的导航信息传输给从机节点。

通过设计航迹发生器分别模拟实际环境下无人机加速、爬升、转向等运动,无人机的仿真初始条件如表1所示,主从机的三维仿真轨迹如图3所示。

表1 仿真初始条件

图3 主从机三维仿真航迹

利用航迹发生器的数据分别模拟陀螺仪、加速度计、GNSS、相对测距/测角信息。其中,陀螺仪、加速度计的采样频率设置为50 Hz,测距/测角和高度计的采样频率设置为 1 Hz。仿真的传感器性能参数设置如表2所示。假设主从机之间的异步时钟误差为20 ns,IEKF迭代次数为100,仿真总时长为700 s。

表2 传感器性能参数设置

为了充分对比算法的性能,本文开展100次蒙特卡罗仿真实验。采用从机位置解算值与真值误差的均方根误差(RMSE)衡量定位精度。从机定位精度算式为

(12)

本文将未考虑5G信号异步时钟误差的传统方法与对异步时钟误差进行补偿的本文方法在定位误差、定位均方根误差以及水平方向总体定位RMSE和方差等方面分别进行对比。传统方法与本文方法的定位误差对比如图4所示,传统方法与本文方法的总体定位均方根误差如表3所示。

表3 传统方法与本文方法的定位均方根误差对比

图4 传统方法与本文方法的定位误差对比

传统方法与本文方法的水平定位RMSE均值和方差对比如图5所示。按照仿真设置的20 ns主从机异步时钟误差计算,对应导致的距离误差应为6 m。从机一的状态量第19维如图6所示,根据系统状态量设置,系统状态量第19维估计了由异步时钟误差导致的相对测距误差。

图5 传统方法和本文方法水平定位RMSE均值和方差对比

图6 对异步时钟误差造成的相对测距误差估计效果

本文方法对异步时钟误差造成的相对测距误差估计最终收敛到6 m附近,符合本文仿真验证中设置的20 ns的异步时钟误差,说明本文方法对异步时钟误差造成的相对测距误差的估计达到了较好的效果。

上述仿真结果表明:

1) 本文方法能够较好地估计出主从机节点间的异步时钟误差,提高测距精度,从而提高从机定位精度,在仿真条件下,本文方法相比未考虑异步时钟误差的传统方法可以将从机一水平定位精度提升21.33%从机二水平定位精度提升29.25%,从机水平定位精度平均提升25.29%;

2) 由于本文方法对主从机节点间的异步时钟误差导致的测距误差进行了补偿,提高了测距观测对噪声的约束效果,降低了从机定位结果的方差,提高了从机定位的稳定性。

4 结论

本文针对数据链相对测距/测角模式,提出一种基于5G信号异步时钟误差补偿的多无人机相对导航方法。在对机载惯导系统误差建模的基础上,将异步时钟误差作为系统状态量,构建基于异步时钟误差数据链相对测距误差模型。结合无人机之间高精度相对测距/测角信息以及机载气压高度计信息,构建多无人机协同定位位置观测模型。通过设计迭代扩展卡尔曼滤波器在对从机惯导系统误差实时估计与补偿的同时,对从机异步时钟误差进行联合最优估计。相对于传统方法,本文方法能有效提高无人机编队的定位精度和定位稳定性。