基于多傳感器組合導航系統的自適應算法研究*

侯 琪 李 沅 李 皓 王 豐

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

1 引言

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

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

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

2 多源組合導航濾波模型

2.1 系統方程

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

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

2.2 量測方程

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

1）GPS系統量測模型

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

2）偏振光導航系統量測模型

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

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

3 自適應卡爾曼濾波參數調整

3.1 量測噪聲方差陣R估計

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

3.1.1 GPS量測噪聲方差估計

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

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

因短時間內系統誤差變化量很小，又慣導短時間內精度高，誤差累計小，所以有：

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

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

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

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

后續對偏振光導航系統航向角的方差估計計算與3.1.1 小節同理，得到偏振光航向角量測噪聲方差估計為

3.2 修正系統噪聲方差陣

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

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

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

4 數據仿真與結果分析

4.1 仿真條件

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

圖1 運動軌跡

4.2 結果分析

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

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

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

圖2 航向角濾波誤差對比

圖3 經度濾波誤差對比

圖4 緯度濾波誤差對比

5 結語

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