高紅蓮， 尤 杰， 曹松銀

(揚(yáng)州大學(xué) 信息工程學(xué)院，江蘇 揚(yáng)州 225127)

近年來，隨著科學(xué)技術(shù)的飛速發(fā)展，導(dǎo)航技術(shù)也在不斷進(jìn)步.單一的導(dǎo)航系統(tǒng)已經(jīng)難以滿足現(xiàn)代復(fù)雜導(dǎo)航環(huán)境的需求，因此將多種導(dǎo)航系統(tǒng)進(jìn)行組合已成為研究的熱點(diǎn)[1].捷聯(lián)式慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System，SINS)和全球定位系統(tǒng)(Global Positioning System，GPS)的組合方式就是其中一種[2].SINS是一種利用慣性敏感器件測量運(yùn)載體實(shí)時(shí)信息，并通過計(jì)算機(jī)進(jìn)行解算得到運(yùn)載體姿態(tài)、位置、速度等參數(shù)的導(dǎo)航技術(shù)[3]，具有穩(wěn)定性強(qiáng)、刷新速率快、短時(shí)間精度高、可輸出完整導(dǎo)航信息等特點(diǎn).而GPS作為一種衛(wèi)星導(dǎo)航系統(tǒng)，雖然其精度在短時(shí)間內(nèi)與SINS存在一定差距，但不會隨著時(shí)間累積誤差，并且能在全天候、全時(shí)段以及全球范圍內(nèi)提供精準(zhǔn)的導(dǎo)航信息[4].由此可見，兩種導(dǎo)航系統(tǒng)能夠取長補(bǔ)短，彌補(bǔ)各自不足.一方面，GPS具有誤差不隨時(shí)間積累的特點(diǎn)，通過GPS的測量信息能夠?qū)崟r(shí)對SINS的誤差進(jìn)行校正，解決SINS誤差隨時(shí)間累積的問題[5].另一方面，利用SINS的高穩(wěn)定性、高動態(tài)性等特點(diǎn)可以克服GPS信號易受遮擋、周跳等問題，兩者的結(jié)合能有效降低單一導(dǎo)航系統(tǒng)的局限性[6].

對于SINS/GPS組合導(dǎo)航系統(tǒng)，濾波算法是關(guān)鍵技術(shù)之一.濾波算法將SINS解算后的數(shù)據(jù)與GPS數(shù)據(jù)進(jìn)行融合，以此修正導(dǎo)航系統(tǒng)的誤差，大大提高了導(dǎo)航性能[7].針對含有多源干擾的慣性導(dǎo)航系統(tǒng)，文獻(xiàn)[8]提出了一種基于魯棒多目標(biāo)濾波器的線性初始對準(zhǔn)方法，采用漂移估計(jì)器估計(jì)慣性傳感器誤差，混合H2/H∞濾波器抑制高斯噪聲和范數(shù)有界干擾.針對空間噪聲的不確定性導(dǎo)致卡爾曼濾波性能降低的問題，文獻(xiàn)[9]設(shè)計(jì)了一種基于擴(kuò)展H∞濾波的魯棒濾波算法來實(shí)現(xiàn)多傳感器信息融合.針對非高斯噪聲，通過最大相關(guān)熵和殘差正交原理構(gòu)造相關(guān)代價(jià)函數(shù)，文獻(xiàn)[10]提出了一種基于自適應(yīng)信息熵理論的魯棒容積卡爾曼濾波器.針對導(dǎo)航系統(tǒng)存在的非線性動力學(xué)、器件漂移、參數(shù)不確定性等多源干擾，文獻(xiàn)[11]提出了一種基于干擾抵消和抑制的多目標(biāo)濾波方法，并用于慣導(dǎo)系統(tǒng)非線性初始對準(zhǔn)問題.針對系統(tǒng)噪聲不確定性對無跡卡爾曼濾波(Unscented Kalman Filter，UKF)性能造成的影響，文獻(xiàn)[12]提出了一種基于最大似然原理的自適應(yīng)UKF算法來估計(jì)過程噪聲的協(xié)方差.此外，粒子濾波(Particle Filter，PF)由于其適用于非高斯、非線性的情形，已廣泛應(yīng)用于組合導(dǎo)航系統(tǒng).但是，隨著時(shí)間增加，PF的重要性權(quán)值可能會集中到少數(shù)粒子上，出現(xiàn)粒子退化現(xiàn)象[13].粒子退化會造成計(jì)算量都浪費(fèi)在無用粒子上，降低算法效率，甚至導(dǎo)致濾波發(fā)散.

當(dāng)前，許多SINS/GPS組合導(dǎo)航系統(tǒng)考慮小姿態(tài)誤差角下的線性誤差模型，當(dāng)運(yùn)載體處于大失準(zhǔn)角運(yùn)動狀態(tài)時(shí)，該系統(tǒng)模型具有一定的模型誤差[14].因此，針對UKF和PF等濾波方法的不足以及運(yùn)載體處于大失準(zhǔn)角下的情況，本文提出一種基于誤差四元數(shù)模型的PF-UKF組合濾波算法.摒棄傳統(tǒng)的小失準(zhǔn)系統(tǒng)誤差方程，利用誤差四元數(shù)代替狀態(tài)向量中的3個(gè)失準(zhǔn)角，使得系統(tǒng)模型能夠在大失準(zhǔn)角狀態(tài)下保持相對精準(zhǔn)度.此外，結(jié)合UKF和PF兩種濾波算法的優(yōu)點(diǎn)，將采樣粒子分為由概率密度函數(shù)采集的隨機(jī)粒子和由UKF采集Sigma點(diǎn)求取后的狀態(tài)值構(gòu)成的確定粒子，不僅能夠克服UKF對噪聲統(tǒng)計(jì)特性的約束，同時(shí)能有效降低PF存在的粒子退化問題.仿真結(jié)果表明：與PF相比，所提出的濾波算法具有更好的濾波精度，并當(dāng)系統(tǒng)存在非高斯噪聲和干擾時(shí)，具有很好的抗干擾能力.

1 SINS/GPS組合導(dǎo)航系統(tǒng)誤差模型

1.1 狀態(tài)方程

利用誤差四元數(shù)建立新的系統(tǒng)誤差模型，狀態(tài)方程由SINS的誤差模型來表示.設(shè)導(dǎo)航坐標(biāo)系為東(E)、北(N)、天(U)坐標(biāo)系.16維的系統(tǒng)狀態(tài)向量如下：

建立SINS/GPS組合導(dǎo)航系統(tǒng)的非線性狀態(tài)方程為

(1)

式中：t為系統(tǒng)連續(xù)狀態(tài)的時(shí)間；f(X(t),t)為系統(tǒng)非線性函數(shù)；G(t)為系統(tǒng)噪聲驅(qū)動矩陣；W(t)為系統(tǒng)噪聲，且

1.2 量測方程

將SINS位置和GPS接收機(jī)輸出位置的差值以及SINS速度和GPS接收機(jī)輸出速度的差值作為量測信息，建立量測方程：

(2)

(3)

式中：h(X(t),t)為非線性部分；λI、LI、hI為SINS位置輸出值；λG、LG、hG為GPS位置輸出值；vIE、vIN、vIU為SINS速度輸出值；vGE、vGN、vGU為GPS速度輸出值；δλI、δLI、δhI為SINS位置誤差；δvGE、δvGN、δvGU為GPS速度誤差；V(t)為量測噪聲.

對系統(tǒng)(1)和(2)離散化，得到SINS/GPS組合導(dǎo)航系統(tǒng)的離散非線性誤差模型為

XK+1=f(XK)+WK

(4)

ZK=h(XK)+VK

(5)

式中：下標(biāo)K表示時(shí)刻.與UKF相比，并不默認(rèn)WK和VK兩種噪聲為獨(dú)立的白噪聲，不規(guī)定噪聲類型.

2 PF-UKF組合濾波器設(shè)計(jì)

結(jié)合PF和UKF算法，將采樣粒子分為兩部分進(jìn)行采集，降低了粒子退化的程度，具體過程如下.

(1) 初始化.由先驗(yàn)狀態(tài)分布抽取初始值X0，確定初始狀態(tài)矢量和初始協(xié)方差：

(6)

(7)

式中：E(·)為期望函數(shù).

計(jì)算2r+1個(gè)Sigma點(diǎn)和權(quán)值：

(8)

(9)

(10)

W0=τ/(r+τ)

(11)

Wi=1/[2(r+τ)]

(12)

Wi+r=1/[2(r+τ)]

(13)

Xi, K/K-1=f(Xi, K-1)

(14)

(15)

狀態(tài)一步預(yù)測均方差為

(16)

Zi, K/K-1=h(Zi, K/K-1)

(17)

(18)

(19)

(20)

式中：RK為K時(shí)刻的量測噪聲.

濾波器增益值為

(21)

更新系統(tǒng)狀態(tài)變量估計(jì)值為

(22)

狀態(tài)變量估計(jì)均方差方程為

(23)

式中：PK為后驗(yàn)協(xié)方差.

(24)

式中：q(·)為重要性概率密度函數(shù)；N(·)為正態(tài)分布.

(4) 將得到的隨機(jī)粒子和確定粒子統(tǒng)一進(jìn)行權(quán)值計(jì)算，對其進(jìn)行權(quán)值歸一化：

(25)

(26)

式中：P(·)為后驗(yàn)概率分布函數(shù).

(5) 為了使所采集的隨機(jī)粒子和確定粒子的數(shù)量能夠保證規(guī)定的有效粒子數(shù)，在不滿足時(shí)仍對其進(jìn)行重采樣步驟，使得有效粒子的權(quán)值為1/N，此時(shí)計(jì)算狀態(tài)估值：

(27)

3 實(shí)驗(yàn)仿真結(jié)果及分析

3.1 仿真條件

為了驗(yàn)證SINS/GPS組合導(dǎo)航系統(tǒng)下的PF-UKF組合濾波器的性能，將其與傳統(tǒng)的PF算法進(jìn)行仿真對比.設(shè)置初始條件如下：初始位置為東經(jīng)40°、北緯50°、高800 m，SINS初始速度誤差為 0.1 m/s，東、北、天3向失準(zhǔn)角誤差為1°、1°、5°，加速度計(jì)初始偏差為1×10-4g，加速度計(jì)隨機(jī)偏差為5×10-5g，陀螺儀常值漂移為0.05(°)/h，陀螺儀隨機(jī)漂移為0.1(°)/h，慣性器件采樣時(shí)間為0.05 s.GPS的速度誤差為0.1 m/s，水平位置誤差為2.5 m，高度位置誤差為5 m，GPS信號采樣周期為0.1 s.考慮大失準(zhǔn)角下的運(yùn)載體狀態(tài)，采用四旋翼無人機(jī)設(shè)備進(jìn)行數(shù)據(jù)采集，如圖1所示.其飛行軌跡設(shè)置為從初始位置起勻速直行50 s, 左轉(zhuǎn)90° 勻速飛行50 s，右轉(zhuǎn)45° 勻速飛行50 s，爬升20° 勻速飛行100 s，下降20° 勻速飛行100 s.

圖1 四旋翼無人機(jī)Fig.1 Quadrotor UAV

3.2 結(jié)果與分析

對PF算法和PF-UKF組合濾波算法下的位置誤差和速度誤差進(jìn)行對比.圖2分別為在PF以及PF-UKF下的經(jīng)度、緯度和高度位置誤差，圖3分別為在PF以及PF-UKF下的東、北、天3向的速度誤差；表1為兩種方法下的速度誤差和位置誤差的標(biāo)準(zhǔn)差.

圖2 不同算法下的位置誤差Fig.2 Position errors of different algorithms

圖3 不同算法下的速度誤差Fig.3 Velocity errors of different algorithms

表1 PF和PF-UKF下的速度誤差和位置誤差標(biāo)準(zhǔn)差Tab.1 Standard deviations of velocity error and position error based on PF and PF-UKF

圖2中，雖然PF和PF-UKF都能使系統(tǒng)速度誤差趨于穩(wěn)定狀態(tài)，但PF下的位置誤差的波動明顯大于PF-UKF，并且PF-UKF下的位置誤差收斂速度更快，精度也更高.圖3中，雖然PF和PF-UKF下的速度誤差在開始階段都存在一定的波動性，但PF-UKF算法下的速度誤差收斂速度更快，大約在50 s后逐漸收斂，并趨于平穩(wěn).由表1可知，相比于PF，PF-UKF濾波下的位置誤差和速度誤差標(biāo)準(zhǔn)差均較小，說明該濾波器更加穩(wěn)定.無論是位置誤差還是速度誤差，導(dǎo)航系統(tǒng)的誤差在PF-UKF濾波下得到了更有效的收斂，并且相比于傳統(tǒng)的PF濾波，所提濾波算法在大失準(zhǔn)角的狀態(tài)下，效果也更優(yōu)異，有效克服了傳統(tǒng)PF濾波存在的粒子退化問題，降低了粒子退化程度.由此可見，在PF-UKF濾波下，SINS/GPS組合導(dǎo)航系統(tǒng)能夠在大失準(zhǔn)角的狀態(tài)下獲得了較好的濾波效果，有效提高了系統(tǒng)的導(dǎo)航精度和收斂速度.

4 結(jié)語

傳統(tǒng)的SINS/GPS組合導(dǎo)航系統(tǒng)誤差模型主要應(yīng)用于小失準(zhǔn)角的情況，但實(shí)際上運(yùn)載體常處于大失準(zhǔn)角的狀態(tài)，且系統(tǒng)噪聲特性未知.而傳統(tǒng)PF又存在粒子退化問題，本文將PF算法和UKF算法相結(jié)合，提出了一種基于PF-UKF的組合濾波算法.利用誤差四元數(shù)代替狀態(tài)向量中的3個(gè)姿態(tài)角，使得系統(tǒng)模型能夠在大失準(zhǔn)角的狀態(tài)下保持相對精準(zhǔn)度.此外，為了降低傳統(tǒng)PF算法中粒子退化的程度，將PF-UKF濾波算法所要處理的粒子分為由UKF采集Sigma點(diǎn)后求取的狀態(tài)值構(gòu)成的確定粒子和由PF的概率密度函數(shù)采集的隨機(jī)粒子.結(jié)果表明：該濾波算法不僅能夠克服UKF對噪聲要求的問題，同時(shí)能夠有效克服PF存在的粒子退化的問題，具有較好的魯棒性.