基于雙歐拉角的UKF組合導(dǎo)航濾波算法

趙仁杰, 胡柏青, 呂 旭, 田佳玉

(海軍工程大學(xué)電氣工程學(xué)院, 湖北 武漢 430033)

0 引 言

組合導(dǎo)航技術(shù)通過對多種導(dǎo)航系統(tǒng)信息綜合處理,有效克服了單一系統(tǒng)局限性,提升了導(dǎo)航的精度與可靠性,被廣泛應(yīng)用在航空航天、武器裝備、無人系統(tǒng)等領(lǐng)域[1]。目前,捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system, SINS)/全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system, GNSS)是最常見的組合導(dǎo)航系統(tǒng)之一,兩者良好互補(bǔ),具有導(dǎo)航信息全面、動(dòng)態(tài)性好、自主性強(qiáng)、精度高等優(yōu)勢[2-3]。

信息融合是組合導(dǎo)航的關(guān)鍵,其實(shí)現(xiàn)依賴于現(xiàn)代濾波技術(shù),其中最主要的手段是卡爾曼濾波[4]。根據(jù)系統(tǒng)狀態(tài)選取不同,卡爾曼濾波可分為直接法和間接法。其中,間接法系統(tǒng)狀態(tài)為導(dǎo)航參數(shù)誤差,模型經(jīng)過線性近似,容易處理,在工程中應(yīng)用廣泛,而直接法直接選取導(dǎo)航參數(shù)作為狀態(tài),可以無損傳遞運(yùn)動(dòng)信息,但狀態(tài)方程非線性,存在濾波器設(shè)計(jì)復(fù)雜、計(jì)算量大缺點(diǎn)[5-6]。隨著計(jì)算機(jī)與非線性濾波技術(shù)的發(fā)展,擴(kuò)展卡爾曼濾波(extend Kalman filter, EKF)、無跡卡爾曼濾波(unscented Kalman filter, UKF)、粒子濾波(particle filter,PF)等非線性濾波算法[7-9]逐漸凸顯出直接法的優(yōu)勢。其中,UKF利用Sigma點(diǎn)采樣近似估計(jì)非線性函數(shù)概率密度,避免了EKF模型線性化以及求解復(fù)雜雅克比矩陣的缺點(diǎn),具有適中的計(jì)算量和較高的濾波精度。

在組合導(dǎo)航系統(tǒng)中,姿態(tài)參數(shù)和其他導(dǎo)航參數(shù)相耦合,無法直接獲取,需要通過狀態(tài)濾波估計(jì)得到,其估計(jì)好壞直接影響著系統(tǒng)精度,因此姿態(tài)估計(jì)研究十分重要。其中,姿態(tài)描述是研究基礎(chǔ),常見表示方法有方向余弦矩陣、四元數(shù)、歐拉角、羅德里格斯參數(shù)(Rodrigues parameter, RP)和修正RP(modified RP, MRP)等[10]。方向余弦矩陣可以全姿態(tài)表達(dá),但求解計(jì)算量大;歐拉角、RP和MRP都是維數(shù)最小的姿態(tài)表示法,但存在奇異性;四元數(shù)憑借著全局非奇異、運(yùn)動(dòng)學(xué)方程為線性等優(yōu)勢,成為應(yīng)用最廣泛的姿態(tài)表達(dá)形式[11-12]。

四元數(shù)參數(shù)之間相互并不獨(dú)立,存在著規(guī)范性約束,若直接與非線性濾波結(jié)合,容易造成濾波發(fā)散。對此,Crassidis等[13-14]提出四元數(shù)無跡估計(jì)器(unsented quaternion estimator, USQUE)算法,利用分層濾波成功解決了約束問題,但算法結(jié)構(gòu)復(fù)雜,計(jì)算量大。文獻(xiàn)[15]提出一種改進(jìn)的USQUE算法,通過四元數(shù)與MRP線性變換,降低了計(jì)算量。Karlgaard等[16]采用MPR表示姿態(tài),通過MRP和其影子MRP相互切換避免了奇異性。后續(xù)許多學(xué)者也對此展開了深入研究[17-18]。目前,在非線性姿態(tài)估計(jì)中,姿態(tài)表示常用四元數(shù)和Rodrigues參數(shù)族,很少采用歐拉角,但歐拉角意義明確,參數(shù)獨(dú)立且求解姿態(tài)矩陣無需正交化處理,針對奇異性問題,黃雪樵[19]提出雙歐法,仿真表明該方法可以有效避免奇異性;陳廷楠等[20-21]證明雙歐法克服歐拉方程的奇異性要優(yōu)于四元數(shù)法;文獻(xiàn)[22]對雙歐法進(jìn)行改進(jìn),實(shí)現(xiàn)了飛行器的任意姿態(tài)連續(xù)解算;Ran等[23]將雙歐法應(yīng)用在水下航行器的非線性對準(zhǔn)中,取得較好效果。

針對USQUE算法的缺點(diǎn),本文采用雙歐拉角姿態(tài)表示,提出一種基于雙歐拉角姿態(tài)表示的無跡卡爾曼濾波(dual-Euler unscented Kalman filter, DEUKF)非線性濾波算法,比較分析了兩種算法的姿態(tài)估計(jì)過程,并在SINS/全球定位系統(tǒng)(global positioning system,GPS)直接式組合導(dǎo)航中應(yīng)用,仿真和車載實(shí)驗(yàn)結(jié)果表明所提算法具有較好的精度，且計(jì)算量明顯減小。

1 姿態(tài)表示

定義坐標(biāo)系:導(dǎo)航坐標(biāo)系取“東-北-天”地理坐標(biāo)系,記為n系,載體坐標(biāo)系為b系,取“右-前-上”,慣性坐標(biāo)系為i系,地球坐標(biāo)系為e系。

1.1 四元數(shù)

四元數(shù)是一種特殊的四參量姿態(tài)表示法,具有全局非奇異性,定義為q=[q0，ρ]T,q0為標(biāo)量,ρ=[q1，q2，q3]T為三維矢量。姿態(tài)四元數(shù)微分方程表示如下:

(1)

(2)

式中:(·×)表示反對稱陣。

1.2 RP族

RP族是一種三參量姿態(tài)表示法,與旋轉(zhuǎn)矢量和姿態(tài)四元數(shù)關(guān)系如下:

Rfamily=etan[φ/(2N)]=fρ/(a+q0)

(3)

N=1時(shí),Rfamily=etan(φ/2),表示RP,此時(shí),f=1,a=0;N=2時(shí),Rfamily=etan(φ/4),表示MRP,一般記作σ,此時(shí),f=a=1;N>2時(shí),Rfamily表示高階RP(high order RP,HORP)[15,18]。

MRP與四元數(shù)q的轉(zhuǎn)換關(guān)系如下:

σ=ρ/(1+q0)

(4a)

(4b)

1.3 歐拉角

(5)

(6)

1.4 雙歐拉角

分析式(5),在θ=±π/2處,姿態(tài)微分方程無法求解,存在奇異性,這也是歐拉角無法全姿態(tài)表達(dá)的原因。雙歐法通過重新定義一組反歐拉角,利用正、反歐拉角切換,有效避開了奇異點(diǎn)。

(7)

(8)

式中:符號(hào)標(biāo)記方式同第1.3節(jié)。

比較式(5)和式(7),在正歐拉微分方程中,θ=±π/2為奇異點(diǎn),而在θ=0和θ=±π處,cosθ=1,方程求解精度高,稱為精華點(diǎn);在反歐拉微分方程中,γr=±π/2為奇異點(diǎn),在γr=0和γr=±π處,cosγr=1,是其精華點(diǎn)。

(9)

分析式(9),θ→±π/2,γr→0或γr→±π;γr→±π/2,θ→0或θ→±π,可以看出,正歐拉微分方程的奇異點(diǎn)對應(yīng)反歐拉微分方程的精華點(diǎn),而反歐拉微分方程的奇異點(diǎn)對應(yīng)正歐拉微分方程的精華點(diǎn),兩者相互倒掛。

雙歐法工作原理是到達(dá)正歐拉微分方程奇異點(diǎn)前,通過姿態(tài)矩陣進(jìn)行正、反歐拉角切換,再利用反歐拉微分方程進(jìn)行更新,反之亦然,進(jìn)而實(shí)現(xiàn)全姿態(tài)工作。文獻(xiàn)[25]驗(yàn)證了正、反歐拉角的最佳切換角度為±π/4和±3π/4,即精華點(diǎn)和奇異點(diǎn)區(qū)間的二等分點(diǎn)是最佳切換點(diǎn)。切換示意圖如圖1所示,圖中字母A表示精華點(diǎn),B表示切換點(diǎn),C表示奇異點(diǎn),白色區(qū)間代表微分方程的精華區(qū),陰影區(qū)間代表奇異區(qū)。對于正歐拉微分方程,圖1對象是正歐拉俯仰角θ,而對于反歐拉微分方程,對象為反歐拉橫滾角γr。

圖1 切換示意圖Fig.1 Switching diagram

2 非線性濾波算法

2.1 UKF算法

考慮如下離散非線性系統(tǒng):

(10)

式中:狀態(tài)方程非線性,量測方程線性,噪聲為加性噪聲。其中，Xk和Yk分別表示k時(shí)刻的n維狀態(tài)量和m維量測量;f(·)為非線性狀態(tài)函數(shù)；Hk為量測矩陣；Wk-1和Vk分別為系統(tǒng)噪聲與量測噪聲向量,兩者互不相關(guān),且滿足如下關(guān)系:

(11)

式中:δkj為克羅內(nèi)克函數(shù)。

對于上述非線性系統(tǒng),給出UKF算法步驟。

步驟 1狀態(tài)初始化

步驟 2時(shí)間更新

(12a)

χi,k/k-1=f(χi,k-1/k-1)

(12b)

(12c)

(12d)

式中:χi,k-1/k-1為k-1時(shí)刻的sigma點(diǎn);sigma(·)為sigma點(diǎn)采樣方程,可表示為

(13)

(14)

步驟 3量測更新

(15a)

(15b)

Pk/k=(In-KkHk)Pk/k-1

(15c)

2.2 USQUE算法

利用四元數(shù)進(jìn)行非線性濾波姿態(tài)估計(jì),雖然不會(huì)出現(xiàn)奇異,但存在著歸一化約束以及濾波方差矩陣匹配約束問題。USQUE算法將濾波分成內(nèi)外兩層,內(nèi)層采用誤差MRP傳遞采樣點(diǎn),外層利用四元數(shù)完成姿態(tài)更新,有效解決了上述問題。給出具體流程如下:

為簡化說明,下文僅對USQUE算法的關(guān)鍵姿態(tài)估計(jì)流程進(jìn)行介紹[26]。

(1) 時(shí)間更新

外層姿態(tài)更新:

外層姿態(tài)更新過程可分為3步。

(16)

內(nèi)層姿態(tài)遞推可分為2步:

(17)

狀態(tài)估計(jì)及均方誤差一步預(yù)測同式(12c)和式(12d)。

(2) 量測更新

量測更新過程參照UKF算法。

(3) 姿態(tài)更新

(18)

2.3 DEUKF算法

雙歐法利用正、反歐拉微分方程奇異點(diǎn)和精華點(diǎn)倒掛性質(zhì),通過正、反歐拉角切換來實(shí)現(xiàn)全姿態(tài)表達(dá)。本文提出的DEUKF算法設(shè)計(jì)了一種新的姿態(tài)估計(jì)流程,介紹之前,給出以下兩個(gè)切換判斷條件,與文獻(xiàn)[24]僅用θ作為切換條件判斷對象有所不同。

圖2 DEUKF算法姿態(tài)估計(jì)流程圖Fig.2 Flow chart of attitude estimation of DEUKF algorithm

2.4 算法比較

在直接式組合導(dǎo)航中,USQUE和DEUKF算法均在UKF框架下,利用SINS基本方程進(jìn)行狀態(tài)濾波更新,理論上沒有模型近似,濾波精度較高,不同之處在于姿態(tài)表示和估計(jì)流程。

3 仿真試驗(yàn)研究

3.1 SINS/GPS組合導(dǎo)航模型

本文建立SINS/GPS直接式組合導(dǎo)航系統(tǒng)模型,由SINS和GPS測量載體運(yùn)動(dòng)參數(shù),構(gòu)建系統(tǒng)狀態(tài)方程和量測方程,通過UKF濾波器估計(jì)出導(dǎo)航參數(shù),系統(tǒng)結(jié)構(gòu)如圖3所示。

圖3 SINS/GPS直接式組合導(dǎo)航結(jié)構(gòu)框圖Fig.3 Structure diagram of SINS/GPS direct integrated navigation system

系統(tǒng)狀態(tài)模型采用SINS基本方程,不同姿態(tài)表示的微分方程見第1.1節(jié),比力方程和位置微分方程具體如下:

(19)

(20)

對于中低精度SINS,系統(tǒng)狀態(tài)一般還需考慮器件誤差、陀螺儀漂移和加速度計(jì)零偏。陀螺儀和加速度計(jì)誤差方程分別為

(21a)

(21b)

(22a)

(22b)

量測模型采用速度和位置松組合模式,得到線性離散量測方程:

(23)

式中:ηv k和ηp k分別是零均值速度和位置高斯白噪聲。

3.2 仿真試驗(yàn)

為驗(yàn)證算法有效性,利用軌跡發(fā)生器模擬飛機(jī)大機(jī)動(dòng)運(yùn)動(dòng),其軌跡如圖4所示,包含勻速、加速、爬升、轉(zhuǎn)彎、俯降、滾轉(zhuǎn)等狀態(tài)。圖5和圖6分別給出載體姿態(tài)和速度變化。軌跡初始地理位置參數(shù)為東經(jīng)108.91°，北緯34.25°，高度為1 000 m,初始姿態(tài)角為0°,初始速度為0 m/s,仿真步長為0.01 s,總時(shí)長為700 s。

圖4 運(yùn)動(dòng)軌跡仿真Fig.4 Simulation of motion trajectory

圖5 載體姿態(tài)變化Fig.5 Change of carrier attitude

圖6 載體速度變化Fig.6 Change of carrier velocity

模型采用速度和位置松組合,算法對速度和位置狀態(tài)估計(jì)比較準(zhǔn)確,因此僅對比分析兩種算法姿態(tài)估計(jì)效果,參考運(yùn)動(dòng)姿態(tài)信息,給出姿態(tài)誤差對比結(jié)果,如圖7和圖8所示。其中,直線表示USQUE算法,虛線表示DEUKF算法。

圖7 俯仰角和橫滾角估計(jì)誤差對比Fig.7 Comparison of pitch angle and roll angle estimation errors

從圖7和圖8中可以看出,對于中低精度SINS/GPS組合導(dǎo)航系統(tǒng),在大初始姿態(tài)誤差角下,兩種算法的姿態(tài)濾波收斂速度較快,其中俯仰角和橫滾角均在20 s左右穩(wěn)定收斂,航向角估計(jì)效果相對水平姿態(tài)角要差,在20 s左右快速收斂至0.6°以內(nèi),在450 s左右達(dá)到穩(wěn)定,誤差收斂到0.1°以內(nèi),原因在于航向角和水平姿態(tài)角估計(jì)精度分別取決于陀螺儀和加速計(jì)的性能;對比仿真結(jié)果,兩種算法的水平姿態(tài)角濾波估計(jì)精度相當(dāng),航向角誤差收斂趨勢一致,其中USQUE穩(wěn)態(tài)精度略優(yōu)于DEUKF,但在運(yùn)行時(shí)間上,DEUKF相比USQUE濾波結(jié)構(gòu)簡單,計(jì)算時(shí)間明顯減小。

圖8 航向角估計(jì)誤差對比Fig.8 Comparison of yaw angle estimation error

為驗(yàn)證算法穩(wěn)定性與有效性,保持初始條件不變,進(jìn)行50次蒙特卡羅仿真試驗(yàn),用均方根誤差(root mean squared error,RMSE)和平均運(yùn)行時(shí)間來衡量算法的性能,RMSE的表達(dá)式為

(24)

姿態(tài)RMSE和算法運(yùn)行時(shí)間如表1所示(CPU of Inter core i5-8250U,1.60 GHz,Matlab R2015a)。仿真結(jié)果表明,DEUKF算法航向角均方根誤差為5.83′,USQUE略優(yōu)于DEUKF，為1.72′;若以平均運(yùn)行時(shí)間衡量算法的計(jì)算量,USQUE為394 s,DEUKF為160 s,由此可見,本文所提算法計(jì)算量明顯減小,驗(yàn)證了第2.4節(jié)的理論分析。

表1 姿態(tài)RMSE與算法運(yùn)行時(shí)間

相比USQUE每一次迭代濾波都要進(jìn)行兩次內(nèi)層誤差MRP與外層四元數(shù)相互切換,每一時(shí)刻DEUKF只利用一組歐拉回路進(jìn)行濾波更新,且僅在相應(yīng)歐拉角超出精華區(qū)時(shí)進(jìn)行切換,實(shí)際次數(shù)并不多,其正、反歐拉角切換和姿態(tài)角均方誤差變化分別如圖9和圖10所示。在圖9中,flag為標(biāo)簽,flag=0和flag=1分別表示正、反歐拉角,對照圖5載體姿態(tài)角(正歐拉角),第一次切換發(fā)生60 s左右,此時(shí)θ超出45°,切換進(jìn)入反歐拉回路,以γr作為判斷對象,在200 s左右,θ開始進(jìn)入45°以內(nèi),γ為0°,由式(9)可知,sinγr=cosθsinγ,γr也為0°,仍在反歐拉微分方程精華區(qū),不同于文獻(xiàn)[24],此處無需切換,第二次切換在280 s左右,此時(shí),θ為0°,γ開始小于-45°,γr也開始小于-45°,進(jìn)入奇異區(qū),反歐拉角切換為正歐拉角,后續(xù)切換分析同上。

圖9 正、反歐拉角切換圖Fig.9 Switching diagram of positive and reverse Euler angles

分析圖10,可以看出濾波中姿態(tài)的均方誤差快速收斂,在正、反歐拉角切換處并未出現(xiàn)波動(dòng),可見切換對姿態(tài)均方誤差的影響較小。

圖10 姿態(tài)均方誤差變化圖Fig.10 Changing diagram of attitude mean square error

3.3 車載實(shí)驗(yàn)

大機(jī)動(dòng)仿真試驗(yàn)表明,DEUKF濾波精度較高、計(jì)算量小,若將算法運(yùn)用在小機(jī)動(dòng)性場景中,狀態(tài)切換減少,理論上更具優(yōu)勢,為驗(yàn)證有效性,進(jìn)行車載組合導(dǎo)航實(shí)驗(yàn)。

搭建車載實(shí)驗(yàn)平臺(tái),主要由XW-ADU7612型高精度航向姿態(tài)參考系統(tǒng)(attitude and heading reference system, AHRS)和XW-IMU5220型微機(jī)電慣性導(dǎo)航系統(tǒng)(micro electro mechanical system,MEMS)組成,其中,AHRS包含光纖陀螺儀、硅加速度計(jì)及GPS接收天線,GPS有效工作時(shí),AHRS提供的精度指標(biāo)為姿態(tài)0.05°,航向0.1°,速度小于0.1 m/s,定位小于2 m;主要性能指標(biāo)如表2所示。

表2 XW-IMU5220主要性能指標(biāo)

車載實(shí)驗(yàn)在空曠的操場上進(jìn)行,繞操場跑道行駛150 s,軌跡和速度分別如圖11和圖12所示,運(yùn)動(dòng)軌跡高度基本相同且天向速度為0 m/s。

圖11 實(shí)驗(yàn)運(yùn)動(dòng)軌跡Fig.11 Experiment motion trajectory

圖12 車輛運(yùn)動(dòng)速度Fig.12 Velocity of the vehicle motion

運(yùn)動(dòng)全程GPS信號(hào)接收良好,利用GPS測得的東向、北向速度和經(jīng)/緯度位置信息作為觀測量進(jìn)行組合導(dǎo)航,系統(tǒng)噪聲和量測噪聲方差根據(jù)傳感器指標(biāo)和經(jīng)驗(yàn)值進(jìn)行設(shè)置,得到兩種算法姿態(tài)估計(jì)效果如圖13～圖15所示,其中實(shí)線為AHRS提供的姿態(tài)參考基準(zhǔn),點(diǎn)劃線和虛線分別表示USQUE和DEUKF算法。

圖13 俯仰角與估計(jì)誤差Fig.13 Pitch angle and estimation error

圖14 橫滾角與估計(jì)誤差Fig.14 Roll angle and estimation error

圖15 航向角與估計(jì)誤差Fig.15 Yaw angle and estimation error

從圖13～圖15中可以看出,在MEMS/GPS直接式組合導(dǎo)航車載實(shí)驗(yàn)中,兩種算法的姿態(tài)估計(jì)精度相當(dāng),由于航向角可觀測性弱,在載體運(yùn)動(dòng)直線段,航向角基本不變,濾波估計(jì)效果不好,估計(jì)誤差有增大趨勢,最大誤差約達(dá)到9°,當(dāng)載體轉(zhuǎn)向時(shí),可觀性增強(qiáng),航向角快速收斂,誤差減小;相比航向角,水平姿態(tài)角自身可觀測性較強(qiáng),兩種算法俯仰角和橫滾角濾波估計(jì)誤差在0.5°以內(nèi)波動(dòng),沒有明顯發(fā)散趨勢。

參考AHRS提供的姿態(tài),給出兩種濾波算法姿態(tài)RMSE和運(yùn)行時(shí)間如表3所示。結(jié)果表明,水平姿態(tài)角估計(jì)效果要比航向角好,對于航向角,USQUE算法估計(jì)精度略優(yōu)于DEUKF,分別為3.334°和3.426°,但在算法運(yùn)行時(shí)間方面,DEUKF時(shí)間約為56 s,USQUE約為97 s,分別為總實(shí)驗(yàn)時(shí)間的37.3%和64.7%,可見DEUKF的計(jì)算量更小。

表3 姿態(tài)RMSE和算法運(yùn)行時(shí)間

4 結(jié) 論

USQUE算法利用姿態(tài)四元數(shù)和誤差MRP相互轉(zhuǎn)換進(jìn)行雙層濾波,有效避免規(guī)范性約束的同時(shí),計(jì)算負(fù)擔(dān)明顯加重,實(shí)時(shí)性較差,相比姿態(tài)四元數(shù),歐拉角維數(shù)更小,求解姿態(tài)矩陣無需正交化處理,本文采用無奇異的雙歐拉角進(jìn)行姿態(tài)表示,提出一種基于UKF框架的DEUKF組合導(dǎo)航算法,并同USQUE算法進(jìn)行理論分析對比,給出了具體的切換條件和流程。最后,通過SINS/GPS直接式速度與位置松組合導(dǎo)航仿真和車載實(shí)驗(yàn),得出相比USQUE算法,DEUKF算法計(jì)算量小、實(shí)時(shí)性好,水平姿態(tài)角估計(jì)精度與USQUE相當(dāng)?shù)慕Y(jié)論,可以為實(shí)際場景組合導(dǎo)航算法的選擇提供一種參考。