基于自適應迭代擴展卡爾曼濾波算法的INS/BDS組合導航系統

張源　詹金林　韓冰　陳偉

摘要

為克服慣性導航系統（INS）的積累誤差，提高誤差的修正精度，提出了基于多天線北斗差分載波相位的北斗/慣性導航系統組合導航算法。該算法建立并線性化慣性導航系統（INS）和北斗導航系統（BDS）的狀態方程和量測方程，對系統的運動狀態參數應用自適應迭代擴展卡爾曼濾波（adaptive iterated extended Kakman filter，AIEKF）算法進行估計。仿真結果表明，自適應迭代擴展卡爾曼濾波算法能夠提高INS/BDS組合導航系統的精度和抗干擾能力，驗證了自適應迭代擴展卡爾曼濾波算法的有效性。

關鍵詞

INS;BDS;組合導航;自適應卡爾曼濾波

中圖分類號： U666.1 ? ? ? ? ? ? ? ? ? ? ? ? 文獻標識碼： A

DOI：10.19694/j.cnki.issn2095-2457.2020.04.81

慣性導航系統（Inertial Navigation System， INS）和北斗衛星導航系統（Beidou Navigation Satellite System，BDS）是目前兩種重要的艦船導航系統。慣性導航系統（INS）是自主導航系統，僅依靠自身就能進行連續的導航和定位，具有自主、隱蔽等特性，所獲取艦船的運動信息完備，但其定位誤差是積累的，隨著時間的積累而不斷增大[1]。北斗衛星導航系統（BDS）的定位精度系統與第3代GPS定位精度相當，具有觀測時間短、定位連續、精度高、誤差不隨時間積累等優點，可提供覆蓋全球的精準定位、導航和授時（Positioning， Navigation and Timing， PNT）服務[2]。由于兩種系統所具備優勢具有互補的特性，將兩者結合起來構成更高精度的組合導航系統備受人們關注。通常將BDS導航信息和INS相應的導航信息進行比對，采用卡爾曼濾波器根據觀測差值在線估計INS的各類誤差并進行相應補償。在以往文獻中，大多將GPS輸出的位移和速度信息輸入到Kalman濾波器中，濾波輸出的位移、速度和姿態信息又反饋回INS，修正INS的積累誤差[3]。艦艇組合導航系統要求具備更高的系統精度和抗干擾能力，提高外部觀測數據的信息數量和質量可以較好地提高INS各類誤差的可觀測性，而高精度北斗姿態測量系統可以提供載體的位置、速度、航向和姿態等信息。文中將高精度北斗姿態測量系統與INS進行組合，將BDS輸出的位置、速度和姿態信息作為INS的外部修正信息，以獲得綜合性能更加優良的艦船組合導航系統。

標準的卡爾曼濾波器有一系列必要條件，這些條件包括：狀態模型和量測模型均為線性，狀態噪聲和量測噪聲均為零期望白噪聲，狀態噪聲、量測噪聲和狀態三者之間不相關，初值或其統計特性（期望和協方差矩陣）已知。當滿足這些條件時，濾波結果在無偏最小方差意義上是最優的;反之，如果不滿足這些條件，濾波結果不是最優的。但在實際中，BDS的測量噪聲會受到多種因素（如：多路徑效應、內部噪聲等）的影響發生變化，標準的卡爾曼濾波器由于其局限性，無法對上述的變化進行相應的檢測和調整，導致量測噪聲和狀態噪聲建模不準確，會出現嚴重的估計偏差。為解決此類問題，大量學者致力于自適應濾波算法的研究工作，通過實時調整模型參數和噪聲統計特性優化算法的濾波精度。文獻所報道的自適應濾波算法主要集中于基于新息估計的卡爾曼濾波器（IAE）[4]和多模型卡爾曼濾波器（MMAE）[5]設計。其中。基于新息估計的自適應濾波算法主要采用協方差匹配、輸出相關技術和極大似然估計等思想[6]，如Sage-Husa自適應濾波算法可自適應噪聲協方差矩陣[7]，Sasiadek根據新息的方差與均值自動調整加權擴展卡爾曼濾波器的權值[8]，Loebis通過診斷新息方差變化，對濾波器使用的系統噪聲方差陣R的增量進行調整獲取系統的穩定性能[5]，Zhang基于新息對系統噪聲方差陣R進行系數調整[9]。仿真驗證，上述方法不增加系統狀態的維數，計算量低，同時獲得較好的精度。

本文針對BDS觀測誤差較大變化的情況下，通過對實際新息的測量和計算，直接對卡爾曼濾波器的增益矩陣進行自適應修正，提高了濾波器的精度和魯棒性。

1 自適應迭代擴展卡爾曼濾波算法（AIEKF）[10]

對于確定性系統，已知系統初始條件，通過求解系統的微分方程，就可以得到系統在未來任意時刻的準確狀態。但實際中的系統在運行過程中必然受到各種干擾和噪聲的影響，在其運行狀態中產生各種誤差。卡爾曼濾波器是組合導航系統數據處理中常用的最優估計算法，但只針對線性系統，要求狀態方程和量測方程均為線性，采用狀態空間法建立準確的狀態方程和量測方程，在設定狀態噪聲與量測噪聲均為零期望白噪聲統計特性的基礎上，根據系統每一時刻的觀測量，運用遞推算法實現對系統狀態的最優估計[10]。當組合導航的系統模型和觀測模型確定時，解算精度較高。卡爾曼濾波器的增益矩陣K是調節加權權重的參數，其計算需要依賴于事先已知的狀態轉移矩陣Φ、量測矩陣H、系統噪聲方差陣R和狀態噪聲協方差矩陣Q，并不依賴在線的實際觀測量。在算法的實際運行中，狀態轉移矩陣Φ、量測矩陣H、狀態噪聲協方差矩陣Q和系統噪聲方差陣R發生變化，K的計算必然會出現偏差，不再滿足最優估計條件，濾波器將發散。有效提高K值在濾波過程的修正能力，是解決這一問題的根本途徑。

新息是指濾波器的測量估計值與實際測量值之間的差值。當濾波器工作正常時，理想狀態下的新息應為零均值白噪聲序列。當狀態估計由于建模誤差而導致估計誤差增大時，導致新息增大且統計特性復雜，此時，基于新息相應調整濾波器的增益、量測噪聲和觀測噪聲的大小，可能會抑制狀態估計誤差的持續增大，提高狀態估計的精度。

對于離散線性系統模型，其狀態方程和量測方程如下：

式中，Xk為狀態估計;Wk為系統噪聲序列;Vk為量測噪聲序列;Φk，k-1為tk-1時刻至tk時刻的一步轉移矩陣;Гk，k-1為系統噪聲驅動矩陣;Hk為量測矩陣。且Wk和Vk互不相關并滿足：E[Wk]=0，Cov[Wk，Wj]=QKδkj，E[Vk]=0，Cov[Vk，Vj]=RKδkj。以新息方式列寫標準卡爾曼濾波算法，如式（1）～（5）所示。

2 BDS/INS組合導航系統及系統模型

2.1 組合系統總體設計

INS/BDS組合導航系統以慣性導航系統（INS）作為組合導航系統的關鍵子系統，而北斗衛星導航系統（BDS）作為輔助子系統。通過組合，使用BDS輸出的位置、速度和姿態等信息來修正陀螺漂移、加速度計偏差和初始失準角引起的INS位置、速度和姿態的誤差，從而獲得高精度的導航信息。圖1為INS/BDS組合導航系統總體設計圖，INS和BDS之間采取松散耦合方式進行組合，兩個子系統獨立工作，各自輸出導航參數。這種組合方式采取位置、速度和姿態組合，將INS的誤差方程作為組合導航系統的狀態方程，將BDS和INS各自輸出的位置、速度和姿態之差作為觀測量，采取自適應迭代擴展卡爾曼濾波器對INS的位置誤差、速度誤差、姿態誤差及慣性元器件誤差進行最優估計，然后對INS進行輸出反饋校正。在系統中，北斗導航姿態測量模塊根據多天線北斗載波觀測數據，建立北斗雙差載波相位觀測模型，利用系統輸出信息輔助整周模糊度解算、周跳探測與修復，最終解算出整周模糊度，解算出高精度的艦船姿態信息。

這種組合方式的優點是可以估計出組合導航系統的速度誤差和位置誤差，由于BDS提供INS修正所必需的位置、速度、航向和縱搖6種信息，可以較好地提高INS誤差狀態的可觀測性和可觀測度。通過補償能夠大幅度地提升系統的定位精度。此外，該系統組合方式原理簡單，工程實現容易，子系統獨立工作，并具有系統冗余，在實際組合導航系統中應用較多。但是，在系統的實際應用中，多種因素會導致BDS的測量誤差不穩定，所以相關的卡爾曼濾波器必須采取相關算法抑制BDS觀測噪聲增大的影響。本文中提出的AIE自適應算法對本組合系統效果進行改善。

2.2 系統誤差模型

2.2.1 慣性導航系統系統誤差模型

系統誤差模型選取半解析式當地水平指北平臺式慣性導航系統，采用東北天坐標系。選用定位誤差、速度誤差、平臺誤差角、陀螺漂移和加速度漂移誤差作為慣性導航系統的狀態變量。

式中：δL，δ？姿為慣導系統在緯度和經度上的定位誤差;δve，δvn為慣導系統的東向速度誤差和北向速度誤差;φe，φn，φu為平臺的東向、北向和方位誤差角;？犖e，？犖n為東向和北向加速度計漂移誤差;εe，εn，εu為東向、北向和方位陀螺儀漂移。

BINS=[02×12;05×7 I5×5;05×12]T，WINS=[01×7 W ?W ?W ?W ?W ]T，WINS中，W ，W 分別為東向、北向加速度計隨機偏差，W ?W ?W 分別為東、北向和方位陀螺隨機漂移，以上各值均設定為零均值、方差為QINS的白噪聲。FINS為12×12的矩陣，如式（12），式中L為艦船所處位置的緯度，g為當地重力加速度;RM、RN分別為地球卯酉圈和子午圈半徑;ωie為地球自轉角速度。

2.2.2 組合系統量測方程

設定組合導航系統狀態模型與慣導系統的誤差模型一致，令慣性導航系統輸出的載體姿態角的測量值分別為ψI=ψ+δψI、θI=θ+δθI、？酌I=？酌+δ？酌I，令BDS測量的姿態角分別為ψB=ψ+δψB、θB=θ+δθB;考慮到當前組合導航系統獲得的INS的位置速度是由計算機運算數字發送實現，而艦船姿態角則依靠平臺高精度的旋轉變壓器測量并通過高精度的數字固態發送實現，上述傳輸過程中的誤差與INS的系統誤差相比較小。所以為了簡化對準時刻的系統觀測方程，忽略INS的測量噪聲，而將BDS姿態測量系統的各種誤差歸為組合系統的量測噪聲。根據試驗分析，假定VBDS是零均值，方差強度可變的BDS測量白噪聲，RBDS為其正常情況下的噪聲協方差。VBDS，WINS互不相關。得到組合導航系統的量測方程如下所示。

令載體坐標系為b系，地理坐標系為t系，平臺坐標系為p系，設載體的航向角、俯仰角和橫滾角分別為ψ、θ和？酌，由坐標變換理論可知C ?=C ?×C ?，得：

（15）

3 仿真結果分析

為驗證方法的精度和魯棒性，對INS/BDS組合導航系統進行計算機仿真測試，仿真測試工具采用MATLAB，仿真時間為3600s，采樣時間為1s。

仿真條件如下所示：艦船航向ψ=45°，θ=1°，？酌=1°，Ve=Vn=5m/s，φ=32°N;INS參數如下：φe=φn=2'，φu=5'，？犖e=？犖n=（1×10-5）g，δφ0=δλ0=0.05"，δVe=δVn=0.1m/s，εe=εn=εu=（1×10-3）（°）/h;INS系統噪聲方差（QINS）為：δ ?=δ ?=（1×10 g） ，δ ?=δ ?=δ ?=（5×10 （°）/h） ，δ ?=δ ?=（0.2m/s） ;BDS量測噪聲方差（RBDS）為δ ?=（3'）2，δ ?=（30"）2，δ ?=δ ?=（0.01"）2，δ ?=δ ?=（0.2m/s）2。

對于標準卡爾曼濾波器，預先設定系統噪聲，計算INS的各項誤差，INS誤差估計效果仿真曲線如圖2、圖3所示中灰線所示。而對于自適應迭代擴展卡爾曼濾波（AIEKF），根據sage開窗估計法，設定記憶窗口長度N=20，系統工作100秒之后，切換到新息自適應控制階段，根據AIEKF算法適時改進卡爾曼增益K的計算，仿真曲線如INS誤差估計效果仿真曲線如圖2、圖3所示中黑線所示。

從仿真結果對比中可以看出：標準的卡爾曼濾波器在BDS量測噪聲發生變化時，由于濾波器魯棒性較差，無法準確跟蹤系統狀態變化，系統噪聲方差矩陣R無法適時調整，導致濾波器輸出的位置誤差、速度誤差、平臺誤差角以及元件誤差等各參數出現較大偏差。在同樣的仿真條件下，當外部量測噪聲變大時，AIEKF算法通過對新息計算，自適應調整濾波器的R值，使增益K自動修正，減弱了量測噪聲對濾波值的影響，表現出較高的精度和抗干擾性。

從曲線中，我們可以看出采用了BDS姿態測量系統的INS/BDS組合導航系統可以較好地對各個參數進行相應估計，所以INS/BDS組合系統無論在初始對準還是動態組合條件下的INS參數誤差能力都比僅采取位置或速度的系統優越。

4 結論

本文將AIEKF算法應用于INS/BDS艦艇用組合導航系統，通過對新息計算，自動根據量測噪聲變化調節增益K，減弱了量測噪聲對濾波值的影響。組合導航需要采取高精度BDS姿態測量系統為INS提供外部修正信息，將載體姿態信息引入系統，增加了系統的觀測量種類，能夠較好地克服由于BDS量測噪聲的不穩定特性造成系統卡爾曼濾波器性能下降。仿真結果表明，相較于Kalman濾波器，AIEKF算法較好地提高了系統的精度和魯棒性。

參考文獻

[1]湯郡郡，胡偉，劉祥水，等.基于SINS/TAN/ADS/MCP的無人機組合導航系統[J].中國慣性技術學報，2018，26（1）：33-38.

TANG Junjun，HU Wei，LIU Xiangshui，et al.Unmanned aerial vehicle integrated navigation system based on SINS/TAN/ADS/MCP[J].Journal of Chinese Inertial Technology，2018，26（1）;33-38.

[2]蔡煊，王長林.基于抗差估計的BDS/ODO 組合列車定位方法[J].鐵道科學與工程學報，2018.15（10）：2654-2660.

CAI Xuan， WAN Changlin，BDS/ODO integrated train positioning method based on robust estimation[J]. Journal of Railway Science and Engineering，2018.15（10） ：2654-2660.

[3]萬德鈞，房建成. 慣性導航初始對準.東南大學出版社P160-165.1998.10.

[4]A.H. Mohamed， K.P. Schwarz. Adaptive kalman filtering for INS/GPS. Journal of Geodesy， Page 193-203， 1999.

[5]D.Loebis， R. Sutton， J. Chudley， W. Naeem. Adaptive tuning of a kalman filter via fuzzy logic for an intelligent AUV navigation system， Page1-7， 2004.

[6]姜浩楠，蔡遠利.帶有噪聲遞推估計的自適應集合卡爾曼濾波[J].控制與決策，2018，33（9）：1567-1574.

JIANG Hao-nan，CAI Yuan-li.Adaptive ensemble Kalman filter with recursive noise estimation[J].Control and Decision， 2018，33（9）：1567-1574.

[7]付心如，孫偉，徐愛功，等.組合導航抗差自適應卡爾曼濾波[J].測繪科學，2018.43（1）：6-10.

FU Xinru，SUN Wei，XU Aigong，et al.Research on robust Kakman filter for integrated navigatuin[J]. Science of Surveying and Mapping[J]，2018.43（1）：6-10.

[8]J.Z. Sasiadek. Sensor fusion. Page203-228， Annual review in control， 2002.

[9]San-tong Zhang， Xue-ye Wei. Fuzzy adaptive kalman filtering for DR/GPS， Proceeding of the second international conference on machine learning and cybernetics， Xian， 2-5 November 2003， p2634-2637.

[10] 卞鴻巍， 金志華，王俊璞，等.組合導航系統新息自適應卡爾曼濾波算法[J]. 上海交通大學學報，2006，40（6）：1009-1014.

BIAN Hongwei，JIN Zhihua，WANG Junpu，et al. The Innovation-Based Estimation Adaptive Kalman Filter Algorithm for INS/GPS Integrated Navigation System[J]. Journal of Shanghai Jiaotong University，2006，40（6）：1009-1014.

[11]R.E. Kalman. A new approach to linear filtering and prediction problems. Journal of Basic Engineering， Pages 35-46， 1960.

[12] 楊艷娟，卞鴻巍，田蔚風，金志華. 一種新的INS/GPS組合導航技術[J].中國慣性技術學報.2004.12（2）：23-26.

YANG Yan-juan，BIAN Hong-wei，TIAN Wei-feng，et al.A New INS/BDS Integrated Navigation Technique[J].Journal of Chinese Inertial Technology.2004.12（2）：23-26.