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

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

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

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

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

鑒于上述的問題，本文通過分析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算法。最后，通過對(duì)比分析驗(yàn)證了本文算法的有效性。

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

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

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

根據(jù)定位原理，可以得到空間幾何模型。將無人機(jī)的絕對(duì)位置坐標(biāo)記為Xu=[xu,yu,zu]T，目標(biāo)的絕對(duì)位置坐標(biāo)記為Xt=[xt,yt,zt]T。相對(duì)導(dǎo)航的目的就是利用無人機(jī)上配置的傳感器測量得到距離和角度信息[ρ,α,β]，計(jì)算出目標(biāo)與無人機(jī)間的相對(duì)位置Δ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)之間的相對(duì)速度和相對(duì)加速度。由式(1)可以建立相對(duì)導(dǎo)航系統(tǒng)的量測方程為

(2)

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

(3)

式中：

選擇勻加速(CA)模型作為相對(duì)運(yùn)動(dòng)模型，離散化狀態(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)化的迭代卡爾曼濾波算法

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

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

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

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

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

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的時(shí)候，IEKF和EKF是等價(jià)的。

2.2 LM-IEKF算法

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

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

(10)

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

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

(11)

由式(10)可得

(12)

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

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時(shí)刻的狀態(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時(shí)刻，當(dāng)i=n時(shí)迭代結(jié)束，由式(17) 和式(27)，可得第k時(shí)刻迭代結(jié)束后，進(jìn)入到下一時(shí)刻k+1的協(xié)方差更新公式：

(28)

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

基于2.2.2節(jié)關(guān)于LM-IEKF算法中的狀態(tài)更新方程以及基于最大似然估計(jì)的協(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)實(shí)際環(huán)境中包含多種非線性的畸變、信號(hào)能量的衰減、運(yùn)行過程中的抖動(dòng)等情況時(shí)，都會(huì)不可避免地引入乘性噪聲。本文假設(shè)傳感器測得的距離值被污染，量測噪聲不僅包含有加性噪聲，也包含有與距離相關(guān)的乘性噪聲，并且距離的量測誤差假定與傳感器至目標(biāo)之間的距離成線性關(guān)系。因此，量測矩陣可寫為

(35)

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

量測噪聲的樣本方差陣為

(36)

樣本方差陣的期望值為

(37)

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

(38)

(39)

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

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

4.1 仿真條件

設(shè)置仿真時(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)動(dòng)。設(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)及近的會(huì)合過程，且無人機(jī)包含有平飛、爬升和轉(zhuǎn)彎等機(jī)動(dòng)過程。本文以MAE(Mean Absolute Error)和RMSE(Root Mean Square Error)來評(píng)價(jià)算法的精度情況。圖3為以目標(biāo)的起始位置為原點(diǎn)的相對(duì)位置示意圖。圖4為仿真設(shè)定的無人機(jī)與目標(biāo)之間的相對(duì)位置和相對(duì)速度(vx,vy,vz)變化的曲線圖。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

算法相對(duì)位置/km相對(duì)速度/(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 包含乘性噪聲情況下的相對(duì)位置和速度精度柱狀圖Fig.10 Histogram of relative position and velocity accuracy containing multiplicative noise

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

5 結(jié) 論

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

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

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