非對稱冗余慣導的在線標定與容錯估計反饋策略

肖 烜，盧建睿，張永炎，沈 凱

（北京理工大學 自動化學院，北京 100081）

慣性導航系統(tǒng)（Inertial Navigation System, INS）不需要依賴外部信號就可以實現(xiàn)運動信息的輸出，使得它成為了所有組合導航系統(tǒng)的公共參考信息提供者。眾多組合導航系統(tǒng)都認為其內(nèi)部的INS不會發(fā)生故障，時刻都能提供可靠的輸出[1,2]。因此組合導航的性能高度依賴于系統(tǒng)內(nèi)慣性元件的可靠性。當單一INS的可靠性無法保證時，飛行器、船舶和高價值地面車輛會安裝兩套以上的慣性測量單元（Inertial Measurement Unit, IMU）構(gòu)成冗余慣性導航系統(tǒng)[3,4]，以提供可靠的慣性信息。

常規(guī)的冗余慣性技術(shù)使用多個規(guī)格相同的IMU以斜置的硬件結(jié)構(gòu)實現(xiàn)[5,6]，容易受到裝配難度和成本等的限制無法廣泛應用。為此，考慮使用非對稱冗余的方法實現(xiàn)慣性導航信息的高可靠性輸出。本文設計的非對稱冗余慣性導航系統(tǒng)（Asymmetric Redundant Inertial Navigation System, ARINS）由兩套精度不同的INS構(gòu)成，其中高精度INS在系統(tǒng)無故障的場景下被選作為基準信息輸出，并被用來對低精度INS的誤差進行在線標定；在高精度INS故障場景下，標定結(jié)果將被用來對低精度INS進行校正，然后利用低精度INS輸出導航解算參數(shù)。

對于理想假設條件下的線性系統(tǒng)，Kalman濾波可以獲得未知參數(shù)的最優(yōu)估計。文獻[7]利用單軸旋轉(zhuǎn)調(diào)制微機電IMU濾波估計固聯(lián)微機電IMU的器件零偏后由固聯(lián)IMU輸出艦船姿態(tài)，避免了旋轉(zhuǎn)調(diào)制下的姿態(tài)輸出鋸齒狀誤差。我國嫦娥探月工程也使用濾波的方法在地月轉(zhuǎn)移與月面上升等階段對IMU進行在軌標定[8]。文獻[9]基于誤差參數(shù)動態(tài)估計的思路實現(xiàn)了對兩套旋轉(zhuǎn)調(diào)制慣導的在線故障監(jiān)測。然而，Kalman濾波必須假設系統(tǒng)過程噪聲與量測噪聲服從零均值高斯分布，系統(tǒng)一旦發(fā)生故障，過程噪聲或者量測噪聲將會偏離假設條件，導致濾波性能下降甚至發(fā)散。為了解決這類問題，研究者提出了眾多自適應Kalman濾波算法，試圖對濾波模型中的過程噪聲和量測噪聲進行辨識[10,11]。其中，基于變分貝葉斯近似方法的自適應Kalman濾波近些年逐漸被研究人員關注[12,13]，它可以在每一個時刻以迭代而不是遞推的形式識別緩慢變化的噪聲。

針對本研究中的ARINS，考慮兩套慣導之間的安裝誤差角、MEMS陀螺常值漂移以及MEMS加速度計的常值零偏，使用兩套慣導之間輸出姿態(tài)與速度之差作為量測量建立了在線標定模型，設計了基于序貫結(jié)構(gòu)的變分貝葉斯自適應Kalman濾波（Variational Bayesian Adaptive Kalman Filter, VB-AKF）以實現(xiàn)對MEMS-INS誤差的估計，給出了一種動態(tài)濾波收斂度分析方法，提出了一種基于收斂度的估計精度量化評價算法并以此為依據(jù)對MEMS-INS進行校正。仿真結(jié)果表明校正后的MEMS-INS水平位置誤差發(fā)散趨勢得到明顯抑制，驗證了本文提出的在線標定和容錯估計反饋策略可以有效提升非對稱冗余慣性導航系統(tǒng)的可靠性與精度。

1 非對稱冗余慣性導航系統(tǒng)總體結(jié)構(gòu)

1.1 結(jié)構(gòu)描述與坐標系定義

本文設計的非對稱冗余慣性導航系統(tǒng)如圖1所示。作為基準系統(tǒng)的FOG-INS（FINS）與輔助備份系統(tǒng)MEMS-INS（MINS）共同安裝在一個剛性基座上，L為兩套INS之間的相對距離即桿臂，在此安裝方式下桿臂可以視為是已知常量或忽略不計，并且不考慮剛性基座的彈性形變。

圖1 非對稱冗余慣性導航系統(tǒng)示意圖Fig.1 Asymmetric redundant inertial navigation system

定義載體坐標系（b系）為載體的“右-前-上”方向，其中 o1x1y1z1是FOG-INS的載體系bf，o2x2y2z2是MEMS-INS的載體系bm；導航坐標系（n系）定義為“東-北-天”方向，表示為 oxnynzn。

由于實際系統(tǒng)安裝誤差的存在，bf系與bm系之間不重合，定義安裝誤差角為 μ=[μx, μy, μz]T，它通常滿足小角度假設，則bf系與bm系之間的變換矩陣可以表示為：

1.2 非對稱冗余慣性導航系統(tǒng)算法框架

對系統(tǒng)中兩套慣導的故障檢測，本文將使用文獻[14]中提出的基于分段處理的序貫概率比檢驗（Sequential Probability Ratio Test, SPRT）算法完成。ARINS的算法框架如圖2所示。VB-AKF估計MINS的誤差參數(shù)并反饋其中的導航誤差，分段SPRT算法檢測到故障后，反饋策略基于估計量化評價補償MEMS器件誤差，故障隔離方法輸出合適的慣性導航信息。為了簡化分析，本文將只考慮FINS存在故障的情況。

圖2 非對稱冗余慣性導航系統(tǒng)算法框架Fig.2 The algorithm framework of the ARINS

2 容錯在線標定方法總體設計

2.1 系統(tǒng)模型的建立

選擇MINS的失準角φ、速度誤差δV、位置誤差δP、相對于FINS的安裝誤差角μ、陀螺漂移ε和加速度計零偏?作為狀態(tài)變量（每個變量包含3個維度的分量，因此X共18維）：

MINS姿態(tài)誤差方程可表示為：

速度和位置誤差方程可以表示為：

式(3)(4)(5)中對應矩陣Mii(i = a, v, p)的形式可以通過慣導誤差方程得到。安裝誤差角、MEMS陀螺漂移和MEMS加速度計零偏視為常量：

狀態(tài)方程可以寫為：

其中， F ∈R18×18為狀態(tài)轉(zhuǎn)移矩陣； w ∈R6為系統(tǒng)過程噪聲，此處可以認為是MEMS慣性元件的角度和速度隨機游走， G ∈R18×6是噪聲傳遞矩陣：

在線標定模型中的量測方程可以通過匹配兩套INS的姿態(tài)和速度信息來構(gòu)建。量測方程可以寫為：

其中， H ∈R6×18是量測矩陣， v ∈R6是測量噪聲。在使用兩套INS的速度輸出進行匹配時需要對FINS的速度桿臂誤差進行補償，修正后的FINS速度為：

在ARINS中，假設FINS解算得到的姿態(tài)沒有誤差，對應姿態(tài)陣；MINS實際解算得到的姿態(tài)存在誤差，對應的姿態(tài)陣是。兩個姿態(tài)陣可以分別表示為：

考慮到安裝誤差角μ，根據(jù)矩陣的鏈式乘法法則：

其中，φ和μ假設為小角度，將式(12)代入式(13)，MINS和FINS姿態(tài)陣元素間的對應關系有：

構(gòu)造與姿態(tài)信息相關的量測方程：

其中，

2.2 變分貝葉斯Kalman濾波

為實現(xiàn)MINS的容錯標定，有必要使用自適應濾波在線辨識模型中的未知時變量測噪聲。變分貝葉斯方法可以逼近狀態(tài)量和量測噪聲的聯(lián)合后驗分布，以得到它們的次優(yōu)估計。設離散化后，系統(tǒng)的狀態(tài)空間模型如下：

式(21)中的積分運算通常是無法獲得解析解的，為此對后驗更新使用變分貝葉斯近似可以得到以上過程可實現(xiàn)的遞推形式。要預測的聯(lián)合后驗概率分布可以表示為高斯分布和逆Gamma分布的乘積：

將觀測噪聲方差建立為啟發(fā)式動態(tài)模型（heuristic dynamics model）：

其中，系數(shù) ρ∈ (0,1]，通常可以選擇為1-exp(-4)。則聯(lián)合預測分布可以表示為式(25)。

根據(jù)式(22)-(25)，現(xiàn)在的后驗更新步驟中，狀態(tài)和量測噪聲方差通過似然函數(shù)耦合，仍然無法得到準確的后驗解析解。為了使式(25)計算易于處理，將對后驗分布進行變分近似，可以得到如下分解形式的近似解：

令分解后的近似分布式(26)與真實后驗概率分布(25)之間的Kullback-Leibler散度最小。 YX（ Xk）服從高斯分布， YR（ Rk）服從逆Gamma分布：

量測噪聲協(xié)方差可以表示為：

后驗分布的結(jié)果可進一步表示為：

為了避免對高維矩陣的求逆運算，減小濾波算法的運行時間并增加算法的數(shù)值穩(wěn)定性，對本節(jié)使用的變分貝葉斯自適應Kalman濾波進行序貫處理，改進后基于序貫結(jié)構(gòu)的VB-AKF算法如算法1所示。提出的ARINS將使用該算法完成對MEMS-INS的動態(tài)標定。

算法1 基于序貫的變分貝葉斯Kalman濾波Algorithm 1 Variational Bayesian adaptive Kalman filter based on sequential structure

3 基于收斂度的估計結(jié)果量化評價與估計反饋策略

3.1 動態(tài)濾波收斂度的定義

狀態(tài)向量的可觀測性及可觀測度分析中，通常使用分段線性定常系統(tǒng)（Piece Wise Constant System,PWCS）理論。然而這種方法只能在系統(tǒng)運行前以離線的方式進行理論分析，考慮到隨機噪聲的影響時，這種理論將不再嚴謹[15]。對此有學者針對導航系統(tǒng)存在隨機噪聲的特點，提出了較為嚴密的可觀測性分析方法[16]，但是仍具有計算量大等缺點，不適合用這些方法評價狀態(tài)變量的收斂性能。

在Kalman濾波中，矩陣Pk反應了各個狀態(tài)之間的協(xié)方差，其中對角線元素代表了狀態(tài)估計的誤差。對于時變系統(tǒng)， Pk對角線元素隨時間的變化可以定量地描述狀態(tài)估計有效性的強度水平。為了在濾波時可以對狀態(tài)變量估計的收斂程度有一個直觀的認識，本文中使用 Pk與其初始化矩陣 P0對角線元素之比的算術(shù)平方根定義為收斂度。對于狀態(tài)向量中的每一個變量（i ），其收斂度可以定義如下：

其中，n為狀態(tài)變量的維數(shù)； οk∈Rn代表了k時刻的收斂度； P0是初始協(xié)方差矩陣，它反映了初始狀態(tài)X0的估計誤差。根據(jù)該定義，收斂度在每一個濾波時刻k都可以獲得更新， οk（i）越大就意味著對應的狀態(tài)變量可以獲得更加精確和快速的估計。

3.2 基于收斂度的估計結(jié)果量化評價

需要注意的是，收斂度οk只是代表了對應狀態(tài)量具有的可以被估計到的水平，并不意味著在某一個時刻k，狀態(tài)量具有了很高的收斂度那么在這個時刻的對應狀態(tài)量的估計結(jié)果就是完全準確的。所以式(33)得到的k時刻收斂度kο就不適合直接應用于對狀態(tài)變量估計精度的評價?？紤]對收斂度kο進行指數(shù)加權(quán)迭代，得到指數(shù)加權(quán)收斂度 kO：

其中，Ok-1為k-1時刻的指數(shù)加權(quán)收斂度，并且O0=0n×1；b定義為遺忘因子，通常的取值范圍是0.95 ＜ b ＜ 0.99。無論收斂度有沒有經(jīng)過指數(shù)加權(quán)，它都沒有具體的上界和下界，并且不同狀態(tài)變量的估計結(jié)果可能對應不同數(shù)量級的收斂度，這就導致無法使用一種一致的、基于收斂度的標準對系統(tǒng)中每一個狀態(tài)變量的估計進行評價。

為了得到狀態(tài)估計的統(tǒng)一量化評價結(jié)果，考慮將式(34)中的指數(shù)加權(quán)收斂度 Ok通過映射函數(shù)進行轉(zhuǎn)換。本文中建立的映射函數(shù) T（ Ok）可以描述為：

其中，ξk定義為狀態(tài)量估計的歸一化評價；t為映射變換參數(shù)，它的取值與本文為了判斷估計結(jié)果是否滿足要求而給定的判斷評價標準S密切相關。圖3展示了不同參數(shù)t下指數(shù)加權(quán)收斂度 kO與狀態(tài)估計歸一化評價ξk之間的關系。

圖3 不同t值下的量化評價與指數(shù)加權(quán)收斂度之間的關系Fig.3 The relationship between Ok and ξk under different t

估計結(jié)果的量化評價通過指數(shù)加權(quán)操作后，將無界的收斂度通過函數(shù) T（ Ok）映射到[0,1]區(qū)間內(nèi)，把不同變量的評價標準統(tǒng)一為本文提出的估計歸一化評價ξk，以此更加直觀地判斷估計精度的可信程度。

3.3 基于量化評價的估計反饋策略

基于狀態(tài)估計的量化評價方法，本節(jié)提出一種估計反饋策略，以保證ARINS故障檢測和導航輸出的可靠性。如果其中待校正的MINS發(fā)生故障，那么該故障被診斷到后ARINS將直接使用FINS的輸出，無法驗證MINS的校正效果，因此本文將只聚焦于FINS的故障來完成對估計反饋策略的介紹。

由于MEMS慣性器件較低的性能，如果MINS中的誤差參數(shù)沒有利用估計結(jié)果進行補償，那么MINS中的導航誤差將會隨時間急劇累積，使得式(3)(4)(5)變得非線性，并使基于線性化假設的VB-AKF估計過程發(fā)散。假設開始導航時刻為0，故障被檢測到的時刻為Tf，那么時間段1就是導航開始到故障被檢測到的這段時間[0, Tf)，時間段2就是[Tf,∞)。則基于估計量化評價的反饋策略（Estimation Feedback Strategy, EFS）可以描述為：

(1) 在時間段1，對MINS的姿態(tài)失準角、速度和位置誤差（即導航誤差）進行反饋，如圖4(a)所示。

(2) 在故障被檢測到的Tf時刻，除了反饋導航誤差，還需要對狀態(tài)變量中包含的MEMS器件誤差完成量化評價后進行反饋。考慮為歸一化評價指標引入一個評判標準 S，只有當對應的狀態(tài)量X?i有ξk（i）＞S 時，該狀態(tài)量的估計結(jié)果才是可信的。根據(jù)實際經(jīng)驗，評判標準S被選擇為0.6826(1σ)。如圖4(b)所示。

(3) 完成故障診斷與誤差校正后（ t＞Tf），MINS將直接通過濾波一步預測過程得到的誤差信息完成對導航信息的校正與輸出，不使用FINS的姿態(tài)與速度對濾波一步預測結(jié)果進行修正，如圖4(c)所示。

圖4 估計反饋策略流程圖Fig.4 The flowchart of estimation feedback strategy

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

4.1 仿真參數(shù)設置

為了驗證提出算法的有效性，本節(jié)使用仿真數(shù)據(jù)進行相關實驗并對結(jié)果進行分析。對于本文設計的ARINS，兩套慣導輸出頻率都為200Hz，慣性器件的誤差參數(shù)如表1所示，兩套慣導之間的安裝誤差角μ設為0°，由于它們在剛性基座上的安裝位置較近，因此忽略桿臂帶來的影響。VB-AKF的濾波周期為0.1 s，其中系數(shù) ρ=1- exp （ -4 ），高斯-牛頓迭代次數(shù)N=5。故障檢測的周期被設置為0.1 s。式(35)中，遺忘因子b=0.99。式(36)中，當評價標準S設置為0.6826時，映射參數(shù)t取5便是合適的。

表1 器件誤差參數(shù)Tab.1 IMU Device Error Parameters

在設置的675 s仿真軌跡中，初始位置對應緯度40 °、經(jīng)度116 °，0~100 s車輛靜止，之后的機動過程中包括了變速、轉(zhuǎn)彎以及小動態(tài)范圍（幾度）的俯仰和橫滾運動。故障情形可以描述為：給FINS的y軸陀螺在570~580 s人為加入0.2 rad/s（11.46 °/s）的軟故障，完成故障診斷后，系統(tǒng)用經(jīng)過校正的MINS輸出載體的導航信息。

4.2 標定結(jié)果與量化評價

基于4.1節(jié)的相關設置，使用VB-AKF濾波殘差的分段SPRT算法可以在延時0.8 s后檢測到加入的故障，作為對照使用KF濾波殘差的分段SPRT算法則在2.1 s后檢測到加入的故障。在加入的故障被檢測到以前，兩種濾波算法對應的MEMS-INS y軸陀螺漂移、z軸陀螺漂移以及y軸加速度計零偏的標定結(jié)果如圖5-7所示。

圖5 MEMS陀螺常值漂移估計（y軸）Fig.5 Estimation of MEMS y-axis gyro drift

圖6 MEMS陀螺常值漂移估計（z軸）Fig.6 Estimation of MEMS z-axis gyro drift

圖7 MEMS加速度計常值零偏的估計（y軸）Fig.7 Estimation of MEMS accelerometer bias of y-axis

對于給定的MEMS慣性器件誤差，在VB-AKF下y軸陀螺漂移、z軸陀螺漂移和y軸加速度計零偏的估計分別是：13.89 °/h、23.83 °/h和2928 μg，對應有估計誤差30.55%、19.15%和2.40%；KF下對應誤差參數(shù)的估計結(jié)果分別是：-11.82 °/h、31.88 °/h和2993 μg，對應有估計誤差：159.10%、59.40%和0.23%，標定結(jié)果如表2所示。對于慣導性能影響最大的陀螺漂移，VB-AKF的估計精度相比于KF的估計分別提升了80.80%和67.76%。在系統(tǒng)存在故障時經(jīng)過VB-AKF算法標定的MEMS器件誤差參數(shù)的準確性明顯高于KF算法的標定結(jié)果。

表2 MEMS慣性器件標定結(jié)果Tab.2 MEMS inertial device calibration results

同時根據(jù)3.2節(jié)設計的基于指數(shù)加權(quán)收斂度的估計結(jié)果量化評價算法，還可以得到基于VB-AKF的部分狀態(tài)估計的量化評價如圖8所示。

圖8 各狀態(tài)的量化評估結(jié)果Fig.8 Quantitative evaluation of corresponding states

對于本文的ARINS，VB-AKF和KF都無法對狀態(tài)變量實現(xiàn)無偏估計。在故障檢測算法存在延遲的時間內(nèi)，KF的估計結(jié)果已經(jīng)發(fā)生了明顯發(fā)散，但是VB-AKF在這段時間內(nèi)的估計結(jié)果并不會產(chǎn)生劇烈的變化，證明VB-AKF對于基準FINS中的故障具有良好的容錯性能。因此，對于考慮基準系統(tǒng)存在故障可能性的在線標定過程中，由于故障檢測不可避免的延時，自適應濾波算法是必不可少的。

MINS的動態(tài)標定模型是線性時變的，其狀態(tài)變量的估計性能與載體的機動方式密切相關。在本節(jié)仿真實驗設置的機動方式下，狀態(tài)變量在故障發(fā)生前都可以獲得較好的估計，明顯高于給定的評價標準S。

4.4 基于估計反饋策略的誤差校正

ARINS在完成故障診斷后，將只依靠MINS完成后續(xù)的慣性導航參數(shù)輸出。根據(jù)4.3節(jié)的結(jié)果，使用KF發(fā)散的錯誤估計結(jié)果補償MEMS器件誤差必將增大MINS的系統(tǒng)誤差，為此考慮以下兩種對MEMS慣性器件誤差的處理情況：（1）不補償（2）故障情況下基于VB-AKF補償。本節(jié)的結(jié)果都是以無故障時理想FINS的水平位置解算輸出作為參考。

由于考慮的情況（1）中沒有對MEMS的慣性器件誤差進行任何補償，此時的水平位置誤差會隨著時間而急劇累積；而情況（2）使用本文提出的VB-AKF和估計反饋策略對MEMS慣性器件的誤差進行容錯估計和補償，可有效抑制后續(xù)導航過程中水平位置誤差的發(fā)散。當故障在570 s發(fā)生后，60 s內(nèi)（即570~630 s）這兩種情況下的ARINS水平位置誤差隨時間變化的趨勢如圖9所示，其中630 s時未經(jīng)過補償?shù)乃轿恢谜`差是173.30 m，經(jīng)過補償后水平位置誤差為77.55 m，精度獲得了55.25%的提升。

圖9 基于VB-AKF和估計反饋策略補償后的ARINS水平位置誤差Fig.9 Horizontal position error of ARINS with compensation based on VB-AKF and EFS

再分別對考慮的兩種情況進行10次仿真實驗，得到630 s時水平位置誤差如表3所示。相比于不補償?shù)那樾危琕B-AKF+EFS補償后水平位置誤差減小了33.59%，該結(jié)果進一步驗證了提出方法的有效性。

表3 水平位置誤差Tab.3 Horizontal Position Error

5 結(jié) 論

針對一種同時裝備有FOG-INS和MEMS-INS的非對稱冗余慣性導航系統(tǒng)，本文建立了MEMS-INS誤差模型，使用變分貝葉斯自適應Kalman濾波進行在線容錯標定并補償導航誤差，基于指數(shù)加權(quán)收斂度的估計精度量化評價算法可以直觀地反饋MEMS慣性器件誤差。仿真實驗驗證了提出算法的有效性。經(jīng)過0.8 s的故障檢測延遲，完成校正的MEMS-INS誤差發(fā)散趨勢得到明顯抑制，與無器件估計誤差反饋的過程相比，系統(tǒng)故障后60 s內(nèi)水平位置精度提升了33.59%，使得冗余結(jié)構(gòu)下系統(tǒng)在成本和體積受約束時也能夠保證可靠的慣性導航參數(shù)輸出。這些成果在特定組合導航應用場景下具有一定工程意義。