改進(jìn)自適應(yīng)強(qiáng)跟蹤容積卡爾曼濾波器在動(dòng)基座大失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用

趙海軍 高大遠(yuǎn) 王超

摘要：捷聯(lián)慣導(dǎo)在動(dòng)基座大方位失準(zhǔn)角初始對(duì)準(zhǔn)時(shí)可能會(huì)出現(xiàn)模型失配和濾波發(fā)散等現(xiàn)象，為提高對(duì)準(zhǔn)性能，對(duì)強(qiáng)跟蹤容積卡爾曼濾波算法（STCKF）進(jìn)行改進(jìn)，引入非線性系統(tǒng)Sage-Husa噪聲估計(jì)器、模型失配判據(jù)和噪聲估計(jì)器收斂性判據(jù)，根據(jù)判據(jù)對(duì)CKF和STCKF進(jìn)行切換。仿真結(jié)果表明：改進(jìn)后的STCKF算法可以有效地解決捷聯(lián)慣導(dǎo)在動(dòng)基座初始對(duì)準(zhǔn)時(shí)出現(xiàn)的模型失配和濾波發(fā)散問(wèn)題。

關(guān)鍵詞：捷聯(lián)慣導(dǎo);初始對(duì)準(zhǔn);STCKF算法;噪聲估計(jì)器

中圖分類號(hào)：U666.1;TH89 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1007-9416（2020）07-0106-05

0 引言

水下無(wú)人航行器（Unmanned Underwater Vehicle，UUV）在海洋探測(cè)及各類活動(dòng)中應(yīng)用廣泛，受其體積、能源和成本的限制，UUV無(wú)法裝備高精度慣導(dǎo)，通常使用低成本的捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System，SINS）或航姿系統(tǒng)。導(dǎo)航精度不僅依賴于其慣性傳感器（陀螺儀和加速度計(jì)）的精度，還依賴于高精度的初始對(duì)準(zhǔn)。

UUV通常位于母船甲板或海面進(jìn)行初始對(duì)準(zhǔn)，受海浪影響其初始航向角難以獲得，水平姿態(tài)角不停變化，導(dǎo)致在對(duì)準(zhǔn)過(guò)程中使用的小失準(zhǔn)角模型不再適用。而在大失準(zhǔn)角條件下，初始對(duì)準(zhǔn)模型變?yōu)榉蔷€性，傳統(tǒng)的卡爾曼濾波難以適用。隨著非線性濾波器方法的發(fā)展，針對(duì)大失準(zhǔn)角條件的初始對(duì)準(zhǔn)方法得到深入研究。Unscented 卡爾曼濾波器（UKF）[1-2]采用unscented變換用數(shù)值方法代替基于Jacob矩陣的線性變換計(jì)算統(tǒng)計(jì)特性，在計(jì)算量不顯著增加的情況下得到較好的非線性濾波效果，文獻(xiàn)[3]應(yīng)用球面徑向容積準(zhǔn)則進(jìn)行相應(yīng)數(shù)值積分計(jì)算，對(duì)強(qiáng)非線性濾波效果較好。對(duì)于實(shí)際的系統(tǒng)，建立非線性模型及后續(xù)進(jìn)行的近似計(jì)算會(huì)帶來(lái)誤差累積，這將導(dǎo)致隨著濾波過(guò)程的進(jìn)行，舊數(shù)據(jù)對(duì)參數(shù)更新的牽制增強(qiáng)而抑制新數(shù)據(jù)在預(yù)測(cè)校正計(jì)算中發(fā)揮的作用。文獻(xiàn)[4]基于漸消記憶濾波理論基礎(chǔ)，提出強(qiáng)跟蹤濾波器，有效解決存在模型不確定性是魯棒性差、濾波發(fā)散等問(wèn)題，得到廣泛應(yīng)用。而將強(qiáng)跟蹤濾波方法與容積卡爾曼濾波方法結(jié)合，形成強(qiáng)跟蹤容積卡爾曼濾波，則繼承了二者非線性濾波效果好、針對(duì)模型不確定時(shí)魯棒性強(qiáng)的特點(diǎn)。文獻(xiàn)[5]分析了強(qiáng)跟蹤平方根容積卡爾曼濾波STRCKF的收斂性，利用船舶動(dòng)力定位仿真實(shí)驗(yàn)驗(yàn)證其有效性。

在STRCKF中，漸消因子的計(jì)算需要利用噪聲統(tǒng)計(jì)特性參數(shù)，而實(shí)際濾波過(guò)程中的噪聲特性難以知曉，需要采用自適應(yīng)方濾波方法進(jìn)行估計(jì)。針對(duì)此問(wèn)題，本文將Sage-Husa噪聲估計(jì)器引入強(qiáng)跟蹤容積卡爾曼濾波算法（STCKF），并加入模型失配判據(jù)和噪聲估計(jì)器收斂性判據(jù)，基于此提出了一種自適應(yīng)強(qiáng)跟蹤容積卡爾曼濾波算法（ASTCKF），并對(duì)此方法進(jìn)行仿真分析。首先建立捷聯(lián)慣導(dǎo)大失準(zhǔn)角對(duì)準(zhǔn)的非線性模型，然后介紹對(duì)STCKF進(jìn)行改進(jìn)的方法和實(shí)施步驟，基于使用MEMS傳感器的SINS進(jìn)行仿真，并對(duì)結(jié)果進(jìn)行分析，最后進(jìn)行小結(jié)。

1 SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)模型

假設(shè)陀螺儀和加速度計(jì)的測(cè)量誤差和主要由隨機(jī)常值和零均值高斯白噪聲組成;同時(shí)當(dāng)UUV在水面時(shí)，SINS進(jìn)行初始對(duì)準(zhǔn)，不考慮高度誤差，假設(shè)水平失準(zhǔn)角較小，方位失準(zhǔn)角為大角度，則可以推導(dǎo)大方位失準(zhǔn)角的誤差模型如下[6-7]：

其中，姿態(tài)誤差矩陣、表示理想導(dǎo)航坐標(biāo)系（n系）、載體坐標(biāo)系（b系）與實(shí)際SINS模擬數(shù)學(xué)平臺(tái)坐標(biāo)系（系）之間的關(guān)系;為系相對(duì)于慣性系（系）的轉(zhuǎn)動(dòng)角速度，、分別為的計(jì)算值和計(jì)算誤差;為系相對(duì)于系的轉(zhuǎn)動(dòng)角速度，為其計(jì)算誤差;為加速度計(jì)的測(cè)量值，為加速度計(jì)測(cè)量誤差;為重力補(bǔ)償誤差;為地球自轉(zhuǎn)角速率;和分別為子午圈曲率半徑和卯酉圈的曲率半徑。

令狀態(tài)向量包含失準(zhǔn)角、（水平）位置誤差和速度誤差、傳感器誤差（不包含垂向加速度誤差），則可寫為：

陀螺和加速度計(jì)的高斯白色噪聲向量為：

觀測(cè)量取衛(wèi)星定位結(jié)果與SINS輸出的位置和速度之差，，則可得到SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)的非線性方程如下：

2.2 非線性Sage-Husa自適應(yīng)濾波算法

上面方法中，在計(jì)算時(shí)，需要知道噪聲統(tǒng)計(jì)特性參數(shù)，而當(dāng)參數(shù)未知時(shí)，可采用自適應(yīng)濾波。傳統(tǒng)自適應(yīng)濾波算法復(fù)雜且計(jì)算量大，很難應(yīng)用于工程實(shí)際。近年來(lái)以Sage-Husa自適應(yīng)濾波為代表的“開(kāi)窗”平滑估計(jì)噪聲的自適應(yīng)濾波算法得到廣泛的應(yīng)用。Sage-Husa自適應(yīng)濾波利用量測(cè)殘差信息，按極大后驗(yàn)噪聲統(tǒng)計(jì)估值定義得到次優(yōu)估值器，然后對(duì)系統(tǒng)噪聲進(jìn)行在線實(shí)時(shí)估計(jì)。根據(jù)極大后驗(yàn)噪聲統(tǒng)計(jì)估值定義設(shè)計(jì)次優(yōu)無(wú)偏估值器，并將其擴(kuò)展到非線性系統(tǒng)，則可以得到非線性Sage-Husa的次優(yōu)無(wú)偏遞推噪聲統(tǒng)計(jì)估值器：

其中，、分別表示通過(guò)非線性變換和后的均值，，為遺忘因子，濾波增益、最優(yōu)估計(jì)值和輸出一步預(yù)測(cè)方差陣等由濾波器給出。

非線性系統(tǒng)Sage-Husa噪聲估計(jì)器不需要計(jì)算雅克比矩陣，計(jì)算量小;而且通過(guò)引入遺忘因子以增大新近時(shí)刻的數(shù)據(jù)加權(quán)系數(shù)，減小陳舊數(shù)據(jù)的加權(quán)系數(shù)，從而達(dá)到對(duì)時(shí)變?cè)肼暯y(tǒng)計(jì)量、、和的自適應(yīng)估計(jì)。

2.3 模型失配判據(jù)

通過(guò)強(qiáng)跟蹤容積卡爾曼濾波算法的推導(dǎo)[4]，可以看出對(duì)誤差陣的調(diào)整以及對(duì)濾波增益矩陣的優(yōu)選都是通過(guò)調(diào)整漸消因子矩陣而實(shí)現(xiàn)的，因此可以選取漸消因子矩陣為模型失配判據(jù)，即當(dāng)時(shí)，非線性系統(tǒng)模型未失配，采用標(biāo)準(zhǔn)容積卡爾曼濾波算法;否則，說(shuō)明非線性系統(tǒng)模型失配，采用自適應(yīng)強(qiáng)跟蹤容積卡爾曼濾波算法。

2.4 噪聲估計(jì)器收斂性判據(jù)

在穩(wěn)態(tài)條件下，可認(rèn)為非線性系統(tǒng)噪聲統(tǒng)計(jì)特性相對(duì)穩(wěn)定，此時(shí)可以不使用非線性系統(tǒng)Sage-Husa噪聲估計(jì)器，從而降低算法的計(jì)算負(fù)擔(dān)。由于模型失配往往伴隨著系統(tǒng)噪聲特性的改變，因此模型失配判據(jù)是調(diào)用非線性系統(tǒng)Sage-Husa噪聲估計(jì)器的前提條件。同時(shí)增加噪聲估計(jì)器收斂性判據(jù)，其表述形式如下：

如果，則判定噪聲估計(jì)器發(fā)散，調(diào)用非線性系統(tǒng)Sage-Husa噪聲估計(jì)器，否則不調(diào)用。其中為常值小量。

2.5 改進(jìn)ASTCKF算法的具體步驟

根據(jù)上面分析，優(yōu)化的ASTCKF算法步驟如下：

（1）一步預(yù)測(cè)估計(jì)，首先按照式（5）-（7）計(jì)算，即CKF方法的第（i）步至第（iii）步。

（2）按照（12）計(jì)算漸消因子，并進(jìn)行模型失配判定。若，模型未失配，則仍然按照標(biāo)準(zhǔn)容積卡爾曼濾波算法的（7）式計(jì)算方差矩陣估計(jì)，否則采用STCKF方法，用（12）（13）式計(jì)算。

（3）計(jì)算一步測(cè)量預(yù)測(cè)、濾波增益矩陣、狀態(tài)估計(jì)值和狀態(tài)誤差協(xié)方差陣，仍然按照相應(yīng)的（8）-（11）式計(jì)算，只不過(guò)要使用采取漸消因子的結(jié)果。

（4）若判定噪聲估計(jì)器未收斂，采用非線性Sage-Husa自適應(yīng)濾波對(duì)噪聲估計(jì)：

由此實(shí)現(xiàn)改進(jìn)的ASTCKF方法。此方法在濾波正常進(jìn)行時(shí)，是一個(gè)標(biāo)準(zhǔn)的容積卡爾曼濾波器，計(jì)算負(fù)擔(dān)不大，精度有保證，而在模型失配、噪聲估計(jì)發(fā)散時(shí)，通過(guò)簡(jiǎn)單的判據(jù)，采用自適應(yīng)Sage-Husa噪聲估計(jì)器，形成ASTCKF，以提高濾波性能?？傮w來(lái)說(shuō)此方法在計(jì)算負(fù)擔(dān)和濾波性能之間進(jìn)行自動(dòng)切換，具有滿意的效果。

3 仿真研究

3.1 仿真條件

（1）UUV的航跡設(shè)計(jì)。

UUV初始對(duì)準(zhǔn)航跡設(shè)計(jì)如下：水下航行器從（緯度36°N，經(jīng)度120°E）位置出發(fā)，前50秒作勻加速直線運(yùn)動(dòng)，加速度為，然后以（約5節(jié)）的速度勻速直線運(yùn)動(dòng)，總仿真時(shí)間為300秒。

（2）濾波器初始參數(shù)設(shè)置。

MEMS陀螺儀的常值漂移為，隨機(jī)漂移均為;三個(gè)加速度計(jì)的零偏均為10mg，隨機(jī)白噪聲標(biāo)準(zhǔn)差為1mg;忽略陀螺儀和加速度計(jì)的安裝誤差;初始失準(zhǔn)角為。仿真中系統(tǒng)狀態(tài)的初始方差陣、系統(tǒng)噪聲初始方差陣以及觀測(cè)噪聲初始方差陣分別設(shè)置為：

3.2 仿真驗(yàn)證

為了驗(yàn)證改進(jìn)STCKF算法的有效性，分別采用本文提出的改進(jìn)ASTCKF濾波算法、強(qiáng)跟蹤容積卡爾曼濾波算法（STCKF）和標(biāo)準(zhǔn)容積卡爾曼濾波算法（CKF），來(lái)進(jìn)行捷聯(lián)慣導(dǎo)系統(tǒng)大方位失準(zhǔn)角海上動(dòng)基座對(duì)準(zhǔn)仿真實(shí)驗(yàn)，仿真實(shí)驗(yàn)分為無(wú)海浪和有海浪兩種情況。

（1）無(wú)海浪情況。

當(dāng)沒(méi)有海浪作用時(shí)，認(rèn)為系統(tǒng)噪聲和觀測(cè)噪聲為零均值的高斯白噪聲，仿真結(jié)果如圖1所示。

可以看出，在沒(méi)有海浪影響的情況下，系統(tǒng)噪聲和觀測(cè)噪聲可以看作零均值的高斯白噪聲，此時(shí)利用CKF、STCKF、優(yōu)化ASTCKF三種濾波算法對(duì)SINS進(jìn)行初始對(duì)準(zhǔn)，均具有較好估計(jì)精度。穩(wěn)定后，三種方法的北向失準(zhǔn)角估計(jì)誤差為20.84`、23.9`，28.12`，天向失準(zhǔn)角估計(jì)誤差分別為：31.01`、34.56`、37.24`。優(yōu)化的STCKF算法的精度略好于其他兩種，而收斂速度要優(yōu)于其它兩種算法。

（2）有海浪情況。

假設(shè)UUV的實(shí)際姿態(tài)角在海浪的作用下分別以如下形式做正弦搖擺運(yùn)動(dòng)：

其中、、分別表示各姿態(tài)角的搖擺幅值;、、分別為搖擺角頻率;而，，為相應(yīng)的搖擺周期;為UUV真實(shí)航向;仿真過(guò)程中分別?。海?，，，，，。此時(shí)，系統(tǒng)噪聲和量測(cè)噪聲為相互獨(dú)立的帶時(shí)變均值和協(xié)方差的正態(tài)白噪聲序列。仿真結(jié)果如圖2所示。

當(dāng)有海浪影響的情況下，系統(tǒng)噪聲和量測(cè)噪聲可以認(rèn)為是相互獨(dú)立的帶時(shí)變均值和協(xié)方差的正態(tài)白噪聲序列，其統(tǒng)計(jì)特性未知，此時(shí)，STCKF對(duì)于方向失準(zhǔn)角的濾波收斂速度和估計(jì)精度都明顯優(yōu)于CKF，優(yōu)化ASTCKF的濾波收斂速度更快，估計(jì)精度也更高。穩(wěn)定后優(yōu)化方法的北向失準(zhǔn)角和天向失準(zhǔn)角誤差分別為30.61′和53.26′，STCKF算法的失準(zhǔn)角誤差分別為43.65′和77.34′，CKF算法的失準(zhǔn)角誤差分別為80.70′和153.30′，可以看出，優(yōu)化ASTCKF在估計(jì)精度上優(yōu)勢(shì)明顯。這是由于是優(yōu)化ASTCKF引入非線性系統(tǒng)Sage-Husa噪聲估計(jì)器，能夠?qū)崟r(shí)調(diào)整和估計(jì)噪聲統(tǒng)計(jì)特性的變化，從而克服系統(tǒng)噪聲統(tǒng)計(jì)特性不準(zhǔn)確的局限性。同時(shí)引入了強(qiáng)跟蹤濾波器，可以對(duì)于系統(tǒng)部分參數(shù)失配引起的模型誤差具有較強(qiáng)的跟蹤能力。因此對(duì)北向失準(zhǔn)角和天向失準(zhǔn)角的估計(jì)能夠保持較高的估計(jì)性能。

4 結(jié)語(yǔ)

在復(fù)雜海洋環(huán)境的影響下，UUV在水面或近水面時(shí)SINS進(jìn)行初始對(duì)準(zhǔn)的過(guò)程存在模型失配和濾波發(fā)散等問(wèn)題，針對(duì)上述問(wèn)題，本文將非線性系統(tǒng)Sage-Husa噪聲估計(jì)器、模型失配判據(jù)和噪聲估計(jì)器收斂性判據(jù)融入到強(qiáng)跟蹤容積卡爾曼濾波算法中，推導(dǎo)了優(yōu)化的強(qiáng)跟蹤容積卡爾曼濾波算法，最后通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了此方法對(duì)SINS進(jìn)行大方位失準(zhǔn)角初始校準(zhǔn)的有效性。

參考文獻(xiàn)

[1] Julier S，Uhlmann J，Durrant-Whyte H F.A New Method for the Nonlinear Transformations of Means and Covariances in Filters and Estimators[J].IEEE Transactions on Automatic Control，2004，45（3）：477-482.

[2] 丁楊斌，王新龍，王鎮(zhèn)，等.Unscented卡爾曼濾波在SINS靜基座大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用研究[J].宇航學(xué)報(bào)，2006，27（6）：1201-1204.

[3] Arasaratnam I，Hayken S.Cubature Kalman filters[J].IEEE Trans. Automatic Control，2009，54（6）：1254-1269.

[4] 周東華，席裕庚，張鐘俊.一種帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器[J].自動(dòng)化學(xué)報(bào)，1991，16（7）：689-695.

[5] 徐樹(shù)生，林孝工，趙大威，等.強(qiáng)跟蹤SRCKF及其在船舶動(dòng)力定位中的應(yīng)用[J].儀器儀表學(xué)報(bào)，2013，34（6）：1266-1272.

[6] M.J Yu.Equivalent Nonlinear Error Models of Strapdown Inertial Navigation System[M].presented at AIAA conference proceedings，Guidance，Navigation and Control，1997.

[7] 嚴(yán)恭敏，嚴(yán)衛(wèi)生，徐德民.基于歐拉平臺(tái)誤差角的SINS非線性誤差模型的研究[J].西北工業(yè)大學(xué)學(xué)報(bào)，2009，27（4）：511-516.

[8] 孫楓，唐李軍.基于CKF的SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)[J].儀器儀表學(xué)報(bào)，2012，33（2）：327-333.