專利名稱：一種sins/gps組合導(dǎo)航系統(tǒng)自適應(yīng)濾波方法
技術(shù)領(lǐng)域：
本發(fā)明涉及的是一種SINS/GPS組合導(dǎo)航系統(tǒng)自適應(yīng)濾波方法，可有效地抑 制由時(shí)變?cè)肼曇鸬膫鹘y(tǒng)卡爾曼濾波精度下降的情況。
(二)
背景技術(shù)：
SINS/GPS組合導(dǎo)航系統(tǒng)因其結(jié)合了慣導(dǎo)系統(tǒng)與GPS全球定位系統(tǒng)的優(yōu)勢(shì)， 一直吸引大量科研人員進(jìn)行深入的理論研究，并在車載、艦船、飛行器中有著廣 泛的應(yīng)用。卡爾曼濾波作為通用的SINS/GPS組合導(dǎo)航系統(tǒng)信息融合算法，要求 精確獲知量測(cè)噪聲的統(tǒng)計(jì)信息；當(dāng)噪聲模型不準(zhǔn)確或無(wú)法預(yù)知的情況下，傳統(tǒng)卡 爾曼濾波精度會(huì)大大降低。而GPS全球定位系統(tǒng)的定位精度不僅取決于器件誤 差，同時(shí)還依賴于星可見(jiàn)度及載體運(yùn)行環(huán)境，僅按經(jīng)驗(yàn)值預(yù)置GPS觀測(cè)噪聲無(wú) 法適應(yīng)GPS的工作特性，如何有效地解決時(shí)變GPS噪聲對(duì)卡爾曼濾波算法的影 響，對(duì)提高SINS/GPS組合導(dǎo)航系統(tǒng)精度具有重要意義。
近年來(lái)，許多學(xué)者對(duì)自適應(yīng)濾波進(jìn)行了深入的研究，如協(xié)方差及系統(tǒng)噪聲縮 放算法，多模自適應(yīng)估計(jì)算法(顧AE)，引入模糊邏輯的自適應(yīng)算法，基于新息 序列的自適應(yīng)算法，等等。多模自適應(yīng)估計(jì)算法采用多個(gè)并行卡爾曼濾波器實(shí)現(xiàn) 對(duì)模型及噪聲的估計(jì)，但由于其計(jì)算量大，并沒(méi)有被廣泛的應(yīng)用于實(shí)際工程中。 常用的Sage-Husa噪聲統(tǒng)計(jì)估值器采用新息序列均值法對(duì)噪聲進(jìn)行估計(jì)， 4=li>,《=^^4_1+^,，滑動(dòng)窗長(zhǎng)度為k(k為當(dāng)前時(shí)刻)，易知，隨著時(shí)間的增加，
當(dāng)前新息對(duì)估值的作用逐漸降低，fc^，"v〖—0。系統(tǒng)長(zhǎng)時(shí)間運(yùn)行后，當(dāng)由于環(huán)
境或載體機(jī)動(dòng)引起噪聲變化時(shí)，Saga-Husa算法無(wú)法解決該情況的噪聲估算問(wèn)題。 本發(fā)明基于新息序列的自適應(yīng)算法，提出了根據(jù)基于當(dāng)前量測(cè)殘差自動(dòng)調(diào)節(jié)滑動(dòng) 窗長(zhǎng)度的方法，進(jìn)行在線噪聲估計(jì)，將新息值與噪聲估計(jì)有效地結(jié)合起來(lái)，既保 證了系統(tǒng)的原有平穩(wěn)特性，又充分利用新息序列對(duì)時(shí)變?cè)肼曔M(jìn)行有效的在線估 計(jì)，具有實(shí)際的工程應(yīng)用意義。
(三) 發(fā)明內(nèi)容本發(fā)明的目的在于提供一種能夠有效地提高GPS噪聲未知的情況下組合導(dǎo) 航系統(tǒng)的定位精度的一種SINS/GPS組合導(dǎo)航系統(tǒng)自適應(yīng)濾波方法。
本發(fā)明的目的是這樣實(shí)現(xiàn)的
步驟l:用捷聯(lián)慣導(dǎo)系統(tǒng)實(shí)時(shí)采集陀螺加速度信息，對(duì)實(shí)時(shí)采集的陀螺加速 度信息進(jìn)行位置速度姿態(tài)解算；
步驟2:通過(guò)串口實(shí)時(shí)采集GPS信號(hào)，按照數(shù)據(jù)格式從緩沖區(qū)中讀取GPS提 供的位置速度信息；
步驟3:根據(jù)SINS動(dòng)態(tài)誤差模型建立組合導(dǎo)航系統(tǒng)的狀態(tài)方程，利用SINS 與GPS的位置速度誤差建立量測(cè)方程；
步驟4:依組合導(dǎo)航解算頻率，根據(jù)解算出的SINS位置、速度信息與GPS 信號(hào)計(jì)算新息大小，及量測(cè)殘差值，量測(cè)殘差大小反映了GPS信號(hào)噪聲大??；
步驟5:利用量測(cè)殘差值，對(duì)滑動(dòng)窗口長(zhǎng)度進(jìn)行自動(dòng)調(diào)節(jié)；
步驟6:依滑動(dòng)窗大小計(jì)算新息均值，對(duì)GPS量測(cè)信號(hào)噪聲協(xié)方差進(jìn)行實(shí)時(shí) 計(jì)算；
步驟7:將噪聲協(xié)方差代入到濾波預(yù)測(cè)過(guò)程及量測(cè)更新過(guò)程，計(jì)算濾波增益，
進(jìn)行組合導(dǎo)航解算。
本發(fā)明根據(jù)當(dāng)前新息自動(dòng)調(diào)節(jié)滑動(dòng)窗長(zhǎng)度，對(duì)時(shí)變?cè)肼曔M(jìn)行計(jì)算的自適應(yīng)組
合導(dǎo)航方法，具體步驟為
1、 根據(jù)SINS與GPS觀測(cè)量及觀測(cè)方程求取當(dāng)前新息及量測(cè)殘差值(第k 時(shí)刻)。
《—，
Ct=M[vt《] +&
a=《c:'v, = (zt—《_,)r(/ +,,'//'r+w(z廣《—■) 其中，^為新息值，G為新息均方差，^為量測(cè)殘差值。
2、 根據(jù)當(dāng)前量測(cè)殘差值求取滑動(dòng)窗長(zhǎng)度。
w=i, " 鵬
W = 4 , c < mi
W = J"teger(;t x A"—"腿)，A < 1， W之1 , amln < d < amax 易知，滑動(dòng)窗最小長(zhǎng)度為1，即W", G只取決于當(dāng)前新息，與歷史新息序
5列無(wú)關(guān)。此時(shí)，當(dāng)前新息比重對(duì)噪聲估值起了最大作用，殘差值大于設(shè)置的最大 閾值，即"> _。
殘差值小于設(shè)置的最小閾值時(shí)，滑動(dòng)窗取最大長(zhǎng)度w"，此時(shí)與Sage-Husa 極大后驗(yàn)噪聲統(tǒng)計(jì)估值器一致，即6 4i;w = Y(t, +乂 。
當(dāng)判斷出殘差值介于兩者之間時(shí)，即，滑動(dòng)窗長(zhǎng)度為
w=/"teg^t"" )，義為調(diào)節(jié)因子，它的大小決定了滑動(dòng)窗口隨量測(cè)殘差大小的變
化趨勢(shì)，hi，化i。
3、 依滑動(dòng)窗大小(濾波窗口大小)計(jì)算新息均值，計(jì)算時(shí)變信號(hào)噪聲協(xié)方 差的方法為；
根據(jù)滑動(dòng)窗大小計(jì)算K， K-1, K-2，…,K-N+l時(shí)刻新息方差的均值，作為進(jìn)一 步估算觀測(cè)噪聲協(xié)方差陣的依據(jù)。
為了避免矩陣奇異，當(dāng)《4<0時(shí)，進(jìn)行規(guī)一化處理，令&=0
4、 將噪聲協(xié)方差代入到濾波預(yù)測(cè)過(guò)程及量測(cè)更新過(guò)程，根據(jù)噪聲方差調(diào)節(jié)
濾波增益，進(jìn)行組合導(dǎo)航解算
A =(/-
其中，,為狀態(tài)變量預(yù)測(cè)值，對(duì)于非線性系統(tǒng)，可通過(guò)一階泰勒展開獲得； 、為新息；^為新息協(xié)方差；么—,為自適應(yīng)濾波中的系統(tǒng)噪聲協(xié)方差；w,為量測(cè)
噪聲協(xié)方差；g,w為一步預(yù)測(cè)誤差均方差；^為濾波增益陣。
本發(fā)明利用了通用的導(dǎo)航設(shè)備，通過(guò)對(duì)噪聲的實(shí)時(shí)計(jì)算，調(diào)節(jié)濾波增益，有 效地解決了由時(shí)變?cè)肼晫?dǎo)致的傳統(tǒng)卡爾曼濾波精度下降的情況。常用的Sage-Husa噪聲統(tǒng)計(jì)估值器作為一種極大后驗(yàn)估值方法，適用于對(duì)未知常值噪聲 進(jìn)行估計(jì)，其缺點(diǎn)和局限性是不能處理時(shí)變?cè)肼?。本發(fā)明通過(guò)對(duì)滑動(dòng)窗大小的自 動(dòng)調(diào)節(jié)，有效地將新息引入到噪聲估值中來(lái)，可在量測(cè)信號(hào)噪聲增大時(shí)，減小濾 波增益，降低量測(cè)信號(hào)對(duì)組合導(dǎo)航系統(tǒng)定位的作用；在量測(cè)信號(hào)噪聲減小時(shí)，加 大濾波增益，增大新息對(duì)組合導(dǎo)航的影響，自動(dòng)延長(zhǎng)的滑動(dòng)窗口又保持了系統(tǒng)的 平穩(wěn)特性，有效的解決了由時(shí)變?cè)肼晫?dǎo)致的傳統(tǒng)卡爾曼濾波精度下降的問(wèn)題，提 高了組合導(dǎo)航系統(tǒng)的定位精度。
(四)


圖1為本發(fā)明中，SINS/GPS自適應(yīng)組合導(dǎo)航系統(tǒng)結(jié)構(gòu)圖。
圖2為本發(fā)明中，量測(cè)噪聲未知情況下自適應(yīng)擴(kuò)展卡爾曼濾波流程圖。
具體實(shí)施方式
下面結(jié)合附圖舉例對(duì)本發(fā)明做更詳細(xì)地描述
步驟l、根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)的誤差模型建立系統(tǒng)方程。 捷聯(lián)慣導(dǎo)誤差方程為
<formula>formula see original document page 7</formula>
其中，^為被估計(jì)狀態(tài)；^為噪聲序列；cD^為"至^時(shí)刻一步轉(zhuǎn)移陣； IV,為系統(tǒng)噪聲驅(qū)動(dòng)陣；^為量測(cè)陣；^為量測(cè)噪聲序列； 一，"表示緯度、經(jīng)度 誤差；^;,W,表示東向、北向速度誤差；K,&表示東向、北向速度；O,表示
北向、東向水平失準(zhǔn)角；么表示方位失準(zhǔn)角；^SA表示陀螺漂移: 連續(xù)系統(tǒng)的狀態(tài)方程為一階泰勒展開，
》=/wo)+r(，， =e(')《('-o
其中，単)=, rA = *f r(,)w ， ￡ r = a, A =叫)，L = r(", a =抓)
SAT J
0w + ^(;)r
取SINS與GPS系統(tǒng)的位置速度誤差為觀測(cè)量，因而可以直接建立離散時(shí)間 t測(cè)方程
4 =
、脫'—^> /\V
p力
步驟2、計(jì)算當(dāng)前新息值及量測(cè)殘差大小，根據(jù)殘差值求取滑動(dòng)窗長(zhǎng)度
v* = Z廣股4," a =《ca = (z'-, f(仏尸w,w〖+ t )扁'(z'. —
w = i,""脆 w = /t, < 隱
W = /"tege/^/t x V—。訓(xùn)')，；i < 1, W》1 , a關(guān)< d < max
步驟3、量測(cè)噪聲未知時(shí)，本發(fā)明中涉及到的自適應(yīng)擴(kuò)展卡爾曼濾波預(yù)測(cè)方
程與更新過(guò)程為
S碌-A&o〖十r必r:
8
權(quán)利要求
1、一種SINS/GPS組合導(dǎo)航系統(tǒng)自適應(yīng)濾波方法，其特征是步驟1用捷聯(lián)慣導(dǎo)系統(tǒng)實(shí)時(shí)采集陀螺加速度信息，對(duì)實(shí)時(shí)采集的陀螺加速度信息進(jìn)行位置速度姿態(tài)解算；步驟2通過(guò)串口實(shí)時(shí)采集GPS信號(hào)，按照數(shù)據(jù)格式從緩沖區(qū)中讀取GPS提供的位置速度信息；步驟3根據(jù)SINS動(dòng)態(tài)誤差模型建立組合導(dǎo)航系統(tǒng)的狀態(tài)方程，利用SINS與GPS的位置速度誤差建立量測(cè)方程；步驟4依組合導(dǎo)航解算頻率，根據(jù)解算出的SINS位置、速度信息與GPS信號(hào)計(jì)算新息大小，及量測(cè)殘差值，量測(cè)殘差大小反映了GPS信號(hào)噪聲大?。徊襟E5利用量測(cè)殘差值，對(duì)滑動(dòng)窗口長(zhǎng)度進(jìn)行自動(dòng)調(diào)節(jié)；步驟6依滑動(dòng)窗大小計(jì)算新息均值，對(duì)GPS量測(cè)信號(hào)噪聲協(xié)方差進(jìn)行實(shí)時(shí)計(jì)算；步驟7將噪聲協(xié)方差代入到濾波預(yù)測(cè)過(guò)程及量測(cè)更新過(guò)程，計(jì)算濾波增益，進(jìn)行組合導(dǎo)航解算。
2、 根據(jù)權(quán)利要求1所述的一種SINS/GPS組合導(dǎo)航系統(tǒng)自適應(yīng)濾波方法，其特征是所述的計(jì)算新息及量測(cè)殘差的具體步驟為v,Z廣喊4—,其中，n為新息值，G為新息均方差，d為量測(cè)殘差值，zt，之為系統(tǒng)的量測(cè) 向量與狀態(tài)向量，//為系統(tǒng)量測(cè)矩陣，^為量測(cè)噪聲協(xié)方差，/V,為一步預(yù)測(cè)誤 差協(xié)方差。
3、 根據(jù)權(quán)利要求1或2所述的一種SINS/GPS組合導(dǎo)航系統(tǒng)自適應(yīng)濾波方 法，其特征是所述的利用量測(cè)殘差值，對(duì)系統(tǒng)k時(shí)刻滑動(dòng)窗口長(zhǎng)度進(jìn)行自動(dòng) 調(diào)節(jié)，具體步驟為w = i， "> 腿<formula>formula see original document page 3</formula>其中，N為滑動(dòng)窗長(zhǎng)度；"^與"皿分別為預(yù)先設(shè)置的最大及最小閾值，當(dāng)量測(cè)殘差大于"_時(shí)，滑動(dòng)窗口取最小值1;當(dāng)量測(cè)殘差小于"自時(shí)，滑動(dòng)窗口取最大值k， k為當(dāng)前時(shí)刻；A為調(diào)節(jié)因子，它的大小決定了滑動(dòng)窗口隨量測(cè)殘 差大小的變化趨勢(shì)，取整計(jì)算滑動(dòng)窗大小。
4、 根據(jù)權(quán)利要求1或2所述的一種SINS/GPS組合導(dǎo)航系統(tǒng)自適應(yīng)濾波方 法，其特征是所述的依滑動(dòng)窗大小計(jì)算新息均值，實(shí)時(shí)計(jì)算量測(cè)信號(hào)噪聲協(xié) 方差，其具體步驟為依據(jù)滑動(dòng)窗大小計(jì)算相關(guān)新息序列均值，計(jì)算量測(cè)噪聲； <formula>formula see original document page 3</formula>為了避免矩陣奇異，當(dāng)《4<0時(shí)，進(jìn)行規(guī)一化處理，令《4=0。
5、 根據(jù)權(quán)利要求3所述的一種SINS/GPS組合導(dǎo)航系統(tǒng)自適應(yīng)濾波方法，其特征是所述的依滑動(dòng)窗大小計(jì)算新息均值，實(shí)時(shí)計(jì)算量測(cè)信號(hào)噪聲協(xié)方差， 其具體步驟為依據(jù)滑動(dòng)窗大小計(jì)算相關(guān)新息序列均值，計(jì)算量測(cè)噪聲；為了避免矩陣奇異，當(dāng)w,O時(shí)，進(jìn)行規(guī)一化處理，令&=0。
全文摘要
本發(fā)明提供的是一種SINS/GPS自適應(yīng)組合導(dǎo)航方法，其特征是將捷聯(lián)慣導(dǎo)系統(tǒng)與GPS全球定位系統(tǒng)進(jìn)行信息融合，并按組合頻率，對(duì)由串口接收到的GPS信號(hào)進(jìn)行噪聲實(shí)時(shí)計(jì)算。本發(fā)明以量測(cè)殘差為判斷基準(zhǔn)，通過(guò)對(duì)滑動(dòng)窗大小的自動(dòng)調(diào)節(jié)，有效地將新息引入到量測(cè)信號(hào)噪聲的實(shí)時(shí)計(jì)算中來(lái)，可在GPS量測(cè)信號(hào)噪聲增大時(shí)，減小濾波增益，降低量測(cè)信號(hào)對(duì)組合導(dǎo)航系統(tǒng)定位的作用；在量測(cè)信號(hào)噪聲減小時(shí)，加大濾波增益，增大新息對(duì)組合導(dǎo)航的影響，自動(dòng)延長(zhǎng)的滑動(dòng)窗口又保持了系統(tǒng)的平穩(wěn)特性，有效地解決了由時(shí)變?cè)肼晫?dǎo)致的傳統(tǒng)卡爾曼濾波精度下降的問(wèn)題，提高了組合導(dǎo)航系統(tǒng)的定位精度。
文檔編號(hào)G01C21/20GK101464152SQ20091007123
公開日2009年6月24日 申請(qǐng)日期2009年1月9日 優(yōu)先權(quán)日2009年1月9日
發(fā)明者強(qiáng) 于, 奔粵陽(yáng), 楓 孫, 博 徐, 郝燕玲, 真 郭, 偉 高 申請(qǐng)人:哈爾濱工程大學(xué)