基于聯邦濾波的偏振光/SINS/BDS/地磁組合導航算法*

馬 偉, 李 沅, 康 健, 侯 琪, 李 皓, 王 豐

(中北大學 信息與通信工程學院,山西 太原 030051)

0 引 言

世界上單一的導航信息源建立較早的有衛星導航、慣性導航系統(inertial navigation system,INS)、地磁導航等[1]。INS不依賴外界環境可實現完全自主導航,精度高，穩定性好,但其定位誤差隨時間積累,長時間工作易產生較大的誤差[2]；衛星導航可實現全天候連續精密導航與定位,但衛星易受人為或自然因素影響[3]；地磁導航可實現全天候、全地域導航,但需存儲大量地磁數據,且易受外界電磁干擾[4]；多源組合導航可以充分利用單一導航的優勢,提高導航的容錯性與魯棒性,增加觀測冗余度[5]。偏振光導航具有抗干擾性強、誤差不隨時間積累、易于微小型化等優點,因此，國內外學者結合偏振光進行了一系列組合導航研究[6]。

本文主要采用偏振光/捷聯慣性導航系統(strapdown inertial navigation system,SINS)/北斗衛星導航系統(BeiDou navigation satellite system,BDS)/地磁的多源組合導航模型,針對多源融合非線性特點,設計了一種基于聯邦卡爾曼濾波的偏振光/SINS/BDS/地磁組合導航算法,算法中主濾波器負責最優融合,子濾波器進行姿態、位置局部估計,采用偏振光、地磁輔助SINS定姿,地磁、BDS輔助SINS定位,提高組合導航的姿態、定位測量精度,最后進行試驗,其結果表明,該方法能夠滿足平臺、地面、空間導航。

1 聯邦濾波組合導航系統模型

1.1 聯邦濾波組合導航設計方案

本文設計的聯邦濾波器中,以SINS為公共的信息參考系統,使用4個信息源,因此設計3個子濾波器,1個主濾波器。每一個子濾波器都將各自的量測信息處理得到局部估計量,主濾波器將各子濾波器的局部估計量進行最優估計,圖1為整個系統的結構圖。

圖1 多源組合導航系統框圖

1.2 聯邦濾波組合導航狀態方程

SINS作為公共的信息參考系統,采用東北天地理參考系作為導航坐標系。系統的公共狀態方程為

X(t)=[δL,δλ,δh,δVE,δVN,δVU,φE,φN,φU,εrx,εry,

(1)

X=F(t)X(t)+G(t)W(t)

(2)

式中F(t)為SINS的狀態轉移矩陣,G(t)為SINS的系統噪聲驅動矩陣,系統噪聲為W(t)。

1.3 聯邦濾波組合導航量測方程

本文設計的聯邦濾波器主濾波器無信息分配,主濾波器不進行濾波任務,主濾波器只針對各子濾波器的估計值進行最優融合得到全局的最優估計。以下為各子濾波器的量測方程。

1)SINS/北斗組合的量測方程

SINS與BDS的位置量測方程

(3)

(4)

式中λ,L,h為經度、緯度和高度的參考值；Me,Mn,Mu為BDS在東北天3個方向上的位置誤差。

2)SINS/偏振光組合的量測方程

由于姿態角的誤差角度為小角度,把偏振光解算出的航向角φp和系統解算出來的航向角φx之差作為航向角的觀測量。航向角的誤差關系式如式(5)所示

(5)

式中θx與φx分別為系統解算出的俯仰角與航向角。偏振光的量測方程為

(6)

3)SINS/地磁組合的量測方程

磁力計給出的航向角解算值是φ′,實際的航向角為φ,因此,航向角的計算誤差為δφ,所以有φ′=φ+δφ。由姿態矩陣與姿態角,坐標變換理論的關系能夠得到

(7)

式中t為數學平臺坐標系。式(8)如下

(8)

聯立式(7)與式(8)可得

(9)

tanφ′=tan(φ+δφ)=-12/22

(10)

由于δφ是一個小量,因此,可近似認為tanδφ=δφ,所以,將式(10)用泰勒級數展開得出

(11)

tan(φ+δφ)=tanφ+(1+tan2φ)δφ

(12)

聯立式(11)與式(12)可得

(13)

故SINS/地磁組合量測方程為

(14)

2 聯邦濾波器設計

本文設計的聯邦濾波器,子濾波器進行量測更新,主濾波器進行噪聲協方差陣更新和誤差協方差陣時間更新。將上述狀態方程和量測方程離散化為

(15)

整個系統中子濾波器與主濾波器是存在信息分配守恒定律,但主濾波器不參與濾波,所以主濾波器分配系數為零,子濾波器之間的分配系數關系為

(16)

協方差陣時間更新為

(17)

狀態估計更新和濾波估計為

(18)

子濾波器的增益為

(19)

子濾波器的估計均方差為

(20)

子濾波器完成相應的局部估計后主濾器進行全局的最優估計,主濾波器的最優估計為

(21)

3 實驗結果與分析

為了對本文設計的聯邦卡爾曼濾波器的性能進行測試與驗證,進行了基于車載運行軌跡的多源融合實驗。 實驗地點為中北大學 (東經：112.451 52°,北緯：38.018 68°),實驗時間為2021年3月5日,實驗天氣晴朗無云,實驗條件為空曠地域進行車載2 300 m的往返實驗,具體實驗結果如圖2～圖6所示。圖7為實驗場地示意。

圖2 導航軌跡

圖3 融合軌跡

圖4 航向角解算曲線

圖5 單一導航誤差曲線

圖6 組合導航誤差曲線

圖7 實驗場地示意

由圖5(a)可知在數據長度在200～300時地磁東向誤差曲線發生突變,是由于剛好路過一處電磁干擾地,因此，地磁信號受到干擾導致誤差急劇加大。

從圖2～圖6可以看出：單一導航情況下地磁的位置誤差較大,兩兩組合導航的情況下較單一信息源導航的定位精度高,在兩個不同傳感器的組合情況下,偏振光/地磁的組合誤差較其他兩兩組合的誤差大,從表1～表2數據可知SINS/偏振光組合比SINS/地磁、偏振光/地磁組合導航的定位精度高，且定位穩定性也相對可靠,而偏振光/SINS/BDS/地磁多個信息源的組合導航其定位精度明顯提高,其東向誤差為3.230 5 m,北向誤差為3.741 9 m。整個實驗表明本算法可以實現多種傳感器較為可靠的融合定位,可以有效地提高測量精度,得到可靠的定位信息。

表1 偏振光/SINS/BDS/地磁組合導航與單一導航最大誤差對比 m

表2 兩兩組合導航誤差對比

4 結 論

本文針對高穩定目標導航參數獲取對多傳感器融合算法的需求,利用偏振光的偏振信息與地磁信息,提出一種偏振光/SINS/BDS/地磁組合導航方法。多源組合導航充分利用偏振光與地磁誤差不隨時間積累,可以提供高精度的姿態信息,結合SINS,BDS可以在不同環境情況下優勢互補,提高組合導航的可靠性與姿態測量精度。實驗表明:本文算法能夠提供精準的航向角和位置估計,偏振光/SINS/BDS/地磁組合導航能夠可靠提供定位信息,綜上所述本方法適用于平臺、地面、空間導航。