[發明專利]基于約束卡爾曼濾波的集群組網協同導航方法及系統

有效

• 申請號: 202110361651.3
• 申請日: 2021-04-02
• 公開（公告）號: CN113175931B
• 公開（公告）日: 2022-08-16
• 發明（設計）人: 秦峰;宋振華;劉偉鵬;崔閃;李云龍;吳鎮;李勇;袁杰波;韓琳;王暉輝
• 申請（專利權）人: 上海機電工程研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G01C25/00
• 代理公司: 上海漢聲知識產權代理有限公司 31236
• 代理人: 胡晶

本發明提供了一種基于約束卡爾曼濾波的集群組網協同導航方法及系統，涉及協同導航技術領域，該方法包括：步驟S1：確定集群組網節點數量，并測量各節點間的相對距離；步驟S2：基于測量的各節點間的相對距離，建立集群組網協同導航濾波方程；步驟S3：設計集群組網各節點慣導位置誤差加權和最小約束條件；步驟S4：構建集群組網協同導航的約束卡爾曼濾波；步驟S5：利用約束卡爾曼濾波估計出的各節點慣導誤差估計值對純慣導誤差補償。本發明能夠實時遞推估計出慣導誤差對慣導系統進行修正，抑制各個節點純慣導誤差發散。

技術領域

本發明涉及協同導航技術領域，具體地，涉及一種基于約束卡爾曼濾波的集群組網協同導航方法及系統。

背景技術

目前，慣性導航技術已經在航天、航空、無人駕駛、水下導航、室內定位等生活中各個場景廣泛應用，成為一種常用的導航方式。

公開號為CN110207691A的中國發明專利，公開了一種基于數據鏈測距的多無人車協同導航方法，利用數據鏈對無人車之間相對距離進行測量，進而通過多導航傳感器信息融合方法對編隊中每輛無人車節點的位置進行估計。無人車編隊中每輛無人車節點搭載慣性導航系統、車輛里程計、數據鏈收發裝置，首先通過卡爾曼濾波器對每輛無人車節點中的慣性導航系統、車輛里程計進行融合，得到每輛無人車節點的速度、位置、姿態信息；進而利用數據鏈測得的無人車之間的距離信息，通過等式約束對每輛無人車的位置估計結果進行優化。通過本方法能夠提高無人車編隊在衛星拒止條件下的導航定位精度。

綜上所述，在現有技術當中，協同導航時必須使用其它導航傳感器輔助構建約束條件，如車輛里程計等，帶來了額外的經濟成本，且在許多場景使用受限。本發明在僅依靠每個載體自身所帶慣性導航系統，利用數據鏈測距，通過設計幾何約束條件，構建約束卡爾曼濾波，解決慣導系統存在長時誤差快速發散，無法用于長時間導航的問題。

發明內容

針對現有技術中的缺陷，本發明的目的是提供一種基于約束卡爾曼濾波的集群組網協同導航方法及系統。

根據本發明提供的一種基于約束卡爾曼濾波的集群組網協同導航方法及系統，所述方案如下：

第一方面，提供了一種基于約束卡爾曼濾波的集群組網協同導航方法，所述方法包括：

步驟S1：確定集群組網節點數量，并測量各節點間的相對距離；

步驟S2：基于測量的各節點間的相對距離，建立集群組網協同導航濾波方程；

步驟S3：設計集群組網各節點慣導位置誤差加權和最小約束條件；

步驟S4：構建集群組網協同導航的約束卡爾曼濾波；

步驟S5：利用約束卡爾曼濾波估計出的各節點慣導誤差估計值對純慣導誤差補償。

優選的，所述步驟S2中的集群組網協同導航濾波方程，包括狀態方程，所述狀態方程具體為：

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

式中，分別為集群中各個節點的地理坐標系下的三軸位置誤差，n為集群節點數量，F(t)為狀態轉移矩陣，G(t)為系統噪聲系數矩陣，W(t)為系統噪聲矩陣；

其中，

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

優選的，所述步驟S2中的集群組網協同導航濾波方程，還包括測量方程，所述測量方程具體為：