改進自適應強跟蹤容積卡爾曼濾波器在動基座大失準角初始對準中的應用

趙海軍 高大遠 王超

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

關鍵詞：捷聯(lián)慣導;初始對準;STCKF算法;噪聲估計器

中圖分類號：U666.1;TH89 文獻標識碼：A 文章編號：1007-9416（2020）07-0106-05

0 引言

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

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

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

1 SINS大方位失準角初始對準模型

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

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

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

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

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

2.2 非線性Sage-Husa自適應濾波算法

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

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

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

2.3 模型失配判據(jù)

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

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

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

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

2.5 改進ASTCKF算法的具體步驟

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

（1）一步預測估計，首先按照式（5）-（7）計算，即CKF方法的第（i）步至第（iii）步。

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

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

（4）若判定噪聲估計器未收斂，采用非線性Sage-Husa自適應濾波對噪聲估計：

由此實現(xiàn)改進的ASTCKF方法。此方法在濾波正常進行時，是一個標準的容積卡爾曼濾波器，計算負擔不大，精度有保證，而在模型失配、噪聲估計發(fā)散時，通過簡單的判據(jù)，采用自適應Sage-Husa噪聲估計器，形成ASTCKF，以提高濾波性能。總體來說此方法在計算負擔和濾波性能之間進行自動切換，具有滿意的效果。

3 仿真研究

3.1 仿真條件

（1）UUV的航跡設計。

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

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

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

3.2 仿真驗證

為了驗證改進STCKF算法的有效性，分別采用本文提出的改進ASTCKF濾波算法、強跟蹤容積卡爾曼濾波算法（STCKF）和標準容積卡爾曼濾波算法（CKF），來進行捷聯(lián)慣導系統(tǒng)大方位失準角海上動基座對準仿真實驗，仿真實驗分為無海浪和有海浪兩種情況。

（1）無海浪情況。

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

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

（2）有海浪情況。

假設UUV的實際姿態(tài)角在海浪的作用下分別以如下形式做正弦搖擺運動：

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

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

4 結(jié)語

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

參考文獻

[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靜基座大方位失準角初始對準中的應用研究[J].宇航學報，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)漸消因子的擴展卡爾曼濾波器[J].自動化學報，1991，16（7）：689-695.

[5] 徐樹生，林孝工，趙大威，等.強跟蹤SRCKF及其在船舶動力定位中的應用[J].儀器儀表學報，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] 嚴恭敏，嚴衛(wèi)生，徐德民.基于歐拉平臺誤差角的SINS非線性誤差模型的研究[J].西北工業(yè)大學學報，2009，27（4）：511-516.

[8] 孫楓，唐李軍.基于CKF的SINS大方位失準角初始對準[J].儀器儀表學報，2012，33（2）：327-333.