基于TDOA的無人機(jī)集群協(xié)同單目標(biāo)定位

何勝陽(yáng), 杜杰朋, 趙雅琴, 王寶瑩, 趙 亮, 吳龍文,*

(1. 哈爾濱工業(yè)大學(xué)電子與信息工程學(xué)院, 黑龍江 哈爾濱 150001; 2. 聯(lián)發(fā)博動(dòng)科技(北京)有限公司, 北京 100015; 3. 中國(guó)鐵路哈爾濱局集團(tuán)有限公司工電檢測(cè)所, 黑龍江 哈爾濱 150001)

0 引 言

基于到達(dá)時(shí)差(time difference of arrival, TDOA)的無源定位，利用目標(biāo)輻射的電磁波到達(dá)不同觀測(cè)站的到達(dá)時(shí)間差，再乘以電磁波傳播速度，可以得到距離差，從而繪制出多組以觀測(cè)站為焦點(diǎn)、距離差為長(zhǎng)軸的雙曲線，通過多個(gè)雙曲線交點(diǎn)實(shí)現(xiàn)定位。該方法又被稱為雙曲線定位法。

定位方程解算方法的研究是一個(gè)十分經(jīng)典的問題,根據(jù)TDOA定位方程高度非線性的特點(diǎn),解決該問題大致可以通過3類解算方法。第1種方法是迭代法,該方法可以得到數(shù)值解,其典型代表是泰勒級(jí)數(shù)法[1]。該方法需要一個(gè)迭代初始值,在該值處進(jìn)行一階泰勒展開并進(jìn)行循環(huán)迭代,將每次迭代的誤差與預(yù)先設(shè)置的閾值比較,當(dāng)小于閾值時(shí)，即得到最終結(jié)果。文獻(xiàn)[2]基于約束總體最小二乘的思想,利用牛頓法迭代得到數(shù)值解。文獻(xiàn)[3]設(shè)立多個(gè)不同的參考站,用泰勒展開法求解TDOA定位的最大似然問題。第2種方法是解析法,該方法可以得到閉式解,球面插值法[4]、球面相交法[5]以及平面相交法[6]是該方法早期的典型代表。不同于迭代法,其最大特點(diǎn)是計(jì)算量小,無需迭代初值,但由于沒有修正過程,其精度較差。在此基礎(chǔ)上,兩步加權(quán)最小二乘[7]法被提出,該方法在第一步引入冗余變量線性化TDOA方程,估計(jì)出位置的粗略值;該方法在第二步利用輻射源位置和冗余變量的關(guān)系修正估計(jì)值,稱該算法為Chan算法。文獻(xiàn)[8]針對(duì)加權(quán)非線性最小二乘法需要進(jìn)行復(fù)雜矩陣運(yùn)算的問題,提出一種非線性期望最大化算法,在取得與原始算法接近性能的同時(shí)大大減少了計(jì)算量。對(duì)于運(yùn)動(dòng)目標(biāo)定位,針對(duì)Ho等人提出的經(jīng)典兩步加權(quán)最小二乘[9]存在閾值效應(yīng)的問題,文獻(xiàn)[10]、文獻(xiàn)[11]對(duì)最小二乘式進(jìn)行不同的改進(jìn),定位性能在較高噪聲水平下有了一定提高。第3種方法可以描述為基于最大似然目標(biāo)函數(shù)的優(yōu)化算法。粒子群算法[12-13]利用粒子種群對(duì)位置的多次迭代得到每個(gè)個(gè)體的局部最優(yōu)解,進(jìn)而通過搜索獲得全局最優(yōu)解,精度較高。另外，遺傳算法[14]、蟻群算法[15]等也逐漸被引入,解決了該最大似然優(yōu)化問題。文獻(xiàn)[16]利用電磁波到達(dá)方向(direction of arrival, DOA)和TDOA的信息融合來更好地估計(jì)輻射源位置,該方法先利用DOA得到更加準(zhǔn)確的時(shí)差,再利用Chan算法進(jìn)行位置解算。

以上算法不考慮站址誤差,但無人機(jī)集群是高機(jī)動(dòng)、高靈活性平臺(tái),往往存在較大站址誤差。在利用TDOA建模求解的過程中,需要考慮誤差因素。在文獻(xiàn)[9]的基礎(chǔ)上,Ho研究了考慮觀測(cè)站位置誤差的前提下,基于TDOA[17]以及TDOA/到達(dá)頻差(frequency difference of arrival, FDOA)[18]的算法改進(jìn),將位置誤差項(xiàng)加入到加權(quán)最小二乘式中,使定位精度得到提高。此后,文獻(xiàn)[19]、文獻(xiàn)[20]對(duì)該閉式算法進(jìn)行了改進(jìn),提高了噪聲魯棒性。文獻(xiàn)[21]在聯(lián)合時(shí)頻差對(duì)移動(dòng)輻射源進(jìn)行位置速度估計(jì)基礎(chǔ)上,引入了多普勒頻移,通過仿真證明小噪聲時(shí)該算法可以接近克拉美羅下界(Cramer-Rao lower bound, CRLB)。針對(duì)傳感器位置隨機(jī)且難以精確估計(jì)的問題,Ho提出了通過放置校準(zhǔn)發(fā)射器來提高傳感器的位置估計(jì)精度的方法[22],推導(dǎo)出校準(zhǔn)器的最佳位置準(zhǔn)則。文獻(xiàn)[23]基于時(shí)頻差測(cè)量,利用半定規(guī)劃法求解,并將定位場(chǎng)景擴(kuò)展到了非視距傳輸和多徑衰落等場(chǎng)景。文獻(xiàn)[24]模擬設(shè)置了多種不同的校準(zhǔn)發(fā)射器,通過測(cè)量TDOA并開發(fā)了一種半定規(guī)劃算法，有效地降低了站址誤差對(duì)定位精度的影響。在此基礎(chǔ)上,文獻(xiàn)[25]進(jìn)一步將時(shí)鐘同步偏差考慮進(jìn)來,擴(kuò)大了算法的應(yīng)用場(chǎng)景。文獻(xiàn)[26]提出一種基于加權(quán)最小二乘的閉式高精度定位方法,該方法無需迭代運(yùn)算,計(jì)算量較小。文獻(xiàn)[27]提出基于到達(dá)時(shí)間(time of arrival, TOA)變化率的高精度定位方法,忽略徑向加速度，進(jìn)行有偏估計(jì)。

本文首先基于無人機(jī)集群對(duì)單目標(biāo)定位進(jìn)行建模,研究TDOA單目標(biāo)定位方程的解算方法,在考慮站址誤差的前提下改進(jìn)Chan算法和泰勒算法,改進(jìn)后的算法分別稱為解析法和迭代法。將粒子群算法的解作為泰勒迭代的初值,提出粒子群泰勒協(xié)同算法,仿真實(shí)驗(yàn)證明該方法具有較高的定位精度。

1 TDOA單目標(biāo)定位模型

在TDOA定位中,輻射源到達(dá)每個(gè)輔站與主站的時(shí)間會(huì)存在一組時(shí)間差,將時(shí)間差乘以電磁波傳播速度即得到距離差,每組距離差可確定一組雙曲線,多組雙曲線的交點(diǎn)即為輻射源的位置。

二維定位場(chǎng)景原理如圖1所示。要實(shí)現(xiàn)單目標(biāo)定位，至少需要兩組雙曲線,即至少需要3架無人機(jī)。設(shè)3架無人機(jī)位于s1=(x1,y1)T、s2=(x2,y2)T、s3=(x3,y3)T處。把s1設(shè)為主機(jī),則另外兩架飛機(jī)分別可與主機(jī)確定一組雙曲線,輻射源就位于雙曲線交點(diǎn)上。為了去除虛假定位點(diǎn)并提高定位精度,往往需要增加無人機(jī)的數(shù)量。

圖1 二維場(chǎng)景下TDOA定位原理Fig.1 Schematic diagram of two-dimensional TDOA location

三維空間單目標(biāo)定位模型如圖2所示,每架輔機(jī)都可與主機(jī)確定一組三維空間中的雙曲面,在不存在TDOA觀測(cè)誤差的情況下,至少需要4架無人機(jī)才能確定單目標(biāo)的位置,要想去除虛假點(diǎn),需要進(jìn)一步增加無人機(jī)的數(shù)量。

圖2 TDOA單目標(biāo)定位模型Fig.2 TDOA single target location model

(1)

輔機(jī)i與主機(jī)接收到信號(hào)的實(shí)際時(shí)間差為

(2)

(3)

式中:ni1=c·Δti1;v為由到達(dá)時(shí)間差測(cè)量誤差導(dǎo)致的距離差誤差向量。誤差的協(xié)方差矩陣Q=E[vvT]。

另外,作為高機(jī)動(dòng)平臺(tái),無人機(jī)自身位置一般是由全球定位系統(tǒng)測(cè)量的,準(zhǔn)確性和實(shí)時(shí)性難以保證,自身位置會(huì)存在誤差,則定位過程中所用的帶有誤差的站址為

(4)

式中:Δsi為無人機(jī)位置的誤差向量。

2 考慮站址誤差時(shí)的TDOA單目標(biāo)定位解算方法

將不考慮站址誤差的Chan算法和Taylor算法直接應(yīng)用到具有較大站址誤差的無人機(jī)集群定位場(chǎng)景中時(shí),會(huì)出現(xiàn)定位精度嚴(yán)重偏離CRLB的情況。根據(jù)文獻(xiàn)[28],當(dāng)無人機(jī)高低角為30°,飛行高度為5 km時(shí),衛(wèi)星對(duì)其位置估計(jì)的誤差約為30 m。根據(jù)文獻(xiàn)[29],由于近年來衛(wèi)星定位精度的提高,無人機(jī)在低空慢速飛行時(shí),誤差可控制在10 m以內(nèi)。本節(jié)將站址誤差考慮進(jìn)來,將改進(jìn)后的Chan算法稱為解析法[29],改進(jìn)后的Taylor算法稱為迭代法[29],并針對(duì)Taylor算法需要較為精確的初始值的問題,提出了一種粒子群泰勒協(xié)同的算法。

2.1 解析法

(5)

ε1=h1-G1u1

(6)

式中:

(7)

(8)

(9)

式中：0是一個(gè)3×1的列向量。然而，B1和D1中均包含無誤差的位置和距離,實(shí)際中無法得到,只能取近似值,近似方法在文獻(xiàn)[17]、文獻(xiàn)[18]的W1初始值獲取中已有說明。為了獲得W1初始值,在目標(biāo)位置遠(yuǎn)離無人機(jī)集群時(shí),有r1≈r2≈…≈rM,則B1可近似為2r1[I],D1可近似為2r1[-1P],1是大小為(M-1)×3的矩陣,P是大小為(M-1)×3(M-1)的對(duì)角矩陣,定義為

(10)

加權(quán)矩陣W1近似寫為

(11)

將式(11)代入式(8)中，可得到初始目標(biāo)位置u1,再通過計(jì)算B1和D1得到性能更優(yōu)的W1,進(jìn)行第一次加權(quán)最小二乘。大量仿真結(jié)果表明[18],定位精度對(duì)加權(quán)矩陣中的噪聲不敏感,所以對(duì)初始加權(quán)矩陣W1近似是可行的。u1的協(xié)方差矩陣由下式給出：

(12)

接下來，進(jìn)行第二次加權(quán)最小二乘

ε2=h2-G2u2

(13)

式中:

(14)

則式(13)的解為

(15)

式中:

(16)

(17)

B2和D2中的無人機(jī)位置和輻射源位置可以將第一步加權(quán)最小二乘解代入。根據(jù)u2與待求輻射源位置關(guān)系,并在消除符號(hào)模糊后,求得最終的目標(biāo)位置為

(18)

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

解析法在Chan算法的基礎(chǔ)上將站址誤差考慮進(jìn)來,得到的定位精度有一定提升,但與Chan算法類似,解析法在求解過程中做了兩次近似,因此精度的提升有限。

2.2 迭代法

f(θ)=T=M-E

(19)

(20)

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

(21)

式中：δθ為一個(gè)3M+3維的列向量,其值為

(22)

A是一個(gè)(4M-1)×(3M+3)的矩陣,其計(jì)算方法為

(23)

(24)

式中:

θ=θg=I

(25)

式中:0是一個(gè)3×1的列向量;O是3M×3的零矩陣;I是3M的單位矩陣,式(21)可重新寫為

Aδθ=W-E

(26)

式中:

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

(27)

當(dāng)加權(quán)矩陣為Q-1時(shí),δθ的加權(quán)最小二乘估計(jì)為

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

(28)

得到的解等于

θ(1)=θg+δθ

(29)

(30)

式中:ε為控制迭代的預(yù)設(shè)閾值。

根據(jù)式(30)進(jìn)行循環(huán)迭代,直到誤差小于預(yù)設(shè)的終止值,停止迭代,目標(biāo)的估計(jì)位置即為向量θ的前3個(gè)值,且目標(biāo)位置后的每3個(gè)值就是一架飛機(jī)的位置估計(jì)。

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

由文獻(xiàn)[30]、利用兩步定位法，將在傳感器獲取的目標(biāo)距離信息中獲得的目標(biāo)位置估計(jì)作為初值啟發(fā),粒子群算法屬于先根據(jù)最大似然原理設(shè)定目標(biāo)函數(shù)、再利用粒子群搜索目標(biāo)函數(shù)最優(yōu)值的方法。將其用在時(shí)差定位方程的求解時(shí),關(guān)鍵是找到非線性最大似然目標(biāo)函數(shù)。

粒子群算法的計(jì)算過程如下。在D維搜索空間中,限定搜索范圍,并生成N個(gè)粒子,每個(gè)粒子所在位置都可能是問題的解,粒子通過自身慣性、個(gè)體經(jīng)驗(yàn)和群體經(jīng)驗(yàn)三大因素得到每次迭代的速度更新值,進(jìn)而得到本次迭代的位置更新公式。設(shè)第i個(gè)粒子當(dāng)前位置為xi=[xi,1,xi,2,…,xi,D]T(i=1,2,…,N)，飛行速度是[vi,1,vi,2,…,vi,D]T(i=1,2,…,N),該粒子通過不斷學(xué)習(xí)更新目前搜索到的個(gè)體的最優(yōu)解為pi=[pi,1,pi,2,…,pi,D]T(i=1,2,…,N)。經(jīng)過一定次數(shù)的學(xué)習(xí)迭代后,每個(gè)粒子都到達(dá)一個(gè)自身最優(yōu)位置,稱為個(gè)體最優(yōu)解。由于群體的社會(huì)性,粒子之間可進(jìn)行信息共享,N個(gè)粒子的種群就可以從中選出群體最優(yōu)解,使得目標(biāo)函數(shù)最優(yōu)。設(shè)全局最優(yōu)解為pg,pg=[pg,1,pg,2,…,pg,D]T(i=1,2,…,N)。當(dāng)?shù)K止時(shí),pg即為粒子群算法求出的最優(yōu)解。粒子的更新公式為

(31)

式中:ω稱為慣性權(quán)重,表征上代速度對(duì)當(dāng)代速度的影響程度；c1和c2為學(xué)習(xí)因子,分別表征自身經(jīng)驗(yàn)和社會(huì)經(jīng)驗(yàn)對(duì)下一步動(dòng)作的影響程度；r1和r2是對(duì)角線元素，服從[0,1]均勻分布的對(duì)角陣。式(31)中的兩個(gè)式子分別是速度更新公式和位置更新公式。令算法經(jīng)過t次迭代后停止。

在時(shí)差定位中,若不存在測(cè)量誤差,則每個(gè)觀測(cè)值確定的雙曲線最終交于一點(diǎn),可得到精確定位結(jié)果,但存在觀測(cè)誤差時(shí),雙曲線不能交于一點(diǎn),可以利用誤差概率密度函數(shù)得出最大似然式,并將其作為目標(biāo)函數(shù),對(duì)目標(biāo)函數(shù)利用粒子群算法求解。假設(shè)TDOA的測(cè)量噪聲為高斯噪聲,服從ni,1～N(0,δ2)分布:

(32)

式中:r的定義如式(3)所示,ri=[r2,r3,…,rM]T,r1=[r1,r1,…,r1]T。觀察式(32),若使該函數(shù)最大,則等價(jià)于求下式的最小值

(33)

式(32)進(jìn)一步可等價(jià)為

(34)

考慮站址誤差時(shí),解析法依舊存在較大誤差,迭代法在精度上有較大優(yōu)勢(shì),但需要較為精確的初始值,否則在噪聲較大時(shí)存在不收斂的問題。這時(shí)，解析法的結(jié)果不能再作為迭代法的初值代入,應(yīng)考慮將一種精度較高算法的結(jié)果作為迭代初值,粒子群算法符合這一要求,其代價(jià)函數(shù)需要在存在站址誤差時(shí)進(jìn)行修改。原本的代價(jià)函數(shù)為

(35)

(36)

(37)

存在站址誤差時(shí),改進(jìn)的Chan算法存在較大誤差,粒子群算法雖然相對(duì)于CRLB有一定偏離,但其結(jié)果可以作為初始值代入改進(jìn)的泰勒算法,并能收斂到較為精確的結(jié)果,稱該算法為粒子群泰勒(Taylor-PSO-Mod)算法。

3 仿真實(shí)驗(yàn)

實(shí)驗(yàn) 1解析法定位解算性能研究

設(shè)集群中包含5架無人機(jī),按矩形布站,輻射源位置為(5,10,0)km,飛機(jī)位置為(0,0,3.3)km、(-3,0,3)km、(3,0,3)km、(0,3,3)km、(0,-3,3)km。

為了研究存在無人機(jī)位置誤差時(shí)解析法對(duì)Chan算法的改進(jìn)效果,設(shè)置站址誤差范圍為1～40 m,進(jìn)行1 000次蒙特卡羅實(shí)驗(yàn),以均方根誤差(root mean square error, RMSE)作為定位結(jié)果的評(píng)價(jià)標(biāo)準(zhǔn),圖3顯示了定位結(jié)果的對(duì)比。

圖例中帶有Mod標(biāo)識(shí)的為改進(jìn)方法,下同。由仿真結(jié)果和上述理論推導(dǎo),由于在求解過程中做了兩次近似,所以解析法對(duì)Chan算法的改進(jìn)并不明顯,在站址誤差較低時(shí)就出現(xiàn)了閾值效應(yīng),抗噪聲性能并不理想。

實(shí)驗(yàn) 2迭代法定位解算性能研究

設(shè)置實(shí)驗(yàn)條件與實(shí)驗(yàn)1相同,圖4顯示了迭代法對(duì)Taylor算法的改進(jìn)效果。

由仿真結(jié)果可知,在站址誤差較大時(shí),迭代法相對(duì)于泰勒算法有較好的抗噪聲性能,RMSE明顯降低,但在站址誤差較小時(shí)偏離了CRLB。且迭代法需要一個(gè)較為準(zhǔn)確的初始估計(jì)值,否則會(huì)無法收斂,實(shí)驗(yàn)1的結(jié)果表明定位誤差較大的解析法已不能滿足要求。

圖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

實(shí)驗(yàn) 3Taylor-PSO-Mod算法定位解算性能研究

設(shè)目標(biāo)、無人機(jī)位置與實(shí)驗(yàn)1中的位置相同,圖5給出了解析法、迭代法、改進(jìn)粒子群算法以及Taylor-PSO-Mod算法隨站址誤差變化的定位精度。

圖5 不同算法RMSE隨站址誤差變化曲線Fig.5 Variation curve of RMSE with station location error of different algorithms

圖5中的紫色線代表本文提出的Taylor-PSO-Mod算法,觀察比較該算法與迭代法、粒子群算法的性能。在低站址誤差時(shí),Taylor-PSO-Mod算法表現(xiàn)最好,在站址誤差較大情況下,Taylor-PSO-Mod算法的定位精度高于粒子群算法,與迭代法有相近的性能,但它解決了迭代法的初值問題。其在較寬的無人機(jī)位置誤差范圍內(nèi),定位結(jié)果與RMSE均最小。

圖6中經(jīng)緯度誤差指的是目標(biāo)的經(jīng)緯度,從曲線可以看出,不論是經(jīng)度誤差還是緯度誤差,解析法的表現(xiàn)均較差,粒子群算法的緯度誤差較大,而Taylor-PSO-Mod算法在經(jīng)緯度誤差上的表現(xiàn)均良好,這就使得接下來的工作有了一定的仿真基礎(chǔ)。

圖6 不同算法經(jīng)緯度誤差隨站址誤差變化曲線Fig.6 Variation curve of longitude and latitude error of different algorithms with station location error

為了進(jìn)一步探究當(dāng)無人機(jī)與目標(biāo)相距不同距離時(shí)各種算法的定位性能變化,在以上初始位置的基礎(chǔ)上,設(shè)置站址誤差為5 m,時(shí)差測(cè)量誤差為20 ns,令無人機(jī)以v=(40,70,0)m/s的速度向目標(biāo)飛行,則隨飛行時(shí)間變化位置估計(jì)的RMSE曲線如圖7所示。

根據(jù)仿真結(jié)果,在時(shí)差測(cè)量誤差和站址誤差確定時(shí),隨著無人機(jī)飛行時(shí)間增大，Taylor-PSO-Mod算法對(duì)目標(biāo)定位的RMSE始終保持最小,說明了所提算法的穩(wěn)定性。且無人機(jī)與輻射源相距越近,精度越高。

圖7 位置估計(jì)RMSE隨飛行時(shí)間變化曲線Fig.7 Variation curve of localization RMSE with flight time

進(jìn)一步地,為觀察密集布站情況下所提方法的定位性能和穩(wěn)定性,參考文獻(xiàn)[31]中的布站方式,新增一架無人機(jī),無人機(jī)集群分別位于(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。觀察密集布站條件下所提算法的性能。該實(shí)驗(yàn)對(duì)曲線作了如下處理,因?yàn)樗O(shè)站址誤差較小,所以對(duì)站址誤差取了對(duì)數(shù)米,得到更為平滑的曲線，以更好地觀察實(shí)驗(yàn)現(xiàn)象,通過實(shí)驗(yàn)獲得的仿真結(jié)果如圖8所示。

圖8 密集布站下RMSE隨站址誤差變化曲線Fig.8 Variation curve of RMSE with station site error under dense station layout

根據(jù)圖8的仿真結(jié)果,當(dāng)設(shè)置較小的站址誤差且密集布站的條件下,所提的Taylor-PSO-Mod算法定位RMSE仍然最小,基本符合CRLB,說明在多種仿真條件下所提算法性能具備穩(wěn)定性。值得注意的是,圖8中的粒子群算法在站址誤差較大時(shí)定位誤差小于CRLB,這是由于粒子群算法是一種有偏估計(jì),而CRLB是無偏估計(jì)的最小方差,在某些布站條件下,當(dāng)目標(biāo)位置估計(jì)偏差較大時(shí),可能出現(xiàn)定位的RMSE小于CRLB的情況。

4 結(jié) 論

本文主要對(duì)基于無人機(jī)集群的時(shí)差測(cè)量單目標(biāo)定位方法進(jìn)行研究,建立了單目標(biāo)定位模型。作為高機(jī)動(dòng)平臺(tái),無人機(jī)無法得到自身精確位置,需要在經(jīng)典算法的基礎(chǔ)上將站址誤差考慮進(jìn)來。本文重點(diǎn)研究存在站址誤差情況下提高單目標(biāo)定位精度的方法。首先，對(duì)Chan算法和Taylor算法進(jìn)行改進(jìn),得到解析法和迭代法,在存在站址誤差的情況下算法性能有不同程度的提高；然后，提出一種Taylor-PSO-Mod解算方法,仿真實(shí)驗(yàn)證明了這種方法相對(duì)于解析法和迭代法的優(yōu)點(diǎn),其在多種仿真場(chǎng)景下均能達(dá)到較高的定位精度,定位性能穩(wěn)定,同時(shí)還解決了迭代法的初值問題。從圖5各個(gè)算法的表現(xiàn)來看,站址誤差較大時(shí)，其RMSE誤差比性能較好的改進(jìn)的粒子群算法減小了300 m左右。