[發明專利]微型組合導航系統及自適應濾波方法

無效

• 申請號: 200710022446.4
• 申請日: 2007-05-18
• 公開（公告）號: CN101059349A
• 公開（公告）日: 2007-10-24
• 發明（設計）人: 劉建業;華冰;曾慶化;趙偉;熊智;賴際舟;李榮冰;黃徽
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/18;G01S5/14
• 代理公司: 南京蘇高專利事務所
• 代理人: 闕如生

1.一種微型組合導航系統，其特征在于，包括余度配置慣性測量單元、微型GPS接收機、導航計算機，該導航計算機包括數據采集、導航解算、自適應平淡粒子濾波器，其中余度配置慣性測量單元采用六個單自由度陀螺和兩個雙軸加速度計組合，該余度配置慣性測量單元輸出數據經過數據采集，輸入到導航解算，該導航解算輸出的慣導信息輸入自適應平淡粒子濾波器，微型GPS接收機的輸出經自適應平淡粒子濾波器后的輸出對慣導導航解算的輸出校正，校正后的輸出與導航解算的輸出一起進入飛控系統。

2.一種如權利要求1所述的微型組合導航系統的自適應濾波方法，包括如下步驟：設計系統狀態方程、設計系統量測方程、UKF濾波，權值歸一化、狀態估計；

所述設計系統狀態方程是以慣性導航系統的誤差量加上GPS時鐘信號測距等效誤差δt_u作為狀態，并結合余度配置MEMS-INS中MEMS傳感器的噪聲狀況，建立系統模型，其狀態方程為：

??????????????????????X^&＝F(t)X(t)+G(t)W(t)

式中：狀態量X＝[φ_E?φ_N?φ_U?δv_E?δv_N?δv_N?δL?δλ?δh?δt_u]^T，由東、北、天向的平臺誤差角φ_E、φ_N、φ_U，東、北、天向的速度誤差δv_E、δv_N、δv_N，緯度、經度和高度的誤差δL、δλ、δh，以及時間測距等效誤差δt_u構成；W為噪聲向量，F，G是狀態方程的狀態轉移矩陣和噪聲系數矩陣。

所述設計系統量測方程，第i顆GPS衛星測得的實際偽距為：

ρ Gi = [ ( x si - x I ) 2 + ( y si - y I ) 2 + ( z si - z I ) 2 ] + δ t u + v ρi , i = 1,2,3,4 ]]>

式中：x_si，y_si，z_si為該衛星的位置，x_I，y_I，z_I為慣性導航位置，v_ρi為偽距測量誤差，δt_u為時間測距等效誤差，(x，y，z)坐標系為地球固定坐標系(ECEF)，而導航計算中是采用經緯高信息表示位置的，因此要進行坐標轉換，下式中的R_N為卯酉圈曲率半徑，L_I，λ_I，h_I表示慣性導航的經、緯、高位置信息，f為地球橢圓偏心率，其他符號意義同上所述，

x I = ( R N + h I - δh ) cos ( L I - δL ) cos ( λ I - δλ ) y I = ( R N + h I - δh ) cos ( L I - δL ) sin ( λ I - δλ ) z I = [ R N ( 1 - f 2 ) + h I - δh ] sin ( L I - δL ) ]]>

得到：

ρ Gi = [ ( x si - ( R N + h I - δh ) cos ( L I - δL ) cos ( λ I - δλ ) ) 2 ]]>

+ ( y si - ( R N + h I - δh ) cos ( L I - δL ) sin ( λ I - δλ ) ) 2 ]]>

+ ( z si - [ R N ( 1 - f 2 ) + h I - δh ] sin ( L I - δL ) ) 2 ] 1 2 ]]>

+ δt u + v ρi ; ]]>

所述UKF濾波，包括初始化、UT變換計算采樣點、時間更新、量測更新；

所述權值歸一化步驟，是指更新樣本權值 w % k = w % k - 1 p ( z k | x k i ) p ( x k i | x k - 1 i ) q ( x k i | x 0 : k - 1 i , z 1 : k ) , ]]>

w k i = w % k Σ w % k , ]]>p()表示條件概率密度，q(x_kⁱ|x_0:k-1ⁱ，z_1:k)為重要性函數：

所述狀態估計步驟，采用下式 x ( k / k ) = Σ i = 1 N x k i ω k i ]]>進行狀態估計；

其特征在于，在系統測量方程與UKF濾波之間還包括粒子生成和門限判斷兩個步驟，在權值歸一化步驟與狀態估計步驟之間還包括重采樣判斷步驟；

所述粒子生成步驟，為了滿足系統性能要求，需要保持粒子數目不低于預定門限M，因此提出粒子數目的自適應決策方法：

根據上式就可以在線調節粒子數目。INT表示取整數，當丟失信號5秒以內，進行小幅度調整粒子數目，當GPS信號丟失超過5秒，則加大幅度調整粒子數目；

所述門限判斷步驟，設定權值控制參數T，在UKF濾波之前進行判斷，如果該粒子權值不小于T，則進行UKF濾波，計算其對應的估計值和狀態方差陣，如果該粒子權值小于T，則不進行UKF濾波，只利用狀態方程進行時間更新X(k+1)＝F(k)X(k)+G(k)W(k)，其中k為當前時刻，k+1為更新后的時刻。從而構成了自適應平淡粒子濾波器，所述權值控制參數 T = 1 N , ]]>N為粒子總數，從幾千個到數萬個不等，但是對每一次濾波而言，N是固定的；

所述重采樣判斷步驟，為了均衡權值在控制參數T左右的粒子，避免太多的粒子權值大于或者小于T，在重采樣環節的判斷處，提出了基于參數平衡原則的重采樣判斷標準：判斷重采樣閾值S_T是否小于 ( N 2 N Σ i = 1 N 1 w i 2 + N 1 N Σ j = 1 N 2 w j 2 ) - 1 , ]]>當N1＞N2，也就是權值大于T的粒子數目較多，則在計算采樣閾值時候，增加權值小于T的粒子的比重；反之，當N1＜N2，也就是權值小于T的粒子數目較多，則在計算采樣閾值時候，增加權值大于T的粒子的比重，其中N1為權值大于T的粒子總數，N2為權值小于T的粒子總數。