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

張源　詹金林　韓冰　陳偉

摘要

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

關鍵詞

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

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

DOI：10.19694/j.cnki.issn2095-2457.2020.04.81

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

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

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

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

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

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

對于離散線性系統(tǒng)模型，其狀態(tài)方程和量測方程如下：

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

2 BDS/INS組合導航系統(tǒng)及系統(tǒng)模型

2.1 組合系統(tǒng)總體設計

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

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

2.2 系統(tǒng)誤差模型

2.2.1 慣性導航系統(tǒng)系統(tǒng)誤差模型

系統(tǒng)誤差模型選取半解析式當?shù)厮街副逼脚_式慣性導航系統(tǒng)，采用東北天坐標系。選用定位誤差、速度誤差、平臺誤差角、陀螺漂移和加速度漂移誤差作為慣性導航系統(tǒng)的狀態(tài)變量。

式中：δL，δ？姿為慣導系統(tǒng)在緯度和經(jīng)度上的定位誤差;δve，δvn為慣導系統(tǒng)的東向速度誤差和北向速度誤差;φ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為當?shù)刂亓铀俣?RM、RN分別為地球卯酉圈和子午圈半徑;ωie為地球自轉(zhuǎn)角速度。

2.2.2 組合系統(tǒng)量測方程

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

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

（15）

3 仿真結(jié)果分析

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

仿真條件如下所示：艦船航向ψ=45°，θ=1°，？酌=1°，Ve=Vn=5m/s，φ=32°N;INS參數(shù)如下：φ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系統(tǒng)噪聲方差（QINS）為：δ ?=δ ?=（1×10 g） ，δ ?=δ ?=δ ?=（5×10 （°）/h） ，δ ?=δ ?=（0.2m/s） ;BDS量測噪聲方差（RBDS）為δ ?=（3'）2，δ ?=（30"）2，δ ?=δ ?=（0.01"）2，δ ?=δ ?=（0.2m/s）2。

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

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

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

4 結(jié)論

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

參考文獻

[1]湯郡郡，胡偉，劉祥水，等.基于SINS/TAN/ADS/MCP的無人機組合導航系統(tǒng)[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] 卞鴻巍， 金志華，王俊璞，等.組合導航系統(tǒng)新息自適應卡爾曼濾波算法[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.