考慮磁差的魯棒DME/VOR/SINS組合導(dǎo)航算法*

方 濤

(中國(guó)西南電子技術(shù)研究所，成都 610036)

0 引 言

全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System,GNSS)、捷聯(lián)慣導(dǎo)系統(tǒng)(Strapdown Inertial Navigation System,SINS)、測(cè)距儀(Distance Measuring Equipment,DME)和伏爾(Very High Frequency Omnidirectional Range,VOR)是民航基于性能的導(dǎo)航中常用的導(dǎo)航系統(tǒng)[1-2]。其中，GNSS因具有導(dǎo)航精度高、全天候和全球性的特點(diǎn)，成為最常用的導(dǎo)航系統(tǒng)。然而GNSS信號(hào)在高動(dòng)態(tài)情況下信號(hào)抗干擾處理復(fù)雜使得飛行管理系統(tǒng)研發(fā)者在設(shè)計(jì)系統(tǒng)時(shí)必須考慮配置有效的導(dǎo)航備份[3-4]。DME/VOR/SINS的組合導(dǎo)航方式中，由于DME和VOR同址安裝且具有信號(hào)覆蓋范圍廣的優(yōu)點(diǎn)，應(yīng)用場(chǎng)景更廣，值得深入研究。

目前國(guó)內(nèi)民航客機(jī)應(yīng)用廣泛的進(jìn)口飛行管理系統(tǒng)中，導(dǎo)航算法封裝于系統(tǒng)內(nèi)部，無(wú)法得知其具體實(shí)現(xiàn)細(xì)節(jié)，且相關(guān)文獻(xiàn)報(bào)道較少。國(guó)內(nèi)針對(duì)VOR/DME/SINS組合導(dǎo)航系統(tǒng)開展了部分理論研究：文獻(xiàn)[5]采用聯(lián)邦卡爾曼濾波器設(shè)計(jì)了機(jī)載多傳感器組合導(dǎo)航系統(tǒng)，并在其子濾波器中設(shè)計(jì)了塔康(Tactical Air Navigation,TACAN)/SINS組合導(dǎo)航系統(tǒng)(TACAN可看作VOR/DME同址安裝在軍用領(lǐng)域的應(yīng)用，兩者輸出完全相同)；文獻(xiàn)[6]采用聯(lián)邦卡爾曼濾波器設(shè)計(jì)了SINS/GNSS/TACAN組合導(dǎo)航系統(tǒng)，在組合導(dǎo)航的同時(shí)實(shí)現(xiàn)動(dòng)態(tài)校正、對(duì)準(zhǔn)和標(biāo)定；文獻(xiàn)[7]設(shè)計(jì)了基本的TACAN/SINS組合導(dǎo)航算法；文獻(xiàn)[8]推導(dǎo)了系數(shù)加權(quán)的聯(lián)邦卡爾曼濾波算法，并將該算法應(yīng)用于SINS/GNSS/TACAN組合導(dǎo)航算法中；文獻(xiàn)[9]設(shè)計(jì)了機(jī)載導(dǎo)航系統(tǒng)的故障檢測(cè)與處理算法；文獻(xiàn)[10]同樣采用聯(lián)邦卡爾曼濾波器設(shè)計(jì)了大飛機(jī)綜合導(dǎo)航算法，其導(dǎo)航系統(tǒng)包括SINS、GNSS、DME、VOR和星敏感器；文獻(xiàn)[11]針對(duì)SINS/大氣數(shù)據(jù)機(jī)/GNSS/TACAN/雷達(dá)綜合導(dǎo)航系統(tǒng)，對(duì)比分析了兩類聯(lián)邦濾波系統(tǒng)級(jí)的故障檢測(cè)方案。上述針對(duì)DME/VOR/SINS組合導(dǎo)航的研究?jī)H停留在理論層面，并未考慮實(shí)際應(yīng)用中存在的問(wèn)題：一是上述研究均假設(shè)TACAN或VOR輸出的方位測(cè)量基準(zhǔn)為真北，而實(shí)質(zhì)上兩者輸出的方位測(cè)量基準(zhǔn)為磁北，后文的測(cè)試結(jié)果將會(huì)看到磁差(地理北與磁北的角度差值)對(duì)組合導(dǎo)航系統(tǒng)有較大的影響；二是當(dāng)飛機(jī)在VOR臺(tái)站上空附近飛行時(shí)VOR輸出的方位信息含較多的野值，野值對(duì)組合導(dǎo)航系統(tǒng)的精度也將產(chǎn)生消極影響。

本文針對(duì)上述兩個(gè)實(shí)際問(wèn)題，提出了一種考慮磁差的魯棒DME/VOR/SINS組合導(dǎo)航算法。實(shí)驗(yàn)測(cè)試結(jié)果表明，考慮磁差的組合導(dǎo)航算法顯著提高了系統(tǒng)的導(dǎo)航精度，同時(shí)由于采用了魯棒的卡爾曼濾波算法實(shí)現(xiàn)信息融合，算法的魯棒性得到了有效提升。

1 問(wèn)題描述

1.1 磁方位與真方位

伏爾系統(tǒng)是一種由機(jī)載設(shè)備直接輸出導(dǎo)航參量的近程無(wú)線電導(dǎo)航測(cè)角系統(tǒng)，導(dǎo)出的導(dǎo)航參量是飛機(jī)相對(duì)于伏爾信標(biāo)臺(tái)的磁方位角，磁方位和真方位的關(guān)系如圖1所示。

圖1 磁方位和真方位關(guān)系示意

圖1中，α=ψ-ψm，ψ為真方位，ψm為磁方位，α為磁差。一般情況下，磁差隨緯度變化，緯度越高，磁差越大。以我國(guó)為例，最北端漠河鎮(zhèn)的磁差大約為11°。顯然，在忽略磁差的情況下，假設(shè)VOR輸出的方位測(cè)量值參考為真北，組合導(dǎo)航系統(tǒng)的導(dǎo)航精度將受到嚴(yán)重影響，第4節(jié)將重點(diǎn)對(duì)比忽略磁差和考慮磁差兩種情況下組合導(dǎo)航系統(tǒng)的精度。

1.2 方位測(cè)量野值對(duì)組合導(dǎo)航的影響

VOR方位測(cè)量輸出野值的原因?yàn)橄嗫仃囂炀€在±60°的頂空盲區(qū)附近增益下降明顯，使普通幅度調(diào)制度降低，產(chǎn)生的15 Hz、135 Hz相位發(fā)生形變，導(dǎo)致方位輸出發(fā)生跳變。某次實(shí)際飛行測(cè)試過(guò)程中VOR方位測(cè)量噪聲如圖2所示，當(dāng)飛機(jī)飛行至臺(tái)站上空附近時(shí)，VOR輸出方位在4 500 s的時(shí)間內(nèi)產(chǎn)生多次跳變。顯然，方位測(cè)量中的野值將對(duì)組合導(dǎo)航系統(tǒng)的魯棒性產(chǎn)生重要影響，在濾波穩(wěn)定性受到嚴(yán)重影響時(shí)，其導(dǎo)航輸出結(jié)果甚至不可靠。第4節(jié)將測(cè)試和分析方位測(cè)量野值對(duì)組合導(dǎo)航系統(tǒng)的影響。

圖2 VOR臺(tái)站上空附近磁方位輸出

2 DME/VOR/SINS組合導(dǎo)航系統(tǒng)設(shè)計(jì)

2.1 算法實(shí)現(xiàn)方案

圖3 DME/VOR/SINS組合導(dǎo)航系統(tǒng)

2.2 組合導(dǎo)航狀態(tài)方程

本文以機(jī)載SINS誤差傳播方程為基礎(chǔ)，建立DME/VOR/SINS組合導(dǎo)航卡爾曼濾波的狀態(tài)方程，狀態(tài)變量X包括三軸姿態(tài)誤差φx、φy和φz；東北天方向的速度誤差δVE、δVN和δVU；經(jīng)度、緯度和高度誤差δλ、δL和δh；三軸陀螺常值漂移εcx、εcy和εcz；三軸加速度計(jì)常值零偏▽cx、▽cy和▽cz。

區(qū)別于已有的研究，本文不再假設(shè)VOR輸出的方位為真方位，將臺(tái)站的磁差納入?yún)?shù)估計(jì)的范疇?？紤]到某一固定位置的磁差在相當(dāng)長(zhǎng)的時(shí)間內(nèi)不會(huì)發(fā)生大的變化，將臺(tái)站磁差α假定為常值：

(1)

在SINS誤差方程的基礎(chǔ)上[12]，考慮式(1)，組合導(dǎo)航卡爾曼濾波的狀態(tài)方程如下：

(2)

式中：矩陣A和B可參考文獻(xiàn)[12]并結(jié)合式(1)得到；系統(tǒng)噪聲序列

εrx、εry和εrz為三軸陀螺隨機(jī)漂移，▽rx、▽ry和▽rz為三軸加速度計(jì)隨機(jī)零偏，ζ為磁差隨機(jī)誤差，近似為高斯白噪聲。

2.3 組合導(dǎo)航量測(cè)方程

(3)

(4)

lU=l+a1δx+a2δy+a3δz。

(5)

DME輸出的斜距可表示為

lS=l+υ。

(6)

式中：υ為DME斜距測(cè)量噪聲。

根據(jù)式(5)和式(6)，斜距差量測(cè)方程可表示為

δl=a1δx+a2δy+a3δz-υ。

(7)

SINS誤差方程中選擇的位置誤差變量為δλ、δL和δh，因而需要將式(7)中的位置誤差δx、δy和δz轉(zhuǎn)換為δλ、δL和δh。文獻(xiàn)[13]給出了上述兩類位置誤差的轉(zhuǎn)換關(guān)系：

(8)

式中：RN為地球卯酉圈半徑。

結(jié)合式(8)，式(7)可進(jìn)一步表示為

δl=b1δλ+b2δL+b3δh-υ。

(9)

式中：b1、b2和b3的具體形式分別為

考慮臺(tái)站磁差α，結(jié)合文獻(xiàn)[8]給出的方位測(cè)量誤差的表達(dá)式，方位測(cè)量誤差表達(dá)式為

(10)

結(jié)合式(9)和式(10)，卡爾曼濾波的量測(cè)方程為

(11)

式中:量測(cè)矩陣H的具體形式為

3 基于M估計(jì)的卡爾曼濾波算法

經(jīng)典的卡爾曼濾波在噪聲統(tǒng)計(jì)特性符合高斯分布時(shí)具有狀態(tài)變量的最優(yōu)估計(jì)，然而，實(shí)際應(yīng)用中噪聲特性不可能完全符合高斯分布，在噪聲嚴(yán)重偏離高斯分布時(shí)狀態(tài)估計(jì)甚至?xí)霈F(xiàn)錯(cuò)誤。在DME/VOR/SINS組合導(dǎo)航應(yīng)用中，如1.2節(jié)所述，VOR方位測(cè)量會(huì)出現(xiàn)野值，若采用經(jīng)典卡爾曼濾波實(shí)現(xiàn)信息融合，狀態(tài)估計(jì)的性能將受到影響。本文采用在工程應(yīng)用中具備良好抗野值能力的基于M估計(jì)的卡爾曼濾波(M-estimation Based Kalman Filter,MKF)算法實(shí)現(xiàn)信息融合。根據(jù)式(2)和式(11)，k時(shí)刻離散化后的組合導(dǎo)航狀態(tài)方程和量測(cè)方程為

式中：Φk,k-1為離散化后的狀態(tài)轉(zhuǎn)移矩陣，Γk-1為離散化的系統(tǒng)噪聲驅(qū)動(dòng)陣，Zk為量測(cè)，Vk為量測(cè)噪聲序列。

濾波算法采用的L1范數(shù)和L2范數(shù)混合的代價(jià)函數(shù)為

(12)

式中：τi為矩陣τ的第i個(gè)元素，κ為調(diào)節(jié)因子。

定義矩陣

(13)

則矩陣ψ對(duì)角線上的第i個(gè)元素ψi可表示為

(14)

MKF算法執(zhí)行流程如下：

Step1 設(shè)置狀態(tài)變量初值X0、估計(jì)均方誤差陣P0、系統(tǒng)噪聲方差陣初值Q0、量測(cè)噪聲方差陣R0、迭代次數(shù)N、調(diào)節(jié)因子κ、k=1。

Step5 計(jì)算τ=MkXk-yk。

Step6 結(jié)合式(12)、(13)和(14)計(jì)算矩陣ψ。

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

4.1 實(shí)驗(yàn)測(cè)試條件

以某次飛行中記錄的機(jī)載SINS慣性器件輸出為基礎(chǔ)，對(duì)本文所設(shè)計(jì)的DME/VOR/SINS組合導(dǎo)航算法進(jìn)行測(cè)試，慣性器件輸出頻率為100 Hz。以機(jī)載SINS/GNSS組合導(dǎo)航系統(tǒng)輸出的導(dǎo)航參數(shù)為參考，對(duì)導(dǎo)航算法的定位精度進(jìn)行定量分析。SINS解算采用最常用的指北方位慣導(dǎo)力學(xué)編排，該力學(xué)編排適用于除極區(qū)以外的絕大多數(shù)區(qū)域。飛機(jī)在水平方向上的相對(duì)運(yùn)動(dòng)軌跡如圖4所示。

圖4 水平面內(nèi)的相對(duì)飛行軌跡

圖4中，DME/VOR臺(tái)站同址安裝，其位置根據(jù)飛機(jī)運(yùn)動(dòng)軌跡設(shè)置得到，緯度1.4°，經(jīng)度1.03°，高度0 m。為確保算法驗(yàn)證的有效性，本文采取半物理仿真的策略模擬DME和VOR的輸出，即采用在DME和VOR輸出的理論真值中疊加實(shí)際測(cè)量噪聲的方法模擬圖4臺(tái)站位置處的DME和VOR的輸出。DME和VOR的輸出頻率為20 Hz，輸出的理論真值根據(jù)臺(tái)站位置與飛機(jī)位置參考計(jì)算得到。本文重點(diǎn)測(cè)試飛機(jī)飛行至VOR臺(tái)站上空時(shí)VOR輸出的磁方位信息包含野值的情況，VOR的實(shí)際測(cè)量噪聲如圖2所示。

4.2 實(shí)驗(yàn)測(cè)試結(jié)果及分析

4.2.1 磁差對(duì)組合導(dǎo)航系統(tǒng)影響測(cè)試

首先測(cè)試組合導(dǎo)航系統(tǒng)考慮磁差和不考慮磁差的兩種情況下組合導(dǎo)航系統(tǒng)的性能。設(shè)VOR臺(tái)站的磁差為-5°，兩種測(cè)試中均采用基于M估計(jì)的卡爾曼濾波實(shí)現(xiàn)信息融合。算法測(cè)試過(guò)程中，為保證迭代收斂，濾波迭代次數(shù)N設(shè)置為10，調(diào)節(jié)因子κ設(shè)置為經(jīng)典參數(shù)1.345。考慮磁差的情況下，磁差的估計(jì)結(jié)果如圖5所示。

圖5 磁差估計(jì)值

兩種測(cè)試方案下組合導(dǎo)航系統(tǒng)的姿態(tài)誤差、速度誤差和位置誤差如圖6～8所示。

圖6 兩種測(cè)試條件下的姿態(tài)誤差曲線

圖7 兩種測(cè)試條件下的速度誤差曲線

圖8 兩種測(cè)試條件下的位置誤差曲線

兩種測(cè)試方案下導(dǎo)航參數(shù)誤差的均方根值如表1所示。

表1 兩種測(cè)試方案下導(dǎo)航參數(shù)誤差的均方根值

如圖5所示，本文所設(shè)計(jì)的組合導(dǎo)航方案可以較為準(zhǔn)確地估計(jì)出臺(tái)站磁差，隨估計(jì)時(shí)間的變長(zhǎng)估計(jì)精度越高。由圖6～8以及表1可知，實(shí)際工程應(yīng)用中，若在組合導(dǎo)航算法的設(shè)計(jì)中不考慮臺(tái)站磁差，系統(tǒng)導(dǎo)航精度將受到顯著影響。本文在組合導(dǎo)航算法的設(shè)計(jì)中考慮了臺(tái)站磁差，相較于不考慮磁差的情況，系統(tǒng)導(dǎo)航精度得到了顯著的提升，在方位、水平速度和水平位置的精度提升方面效果更為明顯。

在測(cè)試中，一個(gè)需要值得關(guān)注的現(xiàn)象是，隨著磁差設(shè)置值的增大，磁差估計(jì)精度會(huì)越來(lái)越高。設(shè)置磁差理論真值從1～11變化，圖9給出了磁差估計(jì)精度隨磁差設(shè)置值變化的曲線，橫軸表示磁差理論真值設(shè)置值，縱軸表示磁差估計(jì)值與理論真值的比值。

圖9 磁差估計(jì)精度

綜合上述分析，本文所設(shè)計(jì)的算法可以有效地估計(jì)出臺(tái)站磁差，具備提升組合導(dǎo)航性能的能力。

4.2.2 方位野值對(duì)組合導(dǎo)航系統(tǒng)影響測(cè)試

本小節(jié)測(cè)試方位測(cè)量野值對(duì)組合導(dǎo)航系統(tǒng)的影響，重點(diǎn)測(cè)試采用基于M估計(jì)的卡爾曼濾波和經(jīng)典卡爾曼濾波算法時(shí)組合導(dǎo)航系統(tǒng)的精度。VOR臺(tái)站的磁差設(shè)置為-5°，兩種濾波算法下的組合導(dǎo)航系統(tǒng)的姿態(tài)誤差、速度誤差和位置誤差如圖10～12所示，兩種濾波算法下導(dǎo)航參數(shù)誤差的均方根值如表2所示。

圖10 兩種濾波算法下的姿態(tài)誤差曲線

圖11 兩種濾波算法下的速度誤差曲線

圖12 兩種濾波算法下的位置誤差曲線

表2 兩種濾波算法下導(dǎo)航參數(shù)誤差的均方根值

由圖10～12以及表2可知，當(dāng)VOR輸出的方位測(cè)量值存在野值時(shí)，基于經(jīng)典的卡爾曼濾波算法實(shí)現(xiàn)DEM/VOR/SINS組合導(dǎo)航的導(dǎo)航精度嚴(yán)重惡化，原因在于量測(cè)野值的存在破壞了理想的高斯分布框架，導(dǎo)致卡爾曼濾波的狀態(tài)估計(jì)性能嚴(yán)重降低；基于M估計(jì)的卡爾曼濾波算法在以上測(cè)試條件下表現(xiàn)出良好的魯棒性，由于該算法是一種組合1范數(shù)和2范數(shù)的組合估計(jì)技術(shù)，保證了DME/VOR/SINS組合導(dǎo)航系統(tǒng)在存在野值時(shí)仍然具備較好的狀態(tài)估計(jì)性能，相較于前者，基于M估計(jì)實(shí)現(xiàn)的組合導(dǎo)航系統(tǒng)具有更高的導(dǎo)航精度。需要補(bǔ)充的一點(diǎn)是，VOR輸出的方位測(cè)量野值通常只在飛機(jī)飛行至頂空盲區(qū)附近才會(huì)出現(xiàn)，當(dāng)飛機(jī)飛行至離臺(tái)較遠(yuǎn)的航路階段，其輸出往往較為穩(wěn)定。當(dāng)方位測(cè)量值不存在野值時(shí)，基于M估計(jì)的卡爾曼濾波退化為經(jīng)典的卡爾曼濾波，此時(shí)采用兩類濾波算法可獲得一致的組合導(dǎo)航精度。因此，本文采用基于M估計(jì)的卡爾曼濾波實(shí)現(xiàn)DEM/VOR/SINS組合導(dǎo)航具備更強(qiáng)的普適性。

5 結(jié) 論

本文針對(duì)DME/VOR/SINS組合導(dǎo)航系統(tǒng)展開研究，并通過(guò)實(shí)驗(yàn)驗(yàn)證了所設(shè)計(jì)的組合導(dǎo)航算法的有效性，主要結(jié)論如下：

(1)VOR輸出的方位是以臺(tái)站磁北為參考的，以VOR輸出的磁方位作為真方位進(jìn)行組合導(dǎo)航，由于忽略了臺(tái)站磁差，組合導(dǎo)航系統(tǒng)的導(dǎo)航精度大幅降低；

(2)飛機(jī)飛行至臺(tái)站頂空盲區(qū)附近時(shí)VOR輸出的方位測(cè)量值中包含野值，由于方位測(cè)量野值的存在，采用常規(guī)的KF實(shí)現(xiàn)DME/VOR/SINS組合導(dǎo)航時(shí)導(dǎo)航誤差急劇增大，導(dǎo)航參數(shù)輸出不可用；

(3)為減少臺(tái)站磁差對(duì)組合導(dǎo)航算法的影響，本文所設(shè)計(jì)的組合導(dǎo)航算法將臺(tái)站磁差納入?yún)?shù)估計(jì)的范疇，在信息融合處理中為應(yīng)對(duì)方位測(cè)量野值對(duì)導(dǎo)航精度的影響，采用了基于M估計(jì)的卡爾曼濾波算法實(shí)現(xiàn)了組合導(dǎo)航。上述兩方面的改進(jìn)工作提高了組合導(dǎo)航算法的適用性和魯棒性，具備較高的工程應(yīng)用價(jià)值。