雙機(jī)協(xié)同無(wú)源定位算法

康 旭，王德江，張 濤

（1.中國(guó)科學(xué)院長(zhǎng)春光學(xué)精密機(jī)械與物理研究所航空光學(xué)成像與測(cè)量重點(diǎn)實(shí)驗(yàn)室，吉林 長(zhǎng)春 130033；2.中國(guó)科學(xué)院大學(xué)，北京 100049）

隨著網(wǎng)絡(luò)技術(shù)的發(fā)展，分布式被動(dòng)探測(cè)系統(tǒng)、無(wú)線傳感器網(wǎng)絡(luò)等探測(cè)體系已在軍事領(lǐng)域得到廣泛應(yīng)用。與其相關(guān)的多站信息融合和聯(lián)合定位目標(biāo)問(wèn)題也得到了深入研究。分布式無(wú)源定位系統(tǒng)利用載機(jī)觀測(cè)到目標(biāo)的到達(dá)角（Angle of Arrival，AOA）來(lái)估計(jì)目標(biāo)位置[1-8]。雙機(jī)協(xié)同能在較廣泛的區(qū)域內(nèi)利用傳感器獲得目標(biāo)方位數(shù)據(jù)，實(shí)現(xiàn)對(duì)目標(biāo)的定位。角度測(cè)量值是目標(biāo)位置的非線性和非凸函數(shù)，不能直接求解；且測(cè)量值受噪聲干擾，對(duì)目標(biāo)的定位精度會(huì)產(chǎn)生很大影響?？梢岳媒y(tǒng)計(jì)學(xué)知識(shí)對(duì)定位誤差進(jìn)行分析，研究出更有效、魯棒性更強(qiáng)、定位精度更高的定位算法。

無(wú)源定位過(guò)程中，獲得目標(biāo)AOA 測(cè)量值的常用方法是在載機(jī)上使用紅外探測(cè)系統(tǒng)或天線陣列。目前，利用AOA 測(cè)量值進(jìn)行目標(biāo)定位已經(jīng)得到廣泛研究。基于AOA 測(cè)量，Stansfield 首先推導(dǎo)了1 種基于AOA 的單站定位方法[9]；Nardone 等建立了基于極大似然原理的估計(jì)方法（Maximum Likelihood，ML），提出了理想估計(jì)器的概念，并給出了克拉美羅下界（Cramer-Rao Lower Bound，CRLB）的估算方法[10]；Torrieri等提出了協(xié)同定位的經(jīng)典極大似然估計(jì)方法[11]；Motti等對(duì)比分析了最大似然估計(jì)器與Stansfield 估計(jì)器的性能[12]；Allen 等提出1 種偽線性估計(jì)算法（Pseudolinear Estimation，PLE），并給出最小二乘解，但存在偏差的問(wèn)題[13]；Kutluyil發(fā)展了偏差減小的偽線性估計(jì)（Bias Reduced Pseudolinear Estimation，BRPLE）并建立了1 種漸進(jìn)無(wú)偏的加權(quán)工具變量估計(jì)器[14]；Alba 等提出了純方位目標(biāo)定位的最小二乘算法（Least Squares，LS），并給出其封閉形式的解[15]；Sohn等利用三角測(cè)量對(duì)目標(biāo)進(jìn)行定位[16]；Sharma等假設(shè)目標(biāo)高度為0，對(duì)地面運(yùn)動(dòng)目標(biāo)進(jìn)行定位，并通過(guò)擴(kuò)展卡爾曼濾波提高定位精度[17]；Cheng 等通過(guò)兩點(diǎn)相交定位對(duì)地面目標(biāo)進(jìn)行定位，并使用最小二乘迭代提高定位精度[18]。

以上方法針對(duì)的是地面目標(biāo)的定位，并不能用于遠(yuǎn)距離空中目標(biāo)。Bai 等提出1 種改進(jìn)的交叉定位算法，提高對(duì)遠(yuǎn)距離空中目標(biāo)的定位精度[19]；Sun等給出了1種基于改進(jìn)極坐標(biāo)系的純方位定位算法的特征向量解[20]。

上述基于AOA 測(cè)量的算法研究，讓我們更加看好雙機(jī)協(xié)同定位的發(fā)展前景，但針對(duì)不同場(chǎng)景如何選擇定位算法仍困擾著我們。本文從基于最小二乘法的定位算法和基于最大似然估計(jì)理論的定位算法兩方面來(lái)分析目標(biāo)定位理論，并使用MATLAB對(duì)不同誤差條件下的目標(biāo)位置估計(jì)的均方根誤差（Root Mean Square Error，RMSE）進(jìn)行對(duì)比，從多個(gè)角度對(duì)比不同場(chǎng)景下算法的性能。

1 基于純角度信息的雙機(jī)定位模型

如圖1 所示，2 架無(wú)人機(jī)si=[xi,yi,zi]T(i=1,2 )，協(xié)同定位輻射源u=[x0,y0,z0]T。每架載機(jī)觀測(cè)到的AOA與輻射源位置間的關(guān)系如下：

圖1 AOA定位模型Fig.1 AOA localization model

式（1）中，θ0i和?0i分別表示輻射源相對(duì)載機(jī)的方位角和俯仰角真值。

實(shí)際中，AOA測(cè)量值不可避免地受到噪聲干擾：

式（2）中，測(cè)量噪聲nθi和ni?是服從均值為0，方差分別為σ2θ和σ2?的獨(dú)立高斯白噪聲。

2 基于AOA測(cè)量值的定位方法

載機(jī)一般通過(guò)攜帶的GPS確定自身實(shí)時(shí)位置，結(jié)合輻射源的AOA 測(cè)量值通過(guò)三角測(cè)量法估計(jì)輻射源位置。受噪聲干擾的測(cè)量值會(huì)導(dǎo)致估計(jì)誤差，利用統(tǒng)計(jì)學(xué)理論可以有效減小誤差。下面介紹基于最小二乘法和極大似然估計(jì)法的定位方法。

2.1 最小二乘類(lèi)定位方法

對(duì)式中的反正切函數(shù)變形得到輻射源與測(cè)量值的關(guān)系可表示為：

將所有載機(jī)的測(cè)量按式（3）的形式堆疊成矩陣：

2.1.1 普通最小二乘法

普通最小二乘法考慮了受噪聲影響的測(cè)量值對(duì)向量b的影響，通過(guò)忽略高階噪聲項(xiàng)的泰勒展開(kāi)，近似得到向量真值為b0=b-eb，其中，eb表示殘差。

通過(guò)最小化殘差平方和

可得輻射源位置坐標(biāo)為：

2.1.2 加權(quán)最小二乘法

普通最小二乘法認(rèn)為測(cè)量值對(duì)結(jié)果的影響具有一致性，但實(shí)際中，方位角和俯仰角的測(cè)量噪聲方差不同。因此，不同噪聲的測(cè)量值對(duì)結(jié)果的影響權(quán)重不同。若已知噪聲nθi和ni?的統(tǒng)計(jì)分布，可得測(cè)量角噪聲協(xié)方差矩陣為

與普通最小二乘法相似，利用最小化殘差平方和

可得輻射源位置坐標(biāo)為：

2.1.3 總體最小二乘法

如前所述，普通最小二乘法一般認(rèn)為向量b受測(cè)量噪聲影響。但式中A和b都由測(cè)量值得到，應(yīng)等價(jià)看待它們。令A(yù)0和b0分別為觀測(cè)矩陣和觀測(cè)向量真值，通過(guò)泰勒展開(kāi)將觀測(cè)矩陣和觀測(cè)向量真值分別表示為A0=A-E和b0=b-e，其中E和e為誤差項(xiàng)。對(duì)此，同時(shí)引入校正向量Δb和校正矩陣ΔA，對(duì)b和A內(nèi)存在的誤差進(jìn)行補(bǔ)償，以抑制噪聲對(duì)輻射源定位精度的影響，從而實(shí)現(xiàn)有誤差的矩陣方程求解向精確矩陣方程的求解的轉(zhuǎn)換：

此時(shí)，總體最小二乘問(wèn)題可用約束最優(yōu)化問(wèn)題來(lái)表示：

方程解為增廣矩陣[A,b]的最小奇異值對(duì)應(yīng)的右奇異向量，也就是[A,b]T[A,b] 的最小特征值對(duì)應(yīng)的特征向量。

2.2 最大似然類(lèi)定位方法

盡管最大似然估計(jì)受初始值的影響存在局部最優(yōu)的問(wèn)題并具有較大的計(jì)算復(fù)雜度，但由于其漸進(jìn)無(wú)偏性，故常作為性能比較的基準(zhǔn)。在高斯噪聲假設(shè)條件下，方位角和俯仰角測(cè)量的似然函數(shù)是1 個(gè)多元高斯概率密度函數(shù)：

式（14）中，J是最大似然代價(jià)函數(shù)：

2.2.1 高斯-牛頓迭代法（GNIM）

一般使用高斯-牛頓迭代法求解ML 估計(jì)問(wèn)題。對(duì)于沒(méi)有封閉解的非線性最小二乘估計(jì)問(wèn)題式，迭代步驟如下：

式（16）中：u(k)是第k次迭代值，利用簡(jiǎn)單的算法或先驗(yàn)知識(shí)可以得到；e(u)=Ψ-ψ(u)，J(k)是u估計(jì)值為u(k)時(shí)的Ψ(u)的雅可比矩陣

一般按式（16）迭代2～4次即可得最優(yōu)解。

2.2.2 加權(quán)工具變量偽線性估計(jì)法（WIVPLE）

在小噪聲假設(shè)下，采用偽線性估計(jì)使最大似然代價(jià)函數(shù)線性化，線性化過(guò)程利用2 個(gè)二次代價(jià)函數(shù)分離方位角和俯仰角：

式（18）中，uxy=u(1 ∶2 )，uz=u( 3)。

由式（18）我們可以發(fā)現(xiàn)：代價(jià)函數(shù)J1(uxy)只取決于方位角測(cè)量；而代價(jià)函數(shù)J2(uxy,uz)同時(shí)取決于方位角和俯仰角。我們將J的最小化求解過(guò)程分解2 步：首先，通過(guò)最小化J1(uxy)，解得uxy；然后，將其代入J2(uxy,uz)，再通過(guò)最小化J2(uxy,uz)，解得uz。

在小噪聲假設(shè)下，(θi-θi(u) )和(?i-?i(u)) 近似為0，可表示為：

式（19）中，sxy,i=si(1 ∶2 )。

式（15）近似為：

由于式（20）求和項(xiàng)中第1項(xiàng)的分母項(xiàng)未知，將其去除后問(wèn)題簡(jiǎn)化為：

同理解得式（20）求和項(xiàng)中第2項(xiàng)：

由于上述對(duì)數(shù)似然函數(shù)線性化過(guò)程中引入了誤差，影響輻射源位置的估計(jì)精度。線性矩陣等式為，誤差項(xiàng)分別為其估計(jì)偏差主要是由F和η間的相關(guān)性引起，因此，可通過(guò)修改式來(lái)消除：

目標(biāo)位置估計(jì)值為：

式（24）中，工具變量H基于目標(biāo)的初始狀態(tài)參數(shù)估計(jì)構(gòu)造，即是利用目標(biāo)位置估計(jì)值得到的方位角。

2.2.3 克拉美羅界（CRLB）

確定任何無(wú)偏估計(jì)量的方差下限在實(shí)際中非常重要，它揭示了所討論的模型中，估計(jì)結(jié)果的均方根誤差下限或估計(jì)精度上限。根據(jù)似然函數(shù)式（15）有：

此外，可通過(guò)Fisher信息矩陣（FIM）計(jì)算得到：

式（26）中：J0是目標(biāo)在真實(shí)位置處的雅可比矩陣；K是噪聲協(xié)方差矩陣。

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

為直觀比較算法優(yōu)劣，本節(jié)我們利用蒙特卡洛數(shù)值模擬進(jìn)行仿真實(shí)驗(yàn)。目標(biāo)定位精度的仿真結(jié)果用RMSE表示，其計(jì)算方式為:

式（27）中：u是源位置的真值；u?i是第i次仿真得出的u估計(jì)值；L=1 000 是蒙特卡洛實(shí)驗(yàn)次數(shù)。

此外，我們使用CRLB 根作為性能評(píng)估的基準(zhǔn)。ML 是漸近無(wú)偏的，但需要迭代和接近真實(shí)源位置的初始值。在實(shí)際中，目標(biāo)的真實(shí)位置是未知的，在此我們以普通最小二乘法的解作為ML迭代初始值。

場(chǎng)景1：按圖1建立笛卡爾坐標(biāo)系，設(shè)置2種幾何構(gòu)型。固定載機(jī)到目標(biāo)間距都是10 km，其中：較好幾何構(gòu)型設(shè)置目標(biāo)相對(duì)兩載機(jī)的張角為90°；較差幾何構(gòu)型設(shè)置目標(biāo)相對(duì)兩載機(jī)的張角為45°°。較好幾何構(gòu)型中載機(jī)位置分別為s1=(1 0,0,0.8 )km 和s2=( - 10,0,0.8) km，輻射源位置坐標(biāo)( 0,10,1) km；較差幾何構(gòu)型中載機(jī)位置分別為s1=(1 0,0,0.8) km 和s2=( 0,10-10 2,0.8 )km，輻射源位置坐標(biāo)( 0,10,1) km。為了便于分析，本文忽略了載機(jī)的位置誤差，并且令方位角和俯仰角的測(cè)量誤差服從均值為0，標(biāo)準(zhǔn)差分別為2ρ和ρ的高斯分布。如圖2所示，當(dāng)載機(jī)與目標(biāo)間的幾何構(gòu)型不同時(shí)，目標(biāo)的定位精度也不同。

圖2 瞬時(shí)定位仿真結(jié)果Fig.2 Simulation results of instantaneous localization

圖2 a）和b）分別描述了較好幾何構(gòu)型與較差幾何構(gòu)型在ρ的標(biāo)準(zhǔn)差從0.5°°～5°°的范圍內(nèi)，目標(biāo)位置的定位精度變化趨勢(shì)。

顯然，較好幾何構(gòu)型可大大提升定位性能。在小誤差范圍內(nèi)，這些算法性能都能接近CRLB，且定位精度相差不多，隨著角度測(cè)量誤差增加，目標(biāo)的定位精度降低。當(dāng)幾何構(gòu)型較差時(shí)，算法間的優(yōu)劣更加明顯。圖2 b）得出：ML類(lèi)的算法中，以普通最小二乘法估計(jì)結(jié)果作為初值的高斯-牛頓迭代法定位精度最高，但是需要迭代，其計(jì)算復(fù)雜度較高；加權(quán)工具變量偽線性估計(jì)不需要迭代計(jì)算，但定位精度較低；LS 類(lèi)算法中，加權(quán)最小二乘法考慮了不同測(cè)量值的權(quán)重，定位性能最高，且LS類(lèi)算法計(jì)算復(fù)雜度相對(duì)ML類(lèi)算法具有天然的優(yōu)勢(shì)。因此，我們應(yīng)該根據(jù)定位性能和計(jì)算復(fù)雜度的需求選擇合適的算法。

下面對(duì)載機(jī)對(duì)目標(biāo)持續(xù)觀測(cè)的場(chǎng)景下進(jìn)行仿真。

場(chǎng)景2：在場(chǎng)景1 的前提下，設(shè)置載機(jī)s1速度為( 0.24,0.24,0 )km/s；載機(jī)s2速度為( 0.24,0.24,0 )km/s。角度傳感器采樣頻率為50 Hz，令載機(jī)對(duì)目標(biāo)連續(xù)觀測(cè)1.5 s，其余參數(shù)設(shè)置同場(chǎng)景1。

如圖3所示，當(dāng)載機(jī)與目標(biāo)間的幾何構(gòu)型不同時(shí)，目標(biāo)的定位精度不同。

圖3 持續(xù)觀測(cè)定位仿真結(jié)果Fig.3 Simulation results of continuous observation and localization

與圖2相比，由于可利用測(cè)量值增多，目標(biāo)定位精度增加。圖3 a）和b）分別描述了較好幾何構(gòu)型與較差幾何構(gòu)型在ρ的標(biāo)準(zhǔn)差從0.5°°～5°°的范圍內(nèi)，目標(biāo)位置的定位精度變化趨勢(shì)。

顯然，較好幾何構(gòu)型可大大提升定位性能。與瞬時(shí)定位不同，較好幾何構(gòu)型的持續(xù)觀測(cè)場(chǎng)景中，ML類(lèi)算法的定位性能降低，且其計(jì)算復(fù)雜度隨著測(cè)量值數(shù)量增加而增大；但較差幾何構(gòu)型中，加權(quán)工具變量偽線性估計(jì)在測(cè)量噪聲較大時(shí)具有最佳的定位性能。LS類(lèi)算法始終具備較好的定位性能。此時(shí)，我們應(yīng)該根據(jù)需求選擇合適的定位算法，但LS 類(lèi)算法具有較大優(yōu)勢(shì)。持續(xù)觀測(cè)的目標(biāo)定位精度變化趨勢(shì)與瞬時(shí)目標(biāo)定位精度相同，都隨著誤差增加而降低；但其定位精度明顯高于瞬時(shí)定位，因?yàn)檩d機(jī)觀測(cè)時(shí)間增加，對(duì)目標(biāo)的觀測(cè)信息增加，目標(biāo)位置的定位精度顯然增加。由圖3可知，隨著角度誤差增大，ML類(lèi)算法估計(jì)目標(biāo)位置的精度逐漸優(yōu)于LS類(lèi)算法估計(jì)目標(biāo)位置的精度；大多數(shù)情況下，偽線性估計(jì)目標(biāo)位置的精度最低，但當(dāng)載機(jī)與目標(biāo)間的幾何構(gòu)型較差且測(cè)角誤差較大時(shí)，其對(duì)目標(biāo)位置的定位精度最高。因此，應(yīng)根據(jù)實(shí)際需求選擇合適的算法。

4 結(jié)論

本文介紹了幾種基于AOA 的無(wú)源目標(biāo)定位算法，并使用蒙特卡洛法進(jìn)行仿真實(shí)驗(yàn)對(duì)比。仿真結(jié)果顯示：在不同幾何構(gòu)型和觀測(cè)條件下，隨著測(cè)量噪聲增大，不同算法對(duì)目標(biāo)定位精度和計(jì)算復(fù)雜度不同，且不同算法間精度存在較大差異；應(yīng)該根據(jù)不同的需求，選擇合適的算法。