双无人机协同测向时差定位的优化仿真

刘云辉，姚敏，赵敏

(南京航空航天大学 自动化学院，江苏 南京 211106)

0 引言

近几年无人机的用途越来越广泛，国内出现一批以大疆为首的无人机民用公司，这使得无人机各方面的技术得到了快速的发展。随着无人机技术的不断改进，无人机以其成本低、无伤亡、体积小的优势，在未来战争中将扮演着重要的角色，而多无人机协同对区域目标进行侦查和打击任务将是未来无人机作战的主要形式[1-2]。为了提高打击精度，兼顾自身的隐蔽性，无源定位被广泛应用。无源定位的电子侦察系统本身不发射电磁波，工作完全是被动的，因此具有很好的隐蔽性。目前，无源定位主要的方法有测向定位、时差定位以及测向时差定位[3-5]等。其中测向定位仅需要2个观测站，系统对时间的同步要求不高，但其定位误差较大，在侧边区尤为明显。时差定位可获得较高的定位精度，但至少需要4个观测站，且对各站的时间同步要求较高。测向时差定位与时差定位相比，减少了观测站的数量，降低了系统复杂性；与测向定位相比，由于综合利用了目标辐射源的方向信息和时差信息，提高了定位精度[6-7]。本文主要研究如何提高测向时差定位的精度。

双机测向时差定位中，对目标位置进行解算，通常的方法是直接解析法[8-9]。由于此方法存在定位模糊问题，文献[7]提出了一种利用余弦定理，适用于三维定位情况的解析算法。但是文献[7]中给出的定位算法存在缺陷，没有对全方位目标的定位精度进行分析。

为了消除全方位的目标定位模糊问题，本文在文献[7]基础上，提出了一种改进的双机测向时差定位算法消除定位模糊问题。为了进一步提高多机协同定位精度，利用跟踪滤波的思路，将主观测站当前时刻及前一时刻的时差和方向观测量进行融合，采用滤波算法进行目标位置估计[10-11]。由于时差和方向观测量都是非线性函数，因此采用了非线性滤波。目前常用的多是扩展卡尔曼滤波(EKF)，它的基本思想是将非线性的量测方程在状态预测处进行泰勒展开并取其一阶近似值，对其线性化滤波后再利用卡尔曼滤波方法进行滤波[12-14]。由于其进行了线性化处理，就会产生较大的误差。近年来发展起来的粒子滤波(PF)算法是一种很好的解决非线性问题的算法[15-16]。

本文在现有文献的基础上，改进测向时差定位方法。仿真结果表明，本文算法能够很好地提高全局目标定位的定位精度，并比较了EKF和PF两种滤波算法对定位精度的改善效果。

1 双机测向时差定位原理及系统模型

双机测向时差目标定位系统由两个无人机组成，其中一个作为主站而另一个作为辅站。主站和辅站均可测得目标相对于无人机观测站的方位角φ和俯仰角ε，同时主站还可测得目标信号到达两站的时间差Δt。根据测的时间差Δt可计算得出目标到达两站的距离差Δr。如图1所示，设主站、辅站和目标在大地直角坐标系下的位置分别为S0(x0,y0,z0)、S1(x1,y1,z1)、S(x,y,z)。则主站和辅站测得的方位角φi、俯仰角εi(i=0、1)和两站距离差Δr可表示为：

(1)

(2)

(3)

传统的目标位置求解方法是根据测得的方位角φi、俯仰角εi和两站距离差Δr，联立式(1)、式(2)、式(3)，先将r0看作已知量解算出目标位置S(x,y,z)，再将S(x,y,z)带入式(3)中求解r0，再根据r0计算出S(x,y,z)的值。由于关于r0的方程是二次方程，所以r0的解不唯一会造成目标位置定位模糊。为了克服这一问题，下节给出通过坐标变换的三维定位无模糊解析算法，并且利用观测站测量的冗余信息，在式(3)中去掉绝对值号，从而得到全方位上无模糊的目标位置。

2 利用坐标变换的双机测向时差定位解算算法

通过坐标变换的三维定位无模糊解析算法是先在以主站为原点的坐标系下解得此坐标系下目标的位置，再变换到大地直角坐标系下。如图2所示，建立以主站为坐标原点的坐标系S0xyz，它是通过将坐标系(oxyz)平移得到的，各坐标轴与坐标系(oxyz)各坐标轴平行。由图2可知，通过求出主站到辐射源的距离r0，就可求得坐标系S0xyz下目标辐射源的位置坐标S′(xs,ys,zs)，进而得到目标在大地直角坐标系下的坐标S(x,y,z)，目标位置解算的解析式如下：

(4)

图2 以主站为原点的坐标系下观测站与目标位置关系

根据式(4)，只要求出r0，就可得到目标的位置坐标。文献[7]给出了基于余弦定位的r0解算方法，该方法中用到Δr=r1-r0并直接将其变换为r1=Δr+r0，实际上Δr=|r1-r0|，要利用测量的冗余信息判别r0和r1的大小后再进行等价代换。否则解算方法只有当r1-r0>0时才能进行目标位置的解算，不能进行全方位上的目标定位。下面给出适用于目标在观测站全方位上时r0的求解。

坐标系S0xyz下，目标的位置坐标为S′(xs,ys,zs)，辅站的坐标为S1′(x1s,y1s,z1s)。根据oxyz和S0xyz两坐标系的变换关系可得目标和辅站在两坐标系下的位置对应关系为:

(5)

坐标系S0xyz下，主站与目标的距离和辅站与目标的距离分别为:

(6)

由Δr=|r1-r0|，当r1-r0>0时可推出：

Δr2=r12-r02-2×Δr×r0

(7)

根据式(6)可知：

r12-r02=x1s2+y1s2+z1s2-2(x1s×xs+y1s×ys+z1s×zs)

(8)

联立式(4)、式(7)、式(8)可得：

(9)

联立式(5)、式(9)得：

(10)

当r1-r0<0时，Δr=r0-r1可得：

Δr2=r12-r02+2×Δr×r0

(11)

此时，可得：

(12)

在大地直角坐标系下，通过合理的布站，使得两无人机在同一高度上，根据式(2)比较两无人机测得的目标辐射源的俯仰角εi即可得出r1-r0的正负。然后，根据r1-r0的正负选择式(11)、式(12)带入式(4)中，得到目标辐射源的位置坐标S(x,y,z)。

3 基于粒子滤波的TDOA/AOA数据融合定位算法

时差和测向数据的测量精度很难提高，在不能满足高精度定位时，可以对目标同一位置进行多次测量，通过对测量结果进行关联处理，以提高定位精度。首先对目标位置进行多次连续观测，将首次测量的观测数据，采用上一节中的算法得到目标的初始位置估计值及相应的零均值估计误差的协方差矩阵，作为滤波算法的初始估计值，然后再利用滤波算法对多次观测值进行处理，从而得到更为精确的目标估计值。由于时差和方向观测量都是非线性函数，为了充分利用观测站获得的信息，解决传统的EKF算法在非线性估计时存在误差较大的问题，本文采用粒子滤波对观测数据进行处理。

粒子滤波的突出特点是适用于任意非线性、非高斯系统的滤波问题，摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约。而双机测向时差定位中的测向误差和测时误差是由多种因素造成的。其中测时误差主要包括两观测站时间基准的对准误差和观测站测量目标信号到达时间的误差，同时测向和时差的观测还受到环境因素引起的随机误差的影响。因此在实际中的系统的状态空间模型很难满足高斯假设，这时粒子滤波将可以很好地解决这一问题。

根据式(1)、式(2)、式(3)，建立双机测向时差定位模型的状态方程为：

X(k)=φk/k-1X(k-1)+ωk-1

(13)

测量方程为：

Y(k)=hk(X(k))+νk

(14)

式中，hk(X(k))=[Δrk,φk,εk]T；νk为均值为0、方差为σ2的高斯观测噪声。

粒子滤波主要有以下3个步骤：产生粒子、计算粒子权重和重要性采样。针对贯序重要性采样算法存在的权值退化现象，本文采用重采样的粒子滤波算法，该算法在每步迭代过程中，根据粒子权值对离散粒子进行重采样，重采样方法舍弃权值较小的粒子，代之以权值较大的粒子。

基于粒子滤波的AOA/TDOA融合定位的算法流程如下：

祖母的故事帮助赛利亚在民族文化身份中找准定位，赛利亚家族的历史与墨西哥和美国的历史紧密相连。芝加哥是赛利亚家族史的一个重要组成部分。她的祖父纳西索曾在芝加哥待了七年来逃避墨西哥革命战乱；墨西哥内战引发的混乱和美国新政所提供的大量的机会使得赛利亚父亲那一辈的墨西哥人离开故土，在芝加哥生存和发展；赛利亚这一代在芝加哥出生成长。

6) 得到k时刻目标估计值：

7) 令k=k+1,重复第二步。

其中初始化是将首次测量的观测数据，采用上一节中的算法得到目标的初始位置估计值及相应的零均值估计误差的协方差矩阵，从而得到初始化粒子。

4 仿真实验分析

为了验证第一节提出的目标定位解算方法的有效性，将其定位误差与理论定位误差的GDOP分布进行对比。基于坐标变换的侧向时差定位解算方法，其定位误差的GDOP通过多次蒙特卡洛得到，即：

仿真参数设定如下：

测角误差σφ0=σε0=3mrad，站址误差σs=10m，测时误差σt=50ns，主站坐标(-20,0,10)km,辅站坐标(20,0,10)km，目标高度z=0km，观测区域在x,y方向均为-60km～60km，蒙特卡洛仿真次数N=100。

图3-图4分别为理论定位误差和基于坐标变换解算的定位误差的GDOP分布图，其观测区域为x∈[-60 60]km，y∈[-60 60]km，z=0km内。这两幅分布图反映的是在指定观测区域内误差的等高图，图中的每条曲线是指对应误差值分布的位置。从两幅分布图中可看出，分布曲线基本相同，在两观测站法线方向分布基本一致，而在靠近两观测站基线方向上本文提出的基于坐标变换解算方法的定位精度更高。图5为观测区域x=0km，y∈[-60 60]km，z=0km内，第一节提出的解算算法的定位误差与理论上的定位误差对比曲线图。从对比曲线进一步反映出，第一节提出的解算算法的定位误差与理论上观测量存在一定误差时出现的定位误差基本一致。因此，通过仿真验证了第一节提出的定位解算算法的有效性。

图3 理论定位误差GDOP分布图

图4 基于坐标变换解算定位误差GDOP分布图

图5 基于坐标变换的定位误差与理论定位误差对比

下面通过仿真实验验证基于粒子滤波的AOA/TDOA数据融合定位方法对目标定位精度的改善效果。对固定目标进行多次测向、时差数据采集，对这些测量数据进行粒子滤波处理得到目标的估计值。因此，蒙特卡洛仿真的定位精度可表示为:

式中:(x,y,z)为目标的真实值；(xpf,ypf,zpf)为每次粒子滤波仿真得到的PF估计值。

仿真参数设置如下：

测角误差σφ0=σε0=3mrad，站址误差σs=5m，测时误差σt=20ns，主站坐标(-20,0,10)km,辅站坐标(20,0,10)km，固定目标坐标(60,60,0)km，进行200次测向、时差数据采集。

图6是基于EKF与PF两种方法对定位误差改善的对比曲线图，其中最上面一条曲线是根据每次时差和方向角的测量值带入到第一节的解算算法中得到的目标位置与目标真实位置之间的误差，下面两条曲线是分别用PF和EKF对观测数据融合后得到的定位误差。从图中可看出，对于将测量值直接带入到位置解算方程进行解算，由于测量值本身存在测量误差，因此解算得到的目标位置与目标真实位置之间也会存在误差。而通过滤波算法与定位解算算法进行结合，对得到的一组测量值进行融合处理。由图6可以看出经过滤波算法融合后的定位误差逐渐收敛，大大提高了定位精度。同时，通过EKF与PF分别对定位解算算法进行改进，由图6可看出，EKF对定位误差的改善整体上趋于收敛，但收敛过程中振荡较为严重。而PF对定位误差进行改善时，其收敛速度比EKF更快，而且PF对定位精度的改善最终趋于稳定，收敛过程比EKF要更加稳定。

图6 EKF与PF定位误差收敛曲线对比

5 结语

本文通过改进测向时差定位的解算方法，使三维目标在全方位上都能进行无模糊的位置解算，仿真验证了改进算法的有效性。同时，提出对目标在同一位置进行多次测量，利用滤波算法对多次测量值进行融合处理来提高定位精度。通过仿真对比可知，经过对测量数据的滤波融合大大提高了定位精度，同时对比了EKF和PF的定位误差收敛曲线，可看出PF更加有效地提高了定位精度。