基于TDOA的无人机集群协同单目标定位

何胜阳, 杜杰朋, 赵雅琴, 王宝莹, 赵 亮, 吴龙文,*

(1. 哈尔滨工业大学电子与信息工程学院, 黑龙江 哈尔滨 150001; 2. 联发博动科技(北京)有限公司, 北京 100015; 3. 中国铁路哈尔滨局集团有限公司工电检测所, 黑龙江 哈尔滨 150001)

0 引 言

基于到达时差(time difference of arrival, TDOA)的无源定位，利用目标辐射的电磁波到达不同观测站的到达时间差，再乘以电磁波传播速度，可以得到距离差，从而绘制出多组以观测站为焦点、距离差为长轴的双曲线，通过多个双曲线交点实现定位。该方法又被称为双曲线定位法。

定位方程解算方法的研究是一个十分经典的问题,根据TDOA定位方程高度非线性的特点,解决该问题大致可以通过3类解算方法。第1种方法是迭代法,该方法可以得到数值解,其典型代表是泰勒级数法[1]。该方法需要一个迭代初始值,在该值处进行一阶泰勒展开并进行循环迭代,将每次迭代的误差与预先设置的阈值比较,当小于阈值时，即得到最终结果。文献[2]基于约束总体最小二乘的思想,利用牛顿法迭代得到数值解。文献[3]设立多个不同的参考站,用泰勒展开法求解TDOA定位的最大似然问题。第2种方法是解析法,该方法可以得到闭式解,球面插值法[4]、球面相交法[5]以及平面相交法[6]是该方法早期的典型代表。不同于迭代法,其最大特点是计算量小,无需迭代初值,但由于没有修正过程,其精度较差。在此基础上,两步加权最小二乘[7]法被提出,该方法在第一步引入冗余变量线性化TDOA方程,估计出位置的粗略值;该方法在第二步利用辐射源位置和冗余变量的关系修正估计值,称该算法为Chan算法。文献[8]针对加权非线性最小二乘法需要进行复杂矩阵运算的问题,提出一种非线性期望最大化算法,在取得与原始算法接近性能的同时大大减少了计算量。对于运动目标定位,针对Ho等人提出的经典两步加权最小二乘[9]存在阈值效应的问题,文献[10]、文献[11]对最小二乘式进行不同的改进,定位性能在较高噪声水平下有了一定提高。第3种方法可以描述为基于最大似然目标函数的优化算法。粒子群算法[12-13]利用粒子种群对位置的多次迭代得到每个个体的局部最优解,进而通过搜索获得全局最优解,精度较高。另外，遗传算法[14]、蚁群算法[15]等也逐渐被引入,解决了该最大似然优化问题。文献[16]利用电磁波到达方向(direction of arrival, DOA)和TDOA的信息融合来更好地估计辐射源位置,该方法先利用DOA得到更加准确的时差,再利用Chan算法进行位置解算。

以上算法不考虑站址误差,但无人机集群是高机动、高灵活性平台,往往存在较大站址误差。在利用TDOA建模求解的过程中,需要考虑误差因素。在文献[9]的基础上,Ho研究了考虑观测站位置误差的前提下,基于TDOA[17]以及TDOA/到达频差(frequency difference of arrival, FDOA)[18]的算法改进,将位置误差项加入到加权最小二乘式中,使定位精度得到提高。此后,文献[19]、文献[20]对该闭式算法进行了改进,提高了噪声鲁棒性。文献[21]在联合时频差对移动辐射源进行位置速度估计基础上,引入了多普勒频移,通过仿真证明小噪声时该算法可以接近克拉美罗下界(Cramer-Rao lower bound, CRLB)。针对传感器位置随机且难以精确估计的问题,Ho提出了通过放置校准发射器来提高传感器的位置估计精度的方法[22],推导出校准器的最佳位置准则。文献[23]基于时频差测量,利用半定规划法求解,并将定位场景扩展到了非视距传输和多径衰落等场景。文献[24]模拟设置了多种不同的校准发射器,通过测量TDOA并开发了一种半定规划算法，有效地降低了站址误差对定位精度的影响。在此基础上,文献[25]进一步将时钟同步偏差考虑进来,扩大了算法的应用场景。文献[26]提出一种基于加权最小二乘的闭式高精度定位方法,该方法无需迭代运算,计算量较小。文献[27]提出基于到达时间(time of arrival, TOA)变化率的高精度定位方法,忽略径向加速度，进行有偏估计。

本文首先基于无人机集群对单目标定位进行建模,研究TDOA单目标定位方程的解算方法,在考虑站址误差的前提下改进Chan算法和泰勒算法,改进后的算法分别称为解析法和迭代法。将粒子群算法的解作为泰勒迭代的初值,提出粒子群泰勒协同算法,仿真实验证明该方法具有较高的定位精度。

1 TDOA单目标定位模型

在TDOA定位中,辐射源到达每个辅站与主站的时间会存在一组时间差,将时间差乘以电磁波传播速度即得到距离差,每组距离差可确定一组双曲线,多组双曲线的交点即为辐射源的位置。

二维定位场景原理如图1所示。要实现单目标定位，至少需要两组双曲线,即至少需要3架无人机。设3架无人机位于s1=(x1,y1)T、s2=(x2,y2)T、s3=(x3,y3)T处。把s1设为主机,则另外两架飞机分别可与主机确定一组双曲线,辐射源就位于双曲线交点上。为了去除虚假定位点并提高定位精度,往往需要增加无人机的数量。

图1 二维场景下TDOA定位原理Fig.1 Schematic diagram of two-dimensional TDOA location

三维空间单目标定位模型如图2所示,每架辅机都可与主机确定一组三维空间中的双曲面,在不存在TDOA观测误差的情况下,至少需要4架无人机才能确定单目标的位置,要想去除虚假点,需要进一步增加无人机的数量。

图2 TDOA单目标定位模型Fig.2 TDOA single target location model

(1)

辅机i与主机接收到信号的实际时间差为

(2)

(3)

式中:ni1=c·Δti1;v为由到达时间差测量误差导致的距离差误差向量。误差的协方差矩阵Q=E[vvT]。

另外,作为高机动平台,无人机自身位置一般是由全球定位系统测量的,准确性和实时性难以保证,自身位置会存在误差,则定位过程中所用的带有误差的站址为

(4)

式中:Δsi为无人机位置的误差向量。

2 考虑站址误差时的TDOA单目标定位解算方法

将不考虑站址误差的Chan算法和Taylor算法直接应用到具有较大站址误差的无人机集群定位场景中时,会出现定位精度严重偏离CRLB的情况。根据文献[28],当无人机高低角为30°,飞行高度为5 km时,卫星对其位置估计的误差约为30 m。根据文献[29],由于近年来卫星定位精度的提高,无人机在低空慢速飞行时,误差可控制在10 m以内。本节将站址误差考虑进来,将改进后的Chan算法称为解析法[29],改进后的Taylor算法称为迭代法[29],并针对Taylor算法需要较为精确的初始值的问题,提出了一种粒子群泰勒协同的算法。

2.1 解析法

(5)

ε1=h1-G1u1

(6)

式中:

(7)

(8)

(9)

式中：0是一个3×1的列向量。然而，B1和D1中均包含无误差的位置和距离,实际中无法得到,只能取近似值,近似方法在文献[17]、文献[18]的W1初始值获取中已有说明。为了获得W1初始值,在目标位置远离无人机集群时,有r1≈r2≈…≈rM,则B1可近似为2r1[I],D1可近似为2r1[-1P],1是大小为(M-1)×3的矩阵,P是大小为(M-1)×3(M-1)的对角矩阵,定义为

(10)

加权矩阵W1近似写为

(11)

将式(11)代入式(8)中，可得到初始目标位置u1,再通过计算B1和D1得到性能更优的W1,进行第一次加权最小二乘。大量仿真结果表明[18],定位精度对加权矩阵中的噪声不敏感,所以对初始加权矩阵W1近似是可行的。u1的协方差矩阵由下式给出：

(12)

接下来，进行第二次加权最小二乘

ε2=h2-G2u2

(13)

式中:

(14)

则式(13)的解为

(15)

式中:

(16)

(17)

B2和D2中的无人机位置和辐射源位置可以将第一步加权最小二乘解代入。根据u2与待求辐射源位置关系,并在消除符号模糊后,求得最终的目标位置为

(18)

式中:P=diag {sgn(u1(1∶3)-s1)}。

解析法在Chan算法的基础上将站址误差考虑进来,得到的定位精度有一定提升,但与Chan算法类似,解析法在求解过程中做了两次近似,因此精度的提升有限。

2.2 迭代法

f(θ)=T=M-E

(19)

(20)

f(θ)|θ=θg+Aδθ≃M-E

(21)

式中：δθ为一个3M+3维的列向量,其值为

(22)

A是一个(4M-1)×(3M+3)的矩阵,其计算方法为

(23)

(24)

式中:

θ=θg=I

(25)

式中:0是一个3×1的列向量;O是3M×3的零矩阵;I是3M的单位矩阵,式(21)可重新写为

Aδθ=W-E

(26)

式中:

W=M-f(θ)|θ=θθ

(27)

当加权矩阵为Q-1时,δθ的加权最小二乘估计为

δθ=[ATQ-1A]-1ATQ-1W

(28)

得到的解等于

θ(1)=θg+δθ

(29)

(30)

式中:ε为控制迭代的预设阈值。

根据式(30)进行循环迭代,直到误差小于预设的终止值,停止迭代,目标的估计位置即为向量θ的前3个值,且目标位置后的每3个值就是一架飞机的位置估计。

2.3 基于粒子群泰勒的解算方法

由文献[30]、利用两步定位法，将在传感器获取的目标距离信息中获得的目标位置估计作为初值启发,粒子群算法属于先根据最大似然原理设定目标函数、再利用粒子群搜索目标函数最优值的方法。将其用在时差定位方程的求解时,关键是找到非线性最大似然目标函数。

粒子群算法的计算过程如下。在D维搜索空间中,限定搜索范围,并生成N个粒子,每个粒子所在位置都可能是问题的解,粒子通过自身惯性、个体经验和群体经验三大因素得到每次迭代的速度更新值,进而得到本次迭代的位置更新公式。设第i个粒子当前位置为xi=[xi,1,xi,2,…,xi,D]T(i=1,2,…,N)，飞行速度是[vi,1,vi,2,…,vi,D]T(i=1,2,…,N),该粒子通过不断学习更新目前搜索到的个体的最优解为pi=[pi,1,pi,2,…,pi,D]T(i=1,2,…,N)。经过一定次数的学习迭代后,每个粒子都到达一个自身最优位置,称为个体最优解。由于群体的社会性,粒子之间可进行信息共享,N个粒子的种群就可以从中选出群体最优解,使得目标函数最优。设全局最优解为pg,pg=[pg,1,pg,2,…,pg,D]T(i=1,2,…,N)。当迭代终止时,pg即为粒子群算法求出的最优解。粒子的更新公式为

(31)

式中:ω称为惯性权重,表征上代速度对当代速度的影响程度；c1和c2为学习因子,分别表征自身经验和社会经验对下一步动作的影响程度；r1和r2是对角线元素，服从[0,1]均匀分布的对角阵。式(31)中的两个式子分别是速度更新公式和位置更新公式。令算法经过t次迭代后停止。

在时差定位中,若不存在测量误差,则每个观测值确定的双曲线最终交于一点,可得到精确定位结果,但存在观测误差时,双曲线不能交于一点,可以利用误差概率密度函数得出最大似然式,并将其作为目标函数,对目标函数利用粒子群算法求解。假设TDOA的测量噪声为高斯噪声,服从ni,1～N(0,δ2)分布:

(32)

式中:r的定义如式(3)所示,ri=[r2,r3,…,rM]T,r1=[r1,r1,…,r1]T。观察式(32),若使该函数最大,则等价于求下式的最小值

(33)

式(32)进一步可等价为

(34)

考虑站址误差时,解析法依旧存在较大误差,迭代法在精度上有较大优势,但需要较为精确的初始值,否则在噪声较大时存在不收敛的问题。这时，解析法的结果不能再作为迭代法的初值代入,应考虑将一种精度较高算法的结果作为迭代初值,粒子群算法符合这一要求,其代价函数需要在存在站址误差时进行修改。原本的代价函数为

(35)

(36)

(37)

存在站址误差时,改进的Chan算法存在较大误差,粒子群算法虽然相对于CRLB有一定偏离,但其结果可以作为初始值代入改进的泰勒算法,并能收敛到较为精确的结果,称该算法为粒子群泰勒(Taylor-PSO-Mod)算法。

3 仿真实验

实验 1解析法定位解算性能研究

设集群中包含5架无人机,按矩形布站,辐射源位置为(5,10,0)km,飞机位置为(0,0,3.3)km、(-3,0,3)km、(3,0,3)km、(0,3,3)km、(0,-3,3)km。

为了研究存在无人机位置误差时解析法对Chan算法的改进效果,设置站址误差范围为1～40 m,进行1 000次蒙特卡罗实验,以均方根误差(root mean square error, RMSE)作为定位结果的评价标准,图3显示了定位结果的对比。

图例中带有Mod标识的为改进方法,下同。由仿真结果和上述理论推导,由于在求解过程中做了两次近似,所以解析法对Chan算法的改进并不明显,在站址误差较低时就出现了阈值效应,抗噪声性能并不理想。

实验 2迭代法定位解算性能研究

设置实验条件与实验1相同,图4显示了迭代法对Taylor算法的改进效果。

由仿真结果可知,在站址误差较大时,迭代法相对于泰勒算法有较好的抗噪声性能,RMSE明显降低,但在站址误差较小时偏离了CRLB。且迭代法需要一个较为准确的初始估计值,否则会无法收敛,实验1的结果表明定位误差较大的解析法已不能满足要求。

图3 解析法与Chan算法定位误差比较曲线Fig.3 Comparison curve of positioning error between the analytical method and Chan algorithm

图4 迭代法与Taylor算法定位误差比较曲线Fig.4 Comparison curve of positioning error between iterative method and Taylor algorithm

实验 3Taylor-PSO-Mod算法定位解算性能研究

设目标、无人机位置与实验1中的位置相同,图5给出了解析法、迭代法、改进粒子群算法以及Taylor-PSO-Mod算法随站址误差变化的定位精度。

图5 不同算法RMSE随站址误差变化曲线Fig.5 Variation curve of RMSE with station location error of different algorithms

图5中的紫色线代表本文提出的Taylor-PSO-Mod算法,观察比较该算法与迭代法、粒子群算法的性能。在低站址误差时,Taylor-PSO-Mod算法表现最好,在站址误差较大情况下,Taylor-PSO-Mod算法的定位精度高于粒子群算法,与迭代法有相近的性能,但它解决了迭代法的初值问题。其在较宽的无人机位置误差范围内,定位结果与RMSE均最小。

图6中经纬度误差指的是目标的经纬度,从曲线可以看出,不论是经度误差还是纬度误差,解析法的表现均较差,粒子群算法的纬度误差较大,而Taylor-PSO-Mod算法在经纬度误差上的表现均良好,这就使得接下来的工作有了一定的仿真基础。

图6 不同算法经纬度误差随站址误差变化曲线Fig.6 Variation curve of longitude and latitude error of different algorithms with station location error

为了进一步探究当无人机与目标相距不同距离时各种算法的定位性能变化,在以上初始位置的基础上,设置站址误差为5 m,时差测量误差为20 ns,令无人机以v=(40,70,0)m/s的速度向目标飞行,则随飞行时间变化位置估计的RMSE曲线如图7所示。

根据仿真结果,在时差测量误差和站址误差确定时,随着无人机飞行时间增大，Taylor-PSO-Mod算法对目标定位的RMSE始终保持最小,说明了所提算法的稳定性。且无人机与辐射源相距越近,精度越高。

图7 位置估计RMSE随飞行时间变化曲线Fig.7 Variation curve of localization RMSE with flight time

进一步地,为观察密集布站情况下所提方法的定位性能和稳定性,参考文献[31]中的布站方式,新增一架无人机,无人机集群分别位于(0.3,0.1,0.15)km,(0.4,0.15,0.1)km,(0.3,0.5,0.2)km,(0.35,0.2,0.1)km,(-0.1,-0.1,-0.1)km,(-0.2,-0.3,-0.2)km。辐射源位置为(285,325,275)m。观察密集布站条件下所提算法的性能。该实验对曲线作了如下处理,因为所设站址误差较小,所以对站址误差取了对数米,得到更为平滑的曲线，以更好地观察实验现象,通过实验获得的仿真结果如图8所示。

图8 密集布站下RMSE随站址误差变化曲线Fig.8 Variation curve of RMSE with station site error under dense station layout

根据图8的仿真结果,当设置较小的站址误差且密集布站的条件下,所提的Taylor-PSO-Mod算法定位RMSE仍然最小,基本符合CRLB,说明在多种仿真条件下所提算法性能具备稳定性。值得注意的是,图8中的粒子群算法在站址误差较大时定位误差小于CRLB,这是由于粒子群算法是一种有偏估计,而CRLB是无偏估计的最小方差,在某些布站条件下,当目标位置估计偏差较大时,可能出现定位的RMSE小于CRLB的情况。

4 结 论

本文主要对基于无人机集群的时差测量单目标定位方法进行研究,建立了单目标定位模型。作为高机动平台,无人机无法得到自身精确位置,需要在经典算法的基础上将站址误差考虑进来。本文重点研究存在站址误差情况下提高单目标定位精度的方法。首先，对Chan算法和Taylor算法进行改进,得到解析法和迭代法,在存在站址误差的情况下算法性能有不同程度的提高；然后，提出一种Taylor-PSO-Mod解算方法,仿真实验证明了这种方法相对于解析法和迭代法的优点,其在多种仿真场景下均能达到较高的定位精度,定位性能稳定,同时还解决了迭代法的初值问题。从图5各个算法的表现来看,站址误差较大时，其RMSE误差比性能较好的改进的粒子群算法减小了300 m左右。