劉云輝，姚敏，趙敏

(南京航空航天大學(xué) 自動(dòng)化學(xué)院，江蘇 南京 211106)

0 引言

近幾年無(wú)人機(jī)的用途越來(lái)越廣泛，國(guó)內(nèi)出現(xiàn)一批以大疆為首的無(wú)人機(jī)民用公司，這使得無(wú)人機(jī)各方面的技術(shù)得到了快速的發(fā)展。隨著無(wú)人機(jī)技術(shù)的不斷改進(jìn)，無(wú)人機(jī)以其成本低、無(wú)傷亡、體積小的優(yōu)勢(shì)，在未來(lái)戰(zhàn)爭(zhēng)中將扮演著重要的角色，而多無(wú)人機(jī)協(xié)同對(duì)區(qū)域目標(biāo)進(jìn)行偵查和打擊任務(wù)將是未來(lái)無(wú)人機(jī)作戰(zhàn)的主要形式[1-2]。為了提高打擊精度，兼顧自身的隱蔽性，無(wú)源定位被廣泛應(yīng)用。無(wú)源定位的電子偵察系統(tǒng)本身不發(fā)射電磁波，工作完全是被動(dòng)的，因此具有很好的隱蔽性。目前，無(wú)源定位主要的方法有測(cè)向定位、時(shí)差定位以及測(cè)向時(shí)差定位[3-5]等。其中測(cè)向定位僅需要2個(gè)觀測(cè)站，系統(tǒng)對(duì)時(shí)間的同步要求不高，但其定位誤差較大，在側(cè)邊區(qū)尤為明顯。時(shí)差定位可獲得較高的定位精度，但至少需要4個(gè)觀測(cè)站，且對(duì)各站的時(shí)間同步要求較高。測(cè)向時(shí)差定位與時(shí)差定位相比，減少了觀測(cè)站的數(shù)量，降低了系統(tǒng)復(fù)雜性；與測(cè)向定位相比，由于綜合利用了目標(biāo)輻射源的方向信息和時(shí)差信息，提高了定位精度[6-7]。本文主要研究如何提高測(cè)向時(shí)差定位的精度。

雙機(jī)測(cè)向時(shí)差定位中，對(duì)目標(biāo)位置進(jìn)行解算，通常的方法是直接解析法[8-9]。由于此方法存在定位模糊問(wèn)題，文獻(xiàn)[7]提出了一種利用余弦定理，適用于三維定位情況的解析算法。但是文獻(xiàn)[7]中給出的定位算法存在缺陷，沒(méi)有對(duì)全方位目標(biāo)的定位精度進(jìn)行分析。

為了消除全方位的目標(biāo)定位模糊問(wèn)題，本文在文獻(xiàn)[7]基礎(chǔ)上，提出了一種改進(jìn)的雙機(jī)測(cè)向時(shí)差定位算法消除定位模糊問(wèn)題。為了進(jìn)一步提高多機(jī)協(xié)同定位精度，利用跟蹤濾波的思路，將主觀測(cè)站當(dāng)前時(shí)刻及前一時(shí)刻的時(shí)差和方向觀測(cè)量進(jìn)行融合，采用濾波算法進(jìn)行目標(biāo)位置估計(jì)[10-11]。由于時(shí)差和方向觀測(cè)量都是非線性函數(shù)，因此采用了非線性濾波。目前常用的多是擴(kuò)展卡爾曼濾波(EKF)，它的基本思想是將非線性的量測(cè)方程在狀態(tài)預(yù)測(cè)處進(jìn)行泰勒展開(kāi)并取其一階近似值，對(duì)其線性化濾波后再利用卡爾曼濾波方法進(jìn)行濾波[12-14]。由于其進(jìn)行了線性化處理，就會(huì)產(chǎn)生較大的誤差。近年來(lái)發(fā)展起來(lái)的粒子濾波(PF)算法是一種很好的解決非線性問(wèn)題的算法[15-16]。

本文在現(xiàn)有文獻(xiàn)的基礎(chǔ)上，改進(jìn)測(cè)向時(shí)差定位方法。仿真結(jié)果表明，本文算法能夠很好地提高全局目標(biāo)定位的定位精度，并比較了EKF和PF兩種濾波算法對(duì)定位精度的改善效果。

1 雙機(jī)測(cè)向時(shí)差定位原理及系統(tǒng)模型

雙機(jī)測(cè)向時(shí)差目標(biāo)定位系統(tǒng)由兩個(gè)無(wú)人機(jī)組成，其中一個(gè)作為主站而另一個(gè)作為輔站。主站和輔站均可測(cè)得目標(biāo)相對(duì)于無(wú)人機(jī)觀測(cè)站的方位角φ和俯仰角ε，同時(shí)主站還可測(cè)得目標(biāo)信號(hào)到達(dá)兩站的時(shí)間差Δt。根據(jù)測(cè)的時(shí)間差Δt可計(jì)算得出目標(biāo)到達(dá)兩站的距離差Δr。如圖1所示，設(shè)主站、輔站和目標(biāo)在大地直角坐標(biāo)系下的位置分別為S0(x0,y0,z0)、S1(x1,y1,z1)、S(x,y,z)。則主站和輔站測(cè)得的方位角φi、俯仰角εi(i=0、1)和兩站距離差Δr可表示為：

(1)

(2)

(3)

傳統(tǒng)的目標(biāo)位置求解方法是根據(jù)測(cè)得的方位角φi、俯仰角εi和兩站距離差Δr，聯(lián)立式(1)、式(2)、式(3)，先將r0看作已知量解算出目標(biāo)位置S(x,y,z)，再將S(x,y,z)帶入式(3)中求解r0，再根據(jù)r0計(jì)算出S(x,y,z)的值。由于關(guān)于r0的方程是二次方程，所以r0的解不唯一會(huì)造成目標(biāo)位置定位模糊。為了克服這一問(wèn)題，下節(jié)給出通過(guò)坐標(biāo)變換的三維定位無(wú)模糊解析算法，并且利用觀測(cè)站測(cè)量的冗余信息，在式(3)中去掉絕對(duì)值號(hào)，從而得到全方位上無(wú)模糊的目標(biāo)位置。

2 利用坐標(biāo)變換的雙機(jī)測(cè)向時(shí)差定位解算算法

通過(guò)坐標(biāo)變換的三維定位無(wú)模糊解析算法是先在以主站為原點(diǎn)的坐標(biāo)系下解得此坐標(biāo)系下目標(biāo)的位置，再變換到大地直角坐標(biāo)系下。如圖2所示，建立以主站為坐標(biāo)原點(diǎn)的坐標(biāo)系S0xyz，它是通過(guò)將坐標(biāo)系(oxyz)平移得到的，各坐標(biāo)軸與坐標(biāo)系(oxyz)各坐標(biāo)軸平行。由圖2可知，通過(guò)求出主站到輻射源的距離r0，就可求得坐標(biāo)系S0xyz下目標(biāo)輻射源的位置坐標(biāo)S′(xs,ys,zs)，進(jìn)而得到目標(biāo)在大地直角坐標(biāo)系下的坐標(biāo)S(x,y,z)，目標(biāo)位置解算的解析式如下：

(4)

圖2 以主站為原點(diǎn)的坐標(biāo)系下觀測(cè)站與目標(biāo)位置關(guān)系

根據(jù)式(4)，只要求出r0，就可得到目標(biāo)的位置坐標(biāo)。文獻(xiàn)[7]給出了基于余弦定位的r0解算方法，該方法中用到Δr=r1-r0并直接將其變換為r1=Δr+r0，實(shí)際上Δr=|r1-r0|，要利用測(cè)量的冗余信息判別r0和r1的大小后再進(jìn)行等價(jià)代換。否則解算方法只有當(dāng)r1-r0>0時(shí)才能進(jìn)行目標(biāo)位置的解算，不能進(jìn)行全方位上的目標(biāo)定位。下面給出適用于目標(biāo)在觀測(cè)站全方位上時(shí)r0的求解。

坐標(biāo)系S0xyz下，目標(biāo)的位置坐標(biāo)為S′(xs,ys,zs)，輔站的坐標(biāo)為S1′(x1s,y1s,z1s)。根據(jù)oxyz和S0xyz兩坐標(biāo)系的變換關(guān)系可得目標(biāo)和輔站在兩坐標(biāo)系下的位置對(duì)應(yīng)關(guān)系為:

(5)

坐標(biāo)系S0xyz下，主站與目標(biāo)的距離和輔站與目標(biāo)的距離分別為:

(6)

由Δr=|r1-r0|，當(dāng)r1-r0>0時(shí)可推出：

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

(7)

根據(jù)式(6)可知：

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

(8)

聯(lián)立式(4)、式(7)、式(8)可得：

(9)

聯(lián)立式(5)、式(9)得：

(10)

當(dāng)r1-r0<0時(shí)，Δr=r0-r1可得：

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

(11)

此時(shí)，可得：

(12)

在大地直角坐標(biāo)系下，通過(guò)合理的布站，使得兩無(wú)人機(jī)在同一高度上，根據(jù)式(2)比較兩無(wú)人機(jī)測(cè)得的目標(biāo)輻射源的俯仰角εi即可得出r1-r0的正負(fù)。然后，根據(jù)r1-r0的正負(fù)選擇式(11)、式(12)帶入式(4)中，得到目標(biāo)輻射源的位置坐標(biāo)S(x,y,z)。

3 基于粒子濾波的TDOA/AOA數(shù)據(jù)融合定位算法

時(shí)差和測(cè)向數(shù)據(jù)的測(cè)量精度很難提高，在不能滿足高精度定位時(shí)，可以對(duì)目標(biāo)同一位置進(jìn)行多次測(cè)量，通過(guò)對(duì)測(cè)量結(jié)果進(jìn)行關(guān)聯(lián)處理，以提高定位精度。首先對(duì)目標(biāo)位置進(jìn)行多次連續(xù)觀測(cè)，將首次測(cè)量的觀測(cè)數(shù)據(jù)，采用上一節(jié)中的算法得到目標(biāo)的初始位置估計(jì)值及相應(yīng)的零均值估計(jì)誤差的協(xié)方差矩陣，作為濾波算法的初始估計(jì)值，然后再利用濾波算法對(duì)多次觀測(cè)值進(jìn)行處理，從而得到更為精確的目標(biāo)估計(jì)值。由于時(shí)差和方向觀測(cè)量都是非線性函數(shù)，為了充分利用觀測(cè)站獲得的信息，解決傳統(tǒng)的EKF算法在非線性估計(jì)時(shí)存在誤差較大的問(wèn)題，本文采用粒子濾波對(duì)觀測(cè)數(shù)據(jù)進(jìn)行處理。

粒子濾波的突出特點(diǎn)是適用于任意非線性、非高斯系統(tǒng)的濾波問(wèn)題，擺脫了解決非線性濾波問(wèn)題時(shí)隨機(jī)量必須滿足高斯分布的制約。而雙機(jī)測(cè)向時(shí)差定位中的測(cè)向誤差和測(cè)時(shí)誤差是由多種因素造成的。其中測(cè)時(shí)誤差主要包括兩觀測(cè)站時(shí)間基準(zhǔn)的對(duì)準(zhǔn)誤差和觀測(cè)站測(cè)量目標(biāo)信號(hào)到達(dá)時(shí)間的誤差，同時(shí)測(cè)向和時(shí)差的觀測(cè)還受到環(huán)境因素引起的隨機(jī)誤差的影響。因此在實(shí)際中的系統(tǒng)的狀態(tài)空間模型很難滿足高斯假設(shè)，這時(shí)粒子濾波將可以很好地解決這一問(wèn)題。

根據(jù)式(1)、式(2)、式(3)，建立雙機(jī)測(cè)向時(shí)差定位模型的狀態(tài)方程為：

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

(13)

測(cè)量方程為：

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

(14)

式中，hk(X(k))=[Δrk,φk,εk]T；νk為均值為0、方差為σ2的高斯觀測(cè)噪聲。

粒子濾波主要有以下3個(gè)步驟：產(chǎn)生粒子、計(jì)算粒子權(quán)重和重要性采樣。針對(duì)貫序重要性采樣算法存在的權(quán)值退化現(xiàn)象，本文采用重采樣的粒子濾波算法，該算法在每步迭代過(guò)程中，根據(jù)粒子權(quán)值對(duì)離散粒子進(jìn)行重采樣，重采樣方法舍棄權(quán)值較小的粒子，代之以權(quán)值較大的粒子。

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

祖母的故事幫助賽利亞在民族文化身份中找準(zhǔn)定位，賽利亞家族的歷史與墨西哥和美國(guó)的歷史緊密相連。芝加哥是賽利亞家族史的一個(gè)重要組成部分。她的祖父納西索曾在芝加哥待了七年來(lái)逃避墨西哥革命戰(zhàn)亂；墨西哥內(nèi)戰(zhàn)引發(fā)的混亂和美國(guó)新政所提供的大量的機(jī)會(huì)使得賽利亞父親那一輩的墨西哥人離開(kāi)故土，在芝加哥生存和發(fā)展；賽利亞這一代在芝加哥出生成長(zhǎng)。

6) 得到k時(shí)刻目標(biāo)估計(jì)值：

7) 令k=k+1,重復(fù)第二步。

其中初始化是將首次測(cè)量的觀測(cè)數(shù)據(jù)，采用上一節(jié)中的算法得到目標(biāo)的初始位置估計(jì)值及相應(yīng)的零均值估計(jì)誤差的協(xié)方差矩陣，從而得到初始化粒子。

4 仿真實(shí)驗(yàn)分析

為了驗(yàn)證第一節(jié)提出的目標(biāo)定位解算方法的有效性，將其定位誤差與理論定位誤差的GDOP分布進(jìn)行對(duì)比?；谧鴺?biāo)變換的側(cè)向時(shí)差定位解算方法，其定位誤差的GDOP通過(guò)多次蒙特卡洛得到，即：

仿真參數(shù)設(shè)定如下：

測(cè)角誤差σφ0=σε0=3mrad，站址誤差σs=10m，測(cè)時(shí)誤差σt=50ns，主站坐標(biāo)(-20,0,10)km,輔站坐標(biāo)(20,0,10)km，目標(biāo)高度z=0km，觀測(cè)區(qū)域在x,y方向均為-60km～60km，蒙特卡洛仿真次數(shù)N=100。

圖3-圖4分別為理論定位誤差和基于坐標(biāo)變換解算的定位誤差的GDOP分布圖，其觀測(cè)區(qū)域?yàn)閤∈[-60 60]km，y∈[-60 60]km，z=0km內(nèi)。這兩幅分布圖反映的是在指定觀測(cè)區(qū)域內(nèi)誤差的等高圖，圖中的每條曲線是指對(duì)應(yīng)誤差值分布的位置。從兩幅分布圖中可看出，分布曲線基本相同，在兩觀測(cè)站法線方向分布基本一致，而在靠近兩觀測(cè)站基線方向上本文提出的基于坐標(biāo)變換解算方法的定位精度更高。圖5為觀測(cè)區(qū)域x=0km，y∈[-60 60]km，z=0km內(nèi)，第一節(jié)提出的解算算法的定位誤差與理論上的定位誤差對(duì)比曲線圖。從對(duì)比曲線進(jìn)一步反映出，第一節(jié)提出的解算算法的定位誤差與理論上觀測(cè)量存在一定誤差時(shí)出現(xiàn)的定位誤差基本一致。因此，通過(guò)仿真驗(yàn)證了第一節(jié)提出的定位解算算法的有效性。

圖3 理論定位誤差GDOP分布圖

圖4 基于坐標(biāo)變換解算定位誤差GDOP分布圖

圖5 基于坐標(biāo)變換的定位誤差與理論定位誤差對(duì)比

下面通過(guò)仿真實(shí)驗(yàn)驗(yàn)證基于粒子濾波的AOA/TDOA數(shù)據(jù)融合定位方法對(duì)目標(biāo)定位精度的改善效果。對(duì)固定目標(biāo)進(jìn)行多次測(cè)向、時(shí)差數(shù)據(jù)采集，對(duì)這些測(cè)量數(shù)據(jù)進(jìn)行粒子濾波處理得到目標(biāo)的估計(jì)值。因此，蒙特卡洛仿真的定位精度可表示為:

式中:(x,y,z)為目標(biāo)的真實(shí)值；(xpf,ypf,zpf)為每次粒子濾波仿真得到的PF估計(jì)值。

仿真參數(shù)設(shè)置如下：

測(cè)角誤差σφ0=σε0=3mrad，站址誤差σs=5m，測(cè)時(shí)誤差σt=20ns，主站坐標(biāo)(-20,0,10)km,輔站坐標(biāo)(20,0,10)km，固定目標(biāo)坐標(biāo)(60,60,0)km，進(jìn)行200次測(cè)向、時(shí)差數(shù)據(jù)采集。

圖6是基于EKF與PF兩種方法對(duì)定位誤差改善的對(duì)比曲線圖，其中最上面一條曲線是根據(jù)每次時(shí)差和方向角的測(cè)量值帶入到第一節(jié)的解算算法中得到的目標(biāo)位置與目標(biāo)真實(shí)位置之間的誤差，下面兩條曲線是分別用PF和EKF對(duì)觀測(cè)數(shù)據(jù)融合后得到的定位誤差。從圖中可看出，對(duì)于將測(cè)量值直接帶入到位置解算方程進(jìn)行解算，由于測(cè)量值本身存在測(cè)量誤差，因此解算得到的目標(biāo)位置與目標(biāo)真實(shí)位置之間也會(huì)存在誤差。而通過(guò)濾波算法與定位解算算法進(jìn)行結(jié)合，對(duì)得到的一組測(cè)量值進(jìn)行融合處理。由圖6可以看出經(jīng)過(guò)濾波算法融合后的定位誤差逐漸收斂，大大提高了定位精度。同時(shí)，通過(guò)EKF與PF分別對(duì)定位解算算法進(jìn)行改進(jìn)，由圖6可看出，EKF對(duì)定位誤差的改善整體上趨于收斂，但收斂過(guò)程中振蕩較為嚴(yán)重。而PF對(duì)定位誤差進(jìn)行改善時(shí)，其收斂速度比EKF更快，而且PF對(duì)定位精度的改善最終趨于穩(wěn)定，收斂過(guò)程比EKF要更加穩(wěn)定。

圖6 EKF與PF定位誤差收斂曲線對(duì)比

5 結(jié)語(yǔ)

本文通過(guò)改進(jìn)測(cè)向時(shí)差定位的解算方法，使三維目標(biāo)在全方位上都能進(jìn)行無(wú)模糊的位置解算，仿真驗(yàn)證了改進(jìn)算法的有效性。同時(shí)，提出對(duì)目標(biāo)在同一位置進(jìn)行多次測(cè)量，利用濾波算法對(duì)多次測(cè)量值進(jìn)行融合處理來(lái)提高定位精度。通過(guò)仿真對(duì)比可知，經(jīng)過(guò)對(duì)測(cè)量數(shù)據(jù)的濾波融合大大提高了定位精度，同時(shí)對(duì)比了EKF和PF的定位誤差收斂曲線，可看出PF更加有效地提高了定位精度。