包含乘性噪聲自適應(yīng)修正的非合作目標(biāo)相對導(dǎo)航算法

朱云峰，孫永榮，趙偉，黃斌，吳玲

南京航空航天大學(xué) 自動化學(xué)院，南京 210016

近年來，隨著無人機(jī)應(yīng)用領(lǐng)域的不斷拓展，其對態(tài)勢感知的要求也越來越高，尤其是對于未知環(huán)境的探索能力，是急需攻克的重點(diǎn)問題。相對導(dǎo)航作為其中的核心技術(shù)之一[1]，在未來的智能物流、集群作戰(zhàn)[2]、搜索救援和編隊飛行[3]等多種場合都至關(guān)重要。因此，實時、高精度、高可靠性的相對狀態(tài)估計算法逐漸成為研究的熱點(diǎn)內(nèi)容[4]。

非合作目標(biāo)的相對導(dǎo)航主要是指在沒有通信條件，或者目標(biāo)上沒有配置主動傳感器的情況下的相對定位問題[5]。合適的傳感器選擇是非常關(guān)鍵的。目前，對于中遠(yuǎn)距離的相對導(dǎo)航，根據(jù)傳感器的測量原理可以分為多種，包括視覺傳感器、雷達(dá)、IRST(Infrared Search and Track)和ESM(Electronic Support Measures)等以波達(dá)角度(Angle of Arrival，AOA)或距離為量測的傳感器[6]，或者以波達(dá)時間(Time of Arrival，TOA)[7]、波達(dá)頻率(Frequency of Arrival，F(xiàn)OA)[8]、波達(dá)時間差(Time Difference of Arrival，TDOA)[9]等為量測值的無線電設(shè)備。本文以視線仰角、視線方位角和無人機(jī)與目標(biāo)之間的直線距離作為傳感器輸出來估計相對狀態(tài)。

對于非線性系統(tǒng)的狀態(tài)估計算法，目前擴(kuò)展卡爾曼濾波(EKF)算法是使用最廣的，但由于其經(jīng)過泰勒展開后取一階線性近似[10-11]，忽略了高階項的非線性特性，在一些強(qiáng)非線性的情況下導(dǎo)致濾波精度嚴(yán)重下降，并且收斂速度緩慢[12]。為了進(jìn)一步改善算法的性能，迭代擴(kuò)展卡爾曼濾波(IEKF)算法發(fā)展起來，并開始得到應(yīng)用。但該算法通過多次使用量測值實現(xiàn)，當(dāng)?shù)踔祷蛘邊f(xié)方差誤差較大時，容易找不到極值點(diǎn)，明顯降低了濾波器的精度[13]。同時，對于非合作目標(biāo)相對導(dǎo)航系統(tǒng)而言，距離的測量往往與信號特性相關(guān)聯(lián)，測量中的抖動振動造成安裝矩陣與基準(zhǔn)之間的偏轉(zhuǎn)以及信號能量的衰減都會引入乘性誤差[14-15]。但目前的相對導(dǎo)航濾波算法基本都將距離量測噪聲設(shè)定為加性噪聲，沒有對因相對距離的增加而引入的乘性量測噪聲進(jìn)行修正，如果只是簡單地以傳感器的標(biāo)稱值或?qū)嶒炇覝y定值設(shè)置濾波中的噪聲陣，必將導(dǎo)致濾波精度的污染[16]，造成仿真實驗和實際的不一致性。

鑒于上述的問題，本文通過分析IEKF算法中的迭代過程，利用LM(Levenberg-Marguardt)優(yōu)化的思想進(jìn)行改進(jìn)，提出了一種LM-IEKF(Levenberg-Marquardt Iterative Extended Kalman Filter)算法，考慮到乘性噪聲情況下的精度下降問題，又進(jìn)一步提出了包含有量測噪聲自適應(yīng)修正的Modified LM-IEKF算法。最后，通過對比分析驗證了本文算法的有效性。

1 非合作目標(biāo)相對導(dǎo)航系統(tǒng)模型

非合作目標(biāo)的相對導(dǎo)航是通過無人機(jī)對目標(biāo)進(jìn)行測角測距來實現(xiàn)的，無人機(jī)與目標(biāo)之間的位置關(guān)系如圖1所示，xyz坐標(biāo)系為地心空間直角坐標(biāo)系，ρ為無人機(jī)測得的目標(biāo)距離,β和α分別為無人機(jī)對目標(biāo)探測得到的視線仰角和視線方位角(本文中規(guī)定視線方位角為與x軸正方向的夾角)。

圖1 相對導(dǎo)航原理示意圖Fig.1 Schematic diagram of relative navigation

根據(jù)定位原理，可以得到空間幾何模型。將無人機(jī)的絕對位置坐標(biāo)記為Xu=[xu,yu,zu]T，目標(biāo)的絕對位置坐標(biāo)記為Xt=[xt,yt,zt]T。相對導(dǎo)航的目的就是利用無人機(jī)上配置的傳感器測量得到距離和角度信息[ρ,α,β]，計算出目標(biāo)與無人機(jī)間的相對位置ΔX=Xt-Xu=[Δx,Δy,Δz]T。由圖2可以得到無人機(jī)與目標(biāo)之間的空間幾何關(guān)系為

(1)

定義狀態(tài)變量為x= [ΔX, ΔX′, ΔX″]T，其中ΔX′和ΔX″分別為無人機(jī)與目標(biāo)之間的相對速度和相對加速度。由式(1)可以建立相對導(dǎo)航系統(tǒng)的量測方程為

(2)

圖2 相對導(dǎo)航幾何空間模型示意圖Fig.2 Relative navigation geometric space model

(3)

式中：

選擇勻加速(CA)模型作為相對運(yùn)動模型，離散化狀態(tài)方程可表示為

xk=Φk|k-1xk-1+Γk-1wk-1

(4)

式中：狀態(tài)轉(zhuǎn)移矩陣Φk|k-1和噪聲輸入矩陣Γk-1分別為

(5)

2 基于Levenberg-Marquardt優(yōu)化的迭代卡爾曼濾波算法

基于上述的相對導(dǎo)航數(shù)學(xué)模型，本節(jié)分析了IEKF算法原理，并引入Levenberg-Marquardt優(yōu)化的思想對其進(jìn)行改進(jìn)，進(jìn)而提出了一種提高濾波穩(wěn)定性的LM-IEKF算法。

2.1 現(xiàn)有的IEKF算法

傳統(tǒng)的EKF算法可以一定程度地解決非線性濾波問題，但由于其在使用泰勒階數(shù)展開時，舍去了高階項，所以濾波過程中產(chǎn)生了截斷誤差。尤其當(dāng)系統(tǒng)模型存在較大的誤差時，經(jīng)非線性函數(shù)放大之后可能會導(dǎo)致濾波結(jié)果偏差很明顯并出現(xiàn)濾波不穩(wěn)定的情況，在實際情況中甚至?xí)霈F(xiàn)濾波發(fā)散。

IEKF算法是EKF的一種改進(jìn)，其以狀態(tài)估計值為依據(jù)，針對觀測矩陣的計算，通過設(shè)置迭代次數(shù)進(jìn)行多次更新，反復(fù)使用觀測數(shù)據(jù)，可以有效地改善狀態(tài)估計的精度[10,12,13]。

算法在k時刻的第次迭代流程如下：

2) 求雅克比矩陣Hk,i：

(6)

3) 求增益陣Kk,i：

Kk,i=Pk|k-1(Hk,i)T(Hk,iPk|k-1(Hk,i)T+Rk)-1

(7)

(8)

5) 協(xié)方差陣遞推Pk|k,i+1：

Pk|k,i+1=(I9-Kk,iHk,i)TPk|k-1

(9)

由上述步驟可以看出，當(dāng)?shù)螖?shù)為1的時候，IEKF和EKF是等價的。

2.2 LM-IEKF算法

利用數(shù)學(xué)方法可以證明IEKF在本質(zhì)上是和高斯牛頓迭代法等價的[17]，可以看作是高斯牛頓迭代在最大似然估計意義上的近似解?？紤]到高斯牛頓在非線性迭代中的表現(xiàn)特點(diǎn)，IEKF在過程噪聲或測量噪聲過大、運(yùn)動模型不準(zhǔn)確、較大的初始誤差等多種情況下，容易出現(xiàn)不穩(wěn)定[10,18]。據(jù)此，本文借鑒了LM方法優(yōu)化高斯牛頓迭代的思想，對IEKF進(jìn)行改進(jìn)，以提高算法的穩(wěn)定性，實現(xiàn)全局的最優(yōu)估計。

2.2.1 狀態(tài)更新方程的推導(dǎo)

(10)

為了簡便，將當(dāng)前時刻的觀測量和狀態(tài)估計值組成一個增廣的觀測向量，同理觀測矩陣也對應(yīng)的進(jìn)行改寫：

Z=[z,x0]T，g(x)=[h(x),x]T

(11)

由式(10)可得

(12)

對于高斯牛頓法來說，迭代公式表示為

xi+1=xi-(r′(xi)Tr′(xi))-1r′(xi)Tr(xi)

(13)

基于LM優(yōu)化的思想[19-20]，將式(13)改進(jìn)為

xi+1=xi-(r′(xi)Tr′(xi)+μI)-1r′(xi)Tr(xi)

(14)

式中：μ為阻尼因子，決定了迭代的方向和步長。

定義：r(x)=S(Z-g(x))

(15)

r′(x)=-Sg′(x)

(16)

其中：

(17)

將式(15)、式(16)代入式(14)中，推導(dǎo)出在k時刻的狀態(tài)變量第i次迭代公式為

xi+1=xi+(g′(xi)TSTSg′(xi)+μI9)-1·

g′(xi)TSTS(Z-g(xi))=xi+

(18)

根據(jù)式(17)、式(18)可以進(jìn)一步推導(dǎo)為

(19)

2.2.2 協(xié)方差陣遞推公式的推導(dǎo)

根據(jù)式(12)的定義，用最大似然函數(shù)L(x)表示Z關(guān)于變量x的概率密度：

(20)

(21)

(22)

(23)

由線性化近似：

(24)

令V=Z-g(x),由式(12)可知,V～(012×1,Q)。

(25)

(26)

G*TQ-1E(VVT)Q-1G*(G*TQ-1G*)-1=

(G*TQ-1G*)-1

(27)

假設(shè)第k時刻，當(dāng)i=n時迭代結(jié)束，由式(17) 和式(27)，可得第k時刻迭代結(jié)束后，進(jìn)入到下一時刻k+1的協(xié)方差更新公式：

(28)

2.2.3 LM-IEKF算法流程推導(dǎo)

基于2.2.2節(jié)關(guān)于LM-IEKF算法中的狀態(tài)更新方程以及基于最大似然估計的協(xié)方差陣遞推公式的研究。本節(jié)推導(dǎo)了算法的整體流程。

1) 設(shè)置濾波器的初值xk0、Pk0。

(29)

3) 求一步預(yù)測的均方差陣Pk|k-1：

Pk|k-1=Φk-1Pk-1Φk-1T+Qk-1

(30)

4) 設(shè)置迭代初值：

(31)

5) 求量測陣的雅克比矩陣Hk,i：

(32)

(33)

7) 求協(xié)方差陣的遞推值Pk|k,i+1：

(34)

8) 判斷終止條件

3 包含乘性噪聲自適應(yīng)修正的Modified LM-IEKF算法

當(dāng)實際環(huán)境中包含多種非線性的畸變、信號能量的衰減、運(yùn)行過程中的抖動等情況時，都會不可避免地引入乘性噪聲。本文假設(shè)傳感器測得的距離值被污染，量測噪聲不僅包含有加性噪聲，也包含有與距離相關(guān)的乘性噪聲，并且距離的量測誤差假定與傳感器至目標(biāo)之間的距離成線性關(guān)系。因此，量測矩陣可寫為

(35)

針對與距離相關(guān)的乘性噪聲，通過儲存一段時間Tm內(nèi)的n個量測噪聲殘差，并計算量測噪聲統(tǒng)計的估計值。自適應(yīng)修正方法在濾波的算法流程中，一方面對未知的噪聲統(tǒng)計參數(shù)進(jìn)行估計，另一方面也利用量測修正狀態(tài)變量的預(yù)測值。

量測噪聲的樣本方差陣為

(36)

樣本方差陣的期望值為

(37)

把式(35)代入式(36)，可以得到量測噪聲協(xié)方差矩陣的估計值為

(38)

(39)

4 實驗與分析

為了驗證本文提出的LM-IEKF算法和Modified LM-IEKF算法的有效性，本文利用MATLAB采用蒙特卡羅方法，在僅含有加性噪聲和包含乘性噪聲兩種情況下展開仿真與分析，并與EKF和IEKF算法進(jìn)行了比較。

4.1 仿真條件

設(shè)置仿真時間為600 s。設(shè)置傳感器的精度(3σ)為[30 m, 2 mrad, 2 mrad]，更新頻率為2 Hz。設(shè)置目標(biāo)的起始位置為[118.300°， 31.0°， 8 000.0 m]，以100 m/s的速度朝北向勻速直線運(yùn)動。設(shè)置無人機(jī)的起始位置為[118.30°, 31.290°, 6 250.0 m]，初始姿態(tài)角為[0°, 0°, 0°]，初始速度為[0, 100 ms, ]。無人機(jī)的軌跡參數(shù)見表1。無人機(jī)與目標(biāo)的軌跡模擬由遠(yuǎn)及近的會合過程，且無人機(jī)包含有平飛、爬升和轉(zhuǎn)彎等機(jī)動過程。本文以MAE(Mean Absolute Error)和RMSE(Root Mean Square Error)來評價算法的精度情況。圖3為以目標(biāo)的起始位置為原點(diǎn)的相對位置示意圖。圖4為仿真設(shè)定的無人機(jī)與目標(biāo)之間的相對位置和相對速度(vx,vy,vz)變化的曲線圖。

表1 無人機(jī)的軌跡參數(shù)Table 1 Trajectory parameter of UAV

圖3 無人機(jī)與目標(biāo)的相對位置示意圖Fig.3 Diagram of relative position between UAV and target

圖4 無人機(jī)與目標(biāo)之間的相對位置和速度關(guān)系變化曲線Fig.4 Variation of relative position and velocity between UAV and target

4.2 僅含加性噪聲情況下的仿真與分析

本節(jié)在僅含加性噪聲的情況下，考察算法的精度。采用蒙特卡羅方法統(tǒng)計100次運(yùn)行的結(jié)果，通過對每個時刻的100組數(shù)據(jù)取平均值繪制相對位置的誤差曲線如圖5(a) 所示，對100組數(shù)據(jù)求解RMSE值繪制對比曲線如圖5(b) 所示。由于LM本質(zhì)是一種優(yōu)化迭代過程的數(shù)值方法，可以有效地減少陷入局域極值的幾率，保證了全局的收斂性，因而也具有更好的穩(wěn)定性。從圖5中初始階段的局部放大圖可以看出，在最初始的一段時間內(nèi)，LM-IEKF算法的收斂速度是要介于IEKF和EKF之間的，相較于其他兩種算法，在性能表現(xiàn)上也顯得更為穩(wěn)健。

圖6為相對速度的誤差曲線和RMSE對比曲線。從初始段的局部放大圖中可以看到，LM-IEKF算法具有更快的收斂速度，在5 s以內(nèi)可以進(jìn)入相對穩(wěn)定的狀態(tài)，而另外兩種算法則要在10 s 左右。并且EKF和IEKF算法在初始段會

圖5 僅含加性噪聲情況下的相對位置誤差和RMSE曲線Fig.5 Errors and RMSE curves of relative position with additive noise only

圖6 僅含加性噪聲情況下的相對速度誤差和RMSE曲線Fig.6 Errors and RMSE curves of relative velocity with additive noise only

產(chǎn)生較大的尖峰,相對速度誤差達(dá)到100 m/s，對比可知LM-IEKF算法具有較好的穩(wěn)定性。

表2為3種算法對相對導(dǎo)航狀態(tài)的誤差統(tǒng)計，圖7(a)為根據(jù)表2數(shù)據(jù)繪制的相對位置精度柱狀圖，圖7(b)為相對速度精度柱狀圖。從圖7的柱狀圖可以看出，在相對位置和相對速度方面，LM-IEKF要優(yōu)于其他2種算法。

本文提出的LM-IEKF算法相較于廣泛使用的EKF算法，在x、y、z三軸上，相對位置MAE分別減小了32%、30%和31%，相對速度MAE分別減小了25%、37%和17%。在RMSE方面，綜合相對位置精度提升了20%，綜合相對速度精度提升了46%。

表2 僅含加性噪聲情況下的誤差統(tǒng)計Table 2 Error statistics with additive noise only

圖7 僅含加性噪聲情況的相對位置和速度精度柱狀圖Fig.7 Histogram of relative position and velocity accuracy with additive noise only

4.3 包含乘性噪聲情況下的仿真與分析

進(jìn)一步討論本文提出的自適應(yīng)量測噪聲修正算法對于乘性噪聲的修正效果，考慮到乘性噪聲與無人機(jī)和目標(biāo)直線距離之間的線性關(guān)系，為了更好地反應(yīng)出算法對乘性噪聲由小到大變化過程的適應(yīng)性，乘性噪聲設(shè)置為均值是0.5，方差為4.9×10-3的高斯白噪聲[14-15]。Modified LM-IEKF算法的滑動窗口取10 s內(nèi)的N組數(shù)據(jù)，N的大小取決于迭代的次數(shù)。采用蒙特卡羅方法運(yùn)行100次進(jìn)行統(tǒng)計比較。

從圖4(a)中可以看出，在x軸和y軸方向上，0～400 s之間為高乘性噪聲段，之后隨著無人機(jī)與目標(biāo)之間距離的逐漸減小，乘性噪聲也逐漸減弱。在z軸方向上，100～200 s之間，乘性噪聲經(jīng)歷了由強(qiáng)到弱再至強(qiáng)的變化過程，400 s后隨著相對位置逐漸接近，乘性噪聲也逐漸衰減。

圖8包含乘性噪聲情況下的相對位置誤差和RMSE曲線圖。從圖中可以看出，第2節(jié)中提出的LM-IEKF算法，有一定的改進(jìn)效果。本文第3節(jié) 中提出的Modified LM-IEKF相較于前面的3種算法可以更好地統(tǒng)計量測噪聲，得到更高精度的相對導(dǎo)航狀態(tài)。

圖9為包含乘性噪聲情況下的相對速度誤差和RMSE曲線圖。由430～470 s之間的局部放大圖可以看到，在速度機(jī)動性較大的階段，Modified LM-IEKF相較于其他3種算法可以更快的做出響應(yīng)，在y軸和z軸方向上更為明顯。

表3為4種算法在包含乘性噪聲情況下的精度對比情況。圖10為根據(jù)表3數(shù)據(jù)繪制的相對位置和相對速度精度柱狀圖。表中數(shù)據(jù)說明了Modified LM-IEKF算法相較于其他3種算法，在存在乘性噪聲污染的情況下，可以較大地提升性能。本文提出的Modified LM-IEKF算法相較于廣泛使用的EKF算法，在x、y、z三軸上，相對

圖8 包含乘性噪聲的相對位置誤差和RMSE曲線Fig.8 Errors and RMSE curves of relative position containing multiplicative noise

圖9 包含乘性噪聲的相對速度誤差和RMSE曲線Fig.9 Error and RMSE curves of relative velocity containing multiplicative noise

表3 包含乘性噪聲情況下的誤差統(tǒng)計Table 3 Error statistics containing multiplicative noise

算法相對位置/km相對速度/(m·s-1)MAERMSEMAERMSEEKFx軸1.39686.644035.3946297.5288y軸1.35436.450045.8651410.6351z軸0.65983.281538.9635290.1457IEKFx軸1.32946.425234.4265282.9053y軸1.27896.341743.1477381.1716z軸0.62163.258336.4685271.5340LM-IEKFx軸1.31086.054132.4549262.7155y軸1.26836.078641.8667368.3108z軸0.61832.919835.3536252.5682Modified LM-IEKFx軸1.17335.931228.5927235.2530y軸1.17356.014734.7493291.5511z軸0.53952.880233.2020234.0510

圖10 包含乘性噪聲情況下的相對位置和速度精度柱狀圖Fig.10 Histogram of relative position and velocity accuracy containing multiplicative noise

位置MAE分別減小了16%、13%和18%，相對速度MAE分別減小了34%、23%和14%。在RMSE方面，綜合相對位置精度提升了10%，綜合相對速度精度提升了23%。

5 結(jié) 論

1) 本文針對無人機(jī)與非合作目標(biāo)之間的中遠(yuǎn)距相對導(dǎo)航問題，提出了一種LM-IEKF相對狀態(tài)估計算法。該算法利用了Levenberg-Marquardt優(yōu)化的思想改進(jìn)IEKF中的迭代過程，以提高濾波器的精度和穩(wěn)定性。通過計算對比分析結(jié)果可知：在僅含加性噪聲情況下，LM-IEKF算法相較于EKF、IEKF具有更好的性能。

2) 考慮到距離傳感器因信號相關(guān)特性所引入的乘性噪聲，進(jìn)一步提出了基于量測噪聲自適應(yīng)修正地Modified LM-IEKF算法。通過對比分析：在包含乘性噪聲的情況下，Modified LM-IEKF算法可以有效的在線估計量測噪聲，在提高相對導(dǎo)航狀態(tài)估計精度方面優(yōu)于其他算法。與目前廣泛使用的EKF算法相比，在綜合相對位置精度上提高了10%，在綜合相對速度精度上提升了23%。

上述研究成果對于無人機(jī)在民用、軍用等領(lǐng)域的發(fā)展具有一定的參考價值。