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

張源　詹金林　韓冰　陳偉

摘要

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

關(guān)鍵詞

INS;BDS;組合導(dǎo)航;自適應(yīng)卡爾曼濾波

中圖分類號： U666.1 ? ? ? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識碼： A

DOI：10.19694/j.cnki.issn2095-2457.2020.04.81

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

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

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

1 自適應(yīng)迭代擴(kuò)展卡爾曼濾波算法（AIEKF）[10]

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

新息是指濾波器的測量估計值與實際測量值之間的差值。當(dāng)濾波器工作正常時，理想狀態(tài)下的新息應(yīng)為零均值白噪聲序列。當(dāng)狀態(tài)估計由于建模誤差而導(dǎo)致估計誤差增大時，導(dǎo)致新息增大且統(tǒng)計特性復(fù)雜，此時，基于新息相應(yī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互不相關(guān)并滿足：E[Wk]=0，Cov[Wk，Wj]=QKδkj，E[Vk]=0，Cov[Vk，Vj]=RKδkj。以新息方式列寫標(biāo)準(zhǔn)卡爾曼濾波算法，如式（1）～（5）所示。

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

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

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

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

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

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

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

式中：δL，δ？姿為慣導(dǎo)系統(tǒng)在緯度和經(jīng)度上的定位誤差;δve，δvn為慣導(dǎo)系統(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 分別為東向、北向加速度計隨機(jī)偏差，W ?W ?W 分別為東、北向和方位陀螺隨機(jī)漂移，以上各值均設(shè)定為零均值、方差為QINS的白噪聲。FINS為12×12的矩陣，如式（12），式中L為艦船所處位置的緯度，g為當(dāng)?shù)刂亓铀俣?RM、RN分別為地球卯酉圈和子午圈半徑;ωie為地球自轉(zhuǎn)角速度。

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

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

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

（15）

3 仿真結(jié)果分析

為驗證方法的精度和魯棒性，對INS/BDS組合導(dǎo)航系統(tǒng)進(jìn)行計算機(jī)仿真測試，仿真測試工具采用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。

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

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

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

4 結(jié)論

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

參考文獻(xiàn)

[1]湯郡郡，胡偉，劉祥水，等.基于SINS/TAN/ADS/MCP的無人機(jī)組合導(dǎo)航系統(tǒng)[J].中國慣性技術(shù)學(xué)報，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].鐵道科學(xué)與工程學(xué)報，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]萬德鈞，房建成. 慣性導(dǎo)航初始對準(zhǔn).東南大學(xué)出版社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]姜浩楠，蔡遠(yuǎn)利.帶有噪聲遞推估計的自適應(yīng)集合卡爾曼濾波[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]付心如，孫偉，徐愛功，等.組合導(dǎo)航抗差自適應(yīng)卡爾曼濾波[J].測繪科學(xué)，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] 卞鴻巍， 金志華，王俊璞，等.組合導(dǎo)航系統(tǒng)新息自適應(yīng)卡爾曼濾波算法[J]. 上海交通大學(xué)學(xué)報，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] 楊艷娟，卞鴻巍，田蔚風(fēng)，金志華. 一種新的INS/GPS組合導(dǎo)航技術(shù)[J].中國慣性技術(shù)學(xué)報.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.