[發(fā)明專利]一種基于聯(lián)邦濾波的組合導(dǎo)航方法

有效

• 申請?zhí)枺?/td>: 202011473337.6
• 申請日: 2020-12-15
• 公開（公告）號: CN112525188B
• 公開（公告）日: 2022-08-05
• 發(fā)明（設(shè)計）人: 龍海輝;王青;李建勛;趙健康;宗軍耀;余亮
• 申請（專利權(quán)）人: 上海交通大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 上海漢聲知識產(chǎn)權(quán)代理有限公司 31236
• 代理人: 黃超宇;胡晶

1.一種基于聯(lián)邦濾波的組合導(dǎo)航方法，其特征在于，包括以下步驟：

步驟1：傳感器及導(dǎo)航設(shè)備誤差機(jī)理分析與誤差建模；

步驟1包括如下步驟：

步驟1.1：捷聯(lián)慣性導(dǎo)航傳感器誤差分析與建模；

步驟1.2：衛(wèi)星導(dǎo)航系統(tǒng)誤差分析與建模；

步驟1.3：大氣數(shù)據(jù)系統(tǒng)誤差分析與建模；

步驟1.4：陸基無線電系統(tǒng)誤差分析與建模；

步驟1.5：儀表著陸系統(tǒng)(ILS)誤差分析與建模；

步驟2：各子濾波系統(tǒng)狀態(tài)空間方程的建立；

步驟2包括如下步驟：

步驟2.1：慣性導(dǎo)航與衛(wèi)星導(dǎo)航子濾波系統(tǒng)；

步驟2.2：慣性導(dǎo)航與大氣數(shù)據(jù)子濾波系統(tǒng)；

步驟2.3：慣性導(dǎo)航與機(jī)載航電子濾波系統(tǒng)；

步驟2.4：性導(dǎo)航與儀表著陸子濾波系統(tǒng)；

步驟3：濾波有效性評估機(jī)制；

步驟3包括如下步驟：

步驟3.1：各子濾波系統(tǒng)可觀性分析

采用奇異值分解方法，設(shè)PWCS離散系統(tǒng)模型為：

其中，X(k)∈Rⁿ,F(j)∈R^n×n,Z(k)∈R^m,H(j)∈R^m×n，j＝1,2,…,r，對每個時間段j，矩陣F_j，H_j都是定常的，系統(tǒng)觀測值可以表示為X(1)的函數(shù)：

總的觀測方程為：

Z＝Q(r)X(1) (31)

這里Z是觀測矢量的全部觀測值：

由于對應(yīng)每一時間段j的可觀測矩陣定義為：

故Q(r)可表示為：

Q(r)為離散PWCS的總可觀測性矩陣，記為離散系統(tǒng)的TOM，定義Q_s(r)為離散PWCS的提取可觀測性矩陣

Q_S(r)＝[Q₁ Q₂ … Q_r]^T (35)

在分段時間足夠短時，可用Q_s(r)的零空間代替Q(r)的零空間，進(jìn)而可用Q_s(r)代替Q(r)分析系統(tǒng)的可觀測性，對系統(tǒng)提取可觀測矩陣進(jìn)行奇異值分解

Q_s^*(j)＝UΛV^T(j＝1,2,…,q) (36)

式中：

r,σ_j分別為的秩和奇異值，并且σ_i＞0,i＝1,2,…,r，U為m.n.j階正交矩陣；V為n階正交矩陣，設(shè)系統(tǒng)滿足PWCS分析定理的要求，則Q^*(j)可代替Q(j)使用，可得到：

X₀＝(UΛV^T)^-1Z (38)

若將正交矩陣U,V分別用各自的列構(gòu)成的列向量表示

U＝[u₁,u₂,…,u_m.n.j]，V＝[v₁,v₂,…,v_m.n.j] (39)

則上式可進(jìn)一步寫成

從數(shù)值上看，σ_i代表X_0,i中取得最大絕對值的元素所對應(yīng)的奇異特性，若σ_i的值較大，則相應(yīng)的系統(tǒng)狀態(tài)可獲得較高精度的估計；若σ_i的值較小，則相應(yīng)的系統(tǒng)狀態(tài)可能會奇異，落入不可觀測空間內(nèi)，由于條帶化可觀測性矩陣Q(j)的階數(shù)隨時段數(shù)量的增加而不斷積累的；

步驟3.2：各子濾波系統(tǒng)穩(wěn)定性分析

對于對非線性系統(tǒng)線性化可得：

若對任意初始狀態(tài)和總有即對任意給定的μ＞0，總可以找到T＝T(μ)＞0，當(dāng)t_k≥t₀+T時，恒成立，則系統(tǒng)稱為一致漸進(jìn)穩(wěn)定的，由Klaman濾波器算法原理知道：

所以判斷濾波器穩(wěn)定可以視為線性系統(tǒng)的穩(wěn)定問題，因此可以根據(jù)一步轉(zhuǎn)移陣(I-K_kH_k)Φ_k來分析濾波穩(wěn)定性，由卡爾曼濾波穩(wěn)定性定理可以推出濾波誤差方差陣的漸進(jìn)性定理，即如果濾波器是一致漸進(jìn)穩(wěn)定的，則系統(tǒng)的均方誤差陣P_k也是漸進(jìn)穩(wěn)定的；

步驟4：自適應(yīng)聯(lián)邦濾波器設(shè)計；

步驟4包括如下步驟：

自適應(yīng)聯(lián)邦濾波器的設(shè)計步驟如下：

步驟4.1：初始化

對聯(lián)邦濾波的各濾波器進(jìn)行初始化如下：

其中，β_i(i＝1,2,…N,m)滿足信息守恒原則，即β₁+β₂+…+β_m＝1；

步驟4.2：各子濾波器及主濾波器進(jìn)行獨(dú)立的時間更新

步驟4.3：各子濾波器進(jìn)行量測更新

步驟4.4：進(jìn)行最優(yōu)融合，得到主濾波器的濾波值和方差信息

建立好四個子系統(tǒng)的狀態(tài)空間方程后，各子系統(tǒng)可基于自適應(yīng)擴(kuò)展卡爾曼濾波進(jìn)行濾波估計，將子濾波系統(tǒng)輸出的狀態(tài)量及方差及各狀態(tài)量的權(quán)重在主濾波器中進(jìn)行最優(yōu)融合，可得到濾波值及方差信息：

按照一定的信息分配原則對各子濾波器進(jìn)行信息的分配與重置，主濾波器可基于加權(quán)最小二乘等方法進(jìn)行最優(yōu)估計；

步驟4.5：按照一定的信息分配原則對各子濾波器進(jìn)行信息的分配與重置：

步驟4.6：重復(fù)步驟4.2及其后面的步驟

聯(lián)邦濾波器的分配因子取決于各變量的可觀度及各系統(tǒng)穩(wěn)定度：

式中，P_k,i和P_0,i分別為第i個子系統(tǒng)在第k和第0時刻的誤差協(xié)方差矩陣，為需要預(yù)先設(shè)計的閾值，X_0ij為第i個系統(tǒng)的客觀度X₀的第j個分量。