超級航姿中基于變閾值判據(jù)的自適應(yīng)Kalman濾波

謝祖輝，楊功流，于東康，李 壯

(1.北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院，北京 100191；2.慣性技術(shù)國防重點(diǎn)實(shí)驗(yàn)室，北京 100191)

0 引言

機(jī)載導(dǎo)航系統(tǒng)通常分為傳統(tǒng)航姿系統(tǒng)和慣導(dǎo)系統(tǒng)[1]。超級航姿系統(tǒng)(super-AHRS)作為兩者之間的銜接導(dǎo)航系統(tǒng)，它在滿足低成本、小體積的同時，還能在主慣導(dǎo)系統(tǒng)和外界輔助信號都失效的情況下，進(jìn)行短時(20～30 min)較高精度的純慣導(dǎo)解算，為機(jī)載武器的發(fā)射提供全面的導(dǎo)航信息。此外，它還能長期輸出一定精度的水平姿態(tài)和航向信息，以確保飛行器能正確返航。

超級航姿系統(tǒng)通常采用精度水平為0.05 ～ 0.10 (°)/h的光纖陀螺，當(dāng)GPS失效時，由于陀螺漂移的影響，無阻尼的慣性導(dǎo)航會使飛行器的姿態(tài)和航向逐漸發(fā)散，導(dǎo)致飛行器無法正確返場[2]。傳統(tǒng)捷聯(lián)航姿系統(tǒng)內(nèi)阻尼算法雖然在一定程度上能抑制飛行器水平姿態(tài)角的發(fā)散，但由于加速度計輸出特性易受周圍環(huán)境的影響，因此，基于定閾值的載體機(jī)動判據(jù)將無法正確判斷出飛行器的機(jī)動狀態(tài)，從而導(dǎo)致飛行器的水平姿態(tài)長期得不到修正，最終引起姿態(tài)角發(fā)散[3-4]。當(dāng)加速度計輸出穩(wěn)定性發(fā)生變化時，基于定量測噪聲的常規(guī)卡爾曼濾波器易振蕩甚至發(fā)散[5]。

本文提出了一種基于模糊推理系統(tǒng)的變閾值載體機(jī)動判據(jù)自適應(yīng)Kalman濾波算法，降低因加表輸出穩(wěn)定性變化所引起的載體機(jī)動狀態(tài)誤判率，并提高了濾波穩(wěn)定性。

1 超級航姿系統(tǒng)內(nèi)阻尼姿態(tài)組合算法

基于載體機(jī)動狀態(tài)自檢驗(yàn)的超級航姿系統(tǒng)內(nèi)阻尼姿態(tài)組合算法的基本原理是：由于加速度計長期穩(wěn)定性好，因此，當(dāng)載體長期處于非機(jī)動狀態(tài)時，可利用加速度計輸出與姿態(tài)角之間的對應(yīng)關(guān)系，將計算得到的載體俯仰角θD和橫滾角γD作為量測量，通過Kalman濾波器對載體的姿態(tài)角進(jìn)行修正，以提高飛行器水平姿態(tài)角精度[6-7]。

1.1 內(nèi)阻尼姿態(tài)角方程

取導(dǎo)航坐標(biāo)系為東、北、天(E、N、U)地理坐標(biāo)系，系統(tǒng)比力方程：

(1)

加速度計在載體坐標(biāo)系下的輸出fb與導(dǎo)航坐標(biāo)系下的投影關(guān)系為

(2)

由式(1)、(2)可得

(3)

(4)

1.2 內(nèi)阻尼Kalman濾波狀態(tài)方程

由文獻(xiàn)[6,8]知，超級航姿系統(tǒng)內(nèi)阻尼Kalman濾波的狀態(tài)量為

(5)

式中：φE，φN，φU為數(shù)學(xué)平臺誤差角；δvE、δvN為載體的速度誤差；εi(i=x,y,z)為陀螺隨機(jī)常值漂移。

平臺失準(zhǔn)角誤差模型為

(6)

載體速度誤差模型為

(7)

式中：ωN，ωU為地球自轉(zhuǎn)角速率在導(dǎo)航坐標(biāo)系下的投影；r為地球半徑；εE、εN、εU為等效陀螺漂移；fE、fN、fU為加表輸出在導(dǎo)航坐標(biāo)系下的投影。

綜上所述可知，超級航姿系統(tǒng)內(nèi)阻尼Kalman濾波狀態(tài)方程為

(8)

式中：X(t)為系統(tǒng)狀態(tài)量；F(t)為誤差傳遞矩陣；G(t)為系統(tǒng)噪聲驅(qū)動矩陣;W(t)為系統(tǒng)激勵噪聲序列。

1.3 內(nèi)阻尼Kalman濾波量測方程

超級航姿系統(tǒng)內(nèi)阻尼Kalman濾波器中的量測量由慣導(dǎo)解算得到的姿態(tài)θI、γI與內(nèi)阻尼姿態(tài)θD、γD之差構(gòu)成，即

N(t)2×1

(9)

(10)

式中：Z(t)為系統(tǒng)t時刻的量測量；H(t)為系統(tǒng)量測矩陣；N(t)為量測噪聲矩陣;θ和ψ分別為載體真實(shí)的俯仰角和方位角。

2 變閾值載體機(jī)動判據(jù)自適應(yīng)Kalman濾波算法

超級航姿系統(tǒng)內(nèi)阻尼姿態(tài)組合導(dǎo)航算法的關(guān)鍵在于正確判斷飛行器的機(jī)動狀態(tài)，適時啟用或斷開加速度計的量測信息，從而更好地達(dá)到校正系統(tǒng)姿態(tài)誤差，提高系統(tǒng)精度的效果[9-10]。通常情況下，載體機(jī)動自檢驗(yàn)法以加速度計輸出均值作為判斷閾值，但當(dāng)加速度計輸出穩(wěn)定性發(fā)生變化且飛行器處于非機(jī)動狀態(tài)時，傳統(tǒng)的基于定閾值的載體機(jī)動判據(jù)將出現(xiàn)誤判，從而斷開內(nèi)阻尼姿態(tài)修正回路，使載體的姿態(tài)角長期得不到修正，最終導(dǎo)致發(fā)散。

2.1 基于模糊推理系統(tǒng)的變閾值載體機(jī)動自檢驗(yàn)法

(11)

(12)

(13)

模糊推理系統(tǒng)(FIS)是一種多輸入單輸出(MISO)或單輸入多輸出(SIMO)系統(tǒng)，它將確定信號根據(jù)知識庫轉(zhuǎn)換為模糊量，再將模糊量進(jìn)行去模糊化處理并轉(zhuǎn)換成精確控制量輸出[11-13]。根據(jù)最小誤判率準(zhǔn)則，本文以式(13)作為FIS的輸入，以判斷閾值d和量測噪聲調(diào)節(jié)因子α作為輸出，構(gòu)成如圖1所示的模糊推理系統(tǒng)。其中，輸入變量采用全交疊Gauss型隸屬函數(shù),而輸出變量采用三角形隸屬函數(shù)，如圖2所示。

圖1 SIMO模糊推理系統(tǒng)

圖2 FIS系統(tǒng)輸出變量圖

在該SIMO模糊推理系統(tǒng)中，用好、一般及差來表示加速度計輸出穩(wěn)定性。根據(jù)加速度計輸出穩(wěn)定性越好，內(nèi)阻尼姿態(tài)誤差越小的原則，同時，為了減小計算量，方便工程上實(shí)時解算，設(shè)計3條模糊推理規(guī)則如表1所示。

表1 SIMO模糊推理規(guī)則

圖與d關(guān)系圖

2.2 基于時變噪聲的自適應(yīng)Kalman濾波算法

變閾值載體機(jī)動自檢驗(yàn)法雖然降低了載體機(jī)動狀態(tài)的誤判率，提高了內(nèi)阻尼姿態(tài)的利用率，但當(dāng)加速度計輸出穩(wěn)定性發(fā)生變化時，基于定量測噪聲的Kalman濾波算法無法根據(jù)加速度計的輸出實(shí)時調(diào)整量測噪聲陣Rk，從而增大或減小Kalman濾波器對量測信息的利用率，嚴(yán)重時將導(dǎo)致濾波器振蕩甚至發(fā)散[14]。

Rk=αR

(14)

圖與α關(guān)系圖

綜上所述可知，本文提出的基于模糊推理系統(tǒng)的變閾值載體機(jī)動判據(jù)自適應(yīng)Kalman濾波算法的基本流程為：當(dāng)由式(11)、(12)計算出的值分別小于d1和d2時(d1和d2均為判斷閾值)，即認(rèn)為載體處于非機(jī)動狀態(tài)，此時啟用變閾值載體機(jī)動判據(jù)自適應(yīng)Kalman濾波算法來修正載體的水平姿態(tài)角，否則系統(tǒng)進(jìn)入純慣導(dǎo)解算模式。

3 仿真結(jié)果與分析

通過設(shè)計兩組仿真實(shí)驗(yàn)來驗(yàn)證文中所提變閾值載體機(jī)動判據(jù)自適應(yīng)Kalman濾波算法的有效性，2組實(shí)驗(yàn)的仿真航跡如表2所示。表中，v、a、ω分別為載體的速度、加速度、角速度。

表2 超級航姿系統(tǒng)航跡仿真過程

表3 載體的初始狀態(tài)信息

3.1 變閾值載體機(jī)動狀態(tài)自檢法仿真實(shí)驗(yàn)

圖5 載體機(jī)動狀態(tài)判斷結(jié)果圖

經(jīng)計算，定閾值法載體機(jī)動判據(jù)的誤判率為15.05%，而變閾值法載體機(jī)動判據(jù)的誤判率僅為3.60%。由圖5可看出,在3 200～4 000 s，即加表穩(wěn)定性突變階段，基于定閾值的載體機(jī)動自檢驗(yàn)法明顯出現(xiàn)較大的誤判率；而基于變閾值的載體機(jī)動自檢法則能根據(jù)加表輸出穩(wěn)定性的高低實(shí)時調(diào)整判斷閾值d的大小，從而降低誤判率。由此可見，基于模糊推理系統(tǒng)的變閾值載體機(jī)動判據(jù)在加表輸出穩(wěn)定性發(fā)生變化時，仍能較好地判斷出飛行器的機(jī)動狀態(tài)，從而適時啟用或斷開內(nèi)阻尼姿態(tài)，更好地起到修正載體水平姿態(tài)角的作用。

3.2 時變噪聲自適應(yīng)Kalman濾波算法仿真實(shí)驗(yàn)

為驗(yàn)證載體水平姿態(tài)經(jīng)過時變噪聲自適應(yīng)Kalman濾波算法修正后的效果，取初始量測噪聲陣R=0.02°；根據(jù)加表穩(wěn)定性的高低，將FIS系統(tǒng)另一輸出變量α的取值范圍設(shè)為[0,5]；其余參數(shù)及加表隨機(jī)漂移的變化情況與3.1節(jié)相同，得到載體水平姿態(tài)誤差角曲線和內(nèi)阻尼姿態(tài)角曲線分別如圖6、7所示。

圖6 載體姿態(tài)誤差角曲線

圖7 內(nèi)阻尼姿態(tài)角曲線

為了便于直觀對比定量測噪聲Kalman濾波算法與時變噪聲自適應(yīng)Kalman濾波算法在上述4個階段的濾波穩(wěn)定性，各階段所對應(yīng)的載體姿態(tài)誤差角穩(wěn)定性計算結(jié)果如表4所示。

表4 載體姿態(tài)角誤差穩(wěn)定性

由圖6可見，在0～3 200 s階段，由于加速度計隨機(jī)漂移保持不變，兩種算法的濾波精度和穩(wěn)定性基本相同，其中，俯仰、橫滾角誤差都抑制在0.1°內(nèi)，且兩種算法的姿態(tài)誤差角穩(wěn)定性基本相同。在3 200～4 000 s階段，由于量測噪聲發(fā)生突變，因此，定量測噪聲法出現(xiàn)較大幅度波動，而時變噪聲法濾波穩(wěn)定性明顯優(yōu)于定量測噪聲法。其中，在2.5倍噪聲時，俯仰、橫滾角誤差穩(wěn)定性分別提高了62.26%和36.50%；在5倍噪聲時，俯仰、橫滾角誤差穩(wěn)定性分別提高了47.88%和62.30%。其原因：由內(nèi)阻尼姿態(tài)融合算法的基本原理可知，加速度計輸出穩(wěn)定性的高低直接決定著內(nèi)阻尼姿態(tài)角穩(wěn)定性的高低。由圖7可看出，加速度計輸出穩(wěn)定性變差的同時，由其解算得到的內(nèi)阻尼姿態(tài)角波動幅度也較大，由于定量測噪聲法的量測噪聲陣Rk為定值，此時，若初始量測噪聲R取值過小，即濾波器的濾波結(jié)果對量測信息的依賴程度較高，則Kalman濾波的穩(wěn)定性隨著內(nèi)阻尼姿態(tài)穩(wěn)定性的變差而變差。時變噪聲法能通過FIS系統(tǒng)自適應(yīng)地調(diào)整噪聲調(diào)節(jié)因子α的大小，從而通過式(14)降低超級航姿系統(tǒng)對內(nèi)阻尼姿態(tài)的利用程度，即提高了濾波精度，也增強(qiáng)了濾波器的魯棒性

4 結(jié)論

1) 本文針對傳統(tǒng)定閾值載體機(jī)動自檢法在加表輸出時，因受外界干擾導(dǎo)致其穩(wěn)定性變差，存在對載體機(jī)動狀態(tài)誤判率高等缺點(diǎn)，提出了一種基于變閾值的載體機(jī)動自檢法。該算法不僅能在加表輸出穩(wěn)定階段正確判斷出載體的機(jī)動狀態(tài)，還能在加表輸出穩(wěn)定性發(fā)生突變時，通過模糊推理系統(tǒng)自適應(yīng)調(diào)節(jié)載體機(jī)動狀態(tài)判斷閾值，從而降低了傳統(tǒng)定閾值法對載體機(jī)動狀態(tài)的誤判率。

2) 本文針對量測噪聲發(fā)生變化時，傳統(tǒng)的基于定量測噪聲的Kalman濾波算法濾波穩(wěn)定性差的缺點(diǎn)，提出了一種基于時變噪聲的自適應(yīng)Kalman濾波算法。該算法以加表輸出穩(wěn)定性作為模糊推理系統(tǒng)的輸入，以調(diào)節(jié)因子α作為輸出，實(shí)時地對量測噪聲陣進(jìn)行調(diào)整，從而提高濾波器對量測信息的利用程度，改善了超級航姿系統(tǒng)在不同狀態(tài)下的濾波精度和濾波穩(wěn)定性。

仿真實(shí)驗(yàn)表明，本文所提算法能有效抑制超級航姿系統(tǒng)在GPS失效時的水平姿態(tài)誤差，具有一定的工程應(yīng)用價值。