專利名稱：一種sins/gps組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種SINS/GPS組合導(dǎo)航系統(tǒng)信息融合的濾波方法，可用于提高飛機、導(dǎo)彈、艦船或地面車輛用SINS/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。
背景技術(shù)：
捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)是一種完全自主的導(dǎo)航系統(tǒng)，可以連續(xù)、實時地提供位置、速度和姿態(tài)信息，其短時精度很高，且具有隱蔽性好，不受氣候條件限制等優(yōu)點，因而廣泛應(yīng)用于航空、航天、航海等領(lǐng)域。但是，SINS誤差隨時間增長，因此常與全球衛(wèi)星定位系統(tǒng)(GPS)組合構(gòu)成SINS/GPS組合導(dǎo)航系統(tǒng)。
在SINS/GPS組合導(dǎo)航系統(tǒng)中，為了提高組合導(dǎo)航系統(tǒng)的精度，現(xiàn)有方法常常采用反饋校正卡爾曼濾波方法進(jìn)行信息融合，其本質(zhì)是將SINS的位置誤差、速度誤差、姿態(tài)誤差和器件誤差作為狀態(tài)量，將GPS提供的位置和速度作為觀測量，通過最優(yōu)估計的方法將SINS的各種誤差估計出來，然后進(jìn)行反饋校正。但是，由于SINS/GPS組合導(dǎo)航系統(tǒng)在通常情況下是不完全可觀測的，一些狀態(tài)量的可觀測度很小，導(dǎo)致這些狀態(tài)的估計值精度很低。將這些精度很低的狀態(tài)估值完全反饋到SINS內(nèi)部，導(dǎo)致SINS內(nèi)部誤差變大，最終組合導(dǎo)航系統(tǒng)精度下降。

發(fā)明內(nèi)容
本發(fā)明的技術(shù)解決問題是克服現(xiàn)有技術(shù)的不足，提供一種SINS/GPS組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法，該方法在組合導(dǎo)航系統(tǒng)不完全可觀測的情況下有效地提高導(dǎo)航精度。
本發(fā)明的技術(shù)解決方案為一種SINS/GPS組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法，其特點在于包括下列步驟(1)建立SINS的誤差方程，利用GPS提供的位置和速度信息作為觀測量，通過卡爾曼濾波方法估計SINS的誤差。
(2)通過系統(tǒng)可觀測度分析方法計算SINS/GPS組合導(dǎo)航系統(tǒng)各個狀態(tài)量的可觀測度。
(3)采用歸一化處理方法對的每個系統(tǒng)狀態(tài)的可觀測度進(jìn)行歸一化處理，然后將歸一化處理后的系統(tǒng)狀態(tài)可觀測度作為加權(quán)反饋因子，采用自適應(yīng)加權(quán)反饋校正方法對SINS系統(tǒng)進(jìn)行反饋校正。
上述的系統(tǒng)可觀測度分析方法為基于奇異值分解的系統(tǒng)可觀測度分析方法，該方法的具體步驟為(1)通過分段線性定常的方法計算條帶可觀測矩陣(SOM)Qsom(r)，Qsom(r)=Q1Q2···Qr]]>其中對應(yīng)每一時間段j的可觀測矩陣定義為 其中，Hj和Fj分別為第j個時間段的系統(tǒng)觀測矩陣和狀態(tài)轉(zhuǎn)移矩陣；(2)對條帶式可觀測矩陣Qsom(r)陣進(jìn)行奇異值分解，得Qsom(r)＝U*S*VT其中U＝[u1，u2，…，um]，V＝[v1，v2，…，vm]都是正交矩陣，S為對角陣S=Λr×r000]]>其中Λ＝diag(σ1，σ2，…，σr)，σ1＞σ2＞…＞σr＞0稱為矩陣Qsom(r)的奇異值。
(3)設(shè)初始狀態(tài)為X(t0)(n維)，量測值為Z(m1維)，則Z=QS(r)*X(t0)=(USVT)X(t0)=(Σi=1rσiuiviT)X(t0)]]>
即Z=Σi=1rσi(viTX(t0))*ui]]>可得X(t0)=Σi=1r(uiT*Zσi)vi]]>(4)對矩陣uiviT進(jìn)行分析，觀察它的各列元素的大小，判斷出每一個奇異值σi對應(yīng)的初始狀態(tài)向量X(t0，i)，奇異值σi的大小直接表明了狀態(tài)向量X(t0，i)可觀測程度的高低。
在上述的SINS/GPS組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法中，自適應(yīng)加權(quán)反饋校正方法可以表示為 其中，ΔYi為系統(tǒng)第i個狀態(tài)量ΔXi對應(yīng)的反饋量；ki為歸一化處理后狀態(tài)量ΔXi的可觀測度； 為狀態(tài)量ΔXi的估值。
上述的歸一化處理方法可以表示為ki=σiσsi,]]>其中，ki為歸一化處理后第i個系統(tǒng)狀態(tài)的可觀測度；σi為歸一化處理之前的第i個系統(tǒng)狀態(tài)的可觀測度；σsi為載體做“S”形機動時第i個系統(tǒng)狀態(tài)的可觀測度，即第i個系統(tǒng)狀態(tài)的最大可觀測度。
本發(fā)明的原理是在SINS/GPS組合導(dǎo)航系統(tǒng)中，現(xiàn)有方法通常采用反饋校正卡爾曼濾波方法將SINS和GPS兩個子系統(tǒng)的數(shù)據(jù)進(jìn)行信息融合，以獲得更高精度的導(dǎo)航結(jié)果，其基本原理是將SINS/GPS組合導(dǎo)航系統(tǒng)的誤差(位置誤差、速度誤差、姿態(tài)誤差和器件誤差等)作為系統(tǒng)的狀態(tài)量，將GPS提供的位置和速度信息作為觀測量，采用卡爾曼濾波方法估計出這些誤差(狀態(tài)量)，并把估計出來的誤差作為反饋量在系統(tǒng)中進(jìn)行校正，以修正系統(tǒng)的誤差，提高系統(tǒng)的導(dǎo)航精度。
但是，SINS/GPS組合導(dǎo)航系統(tǒng)在大多情況下都是不完全可觀測的，只有當(dāng)載體做“S”形機動時，系統(tǒng)的可觀測度達(dá)到最大，SINS/GPS組合導(dǎo)航系統(tǒng)才轉(zhuǎn)變?yōu)橥耆捎^測系統(tǒng)。而SINS/GPS組合導(dǎo)航系統(tǒng)的可觀測度直接決定了卡爾曼濾波方法的估計精度。有些系統(tǒng)狀態(tài)不可觀測或可觀測度很低，其估計精度也很低，若將這些精度很低的狀態(tài)估值反饋，反而降低了組合導(dǎo)航系統(tǒng)的精度。因此，只有根據(jù)每個系統(tǒng)狀態(tài)的可觀測度的決定其反饋量，建立系統(tǒng)狀態(tài)量的反饋量與可觀測度的定量關(guān)系，根據(jù)每個系統(tǒng)狀態(tài)的可觀測度大小判斷反饋量的大小，才能從根本提高組合導(dǎo)航系統(tǒng)的精度。
因此，所述的一種SINS/GPS組合導(dǎo)航系統(tǒng)自適應(yīng)加權(quán)反饋校正濾波的基本原理是首先，將SINS/GPS組合導(dǎo)航系統(tǒng)的誤差作為狀態(tài)量，建立系統(tǒng)的狀態(tài)方程，將GPS提供的位置和速度信息作為觀測量，通過卡爾曼濾波方法估計SINS/GPS組合導(dǎo)航系統(tǒng)的誤差(狀態(tài)量)。然后，采用系統(tǒng)可觀測度分析方法計算每個狀態(tài)量的可觀測度，再將歸一化處理后的系統(tǒng)狀態(tài)可觀測度作為加權(quán)反饋因子，將加權(quán)反饋因子與狀態(tài)估值的積作為反饋量，進(jìn)行自適應(yīng)反饋校正。由于自適應(yīng)加權(quán)反饋校正濾波根據(jù)每個系統(tǒng)狀態(tài)的可觀測度的決定其反饋量，建立系統(tǒng)狀態(tài)量的反饋量與可觀測度的定量關(guān)系，因此從根本提高了組合導(dǎo)航系統(tǒng)的精度。
本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點在于本發(fā)明建立系統(tǒng)狀態(tài)量的反饋量與可觀測度的定量關(guān)系，將歸一化處理后的系統(tǒng)狀態(tài)可觀測度作為加權(quán)反饋因子，對SINS系統(tǒng)進(jìn)行自適應(yīng)加權(quán)反饋校正，可在系統(tǒng)不完全可觀測的情況下有效地提高飛機、導(dǎo)彈、艦船或地面車輛用SINS/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。


圖1為本發(fā)明的原理框圖；圖2為本發(fā)明的卡爾曼濾波基本算法的解算流程圖。
具體實施例方式
如圖1、2所示，本發(fā)明的具體實施方法如下(1)建立SINS/GPS組合導(dǎo)航系統(tǒng)的數(shù)學(xué)模型，包括系統(tǒng)狀態(tài)方程和量測方程，分別如式1和式2所示。
系統(tǒng)狀態(tài)方程X·=FX+GW---(1)]]>其中，X為系統(tǒng)狀態(tài)矢量，W為系統(tǒng)噪聲矢量，F(xiàn)為系統(tǒng)轉(zhuǎn)移矩陣，G為噪聲轉(zhuǎn)換矩陣X＝[φxφyφzδvxδvyδvzδL δλ δh εxεyεzxyz]T F=FINSFSo6×6FM,FS=Cbn03×303×3Cbn03×303×3,FM=
,G=Cbn03×303×3Cbn09×309×3]]>系統(tǒng)的量測方程Z＝HX+ζ (2)其中Z為觀測矢量，H為觀測矩陣，ζ為量測噪聲Z＝[δL δλ δh δVEδVNδVU]TH=03×6I3×303×603×3I3×303×9]]>ζ=ζLζλζhζVEζVNζVUT]]>(2)在SINS/GPS組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型的基礎(chǔ)上，采用卡爾曼濾波方法進(jìn)行信息融合，獲得SINS/GPS組合系統(tǒng)的狀態(tài)量估計值 卡爾曼濾波算法的流程圖如圖2所示，該算法的公式編排如式(3)～(7)所示。
狀態(tài)一步預(yù)測方程XΛk/k-1=φk,k-1XΛk-1---(3)]]>狀態(tài)估值計算方程XΛk=XΛk/k-1+Kk(Zk-HkXΛk/k-1)---(4)]]>濾波增量方程KΛk=PΛk/k-1HkT(HkPk/k-1HkT+Rk)-1---(5)]]>
一步預(yù)測均方誤差方程PΛk/k-1=φk,k-1Pk-1φk,k-1T+Γk-1Qk-1Γk-1T---(6)]]>估計均方誤差方程PΛk=(I-KkHk)Pk/k-1(I-KkHk)T+KkRkKkT---(7)]]>(3)采用基于奇異值分解的可觀測度分析方法計算SINS/GPS組合導(dǎo)航系統(tǒng)中各個狀態(tài)量的可觀測度。
分段線性定常系統(tǒng)(PWCS)可觀測性分析方法是基于奇異值分解的可觀測度分析方法的基礎(chǔ)。SINS/GPS組合導(dǎo)航系統(tǒng)是時變系統(tǒng)，判斷定常系統(tǒng)可觀測性的分析方法都不適用，分段線性定常系統(tǒng)(PWCS)可觀測性分析方法是專門用于判斷時變系統(tǒng)可觀測性的一種方法。在一個足夠小的時間區(qū)間內(nèi)，如果線性時變系統(tǒng)的系數(shù)矩陣變化量可以忽略不計，那么在該時間區(qū)間內(nèi)就可以把時變系統(tǒng)當(dāng)作定常系統(tǒng)處理，這樣的系統(tǒng)稱為分段式定常系統(tǒng)。
一個離散的PWCS可用如下模型表示X(k+1)＝FjX(k)+GjU(k)+Γjw(k) (8)Zj(k)＝HjX(k)式中X(k)∈Rn，F(xiàn)j∈Rn×n，Gj∈Rn×s，U(k)∈Rs，w(k)∈Rl，Γj∈Rn×l，Zj(k)∈Rm，Hj∈Rm×n。j＝1，2，……，r，表示系統(tǒng)分段間隔序號。對每個時間段j，矩陣Fj、Gj和Hj都是恒定的，但對應(yīng)不同的時間段，每個矩陣可以是不同的。系統(tǒng)總的可觀測性矩陣(TOM)和條帶化可觀測性矩陣(SOM)分別表示為Qtom(r)=Q1Q2F1n-1···QrFr-1n-1Fr-2n-1...F1n-1---(9)]]>
Qsom(r)=Q1Q2···Qr---(10)]]>其中對應(yīng)每一時間段j的可觀測矩陣定義為 根據(jù)系統(tǒng)方程和量測方程以及上述可觀測矩陣的定義，由初值表示的系統(tǒng)輸出為Z＝Qtom(r)*X(t0)(12)若矩陣Qtom(r)的秩等于n，則由上式可知，X(t0)有唯一確定的解，表明系統(tǒng)狀態(tài)是完全可觀測的。顯然，直接利用Qtom(r)陣研究離散PWCS的可觀測性計算量相當(dāng)大，而采用SOM來代替TOM來分析系統(tǒng)的可觀測性，可以使問題得到簡化。
用SOM矩陣代替TOM矩陣，隨著時間段的增加，可觀測性矩陣的維數(shù)仍然很高，對其實施奇異值分解的工作量也是相當(dāng)大的。因此，這里采用一種改進(jìn)的基于奇異值分解的系統(tǒng)可觀測度分析方法。
設(shè)某時間段動態(tài)系統(tǒng)的可觀測性矩陣為Qsom，初始狀態(tài)為X(t0)(n維)，量測值為Z(m1維)，則Z＝Qsom*X(t0) (13)對Qsom陣進(jìn)行奇異值分解，得Qsom＝U*S*VT(14)其中U＝[u1，u2，…，um]，V＝[v1，v2，…，vm]都是正交矩陣。
S=Λr×r000---(15)]]>其中Λ＝diag(σ1，σ2，…，σr)，σ1＞σ2＞…＞σr＞0稱為矩陣Qsom的奇異值。將式(14)帶入式(13)中，得
Z=(USVT)X(t0)=(Σi=1rσiuiviT)X(t0)---(16)]]>即Z=Σi=1rσi(viTX(t0))*ui---(17)]]>根據(jù)式(16)可得X(t0)=Σi=1r(uiT*Zσi)vi---(18)]]>傳統(tǒng)的分析方法是，根據(jù)式(18)計算每一個奇異值σi對應(yīng)的初始狀態(tài)向量X(t0，i)。從數(shù)值上看，較大的奇異值可以獲得較好的狀態(tài)估計，反之，對于特別小的奇異值，可能會引起多個X(t0)的奇異，最終落入不可觀測空間內(nèi)。
從線性系統(tǒng)理論的角度分析，狀態(tài)量X(t0)的可觀測性應(yīng)該只取決于系統(tǒng)結(jié)構(gòu)，而與觀測量Z無關(guān)，根據(jù)式(16)對矩陣uiviT進(jìn)行分析，觀察它的各列元素的大小，就可以判斷出每一個奇異值σi對應(yīng)的初始狀態(tài)向量X(t0，i)，這種改進(jìn)的可觀測度分析方法不僅計算簡單，更重要的是可以在沒有實驗測得量測數(shù)據(jù)的情況下分析系統(tǒng)狀態(tài)的可觀測度。
(4)將系統(tǒng)狀態(tài)量的可觀測度進(jìn)行歸一化處理。
通常，如果某一個系統(tǒng)狀態(tài)可觀測，該系統(tǒng)狀態(tài)對應(yīng)的可觀測度即為1。但是在傳統(tǒng)的系統(tǒng)可觀測度分析方法中，未對每個系統(tǒng)狀態(tài)的可觀測度進(jìn)行歸一化處理。由于每個系統(tǒng)狀態(tài)量綱不同，導(dǎo)致每個系統(tǒng)狀態(tài)的可觀測度大小不一，甚至數(shù)量級不同。最為明顯的是，當(dāng)載體進(jìn)行“S”形機動時，此時SINS/GPS組合導(dǎo)航系統(tǒng)為完全可觀測系統(tǒng)，每個狀態(tài)量的可觀測度都應(yīng)該為1。但是采用傳統(tǒng)的系統(tǒng)可觀測度分析方法計算出來的每個系統(tǒng)狀態(tài)的可觀測度，由于量綱的原因大多不等于1，有的很大，有的很小。為了使得可觀測度的定義能更確切地表示系統(tǒng)狀態(tài)的可觀測程度，采用可觀測度的歸一化處理方法，如式(19)所示。
ki=σiσsi---(19)]]>其中，ki為歸一化處理后第i個系統(tǒng)狀態(tài)的可觀測度；σi為歸一化處理之前的第i個系統(tǒng)狀態(tài)ΔXi的可觀測度；σsi為載體做“S”形機動時第i個系統(tǒng)狀態(tài)ΔXi的可觀測度，即第i個系統(tǒng)狀態(tài)ΔXi的最大可觀測度。
(5)將歸一化處理后的系統(tǒng)可觀測度作為加權(quán)反饋因子，將加權(quán)反饋因子與狀態(tài)估值的乘積作為反饋量，對SINS/GPS組合導(dǎo)航系統(tǒng)進(jìn)行自適應(yīng)加權(quán)反饋校正。
傳統(tǒng)的反饋校正方法不管系統(tǒng)狀態(tài)估值的精度，直接進(jìn)行全狀態(tài)反饋。當(dāng)系統(tǒng)中部分系統(tǒng)狀態(tài)可觀測度低時，這些系統(tǒng)狀態(tài)的估計精度也很低，將這些精度很低的系統(tǒng)狀態(tài)估值反饋到系統(tǒng)中，最終導(dǎo)致系統(tǒng)的精度下降。
由于卡爾曼濾波方法估計的精度歸根到底是由每個系統(tǒng)狀態(tài)的可觀測度決定的，只有根據(jù)每個系統(tǒng)狀態(tài)的可觀測度的決定其反饋量，建立系統(tǒng)狀態(tài)量的反饋量與可觀測度的定量關(guān)系，才能從根本提高組合導(dǎo)航系統(tǒng)的精度。因此，自適應(yīng)加權(quán)反饋校正方法可以表示為 其中，ΔYi為系統(tǒng)第i個狀態(tài)量ΔXi對應(yīng)的反饋量；ki為歸一化處理后狀態(tài)量ΔXi的可觀測度； 為狀態(tài)量ΔXi的估值。
最后，將ΔYi在SINS/GPS組合導(dǎo)航系統(tǒng)中進(jìn)行反饋，校正SINS/GPS組合導(dǎo)航系統(tǒng)的誤差，提高了組合導(dǎo)航系統(tǒng)的精度。
權(quán)利要求
1.一種SINS/GPS組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法，其特征在于包括下列步驟(1)建立SINS的誤差方程，利用GPS提供的位置和速度信息作為觀測量，通過卡爾曼濾波方法估計SINS的誤差；(2)通過系統(tǒng)可觀測度分析方法計算SINS/GPS組合導(dǎo)航系統(tǒng)各個狀態(tài)量的可觀測度；(3)采用歸一化處理方法對的每個系統(tǒng)狀態(tài)的可觀測度進(jìn)行歸一化處理，然后將歸一化處理后的系統(tǒng)狀態(tài)可觀測度作為加權(quán)反饋因子，采用自適應(yīng)加權(quán)反饋校正方法對SINS系統(tǒng)進(jìn)行反饋校正。
2.根據(jù)權(quán)利要求1所述的一種SINS/GPS組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法，其特征在于所述的系統(tǒng)可觀測度分析方法為基于奇異值分解的系統(tǒng)可觀測度分析方法，該方法的具體步驟為(1)通過分段線性定常的方法計算條帶可觀測矩陣Qsom(r)，Qsom(r)=Q1Q2···Qr]]>其中對應(yīng)每一時間段j的可觀測矩陣定義為 其中，Hj和Fj分別為第j個時間段的系統(tǒng)觀測矩陣和狀態(tài)轉(zhuǎn)移矩陣；(2)對條帶式可觀測矩陣Qsom(r)陣進(jìn)行奇異值分解，得Qsom(r)＝U*S*VT其中U＝[u1，u2，…，um]，V＝[v1，v2，…，vm]都是正交矩，S為對角陣S=Λr×r000]]>其中Λ＝diag(σ1，σ2，…，σr)，σ1＞σ2＞…＞σr＞0稱為矩陣Qsom(r)的奇異值；(3)設(shè)初始狀態(tài)為X(t0)，為n維，量測值為Z，為m1維，n＞0，m1＞0，則Z=QS(r)*X(t0)=(USVT)X(t0)=(Σi=1rσiuiviT)X(t0)]]>即Z=Σi=1rσi(viTX(t0))*ui]]>可得X(t0)=Σi=1r(uiT*Zσi)vi]]>(4)對矩陣uiviT進(jìn)行分析，觀察它的各列元素的大小，判斷出每一個奇異值σi對應(yīng)的初始狀態(tài)向量X(t0，i)，奇異值σi的大小直接表明了狀態(tài)向量X(t0，i)可觀測程度的高低。
3.根據(jù)權(quán)利要求1所述的一種SINS/GPS組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法，其特征在于所述的自適應(yīng)加權(quán)反饋校正方法公式為 其中，ΔYi為系統(tǒng)第i個狀態(tài)量ΔXi對應(yīng)的反饋量，ki為歸一化處理后狀態(tài)量ΔXi的可觀測度， 為狀態(tài)量ΔXi的估值。
4.根據(jù)權(quán)利要求1所述的一種SINS/GPS組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法，其特征在于所述的歸一化處理方法公式為ki=σiσsi]]>表示，其中，ki為歸一化處理后第i個系統(tǒng)狀態(tài)ΔXi的可觀測度，σi為歸一化處理之前的第i個系統(tǒng)狀態(tài)ΔXi的可觀測度，σsi為載體做“S”形機動時第i個系統(tǒng)狀態(tài)ΔXi的可觀測度，即第i個系統(tǒng)狀態(tài)的最大可觀測度。
全文摘要
本發(fā)明涉及一種SINS/GPS組合導(dǎo)航系統(tǒng)的自適應(yīng)加權(quán)反饋校正濾波方法，其特征是首先利用GPS提供的位置和速度觀測量，通過卡爾曼濾波方法估計SINS的位置誤差、速度誤差、姿態(tài)誤差和器件誤差，然后采用系統(tǒng)可觀測度分析方法計算系統(tǒng)中各個狀態(tài)量的可觀測度，再將歸一化處理后的系統(tǒng)狀態(tài)可觀測度作為加權(quán)反饋因子，對SINS系統(tǒng)進(jìn)行自適應(yīng)加權(quán)反饋校正。本發(fā)明具有精度高、不易發(fā)散的優(yōu)點，可在系統(tǒng)不完全可觀測的情況下有效地提高飛機、導(dǎo)彈、艦船或地面車輛用SINS/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。
文檔編號G01S19/49GK1945212SQ20061011427
公開日2007年4月11日 申請日期2006年11月3日 優(yōu)先權(quán)日2006年11月3日
發(fā)明者房建成, 劉百奇, 楊勝, 宮曉琳, 張鈺, 李金濤 申請人:北京航空航天大學(xué)