基于卡爾曼濾波模型的多傳感器數(shù)據(jù)融合導(dǎo)航定位建模與仿真

宋之卉，趙彥曉

（北京信息科技大學(xué)信息與通信工程學(xué)院，北京 100124）

隨著多傳感器技術(shù)的不斷發(fā)展，可利用的與導(dǎo)航定位相關(guān)的位置傳感器越來越多，標(biāo)識(shí)著同一個(gè)物理信息的數(shù)據(jù)源也呈多樣化。在導(dǎo)航與定位技術(shù)領(lǐng)域，位置標(biāo)識(shí)數(shù)據(jù)源可以包括全球?qū)Ш叫l(wèi)星數(shù)據(jù)、視覺傳感數(shù)據(jù)、慣性傳感器數(shù)據(jù)等，僅僅憑借獨(dú)立傳感器檢測(cè)到的信息已不能實(shí)現(xiàn)定位的要求。為不斷提升導(dǎo)航定位精度，為用戶提供多維度的導(dǎo)航定位結(jié)果，是決定著導(dǎo)航與定位性能的關(guān)鍵環(huán)節(jié)，具有較大的研究?jī)r(jià)值和應(yīng)用潛力。

本論文基于導(dǎo)航定位仿真平臺(tái)，通過采集用戶的GNSS 接收機(jī)、視覺、慣性導(dǎo)航等傳感器的位置數(shù)據(jù)，將這些信息輸入卡爾曼濾波器進(jìn)行信息的預(yù)處理，剔除掉錯(cuò)誤和粗差信息，再根據(jù)最優(yōu)加權(quán)融合估計(jì)算法對(duì)這些數(shù)據(jù)進(jìn)行有效融合，為用戶提供多維度的導(dǎo)航定位結(jié)果。

1 導(dǎo)航定位仿真平臺(tái)的結(jié)構(gòu)

本仿真系統(tǒng)框圖如圖1所示，包含以下模塊：傳感器用戶位置數(shù)據(jù)采集模塊、卡爾曼濾波與數(shù)據(jù)預(yù)處理模塊、數(shù)據(jù)融合算法模塊，結(jié)果圖形化顯示及性能分析模塊。其中傳感器用戶位置數(shù)據(jù)仿真模塊包含GNSS 接收機(jī)、視覺傳感器以及慣性導(dǎo)航三類位置傳感器，首先通過設(shè)置樣本數(shù)量、數(shù)據(jù)范圍及精度，隨機(jī)傳感器仿真數(shù)據(jù)，然后采用卡爾曼濾波模型對(duì)仿真數(shù)據(jù)進(jìn)行預(yù)處理，然后進(jìn)行三路傳感器仿真數(shù)據(jù)濾波后的融合，最后輸出數(shù)據(jù)融合結(jié)果、仿真結(jié)果圖形化顯示以及對(duì)各傳感器定位性能進(jìn)行評(píng)估。

圖1 導(dǎo)航定位系統(tǒng)仿真平臺(tái)結(jié)構(gòu)

2 數(shù)據(jù)預(yù)處理流程

傳感器數(shù)據(jù)與預(yù)處理流程如圖2所示，首先通過GNSS 接收機(jī)、視覺傳感器以及慣性導(dǎo)航對(duì)用戶位置信息進(jìn)行仿真，通過設(shè)定數(shù)據(jù)誤差范圍及精度，給定高斯白噪聲，隨機(jī)生成三組仿真數(shù)據(jù)，然后通過卡爾曼濾波模型進(jìn)行數(shù)據(jù)預(yù)處理，引入相對(duì)誤差和均方根誤差對(duì)預(yù)處理數(shù)據(jù)置信度和精確度進(jìn)行檢驗(yàn)，剔除錯(cuò)誤和粗差數(shù)據(jù)，生成更為精確可信的傳感器仿真數(shù)據(jù)。

3 卡爾曼濾波數(shù)學(xué)模型

采用卡爾曼濾波器進(jìn)行估計(jì)可以實(shí)現(xiàn)線性系統(tǒng)下的方案最優(yōu)，GNSS 接收機(jī)、視覺傳感器以及慣性導(dǎo)航三組傳感器可以近似等效為線性特性。其濾波過程如圖3所示。

圖2 傳感器數(shù)據(jù)與預(yù)處理流程

圖3 卡爾曼濾波過程圖

卡爾曼濾波主要包括預(yù)測(cè)與更新兩個(gè)過程。預(yù)測(cè)當(dāng)前狀態(tài)時(shí)主要依據(jù)系統(tǒng)前一狀態(tài)與控制量來進(jìn)行估計(jì)。更新確定總體預(yù)測(cè)值可信還是傳感器測(cè)量的數(shù)據(jù)更可信。通常情況下濾波器根據(jù)預(yù)測(cè)的當(dāng)前狀態(tài)與更新之后的測(cè)量估計(jì)，給出較精確地?cái)?shù)據(jù)信息。在工作過程中始終對(duì)當(dāng)前狀態(tài)進(jìn)行重復(fù)更新。

假設(shè)線性系統(tǒng)狀態(tài)是k，預(yù)測(cè)時(shí)依據(jù)的表達(dá)式為

狀態(tài)更新時(shí)依據(jù)的表達(dá)式為

4 數(shù)據(jù)融合算法

數(shù)據(jù)融合采用最優(yōu)加權(quán)融合估計(jì)算法，如式（6），合并來自三個(gè)不同位置傳感器的濾波及預(yù)處理后的仿真數(shù)據(jù)，以提供給用戶更為精確的導(dǎo)航定位信息，由于各個(gè)傳感器精度不一樣，為了達(dá)到更好的融合結(jié)果，通過給定隨機(jī)噪聲，根據(jù)不同傳感器設(shè)定權(quán)值，以達(dá)到最優(yōu)的融合結(jié)果。

5 仿真分析

采用matlab 軟件對(duì)三組傳感器仿真數(shù)據(jù)經(jīng)卡爾曼濾波模型預(yù)處理，加入相對(duì)誤差和均方根誤差進(jìn)行置信度和精確度檢驗(yàn)以及卡爾曼濾波仿真和數(shù)據(jù)融合結(jié)果進(jìn)行了仿真，結(jié)果如下：

5.1 數(shù)據(jù)預(yù)處理前后誤差對(duì)比

數(shù)據(jù)預(yù)處理通過卡爾曼濾波模型進(jìn)行，加入相對(duì)誤差和均方根誤差對(duì)濾波前后置信度和精確度進(jìn)行比較和分析。

圖4、5、6分別是GNSS 接收機(jī)、視覺傳感器以及慣性導(dǎo)航傳感器仿真數(shù)據(jù)經(jīng)卡爾曼濾波前后，通過計(jì)算相對(duì)誤差和均方根誤差，進(jìn)行預(yù)測(cè)值與真實(shí)值和濾波值與真實(shí)值的誤差對(duì)比。綜合這三個(gè)結(jié)果對(duì)比分析，可以看出，經(jīng)卡爾曼濾波加入均方根誤差進(jìn)行數(shù)據(jù)預(yù)處理后，得到的仿真數(shù)據(jù)誤差相比如濾波前測(cè)量值誤差明顯減小，分別在0.01、0.005誤差范圍大小附近上下浮動(dòng)，數(shù)據(jù)精度明顯提升，擬合效果好，滿足設(shè)計(jì)要求。

圖4 GNSS接收機(jī)濾波前后誤差對(duì)比圖

圖5 視覺傳感器濾波前后誤差對(duì)比圖

圖6 慣性導(dǎo)航濾波前后誤差對(duì)比圖

5.2 卡爾曼濾波仿真分析

通過多傳感器數(shù)據(jù)仿真及建模仿真系統(tǒng)，采用卡爾曼濾波模型進(jìn)行濾波得到三組位置傳感器仿真數(shù)據(jù)結(jié)果如下。

圖7、8、9分別是GNSS 接收機(jī)、視覺傳感器以及慣性導(dǎo)航傳感器經(jīng)卡爾曼濾波模型進(jìn)行濾波后，真實(shí)值、預(yù)測(cè)值以及濾波值比較。綜合這三個(gè)仿真結(jié)果對(duì)比分析，無論是哪一類型的位置傳感器，經(jīng)卡爾曼濾波后的值，都和真實(shí)值更為接近，且真實(shí)值和測(cè)量值都集中在固定區(qū)域，分別集中在0.069-0.07之間，說明濾波效果明顯提高了精度。

圖7 GNSS接收機(jī)卡爾曼濾波仿真結(jié)果

圖8 視覺傳感器卡爾曼濾波仿真結(jié)果

圖9 慣性導(dǎo)航卡爾曼濾波仿真結(jié)果

5.3 最優(yōu)加權(quán)融合估計(jì)算法數(shù)據(jù)融合結(jié)果

最后，對(duì)三組位置傳感器經(jīng)卡爾曼濾波后的仿真數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，采用最優(yōu)加權(quán)融合估計(jì)算法，合并來自三個(gè)不同位置傳感器的濾波及預(yù)處理后的仿真數(shù)據(jù)，以提供給用戶更為精確的導(dǎo)航定位信息。由于各個(gè)傳感器精度不一樣，為了達(dá)到更好的融合結(jié)果，通過給定隨機(jī)噪聲，根據(jù)不同傳感器設(shè)定權(quán)值，得到最優(yōu)加權(quán)融合估計(jì)算法融合后結(jié)果如圖10所示。

圖10 三組傳感器濾波融合結(jié)果圖

仿真數(shù)據(jù)融合結(jié)果如表1所示：

表1 三種傳感器仿真數(shù)據(jù)融合結(jié)果

將融合結(jié)果與各個(gè)單傳感器濾波值相比較，得到的數(shù)據(jù)融合結(jié)果精度高于任一單獨(dú)位置傳感器定位精度，與真實(shí)值誤差相比有明顯改善，達(dá)到了多傳感器位置信息融合提高導(dǎo)航定位精度的目的。另外，通過卡爾曼濾波前、后，計(jì)算相對(duì)誤差和均方根誤差，進(jìn)行預(yù)測(cè)值與真實(shí)值、濾波值與真實(shí)值的對(duì)比以及單一傳感器卡爾曼濾波仿真分析，通過多傳感器數(shù)據(jù)融合結(jié)果與單一位置傳感器濾波值比較，可以看出，利用信息融合結(jié)果實(shí)現(xiàn)的對(duì)位置傳感器定位性能評(píng)價(jià)的精度較高，且是正確有效的，達(dá)到了預(yù)期設(shè)計(jì)效果。

6 結(jié)束語(yǔ)

本文采用卡爾曼濾波模型對(duì)三種位置傳感器仿真用戶位置數(shù)據(jù)進(jìn)行預(yù)處理，通過計(jì)算相對(duì)誤差和均方根誤差，進(jìn)行仿真分析，對(duì)濾波前后預(yù)測(cè)值與真實(shí)值誤差和濾波值與真實(shí)值誤差進(jìn)行比較，從而對(duì)數(shù)據(jù)預(yù)處理置信度和精確度加以分析。通過卡爾曼濾波圖形化顯示仿真結(jié)果，對(duì)各單一傳感器仿真數(shù)據(jù)真實(shí)值、預(yù)測(cè)值和濾波值做了比較和仿真分析。最后對(duì)三組傳感器仿真數(shù)據(jù)濾波值，采用最優(yōu)加權(quán)融合估計(jì)算法進(jìn)行數(shù)據(jù)融合，并進(jìn)行了仿真分析，結(jié)果表明采用卡爾曼濾波算法和最優(yōu)加權(quán)融合估計(jì)算法后，預(yù)處理數(shù)據(jù)置信度和數(shù)據(jù)精度得到明顯提高，融合處理結(jié)果優(yōu)于單源處理結(jié)果，導(dǎo)航定位精度得到提高，能更好地為用戶提供多維度的導(dǎo)航定位結(jié)果，同時(shí)，本文的研究成果也對(duì)環(huán)境數(shù)據(jù)預(yù)測(cè)和評(píng)估做支撐，具有現(xiàn)實(shí)意義。