田佳玉 胡柏青 李開龍 趙仁杰

（海軍工程大學(xué) 武漢 430033）

1 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System，SINS）基于在價(jià)格體積以及算法等方面的優(yōu)勢，目前已成為研究的熱門領(lǐng)域。慣導(dǎo)系統(tǒng)中慣性測量元件的器件誤差積累等問題，會(huì)導(dǎo)致導(dǎo)航精度的下降，而組合導(dǎo)航正可以就這一問題進(jìn)行改善。以捷聯(lián)慣導(dǎo)為核心，其他導(dǎo)航設(shè)備輔助的組合模式，被稱為捷聯(lián)慣性基組合導(dǎo)航，GPS（Global Positioning System，全球定位系統(tǒng)）［1］和 DVL（Doppler Velocity Log，多普勒計(jì)程儀）［2］常作為輔助導(dǎo)航設(shè)備。組合導(dǎo)航的基礎(chǔ)是現(xiàn)代濾波技術(shù)，根據(jù)慣導(dǎo)系統(tǒng)模型線性或非線性，可將濾波技術(shù)分為線性濾波和非線性濾波。線性濾波中最經(jīng)典的當(dāng)屬卡爾曼濾波（Kalman Filtering，KF）［3］，在非線性濾波中，1995年S.J.Julier和J.K.Uhlmann提出的無跡卡爾曼濾波算法（Unscented Kalman Filter，UKF）估計(jì)精度較高且計(jì)算量一定［4～5］一直是研究的熱點(diǎn)。

除了考慮慣導(dǎo)模型，在捷聯(lián)慣性基組合導(dǎo)航濾波中，姿態(tài)表達(dá)方法也是需要重視的。采用不同的姿態(tài)表達(dá)方法所構(gòu)造的組合導(dǎo)航濾波算法是不同的，常見的姿態(tài)表達(dá)方法有三維矢量的Euler角、羅德里格斯參數(shù)族以及四維矢量的四元數(shù)［6～8］。其中Euler角法僅需要求解三個(gè)微分方程，數(shù)量較少，但存在一對(duì)奇異點(diǎn)［9］在特定點(diǎn)姿角突變，無法得到準(zhǔn)確值。在20世紀(jì)70年代末期，中國飛行試驗(yàn)研究院黃雪樵［8］提出一種雙歐拉法，利用正反兩組歐拉方程結(jié)合使用可以解決奇異性問題。目前國內(nèi)外學(xué)者對(duì)于組合導(dǎo)航濾波算法的研究取得了諸多成果，Hamiltn在進(jìn)行附屬的虛數(shù)部研究中，提出了四元數(shù)概念［11］，在四元數(shù)體系下，乘性拓展卡爾曼濾波是基于四元數(shù)與拓展卡爾曼濾波的結(jié)合［12～13］，2003年，融合四元數(shù)與UKF，Markley、Crassidis提出采用“分層濾波”方式進(jìn)行濾波估計(jì)［14］。

但目前對(duì)于Euler角體系下的線性濾波（Euler-KF）、非線性UKF濾波（Euler-UKF）、以及雙歐拉UKF濾波（DoEuler-UKF）三種組合濾波算法在實(shí)際捷聯(lián)慣性基組合導(dǎo)航系統(tǒng)中的精度、收斂速度和差異性還未進(jìn)行研究分析。本文正是基于此，開展相關(guān)理論研究，通過仿真實(shí)驗(yàn)展開探究三種組合導(dǎo)航濾波方法在應(yīng)用中的優(yōu)缺點(diǎn)，為組合導(dǎo)航濾波的選取與使用提供有益參考。

2 捷聯(lián)慣導(dǎo)基本模型

捷聯(lián)慣導(dǎo)基本方程包括姿態(tài)、速度和位置微分方程，分別為

3 雙歐拉角姿態(tài)解算法

歐拉角雖然使用簡單易懂且獨(dú)立方程數(shù)最少，但是一旦俯仰角達(dá)到±90°時(shí)方程組就會(huì)出現(xiàn)奇異點(diǎn)，進(jìn)而無法確定橫滾角與航向角的大小。于是可以考慮采用正反兩組歐拉方程進(jìn)行交替運(yùn)算，避開各自奇異點(diǎn)。

動(dòng)坐標(biāo)系相對(duì)參考坐標(biāo)系方位，由動(dòng)坐標(biāo)系依次繞3個(gè)不同軸轉(zhuǎn)動(dòng)的3個(gè)轉(zhuǎn)角來確定，如果三個(gè)軸的順序是y-z-x，則令對(duì)應(yīng)的航向角、俯仰角和橫滾角為正歐拉角，分別記為 φ、θ、γ，如圖1（a）所示，角速度為。如果變換順序是y-x-z，則航向角、橫滾角和俯仰角為反歐拉角，分別記為φr、γr、θr如圖1（b）所示。

圖1 正、反歐拉坐標(biāo)系旋轉(zhuǎn)

兩坐標(biāo)矩陣的轉(zhuǎn)換可分解為三個(gè)單一旋轉(zhuǎn)矩陣的連乘，令導(dǎo)航坐標(biāo)系為n，載體坐標(biāo)系為b，則相應(yīng)的正歐拉姿態(tài)矩陣可表示為

Cx表示關(guān)于x的單位旋轉(zhuǎn)矩陣。

角速度與歐拉角的關(guān)系可表示為

由上述可知：θ=±90°為方程組的奇異點(diǎn)，θ=0°或在π附近時(shí)，精度會(huì)提升，此范圍被稱為正歐拉方程的精華區(qū)。

可知，反歐拉方程的奇異點(diǎn)和精華區(qū)與正歐拉方程相反，奇異點(diǎn)在0°或π，精華區(qū)在±90°附近，正歐拉方程與反歐拉方程的精華區(qū)與奇異區(qū)呈現(xiàn)出的是一個(gè)倒掛關(guān)系。在應(yīng)用的過程中，一般是以±45°或±135°為界劃分精華區(qū)和奇異區(qū)，如圖2。

圖2 正、反歐拉角微分方程的精華區(qū)與奇異區(qū)

4 卡爾曼濾波

xk、zk分別為狀態(tài)向量和量測向量，Φk|k-1為狀態(tài)轉(zhuǎn)移矩陣，Γk為噪聲矩陣和Hk為量測矩陣，wk、vk分別為系統(tǒng)噪聲和量測噪聲且

δkj為狄拉克函數(shù)，利用量測量獲得狀態(tài)最優(yōu)估計(jì)值x?k的KF濾波過程由狀態(tài)一步預(yù)測、一步預(yù)測均方差、濾波增益、狀態(tài)最優(yōu)估計(jì)、狀態(tài)估計(jì)均方差陣組成，見式（10）。

5 無跡卡爾曼濾波

UKF是以UT變換為基礎(chǔ)，選取相應(yīng)的Sigma采樣點(diǎn)來近似系統(tǒng)狀態(tài)的先驗(yàn)統(tǒng)計(jì)特性，再直接通過非線性方程演化系統(tǒng)狀態(tài)的后驗(yàn)分布特性。當(dāng)系統(tǒng)的過程噪聲和量測噪聲為加性噪聲且量測方程是線性的時(shí)候能夠得到簡化的UKF算法，簡化的UKF濾波算法如下：

6 基于雙歐拉角的無跡卡爾曼濾波

由于歐拉角存在奇異性，本文采用雙歐拉角法結(jié)合UKF進(jìn)行濾波，基本的濾波流程如圖3所示。

圖3 DoEuler-UKF濾波流程

7 實(shí)驗(yàn)及分析

7.1 組合導(dǎo)航仿真實(shí)驗(yàn)

通過仿真實(shí)驗(yàn)對(duì)以上三種方法的估計(jì)效果進(jìn)行比較，采用位置作為量測量SINS∕GPS松組合方式，仿真產(chǎn)生的航行軌跡如圖4所示，仿真時(shí)長1300s。

圖4 仿真航跡

設(shè)置濾波器的初始條件：

7.2 實(shí)驗(yàn)結(jié)果分析

實(shí)驗(yàn)的主要目的是比較Euler-KF、Euler-UKF和DoEuler-UKF的濾波效果，結(jié)果分別如圖5～6所示。

圖5 姿態(tài)角估計(jì)誤差比較

表1是位置和航向均方根誤差（RMSE）統(tǒng)計(jì)數(shù)據(jù)，RMSE方程由式（12）表示：

表1 位置、航向估計(jì)誤差均方根誤差

其中δx?是RMSE，T是實(shí)驗(yàn)時(shí)長，計(jì)誤差值。

通過分析圖5的姿態(tài)估計(jì)效果，可以發(fā)現(xiàn)DoEuler-UKF的精度要明顯高于Euler-UKF和Euler-KF，Euler-UKF濾波效果好于Euler-KF。圖6是速度誤差估計(jì)，從收斂過程可以看出，DoEuler-UKF的收斂速度明顯比Euler-UKF和Euler-KF要快。通過表1可以看出，在誤差精度方面，DoEuler-UKF要明顯好于其他兩種方法，這是由于Euler-KF方法采用的是線性誤差模型，忽略了非線性部分，導(dǎo)致了精度下滑，而Euler-UKF采用的則是非線性誤差模型，各項(xiàng)參數(shù)考慮更為全面，然而Euler的姿態(tài)表示方法存在一定局限性，一旦達(dá)到奇異點(diǎn)附近，精度便會(huì)受到較大影響，DoEuler-UKF既選用了更為準(zhǔn)確的非線性模型，又克服了歐拉角的奇異性問題，因此達(dá)到了更為理想的濾波估計(jì)效果。

圖6 速度估計(jì)誤差

8 結(jié)語

Euler角法作為旋轉(zhuǎn)物理意義最明顯的姿態(tài)表示方法，奇異點(diǎn)的存在限制了它的精度與應(yīng)用范圍，而雙歐拉法，利用正反歐拉方程的變換，巧妙避開奇異區(qū)，并對(duì)解的精華區(qū)劃分，有效提高了姿態(tài)的精確性。結(jié)合不同的慣導(dǎo)模型，對(duì)Euler-KF，Euler-UKF與DoEuler-UKF濾波比較實(shí)驗(yàn)進(jìn)行分析：DoEuler-UKF對(duì)比Euler-KF和Euler-UKF，航向估計(jì)精度分別提高了69.5%與58.7%，速度估計(jì)精度分別提高了85.7%與73.6%。