一種基于自適應中心差分卡爾曼濾波的mems捷聯慣導初始對準方法
【專利摘要】本發明屬于導航領域，具體涉及一種對量測噪聲統計特性不確定的MEMS捷聯慣導初始對準方法。包括：利用GPS確定載體的初始經度、緯度；采集MEMS加速度計輸出的加速度數據和磁強計輸出的磁場強度數據；選取濾波初始值；采集MEMS陀螺儀輸出的角速度數據和MEMS加速度計輸出的加速度數據；估計出平臺失準角；完成精確的初始對準。本發明在每次迭代時通過監控新息序列的變化，實時估計和調整量測噪聲方差陣，增加了系統的自適應能力，在保證濾波精度的基礎上提高了濾波的穩定性。
【專利說明】一種基于自適應中心差分卡爾曼濾波的MEMS捷聯慣導初 始對準方法
【技術領域】
[0001] 本發明屬于導航領域，具體涉及一種對量測噪聲統計特性不確定的MEMS捷聯慣 導初始對準方法。
【背景技術】
[0002] 微機械電子系統（Micro Electronic Mechanical System-MEMS)，又簡稱為微機 電系統，是采用納米技術加工出的新一代微型機電裝置。它以硅半導體材料為加工對象，采 取專用集成電路制造技術加工出的外形尺寸在毫米量級的具有驅動、控制和信號處理功能 的微型器件。MEMS慣性測量單元是一種包含三軸MEMS陀螺和三軸MEMS加速度計的慣性測 量裝置，其中陀螺和加速度計的三個軸相互垂直，符合右手定則。鑒于MEMS具有體積小、重 量輕、功耗少、成本低、集成化程度高等優點，其將擁有更廣闊的工程應用前景，尤其對于微 小型運載體的導航、制導與姿態控制具有重要意義。
[0003] 初始對準是捷聯慣導系統中的關鍵技術之一，其對準時間和精度直接影響慣導系 統的工作性能，捷聯慣導系統初始對準的主要目的是建立姿態矩陣的初始值。初始對準過 程主要包括粗對準和精對準兩個階段，首先用利用解析法粗略估計出失準角的大小，其次， 建立誤差狀態方程，由于MEMS慣性器件精度不高，在粗對準結束時航向誤差角比較大，因 此初始對準誤差狀態方程是非線性的，最后，利用濾波最優估計方法精確估計出失準角的 大小從而實現精對準。
[0004] CDKF(Central difference Kalman filter)濾波器的基礎是中心差分變換，利用 插值理論的非線性變換方法，求解隨機變量的均值和方差等統計量，CDKF以Sterling插值 公式為出發點，用多項式逼近非線性方程導數的方法來避免復雜的求導運算，采用中心差 分代替泰勒級數展開中的一階和二階導數。CDKF比傳統的非線性濾波EKF精度高，穩定性 好，且不需要計算復雜的雅克比矩陣；與同樣基于Sigma點的非線性濾波UKF相比，具有稍 高的理論精度，但在實際應用時，CDKF會受到系統噪聲不確定和外界環境對濾波模型干擾 因素的影響。系統噪聲主要來源加速度計零偏和陀螺漂移，即慣性測量元件誤差。由于慣 性器件誤差一般初始對準之前都經過實驗建模和補償，因此在實際對準過程中，對系統的 狀態模型的影響可以忽略；考慮到對準時，工作環境的不同，量測噪聲也有所不同，工作環 境的惡劣程度將直接影響量測噪聲的性能，不確定性很大，因此需要對量測噪聲進行實時 估計。將自適應估計方法引入到CDKF算法中，從而保證CDKF在量測噪聲統計模型不準確 時仍能保持正常收斂，并具有更高的估計精度，提高系統穩定性。
【發明內容】
[0005] 本發明的目的是為了解決在量測噪聲統計模型不準確時，提供一種基于自適應中 心差分卡爾曼濾波的MEMS捷聯慣導初始對準方法。
[0006] 本發明的目的是這樣實現的：
[0007] (1)利用GPS確定載體的初始經度、緯度；
[0008] (2)采集MEMS加速度計輸出的加速度數據和磁強計輸出的磁場強度數據，利用解 析法進行粗對準，得到初始矩陣C)f(O)，確定載體姿態信息即縱搖角、橫搖角、航向角，其中 b代表載體坐標系，n'代表計算導航坐標系；
[0009] (3)選取濾波初始值
[0010] x0 = ^fxJ5P0 - ￡[(.?-, -x0)(x0 -i0)r]
[0011] 其中Xtl為狀態變量的初值，Ptl為狀態變量的初始誤差協方差矩陣；
[0012] (4)采集MEMS陀螺儀輸出的角速度數據和MEMS加速度計輸出的加速度數據；
[0013] (5)利用自適應中心差分卡爾曼濾波方法進行濾波，估計出平臺失準角；
[0014] (6)利用步驟（5)估計出來的平臺失準角修正MEMS捷聯慣導系統的初始矩陣 Cf (〇)，得到精確的初始矩陣Q(O)，即q(0) = q. C^(O)，完成精確的初始對準，其他中n 代表導航坐標系。
[0015] 初始矩陣
[0016]
【權利要求】
1. 一種基于自適應中心差分卡爾曼濾波的MEMS捷聯慣導初始對準方法，其特征在 于： (1) 利用GPS確定載體的初始經度、緯度； (2) 采集MEMS加速度計輸出的加速度數據和磁強計輸出的磁場強度數據，利用解析法 進行粗對準，得到初始矩陣(〇)，確定載體姿態信息即縱搖角、橫搖角、航向角，其中 表載體坐標系，η'代表計算導航坐標系； (3) 選取濾波初始值 元=可-\]，C =廠[(? -元)(-\ -元)'] 其中Xtl為狀態變量的初值，Pq為狀態變量的初始誤差協方差矩陣； (4) 采集MEMS陀螺儀輸出的角速度數據和MEMS加速度計輸出的加速度數據； (5) 利用自適應中心差分卡爾曼濾波方法進行濾波，估計出平臺失準角； (6) 利用步驟（5)估計出來的平臺失準角修正MEMS捷聯慣導系統的初始矩陣Cf(O)， 得到精確的初始矩陣C：(〇)，即CT(O) = C^qf(O)，完成精確的初始對準，其他中η代表導航 坐標系。
2. 根據權利要求1所述的一種基于自適應中心差分卡爾曼濾波的MEMS捷聯慣導初始 對準方法，其特征在于：所述的初始矩陣
Θ(Ι是縱搖角，Y (I是橫搖角，科!是航向角。
3. 根據權利要求2所述的一種基于自適應中心差分卡爾曼濾波的MEMS捷聯慣導初始 對準方法，其特征在于：所述的狀態變量包括坐標系三個方向的失準角以及坐標系水平方 向的速度誤差。
4. 根據權利要求3所述的一種基于自適應中心差分卡爾曼濾波的MEMS捷聯慣導初始 對準方法，其特征在于，所述的步驟（5)包括： (5. 1)確定權值：
其中h為中心差分步長，為高斯分布，最優估計值為m為系統狀態變量的維數； (5. 2)構造2m+l維的Sigma點： 4-1 =[Λ；. P-^-I I t 其中為k-1時刻估計出的狀態變量，Plrt為k-1時刻狀態變量的誤差協方差矩陣； (5. 3)在狀態方程為非線性、量測方程線性的條件下進行時間更新： Y k/k-1 = fk-1 ( ζ k-1);
由·％-1和Pi1計算Sigma點ξ Jrf,通過非線性狀態函數fH ( ·)傳播為γ k/Vl,由γ k/H 可得狀態預測毛/H和狀態預測誤差協方差陣Pk/k+ Qlrt為系統噪聲； (5.4)在狀態方程為非線性、量測方程線性的條件下進行量測更新：
其中，Kk為濾波增益矩陣，ek為新息，毛為狀態估計值其中包括平臺失準角，Pk為狀態 誤差協方差矩陣，瓦為量測噪聲的在線估計，1為遺忘因子，取值范圍為〇. 95?0. 99。
5.根據權利要求4所述的一種基于自適應中心差分卡爾曼濾波的MEMS捷聯慣導初始 對準方法，其特征在于，所述的狀態方程為： 文=/認)+ 6^，系統噪聲向量4 4 wt]
其中，心，馬分別為地球子午、卯酉曲率半徑，φχ，φρ小2為三個方向的平臺失準 角；SVx，Svy為速度誤差；L為當地緯度；Wie為地球自轉角速度；4為三個軸向的 陀螺漂移為陀螺零均值高斯白噪聲；為三個軸向的加速度零偏； w^, 為加速度計零均值高斯白噪聲；fx，fy，fZ為加速度輸出比力在計算地理坐標系 上的值；C' u是載體系到計算地理系矩陣中的元素； 量測方程為： Z = HX+V 其中，量測量Z為慣導水平速度誤差δ vx，SVy ;H= [I2X2 02X3]，I2x2為單位二階矩陣， O2x3為2行3列零矩陣；V為量測噪聲。
【文檔編號】G01C25/00GK104374405SQ201410624222
【公開日】2015年2月25日 申請日期:2014年11月6日 優先權日:2014年11月6日
【發明者】王通達, 宋春雨, 李美玲, 徐英蛟, 劉萍, 于天琦 申請人:哈爾濱工程大學