單 斌，侯夢婷，王雪梅，楊 波，許 哲，李 燦

（火箭軍工程大學(xué) 導(dǎo)彈工程學(xué)院，西安 710025）

近幾年，微機電系統(tǒng)（Micro-Electro-Mechanical System,MEMS）以其成本低、體積小、電路可單片集成等優(yōu)勢得到廣泛應(yīng)用。微慣性測量單元（Micro Inertial Measurement Unit,MIMU）從各種微機電系統(tǒng)中脫穎而出，目前已發(fā)展成為構(gòu)建車載慣性導(dǎo)航系統(tǒng)的新興傳感器件之一。由于越來越多的陸基車輛需要導(dǎo)航系統(tǒng)在行駛過程中完成初始對準(zhǔn)任務(wù)，因此，行進間初始對準(zhǔn)成為了一個備受關(guān)注的問題。但受到制造工藝等因素的限制，MIMU 精度不高，不能單獨用于導(dǎo)航，全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System,GNSS）是最常見的高精度外部導(dǎo)航傳感器。

初始對準(zhǔn)通常分為兩個階段，即粗對準(zhǔn)階段和精對準(zhǔn)階段。理想情況下，粗對準(zhǔn)階段為精對準(zhǔn)提供初始姿態(tài)。使用最優(yōu)估計對準(zhǔn)（Optimization-based Alignment,OBA）是一種有效的粗對準(zhǔn)方法，Lu 等[1]結(jié)合梯度下降法減少積分間隔，在構(gòu)建過程中識別MEMS 陀螺儀誤差，提高了OBA 方法在低精度慣導(dǎo)系統(tǒng)中的適用性。王偉等[2]通過建立非線性系統(tǒng)的狀態(tài)方程來估算失準(zhǔn)角和MEMS 陀螺儀的常值漂移，并將其估計結(jié)果反饋到OBA 算法中，提高了動基座對準(zhǔn)的快速性、準(zhǔn)確性。傳統(tǒng)OBA 方法沒有考慮慣性器件誤差帶來的影響，需要很高的傳感器精度和計算能力，然而低成本的慣導(dǎo)系統(tǒng)通常配備了低精度的慣性器件，導(dǎo)致雙矢量誤差隨時間推移而累積，影響算法的可靠性[3]，因此，不適用于低精度捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System,SINS）。

精對準(zhǔn)技術(shù)是以現(xiàn)代控制理論為基礎(chǔ)的狀態(tài)估計方法，以Kalman 濾波為基礎(chǔ)的最優(yōu)估計算法是標(biāo)準(zhǔn)方案，被廣泛應(yīng)用在各類實際系統(tǒng)中。Kalman 濾波的性能依賴準(zhǔn)確的誤差模型和觀測信息，楊咚等[4]建立非線性大方位失準(zhǔn)角誤差模型，提出了一種改進Sage_Husa 自適應(yīng)卡爾曼濾波方法，設(shè)計自適應(yīng)因子，具有較強的控制動力學(xué)模型誤差影響的能力?？煽康腒alman 濾波算法要求有準(zhǔn)確的模型、可靠的觀測信息以及合理的估計方法，當(dāng)系統(tǒng)模型與觀測信息同時出現(xiàn)異常情況時，自適應(yīng)因子容易受到觀測粗差的影響，不能準(zhǔn)確估計，因此，在精對準(zhǔn)過程中需要設(shè)計合理的自適應(yīng)抗差估計方法。

針對上述問題，設(shè)計了一種適用于低精度SINS 的OBA 動基座對準(zhǔn)方法。在這種方法中，對雙矢量積分公式進行分解，縮短積分間隔，減小隨時間推移而不斷累積的加速度計誤差，從而提高雙矢量的精度；同時對系統(tǒng)進行誤差分析，推導(dǎo)低精度慣導(dǎo)系統(tǒng)的陀螺儀誤差方程，結(jié)合自適應(yīng)無跡Kalman 濾波算法對失準(zhǔn)角和陀螺儀誤差進行估計，使得改進OBA 方法適用于低精度SINS 系統(tǒng)，實現(xiàn)GNSS 輔助MIMU 粗對準(zhǔn)。然后利用粗對準(zhǔn)的對準(zhǔn)結(jié)果對卡爾曼濾波參數(shù)進行初始化，提出基于大方位失準(zhǔn)角模型的改進自適應(yīng)抗差無跡卡爾曼濾波（Adaptive Robust Unscented Kalman Filter,ARUKF）精對準(zhǔn)方法，進一步提高姿態(tài)精度。最后通過跑車實驗驗證了本文方法的有效性。

1 本文所用坐標(biāo)系

1）地心慣性坐標(biāo)系（i系，oxiyizi）。原點在地球質(zhì)心，x軸在赤道面并指向春分點，z軸沿自轉(zhuǎn)軸方向指向北極。

2）導(dǎo)航坐標(biāo)系（n系，oxnynzn）。選用當(dāng)?shù)氐牡乩碜鴺?biāo)系，取“東-北-天”坐標(biāo)系作為導(dǎo)航坐標(biāo)系。初始時刻導(dǎo)航坐標(biāo)系用n0表示。

3）地球坐標(biāo)系（e系，oxeyeze）。地球坐標(biāo)系固聯(lián)于地球，隨地球自轉(zhuǎn)運動。初始時刻地球坐標(biāo)系用e0表示。

4）載體坐標(biāo)系（b系，oxbybzb）。載體坐標(biāo)系固聯(lián)于載體，隨載體運動，取“右-前-上”坐標(biāo)系作為載體坐標(biāo)系。通常認(rèn)為載體坐標(biāo)系與慣性器件坐標(biāo)系指向一致。

5）計算坐標(biāo)系（n′系）。由慣導(dǎo)系統(tǒng)機械編排輸出信息所確定的導(dǎo)航坐標(biāo)系記為n′系。n′系與n系之間存在的偏差由慣導(dǎo)系統(tǒng)的器件誤差、模型誤差、計算誤差決定。

2 改進的最優(yōu)估計粗對準(zhǔn)

為了構(gòu)建雙矢量，根據(jù)捷聯(lián)慣導(dǎo)比力方程構(gòu)建雙矢量的觀測值：

在矢量α和β已知的情況下，可以根據(jù)文獻[6]的最小二乘理論，通過計算K的最小特征值的歸一化特征向量，確定姿態(tài)矩陣

傳統(tǒng)OBA 方法應(yīng)用在低精度慣導(dǎo)系統(tǒng)時，慣性器件誤差的長期累積影響雙矢量精度，需要對OBA 方法進行改進。分析式(6)第一個公式，向量α(t)的求取需要對加速度計的輸出fb積分，這樣會導(dǎo)致加速度計誤差長期累積，影響向量α(t)的精度。為減小傳感器誤差帶來的影響，雙矢量α(t)和β(t)可表示為：

式(5)與式(12)相比，式(5)的積分區(qū)間必須從初始時刻開始，而式(12)積分下限可以是當(dāng)前時刻的任意時刻。這一改變使得fb的積分不會隨著時間推移累積，因此，可以大大減少由加速度計偏置引起的誤差。

對式(13)Kalman 濾波器的狀態(tài)方程離散化：

其中，xk是Kalman 濾波器的狀態(tài)量；Fk是狀態(tài)轉(zhuǎn)移矩陣；ωk是系統(tǒng)噪聲，滿足ωk～N(0,Qk)。

用α′ 表示ib′(0)系下的觀測向量時則式(5)可以改寫為：

根據(jù)式(27)，整理得：

將系統(tǒng)的觀測方程表示為：

其中，zk為觀測矩陣；Hk為狀態(tài)觀測矩陣；νk是觀測噪聲，滿足νk～N(0,Rk)，且與系統(tǒng)噪聲ωk不相關(guān)。

改進的OBA 粗對準(zhǔn)算法流程如圖1 所示。

3 改進的自適應(yīng)抗差精對準(zhǔn)

3.1 GNSS 輔助MIMU 動基座精對準(zhǔn)模型

在精對準(zhǔn)過程中，為了更準(zhǔn)確地描述系統(tǒng)模型，建立基于大方位失準(zhǔn)角條件下的誤差模型，記慣導(dǎo)系統(tǒng)機械編排輸出信息所確定的導(dǎo)航坐標(biāo)系為n′系。n系與n′系之間存在的偏差記為失準(zhǔn)角φ＝ [φxφyφz]。此時，n系與n′系之間的變換關(guān)系為：

建立大方位失準(zhǔn)角的非線性誤差方程(21)：

其中，εi表示陀螺儀常值漂移誤差；wgi表示白噪聲；β和σ分別為馬爾科夫過程的相關(guān)系數(shù)和驅(qū)動白噪聲的標(biāo)準(zhǔn)方差。

考慮初始對準(zhǔn)過程時間較短，將加速度計誤差模型近似為常值漂移加白噪聲[9]，即常值零偏?b和隨機誤差wa，誤差模型為：

式(21)-(23)構(gòu)成了大方位失準(zhǔn)角的狀態(tài)方程，定義對準(zhǔn)誤差模型為：

其中，f(x)為狀態(tài)方程非線性部分。

載車行駛過程只考慮東向和北向速度，狀態(tài)量為δVE、δVN，為使系統(tǒng)的誤差方程適合Kalman 濾波，定義狀態(tài)變量為：

取水平方向的速度誤差為觀測向量，定義觀測量為Z＝[δVxδVy]T。采用松組合方式，以水平方向GNSS 和INS 輸出的速度之差作為速度誤差觀測量。構(gòu)造觀測方程為：

其中，系統(tǒng)噪聲變量W＝[waxwaywgxwgywgz]T為N(0,Q)的零均值高斯白噪聲；觀測噪聲η為N(0,R)的零均值高斯白噪聲。

針對低精度MIMU 誤差方程，做如下簡化：

針對低精度MIMU，由于地球自轉(zhuǎn)角速度約為7.2921158 × 10-5rad/s，遠小于陀螺測量誤差，因此，忽略地球自轉(zhuǎn)的影響。

3.2 簡化無跡卡爾曼濾波

本文GNSS 輔助車載MIMU 精對準(zhǔn)過程建立了大方位失準(zhǔn)角的非線性誤差模型，由于系統(tǒng)狀態(tài)空間模型是非線性的，傳統(tǒng)的Kalman 濾波算法不能直接應(yīng)用。無跡卡爾曼濾波基于無跡變換（Unscented Transformation,UT）的思想，具有較高濾波精度[10]。本文采用UKF 算法對狀態(tài)變量進行濾波估計，解算公式與具體步驟參考文獻[11]。

濾波過程中，式(25)為濾波的狀態(tài)變量。根據(jù)式(21)和式(26)可以發(fā)現(xiàn)，系統(tǒng)狀態(tài)方程是非線性方程，量測方程是線性方程，因此，在使用UKF 濾波時，量測更新過程采用線性Kalman 濾波進行簡化，從而減少采樣次數(shù)和計算量。

簡化后的量測更新過程為：

3.3 改進自適應(yīng)抗差算法

抗差估計方法主要通過判斷觀測信息中的隨機變量，并對異常觀測信息的權(quán)重進行調(diào)整，減少異常觀測量對濾波器性能的影響。自適應(yīng)濾波器往往根據(jù)觀測信息的準(zhǔn)確性判斷系統(tǒng)動力學(xué)模型是否準(zhǔn)確，通過構(gòu)造統(tǒng)計量，辨別系統(tǒng)動力學(xué)模型誤差，利用建立的自適應(yīng)因子降低動力學(xué)模型誤差。當(dāng)系統(tǒng)動力學(xué)模型和觀測量同時發(fā)生異常時，為了控制觀測異常和狀態(tài)預(yù)測信息異常對狀態(tài)參數(shù)估值的影響，根據(jù)抗差估計以及自適應(yīng)估計原理可實現(xiàn)自適應(yīng)抗差濾波，構(gòu)造濾波極值原則[10]：

其中，αk（0 ＜αk≤1）為自適應(yīng)因子，控制狀態(tài)模型權(quán)矩陣；Vk為觀測殘差；Σk為觀測向量Zk的抗差等價權(quán)矩陣（即R-1），是觀測向量權(quán)矩陣的自適應(yīng)估值。

自適應(yīng)抗差濾波原則為一個關(guān)于Xk的二次函數(shù)，存在一個極（最）小值。對式(28)中Xk求極值可得：

式(30)即為狀態(tài)參數(shù)向量的自適應(yīng)抗差濾波解，可以通過矩陣恒等變換得到卡爾曼濾波更新公式：

由式(28)可以看出，準(zhǔn)確的觀測噪聲等價權(quán)矩陣Σk和自適應(yīng)因子αk可以有效降低由于異常信息帶來的誤差，因此，完整的自適應(yīng)抗差濾波理論的關(guān)鍵在于構(gòu)建正確的等價權(quán)矩陣以及自適應(yīng)因子[12]。為有效構(gòu)造正確的等價權(quán)矩陣以抑制觀測量粗差的影響，引入抗差等價權(quán)因子λk(0 ＜λk≤1)，得到等價狀態(tài)預(yù)測協(xié)方差陣和等價觀測噪聲協(xié)方差陣。

改進的自適應(yīng)抗差算法分為兩步：

1）構(gòu)造抗差等價權(quán)因子λk：

在當(dāng)前觀測值無故障的情況下，預(yù)測殘差Vk服從零均值的正態(tài)分布，方差為σ2。采用假設(shè)檢驗的方法探測GNSS 故障觀測值，設(shè)計粗差檢驗統(tǒng)計量為：

其中，i表示第i個觀測值。由于服從標(biāo)準(zhǔn)正態(tài)分布，對應(yīng)的零假設(shè)和備擇假設(shè)為：

其中，nδ/2為假設(shè)檢驗對應(yīng)正態(tài)分布的探測邊界值。

2）構(gòu)造自適應(yīng)因子αk

采用預(yù)報殘差法與兩段函數(shù)相結(jié)合的方式構(gòu)造自適應(yīng)因子αk，然而預(yù)報殘差法的前提是GNSS 觀測量可靠，當(dāng)觀測量異常時，該方法構(gòu)造的自適應(yīng)因子無法正確區(qū)分動力學(xué)模型誤差和觀測誤差，會影響導(dǎo)航解的精度和可靠性[13]，因此需先剔除掉觀測異常信息。

設(shè)k時刻第i個觀測值存在粗差δLi，第i個受粗差影響的預(yù)測殘差為：

其中，tr(?)表示矩陣的跡。

在構(gòu)造自適應(yīng)因子時，為了削弱觀測異常帶來的影響，考慮抗差等價權(quán)因子λk構(gòu)造時的判別情況，剔除含有粗差觀測量對應(yīng)的預(yù)測殘差和預(yù)測殘差協(xié)方差，由剩余預(yù)測殘差及協(xié)方差構(gòu)造可靠的判別統(tǒng)計量：

其中，c＝ 1.0～ 2.5。

改進的自適應(yīng)抗差無跡卡爾曼濾波算法流程如圖2 所示。

圖2 改進的自適應(yīng)抗差無跡卡爾曼濾波算法流程圖Fig.2 Flow chart of improved adaptive robust unscented Kalman filtering algorithm

圖3 實驗裝置安裝圖Fig.3 Installation diagram of the test set

4 跑車實驗

4.1 實驗設(shè)置

實驗裝置由一個MSI3000E 微慣性測量組合（內(nèi)嵌三軸MSG7100D 微陀螺儀、三軸MSA6000D 微加速度計）、GNSS 雙天線、直流穩(wěn)壓電源、功分器、計算機、衛(wèi)星接收機以及同步調(diào)理器構(gòu)成。MSI3000E 微慣性測量組合性能指標(biāo)如表1 所示。

表1 MSI3000E 微慣性測量組合性能指標(biāo)Tab.1 Performance Indicators of MSI3000E Micro Inertial Measurement Unit

在實驗車上搭載設(shè)備后進行跑車實驗，在繞城高速進行測試，期間部分位置存在建筑物、樹木遮擋等情況，運動速度不小于15 m/s，實驗軌跡如圖4 所示。

圖4 現(xiàn)場實驗軌跡示意圖Fig.4 Schematic diagram of the trajectory of the field test

圖5-7 分別為測得的載體運動參數(shù)信息。

圖5 載體姿態(tài)角信息Fig.5 Carrier attitude angle information

圖6 載體速度信息Fig.6 Carrier speed information

圖7 載體位置信息Fig.7 Carrier location information

4.2 不同行進間粗對準(zhǔn)方法的比較

為了驗證改進OBA 方法應(yīng)用于低成本慣導(dǎo)系統(tǒng)中的粗對準(zhǔn)效果，設(shè)計跑車實驗方案進行比較。

方案一：傳統(tǒng)OBA 粗對準(zhǔn)方法；

方案二：改進OBA 粗對準(zhǔn)方法；

以高精度組合導(dǎo)航系統(tǒng)為參考，圖8-10 為跑車實驗中改進OBA 方法（方案二）與傳統(tǒng)OBA 方法（方案一）姿態(tài)誤差角對比。

圖8 航向角對準(zhǔn)誤差Fig.8 Heading angle alignment error

圖9 俯仰角對準(zhǔn)誤差Fig.9 Pitch angle alignment error

表2 為粗對準(zhǔn)結(jié)束時刻兩種方法的性能對比。

表2 不同行進間粗對準(zhǔn)方法的對準(zhǔn)結(jié)果Tab.2 Alignment results of different interline coarse alignment methods

由表2 可以看出，所提出的改進OBA 方法更適用于低精度慣導(dǎo)系統(tǒng)粗對準(zhǔn)過程，對準(zhǔn)效果明顯優(yōu)于傳統(tǒng)的OBA 方法。相對于傳統(tǒng)OBA 方法，改進OBA方法的最大水平姿態(tài)角誤差從2.97 °降低到0.96 °，航向角誤差從3.36 °降低到1.71 °。兩種對準(zhǔn)方法都能實現(xiàn)快速粗對準(zhǔn)，改進的OBA 方法克服了傳統(tǒng)OBA 方法無法對慣性器件誤差處理的缺陷，縮短了積分間隔，可以識別并抑制MEMS 慣性器件的偏置誤差，具有很強的準(zhǔn)確性和適應(yīng)性。

4.3 不同行進間精對準(zhǔn)方法的比較

在上一節(jié)粗對準(zhǔn)方法實驗驗證的基礎(chǔ)上，本節(jié)開展GNSS 輔助MIMU 精對準(zhǔn)方法的跑車實驗驗證。

首先設(shè)計常規(guī) ARUKF 精對準(zhǔn)方法與改進ARUKF 精對準(zhǔn)方法對比實驗，車輛在對準(zhǔn)開始時刻速度為零，開始對準(zhǔn)后車輛立即行駛。為了充分反映濾波算法在跑車實驗中收斂性能，將運算時長增加為180 s。設(shè)計對比實驗，以高精度組合導(dǎo)航系統(tǒng)對準(zhǔn)結(jié)果為參考。圖11-13 為精對準(zhǔn)過程常規(guī)ARUKF 與改進ARUKF 方法的失準(zhǔn)角誤差曲線圖。

圖11 東向失準(zhǔn)角誤差Fig.11 Eastward misalignment angle error

圖12 北向失準(zhǔn)角誤差Fig.12 Northward misalignment angle error

圖13 方位失準(zhǔn)角誤差Fig.13 Azimuthal misalignment angle error

對上述實驗數(shù)據(jù)進行相應(yīng)的統(tǒng)計分析，得到ARUKF 與改進ARUKF 方法的精對準(zhǔn)失準(zhǔn)角均方根誤差（Root Mean Square Error,RMSE），如表3 所示。

表3 不同濾波方法失準(zhǔn)角RMSE 比較Tab.3 Comparison of RMSE of misalignment angles for different filtering methods

從表3 可以看出，ARUKF 與改進ARUKF 方法的東向失準(zhǔn)角RMSE 分別為0.30 °、0.15 °，北向失準(zhǔn)角RMSE 分別為0.33 °、0.10 °，在方位角失準(zhǔn)角RMSE分別為0.39 °、0.11 °。相較于ARUKF，改進ARUKF精對準(zhǔn)方案收斂性能明顯較好，并且能夠在較長一段時間保持穩(wěn)定。

圖14-15 為常規(guī)ARUKF 與改進ARUKF 兩種方法的東向速度誤差和北向速度誤差估計結(jié)果，表3 統(tǒng)計了兩種方法的速度RMSE。

圖14 東向速度誤差Fig.14 Eastward velocity error

圖15 北向速度誤差Fig.15 Northward velocity error

從表4 可以看出，ARUKF 與改進ARUKF 方法的東向速度RMSE 分別為0.35 m/s、0.09 m/s，北向速度RMSE分別為0.29 m/s、0.08 m/s。在速度誤差方面，改進ARUKF 方法同樣具有較好的收斂性能。

表4 不同濾波方法速度RMSE 比較Tab.4 Comparison of speed RMSE of different filtering methods

綜合分析跑車對比實驗，相比于常規(guī)ARUKF 算法，基于改進ARUKF 的初始對準(zhǔn)誤差波動幅度明顯降低了很多，提高了組合定姿系統(tǒng)的穩(wěn)定性。通過上述兩組對比實驗可以發(fā)現(xiàn)，當(dāng)GNSS 速度觀測量中含有粗差時，給兩種組合定姿精度帶來了不同程度的影響。這主要是由于UKF 方法不具有修正模型和抵抗粗差的能力，常規(guī)ARUKF 方法在觀測值存在異常的情況下，自適應(yīng)因子易被粗差污染；本文提出的改進ARUKF 方法通過引入抗差因子和自適應(yīng)因子，克服了觀測量異常帶來的影響，有效抵抗粗差并抑制誤差發(fā)散，能夠更好地處理和修正觀測量異常以及動力學(xué)模型誤差所帶來的影響，使組合定姿系統(tǒng)具有一定的自適應(yīng)抗差能力。

5 結(jié)論

本文提出了一種GNSS 輔助車載MIMU 動基座初始對準(zhǔn)方法。在粗對準(zhǔn)中，以最優(yōu)估計理論為基礎(chǔ)，對OBA 對準(zhǔn)算法進行改進，提高了慣性器件輸出精度，適用于低精度慣導(dǎo)系統(tǒng)且計算簡單。在精對準(zhǔn)中，為了保證對準(zhǔn)結(jié)果的可靠性，引入了自適應(yīng)抗差估計方法，能夠有效抑制并消除觀測中的粗差以及動力學(xué)模型異常的影響，具有較高的穩(wěn)定性。跑車實驗證明了本文所提出的行進間對準(zhǔn)方法的正確性和有效性。相比于傳統(tǒng)OBA 對準(zhǔn)方法，在一定程度上可縮短對準(zhǔn)時間，提高初始對準(zhǔn)精度，改進的自適應(yīng)抗差估計能夠平衡濾波估計值對GNSS 觀測值和動力學(xué)模型的信任，提高精對準(zhǔn)的準(zhǔn)確性和可靠性，具有很好的應(yīng)用前景。