[發(fā)明專利]用于使用不變卡爾曼濾波器輔助一隊(duì)交通工具導(dǎo)航的方法和裝置

在審

• 申請(qǐng)?zhí)枺?/td>: 201980053657.5
• 申請(qǐng)日: 2019-07-23
• 公開（公告）號(hào): CN112567203A
• 公開（公告）日: 2021-03-26
• 發(fā)明（設(shè)計(jì)）人: 阿克塞爾·巴勞
• 申請(qǐng)（專利權(quán)）人: 賽峰集團(tuán)
• 主分類號(hào): G01C21/00
• 分類號(hào): G01C21/00
• 代理公司: 北京派特恩知識(shí)產(chǎn)權(quán)代理有限公司 11270
• 代理人: 江海;李雪

本發(fā)明涉及一種用于輔助一隊(duì)交通工具導(dǎo)航的方法，這一隊(duì)交通工具包括主交通工具(1)及相對(duì)于主交通工具(1)可移動(dòng)的副交通工具(2)，所述方法包括以下步驟：·接收由至少一個(gè)傳感器(2，12)獲取的、主交通工具(1)和副交通工具(2)之間的相對(duì)運(yùn)動(dòng)數(shù)據(jù)(Y₁，Y₂)；·通過不變卡爾曼濾波器使用所接收的數(shù)據(jù)(Y₁，Y₂)作為觀測(cè)值，估計(jì)(100，200)這一隊(duì)交通工具的導(dǎo)航狀態(tài)，導(dǎo)航狀態(tài)包括：·第一變量，表示將與主交通工具(1)相關(guān)聯(lián)的位置標(biāo)記關(guān)聯(lián)到參考點(diǎn)的第一剛性變換；以及·第二變量，表示將與主交通工具(1)相關(guān)聯(lián)的位置標(biāo)記關(guān)聯(lián)到與副交通工具(2)相關(guān)聯(lián)的位置標(biāo)記的第二剛性變換，不變卡爾曼濾波器使用包括第一剛性變換和第二剛性變換的逐項(xiàng)組合的法則作為內(nèi)部組合法則。

技術(shù)領(lǐng)域

本發(fā)明涉及一種用于輔助一隊(duì)交通工具導(dǎo)航的方法以及適用于實(shí)現(xiàn)這種方法的裝置。

背景技術(shù)

估計(jì)物理系統(tǒng)的狀態(tài)的問題通常如下所示。系統(tǒng)在時(shí)刻n的狀態(tài)由向量X_n表示，以及在時(shí)刻n可用的觀測(cè)值由另一個(gè)向量Y_n表示。系統(tǒng)的演化寫成這樣：

X_n+1＝f(X_n)

其中，f是已知函數(shù)(通常稱為傳播函數(shù))。觀測(cè)值Y_n通過已知的觀測(cè)函數(shù)h與系統(tǒng)的狀態(tài)相關(guān)：

Y_n＝h(X_n)

從序列(Y_n)_n≥0建立狀態(tài)X_n的良好估計(jì)通常是個(gè)難題，但是在某些情況下可得以簡(jiǎn)化。

“線性系統(tǒng)”是指具有以下形式的系統(tǒng)的特殊情況：

X_n+1＝FX_n+w_n

Y_n＝HX_n+V_n

其中，F(xiàn)是傳播矩陣，H是觀測(cè)矩陣，w_n和V_n是干擾預(yù)測(cè)和測(cè)量的噪聲。

在這種線性情況下，已知的方法包括建立稱為“卡爾曼濾波器”的估計(jì)器。該卡爾曼濾波器實(shí)現(xiàn)以下計(jì)算：

其中，指數(shù)n+1|n和n+1|n+1分別表示在不考慮觀測(cè)值Y_n+1的情況下，在n+1時(shí)刻計(jì)算的估計(jì)值，以及在考慮觀測(cè)值Y_n+1的情況下，在n+1時(shí)刻計(jì)算的估計(jì)值。矩陣K_n稱為“增益矩陣”，其可以使用Riccati(黎卡提)方程來計(jì)算。于是估計(jì)誤差定義為：

(在考慮觀測(cè)值Y_n之后)

(在考慮觀測(cè)值Y_n+1之前)

可以容易地驗(yàn)證該誤差遵循以下演化：

e_n+1|n＝Fe_n|n(在考慮觀測(cè)值Y_n+1之前)

e_n+1|n+1＝(I-K_n+1H)e_n+1|n(在考慮觀測(cè)值Y_n+1之后)

其中，I表示單位矩陣。

上述方程不依賴于X_n，因此可以建立對(duì)系統(tǒng)的任何實(shí)際軌跡都有效的估計(jì)器，而對(duì)于任何非線性系統(tǒng)則不是這種情況。

在非線性系統(tǒng)的情況下，普通的卡爾曼濾波器無法實(shí)現(xiàn)。因此，提出了一種卡爾曼濾波器變型，稱為“擴(kuò)展”卡爾曼濾波器，適用于非線性系統(tǒng)。然而，當(dāng)使用擴(kuò)展卡爾曼濾波器時(shí)，在線性情況下觀測(cè)的簡(jiǎn)化不再發(fā)生，因此得到涉及X_n和的誤差方程。這個(gè)問題是使用擴(kuò)展卡爾曼濾波器時(shí)遇到的大多數(shù)發(fā)散情況的根源。