[發(fā)明專(zhuān)利]基于約束卡爾曼濾波的集群組網(wǎng)協(xié)同導(dǎo)航方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202110361651.3
• 申請(qǐng)日: 2021-04-02
• 公開(kāi)（公告）號(hào): CN113175931B
• 公開(kāi)（公告）日: 2022-08-16
• 發(fā)明（設(shè)計(jì)）人: 秦峰;宋振華;劉偉鵬;崔閃;李云龍;吳鎮(zhèn);李勇;袁杰波;韓琳;王暉輝
• 申請(qǐng)（專(zhuān)利權(quán)）人: 上海機(jī)電工程研究所
• 主分類(lèi)號(hào): G01C21/20
• 分類(lèi)號(hào): G01C21/20;G01C21/16;G01C25/00
• 代理公司: 上海漢聲知識(shí)產(chǎn)權(quán)代理有限公司 31236
• 代理人: 胡晶

1.一種基于約束卡爾曼濾波的集群組網(wǎng)協(xié)同導(dǎo)航方法，其特征在于，包括：

步驟S1：確定集群組網(wǎng)節(jié)點(diǎn)數(shù)量，并測(cè)量各節(jié)點(diǎn)間的相對(duì)距離；

步驟S2：基于測(cè)量的各節(jié)點(diǎn)間的相對(duì)距離，建立集群組網(wǎng)協(xié)同導(dǎo)航濾波方程；

步驟S3：設(shè)計(jì)集群組網(wǎng)各節(jié)點(diǎn)慣導(dǎo)位置誤差加權(quán)和最小約束條件；

步驟S4：構(gòu)建集群組網(wǎng)協(xié)同導(dǎo)航的約束卡爾曼濾波；

步驟S5：利用約束卡爾曼濾波估計(jì)出的各節(jié)點(diǎn)慣導(dǎo)誤差估計(jì)值對(duì)純慣導(dǎo)誤差補(bǔ)償；

所述步驟S2中的集群組網(wǎng)協(xié)同導(dǎo)航濾波方程，包括狀態(tài)方程，所述狀態(tài)方程具體為：

X(t)＝F(t)X(t)+G(t)W(t)

式中，分別為集群中各個(gè)節(jié)點(diǎn)的地理坐標(biāo)系下的三軸位置誤差，n為集群節(jié)點(diǎn)數(shù)量，F(xiàn)(t)為狀態(tài)轉(zhuǎn)移矩陣，G(t)為系統(tǒng)噪聲系數(shù)矩陣，W(t)為系統(tǒng)噪聲矩陣；

其中，

式中，V_eⁱ表示i節(jié)點(diǎn)東向速度；L_i表示i節(jié)點(diǎn)緯度；h_i表示i節(jié)點(diǎn)高度；R_N表示地球卯酉圈曲率半徑；R_M表示地球子午圈曲率半徑；V_nⁱ表示i節(jié)點(diǎn)北向速度；

所述步驟S2中的集群組網(wǎng)協(xié)同導(dǎo)航濾波方程，還包括測(cè)量方程，所述測(cè)量方程具體為：

L(t)＝HX(t)+Δ

式中，L(t)是(n-1)×1維觀測(cè)量矩陣，n-1為觀測(cè)量數(shù)量，即測(cè)距值數(shù)量，H為(n-1)×3n維觀測(cè)系數(shù)矩陣，Δ是(n-1)×1維觀測(cè)噪聲矩陣；

其中，

式中，為集群中節(jié)點(diǎn)i與節(jié)點(diǎn)j間的測(cè)量距離，為節(jié)點(diǎn)i與節(jié)點(diǎn)j間慣導(dǎo)計(jì)算距離，L隨彈群節(jié)點(diǎn)n的數(shù)量變化實(shí)時(shí)變化；D_a為地理坐標(biāo)系下緯經(jīng)高誤差轉(zhuǎn)換至地心地固坐標(biāo)系下XYZ三軸誤差的轉(zhuǎn)換矩陣；

所述步驟S4采用投影法將約束條件投影至傳統(tǒng)卡爾曼濾波，構(gòu)建集群組網(wǎng)協(xié)同導(dǎo)航的約束卡爾曼濾波為如下：

約束條件矩陣：B_k＝G^Tp_xD_a；

狀態(tài)一步預(yù)測(cè)：

約束修正后狀態(tài)一步預(yù)測(cè)：

狀態(tài)估計(jì)：

濾波增益：

一步預(yù)測(cè)均方誤差：P_k/k-1＝φ_k,k-₁P_k-1φ^T_k,k-1+Q_k；

估計(jì)均方誤差：P_k＝(I-K_kH_k)P_k/k-1；

式中，B_k為約束條件矩陣，為一步預(yù)測(cè)狀態(tài)量矩陣，為前次狀態(tài)量矩陣，φ_k,k-1為狀態(tài)轉(zhuǎn)移矩陣，為約束修正后預(yù)測(cè)狀態(tài)量矩陣，P_k-₁為狀態(tài)誤差協(xié)方差矩陣，K_k為濾波增益矩陣，Z_k為觀測(cè)量矩陣，H_k為觀測(cè)系數(shù)矩陣，P_k/k-1為一步預(yù)測(cè)狀態(tài)誤差協(xié)方差矩陣，R_k為觀測(cè)噪聲矩陣，Q_k為狀態(tài)噪聲矩陣，I為單位矩陣；G^T是由H^TPH的零特征值對(duì)應(yīng)的特征向量組成的6×3n維矩陣，P為L(zhǎng)的對(duì)角權(quán)陣，H^TPH為L(zhǎng)的對(duì)角權(quán)陣的正定矩陣，P_x為根據(jù)各節(jié)點(diǎn)慣導(dǎo)系統(tǒng)精度水平確定的待估參數(shù)向量X的3n×3n維權(quán)陣，n為集群節(jié)點(diǎn)數(shù)量。

2.根據(jù)權(quán)利要求1所述的基于約束卡爾曼濾波的集群組網(wǎng)協(xié)同導(dǎo)航方法，其特征在于，所述步驟S3中集群組網(wǎng)各節(jié)點(diǎn)慣導(dǎo)位置誤差加權(quán)和最小約束條件，其中，約束條件包括：

G^Tp_xD_aX＝O

式中，為集群節(jié)點(diǎn)i在地心地固系下慣導(dǎo)位置，P_x隨集群節(jié)點(diǎn)n的數(shù)量變化實(shí)時(shí)變化。