侯 琪 李 沅 李 皓 王 豐

（中北大學信息與通信工程學院 太原 030051）

1 引言

慣性導航系統(tǒng)（Inertial Navigation System，INS）是一種定位定向?qū)Ш皆O備，為載體自主、實時提供姿態(tài)、速度和位置信息［1］，具有不受地理限制、干擾小、結(jié)構(gòu)簡單等優(yōu)點。其本質(zhì)上是一個數(shù)值積分系統(tǒng)［2～3］，在實際應用中，由于器件的各種噪聲以及計算上的誤差會隨著時間的增加而不斷累積并發(fā)散［4］。捷聯(lián)慣性導航系統(tǒng)（Strap-down Inertial Navigation System，SINS）因省去機電式的導航平臺，使得整個系統(tǒng)體積、重量以及成本大大降低，但同時其動態(tài)誤差要比平臺式系統(tǒng)動態(tài)誤差大，因此在長時間內(nèi)很難獨立使用。全球定位系統(tǒng)（Global Positioning System，GPS）作為非自主導航系統(tǒng)，具長期穩(wěn)定定位、量測智能化等特點，但易受到外部環(huán)境因素的干擾導致可靠性下降。隨著偏振技術(shù)的發(fā)展，天空偏振光作為仿生自主導航的重要分支，具有抗干擾能力強的優(yōu)勢［5～6］。多源組合導航可以充分利用單一導航的優(yōu)勢，提高導航的容錯性與魯棒性，增加觀測冗余度［7］。

對于組合導航系統(tǒng)，濾波算法是影響導航性能的主要因素，工程上基本采用Kalman 濾波（KF）算法進行組合導航數(shù)據(jù)融合。模型統(tǒng)計噪聲級在濾波過程之前給出，整個遞推過程中保持不變，實際中眾多因素會使得這種先驗信息不再足以表示真實的統(tǒng)計噪聲水平，不準確的狀態(tài)估計量反饋至導航系統(tǒng)后會造成濾波發(fā)散問題。針對噪聲統(tǒng)計特性的不準確問題，通常采用自適應卡爾曼濾波算法［8］。自適應濾波算法包括衰減記憶濾波、Sage-Husa 自適應濾波等方法，其本質(zhì)都是通過不同參數(shù)調(diào)節(jié)噪聲方差陣進行自適應估計，使其不斷地適應系統(tǒng)的變化［9］。但衰減記憶濾波只能在一定程度上修正模型誤差引起的系統(tǒng)噪聲［10］。Sage-Husa自適應濾波的噪聲協(xié)方差矩陣在多維系統(tǒng)中易失去非負定性并導致濾波發(fā)散［11］。

針對上述問題，本文提出了一種基于SINS/偏振光/GPS 組合導航自適應濾波算法，來解決子系統(tǒng)噪聲方差陣異常或系統(tǒng)動態(tài)模型不準確情況導致的濾波發(fā)散問題。

2 多源組合導航濾波模型

2.1 系統(tǒng)方程

組合導航系統(tǒng)的誤差狀態(tài)向量均定義為SINS誤差，包含9 個SINS 的基本導航參數(shù)誤差和6 個慣導儀表的誤差狀態(tài)量，因此組合導航系統(tǒng)的狀態(tài)向量可表示為

其中，δL、δλ、δh表示經(jīng)、緯、高度的誤差；δV表示東北天方向三維速度誤差；?表示東北天方向的航向姿態(tài)角誤差；ε為隨機漂移，?表示加速度計零偏。

2.2 量測方程

SINS/偏振光/GPS 組合導航系統(tǒng)［6］量測方程中ZP、Zl分別為偏振光、慣性導航系統(tǒng)的觀測矩陣，HP、Hl分別為偏振光、慣性導航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，如下：

1）GPS系統(tǒng)量測模型

將捷聯(lián)慣導系統(tǒng)與GPS 之間的位置差Zl作為系統(tǒng)的位置量測信息。位置量測方程中Zl、Hl和Xl分別為

2）偏振光導航系統(tǒng)量測模型

將系統(tǒng)解算的航向角φs與偏振光導航解算出的航向角φp之差Δφ作為航向角的觀測量，當姿態(tài)角誤差為小角度時，航向角誤差與平臺誤差角的關系為

其中，θs是導航系統(tǒng)解算出來的俯仰角。偏振光量測模型中Zp、Hp和Xp分別為

3 自適應卡爾曼濾波參數(shù)調(diào)整

3.1 量測噪聲方差陣R估計

GPS 信號易受環(huán)境影響導致失鎖［12］，偏振光輔助導航易受天氣影響導致航向角信息誤差增大，而SINS 短期內(nèi)精度高。因此可利用這一互補優(yōu)勢對GPS 和偏振光系統(tǒng)量測噪聲方差統(tǒng)計特性進行有效估計。

3.1.1 GPS量測噪聲方差估計

設K 時刻GPS 和SINS 得到的同一軌跡的位置信息分別為ZGPS(k)和ZSINS1(k)，根據(jù)基于冗余量測噪聲估計定理［9］，有：

其中，f(k)為量測系統(tǒng)誤差，V(k)為動態(tài)噪聲，且為零均值白噪聲，ΔZSINS1(k)、ΔZGPS(k) 分別為SINS、GPS各自的一階自差分序列。

因短時間內(nèi)系統(tǒng)誤差變化量很小，又慣導短時間內(nèi)精度高，誤差累計小，所以有：

其中RSINS1(k)、RGPS(k)分別為SINS 和GPS 測量噪聲方差陣。

則GPS量測噪聲方差估計可表示為

3.1.2 偏振光航向角噪聲方差估計

設K 時刻SINS 計算得到的航向信息為ZSINS2(k)，偏振光探測系統(tǒng)解算得到的航向角信息作為姿態(tài)量測為Zlight(k)，有：

后續(xù)對偏振光導航系統(tǒng)航向角的方差估計計算與3.1.1 小節(jié)同理，得到偏振光航向角量測噪聲方差估計為

3.2 修正系統(tǒng)噪聲方差陣

通過3.1 節(jié)對R 的準確估計，該等式不成立則認為是由P(k，k-1)不準確引起的，P(k，k-1)對等式的影響程度可表示為

P(k，k-1)不準確主要受Q 影響且會呈現(xiàn)與Q相同的趨勢，所以可通過αk實現(xiàn)對Q 的調(diào)整，間接實現(xiàn)對P(k，k-1)的調(diào)整：

其中，αk大于1 則表明系統(tǒng)噪聲偏大，通過式（14）增大Q(k-1)，反之減小。

4 數(shù)據(jù)仿真與結(jié)果分析

4.1 仿真條件

本文利用軌跡發(fā)生器產(chǎn)生的仿真數(shù)據(jù)進行算法對比，其中SINS 采樣頻率為100Hz，偏振光和GPS 導航采樣頻率為1Hz；軌跡初始經(jīng)緯度位置為112.444997°、38.016847°，高度為0m；初始速度為0m/s。設置陀螺儀零偏誤差為0.3°/h；隨機漂移為0.01°/h；加速度計零偏誤差為100ug；仿真時長為2011s，生成軌跡如圖1所示。

圖1 運動軌跡

4.2 結(jié)果分析

為驗證本文多源組合導航自適應算法的優(yōu)勢，在軌跡仿真1000s～1500s 內(nèi)增大誤差干擾，并且與擴展卡爾曼濾波（EKF）算法進行濾波對比，實驗結(jié)果如下圖所示。

如圖2所示，航向角在整體濾波結(jié)果上有明顯改善，在變噪時段濾波后均方根誤差降低了55%。如圖3、圖4所示，在噪聲非突變情況下，EKF 和本文改進自適應算法濾波的誤差結(jié)果基本一致，但在1000s～1500s 的噪聲突變情況下，通過圖3、圖4可以直觀看出本文算法的優(yōu)越性，表1可以看出該自適應算法較EKF 在經(jīng)緯度均方根誤差上分別降低了21.6%、25%。

表1 變噪時段濾波結(jié)果均方根誤差對比

圖2 航向角濾波誤差對比

圖3 經(jīng)度濾波誤差對比

圖4 緯度濾波誤差對比

5 結(jié)語

針對GPS 和偏振光輔助導航系統(tǒng)出現(xiàn)干擾較大，量測噪聲估計不準確以及系統(tǒng)動態(tài)模型不準確的情況，本文提出了一種基于組合導航自適應濾波調(diào)節(jié)方法。最終仿真實驗表明該方法相比較EKF算法對姿態(tài)和位移有更好的修正作用，系統(tǒng)得到了有效的改善。