基于PF-UKF組合濾波的SINS/GPS組合導航系統空中對準方法

高紅蓮， 尤 杰， 曹松銀

(揚州大學 信息工程學院，江蘇 揚州 225127)

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

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

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

1 SINS/GPS組合導航系統誤差模型

1.1 狀態方程

利用誤差四元數建立新的系統誤差模型，狀態方程由SINS的誤差模型來表示.設導航坐標系為東(E)、北(N)、天(U)坐標系.16維的系統狀態向量如下：

建立SINS/GPS組合導航系統的非線性狀態方程為

(1)

式中：t為系統連續狀態的時間；f(X(t),t)為系統非線性函數；G(t)為系統噪聲驅動矩陣；W(t)為系統噪聲，且

1.2 量測方程

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

(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)為量測噪聲.

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

XK+1=f(XK)+WK

(4)

ZK=h(XK)+VK

(5)

式中：下標K表示時刻.與UKF相比，并不默認WK和VK兩種噪聲為獨立的白噪聲，不規定噪聲類型.

2 PF-UKF組合濾波器設計

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

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

(6)

(7)

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

計算2r+1個Sigma點和權值：

(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)

狀態一步預測均方差為

(16)

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

(17)

(18)

(19)

(20)

式中：RK為K時刻的量測噪聲.

濾波器增益值為

(21)

更新系統狀態變量估計值為

(22)

狀態變量估計均方差方程為

(23)

式中：PK為后驗協方差.

(24)

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

(4) 將得到的隨機粒子和確定粒子統一進行權值計算，對其進行權值歸一化：

(25)

(26)

式中：P(·)為后驗概率分布函數.

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

(27)

3 實驗仿真結果及分析

3.1 仿真條件

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

圖1 四旋翼無人機Fig.1 Quadrotor UAV

3.2 結果與分析

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

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

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

表1 PF和PF-UKF下的速度誤差和位置誤差標準差Tab.1 Standard deviations of velocity error and position error based on PF and PF-UKF

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

4 結語

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