本發(fā)明涉及衛(wèi)星導(dǎo)航和卡爾曼濾波
技術(shù)領(lǐng)域:
,特別是一種GPS接收機的自適應(yīng)矢量跟蹤方法。
背景技術(shù):
:GPS是由美國開發(fā)和維護的天基無線電導(dǎo)航系統(tǒng),具有低成本、高精度、全天候的特點。全世界的用戶只需要使用接收機接收至少4顆導(dǎo)航衛(wèi)星的信號就可以實時的獲得自身的精確的三維位置、速度和時間信息。經(jīng)過幾十年的發(fā)展,目前GPS接收機已經(jīng)被廣泛應(yīng)用于武器制導(dǎo)、車輛導(dǎo)航、大地測量等領(lǐng)域,甚至還被應(yīng)用于地震預(yù)測、天氣預(yù)報等領(lǐng)域。目前國內(nèi)外主要使用的成熟技術(shù)都是基于鎖相環(huán)技術(shù)的變量跟蹤環(huán)路,但是對于高動態(tài)和弱信號環(huán)境效果不佳,標(biāo)量跟蹤環(huán)路在高動態(tài)環(huán)境下環(huán)路容易失鎖,標(biāo)量跟蹤環(huán)路在弱信號環(huán)境下不能穩(wěn)定的跟蹤信號。技術(shù)實現(xiàn)要素:本發(fā)明的目的在于提供一種導(dǎo)航精度高、跟蹤性能穩(wěn)定的GPS接收機的自適應(yīng)矢量跟蹤方法,以提高GPS信號跟蹤環(huán)路的魯棒性。實現(xiàn)本發(fā)明目的的技術(shù)解決方案為:一種GPS接收機的自適應(yīng)矢量跟蹤方法,包括下列步驟:步驟1,矢量跟蹤環(huán)路初始化,包括接收機的初始的位置、速度、鐘差、鐘漂和衛(wèi)星星歷;步驟2,利用導(dǎo)航濾波器估計的位置、速度、鐘差、鐘漂和衛(wèi)星星歷計算載波和碼NCO預(yù)測參數(shù);步驟3,導(dǎo)航濾波器自適應(yīng)處理,包括計算標(biāo)度因子和模糊控制器,標(biāo)度因子用來判斷是否存在誤差通道,新息序列方差和量測噪聲方差比值作為模糊控制器輸入,模糊控制器輸出對應(yīng)通道的量測方差調(diào)整參數(shù);步驟4,載波鑒頻器和碼環(huán)鑒別器輸出作為導(dǎo)航濾波器量測信息,用以估計接收機的位置、速度、鐘差和鐘漂誤差,用估計的信息修正接收機位置和速度,然后用修正過的接收機位置和速度結(jié)合衛(wèi)星星歷計算載波和碼NCO,以保持對輸入信號的穩(wěn)定跟蹤。本發(fā)明與現(xiàn)有技術(shù)相比,其顯著優(yōu)點是:(1)采用的矢量跟蹤方法,取消了傳統(tǒng)的獨立、并行的標(biāo)量跟蹤模式,充分利用了各衛(wèi)星通道間的共享信息,可以同時跟蹤所有可視衛(wèi)星;(2)采用了自適應(yīng)跟蹤方法,提高跟蹤環(huán)路在部分通道信號衰減和被遮擋環(huán)境下的環(huán)路的跟蹤穩(wěn)定性。附圖說明圖1是本發(fā)明GPS接收機的自適應(yīng)矢量跟蹤方法的原理框圖。圖2是模糊控制器輸入隸屬函數(shù)圖。圖3是模糊控制器輸出隸屬函數(shù)圖。具體實施方式本發(fā)明GPS接收機的自適應(yīng)矢量跟蹤方法,具體實現(xiàn)如下:在不考慮噪聲的情況下,GPS接收機射頻前端輸出的中頻信號模型為:SIF(t)=2A·D(t-τ)·C(t-τ)·cos[ωIFt+φ(t)]---(1)]]>式中,A為信號強度,D(t)為導(dǎo)航電文,C(t)為C/A碼,τ為傳輸過程中的時間延遲,ωIF為信號中頻,φ(t)為初始載波相位。本地振蕩器發(fā)生的兩路信號分別為:I(t)=2cos[(ωIF+Δω)t+φ0]---(2)]]>Q(t)=2sin[(ωIF+Δω)t+φ0]---(3)]]>式中,(ωIF+Δω)為本地振蕩器產(chǎn)生的載波頻率,Δω為本地載波頻率和輸入的中頻信號頻率的差,φ0為本地信號產(chǎn)生初始載波相位。輸入的中頻信號與本地振蕩器發(fā)生的同相、正交信號相乘,濾除高頻成分后,兩支路的輸出為:SI(t)=A·D(t-τ)·C(t-τ)·cos[φ(t)-Δωt-φ0]---(4)]]>SQ(t)=A·D(t-τ)·C(t-τ)·sin[φ(t)-Δωt-φ0]---(5)]]>兩個支路的輸出信號分別與本地偽碼發(fā)生器生成的即時碼(P)、超前碼(E)和滯后碼(L)相關(guān),并在預(yù)檢測積分時間內(nèi)累加求和。假設(shè)積分間隔內(nèi),載波頻率差和相位差都近似不變,則取均值后的相關(guān)器輸出為:IP=2A2·D·R(δτ)·sin(πTδf)πTδf·cos(πTδf+δφ)---(6)]]>QP=2A2·D·R(δτ)·sin(πTδf)πTδf·sin(πTδf+δφ)---(7)]]>IE=2A2·D·R(δτ-δ)·sin(πTδf)πTδf·cos(πTδf+δφ)---(8)]]>QE=2A2·D·R(δτ-δ)·sin(πTδf)πTδf·sin(πTδf+δφ)---(9)]]>IL=2A2·D·R(δτ+δ)·sin(πTδf)πTδf·cos(πTδf+δφ)---(10)]]>QL=2A2·D·R(δτ+δ)·sin(πTδf)πTδf·sin(πTδf+δφ)---(11)]]>式中,δ為本地C/A碼超前滯后的間隔,T為預(yù)檢測積分時間,δτ為偽碼相位誤差,δf和分別為積分間隔起始時刻本地參考信號與輸入信號之間的載波頻率差和載波相位差,R(τ)為C/A碼的相關(guān)函數(shù)。結(jié)合圖1,本發(fā)明GPS接收機的自適應(yīng)矢量跟蹤方法,包括下列步驟:步驟1,矢量跟蹤環(huán)路初始化,包括接收機的初始的位置、速度、鐘差、鐘漂和衛(wèi)星星歷。步驟2,利用導(dǎo)航濾波器估計的位置、速度、鐘差、鐘漂和衛(wèi)星星歷計算載波和碼NCO預(yù)測參數(shù),具體如下:hk(n)=hx(n)hy(n)hz(n)T---(12)]]>hx(n)=x(n)-x^r^(n),hy(n)=y(n)-y^r^(n),hz(n)=z(n)-z^r^(n)---(13)]]>r^(n)=(x(n)-x^)2+(y(n)-y^)2+(z(n)-z^)2---(14)]]>τ^k(n)=τk-1(n)+(ΔXk,k-1(n)-tk,k-1Vk-1)Thk(n)+tk,k-1c---(15)]]>f^code,k(n)=[1+td,k-1+(Vk-1(n)-Vk-1)Thk(n)]fcode/c---(16)]]>f^carrier,k(n)=[1+td,k-1+(Vk-1(n)-Vk-1)Thk(n)]fcarrier/c---(17)]]>式中,Vk-1分別為k-1時刻解算得到的GPS接收機的速度,x(n)、y(n)、z(n)為衛(wèi)星號為n的GPS衛(wèi)星的位置,為接收機在地心地固坐標(biāo)系下的位置,為衛(wèi)星n與接收機之間的視線矢量,為k-1時刻通過星歷解算出來的衛(wèi)星n的速度;為k時刻的衛(wèi)星位置;是指k時刻的衛(wèi)星位置和k-1時刻衛(wèi)星n的位置的差值;分別為k時刻的碼相位、碼頻率和載波頻率預(yù)測值;為k-1時刻的碼相位解算值;c為真空中光速;td,k-1為k-1時刻的鐘漂;fcode為C/A碼的基準(zhǔn)頻率,為1.023MHz;fcarrier為載波L1的頻率,為1575.42MHz。步驟3,導(dǎo)航濾波器自適應(yīng)處理,包括計算標(biāo)度因子和模糊控制器,標(biāo)度因子用來判斷是否存在誤差通道,新息序列方差和量測噪聲方差比值作為模糊控制器輸入,模糊控制器輸出對應(yīng)通道的量測方差調(diào)整參數(shù),具體如下:(a)導(dǎo)航濾波器模型的狀態(tài)方程:ΔxkΔvx,kΔykΔvy,kΔzkΔvz,kc·tb,kc·td,k=Φk,k-1Δxk-1Δvx,k-1Δyk-1Δvy,k-1Δzk-1Δvz,k-1c·tb,k-1c·td,k-1+wk---(18)]]>式中,Φk,k-1為k-1時刻到k時刻的狀態(tài)轉(zhuǎn)移矩陣,c為真空下光的速度,tb,k和tb,k-1分別為k時刻和k-1時刻的接收機鐘差,td,k和td,k-1分別為k時刻和k-1時刻的接收機鐘漂,Δxk、Δyk、Δzk為地心地固坐標(biāo)系下k時刻位置誤差在x、y、z軸的分量,Δvx,k、Δvy,k、Δvz,k為k時刻地心地固坐標(biāo)系速度誤差在x、y、z軸的分量,wk是系統(tǒng)噪聲,Δxk-1、Δyk-1、Δzk-1為地心地固坐標(biāo)系下k-1時刻位置誤差在x、y、z軸的分量,Δvx,k-1、Δvy,k-1、Δvz,k-1為k-1時刻地心地固坐標(biāo)系下速度誤差在x、y、z軸的分量;wk為系統(tǒng)噪聲矩陣;(b)導(dǎo)航濾波器模型的觀測方程為:觀測量選取各通道的碼相位測量值和載波頻率測量值,觀測量與狀態(tài)量之間的關(guān)系如下:zk=zcode,kzcarrier,k+vk=HkΔxkΔvx,kΔykΔvy,kΔzkΔvz,kc·tb,kc·td,k+vk---(19)]]>zcode,kn=hxnΔxk+hynΔyk+hznΔzk+c·tb,k+vcode,k---(20)]]>zcarrier,kn=hxnΔvx,k+hynΔvy,k+hxnΔvz,k+c·td,k+vcarrier,k---(21)]]>式中,Hk為觀測矩陣,vk為觀測噪聲矩陣,vcode,k和vcarrier,k為觀測誤差噪聲;E{wk}=0,E{wkwjT}=QkE{vk}=0,E{vkvjT}=RkE{vkwjT}=0---(22)]]>式中:wk和wj為系統(tǒng)模型噪聲;vk和vj為系統(tǒng)觀測噪聲;Qk≥0為系統(tǒng)噪聲方差陣;Rk>0為量測噪聲方差陣;(c)導(dǎo)航濾波器:x^k,k-1=Φk,k-1x^k-1---(23)]]>x^k=x^k,k-1+Kk(zk-Hkx^k,k-1)---(24)]]>Kk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1---(25)]]>Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1---(26)]]>Pk=(I-KkHk)Pk,k-1(27)式中:為k-1時刻到k時刻的狀態(tài)預(yù)測,為k時刻的狀態(tài)估計,預(yù)測Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣,Kk為濾波增益矩陣,Pk,k-1和Pk為協(xié)方差矩陣,zk為輸入的量測值,I為單位矩陣,Rk和Qk-1分別為量測噪聲方差陣和系統(tǒng)噪聲方差陣;定義新息序列計算如下:δk=(zk-Hkx^k,k-1)=(Hkx^k+vk-Hkx^k,k-1)=Hk(x^k-x^k,k-1)+vk---(28)]]>新息方差計算公式如下:Ek=1NΣi=k-N+1kδδT---(29)]]>δk=(Zk-HkΦk,k-1X^k-1)---(30)]]>新息方差的理論定義:Ωk=HkPk-1HkT+Rk---(31)]]>標(biāo)度因子:degk=trace(Ek)trace(Ωk)---(32)]]>當(dāng)標(biāo)度因子大于設(shè)定的閾值的時候就認為存在跟蹤通道影響跟蹤環(huán)路結(jié)果,濾波正常的情況下標(biāo)度因子的值在1附近。然后具體對每個通道進行分析。定義矩陣β:βk(i,i)=Ek(i,i)/Ωk(i,i)(33)濾波正常的時候βk(i,i)的值在1附近,范圍為0.9~1.1。(d)模糊控制器模糊控制的輸入是βk(i,i),輸出是Λk(i,i)。輸入有三個模糊集:S=??;N=正常;B=大。輸出有三個模糊集:S=??;N=正常;B=大。圖2是模糊控制器輸入隸屬函數(shù)圖,圖3是模糊控制器輸出隸屬函數(shù)圖。調(diào)整之后的濾波方程如下:x^k,k-1=Φk,k-1x^k-1---(34)]]>x^k=x^k,k-1+Kk(zk-Hkx^k,k-1)---(35)]]>Kk=Pk,k-1HkT(HkPk,k-1HkT+RkΛk)-1---(36)]]>Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1---(37)]]>Pk=(I-KkHk)Pk,k-1(38)式中:為k-1時刻到k時刻的狀態(tài)預(yù)測,為k時刻的狀態(tài)估計,預(yù)測Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣,Kk為濾波增益矩陣,Pk,k-1和Pk為協(xié)方差矩陣,zk為輸入的量測值,I為單位矩陣,Rk和Qk-1分別為量測噪聲方差陣和系統(tǒng)噪聲方差陣,Λk為量測方差調(diào)整矩陣。步驟4,載波鑒頻器和碼環(huán)鑒別器輸出作為導(dǎo)航濾波器量測信息,用以估計接收機的位置、速度、鐘差和鐘漂誤差,用估計的信息修正接收機位置和速度,然后用修正過的接收機位置和速度結(jié)合衛(wèi)星星歷計算載波和碼NCO,以保持對輸入信號的穩(wěn)定跟蹤。a)載波鑒頻器輸出頻率誤差zcarrier,計算公式如下:zcarrier=cross×sign(dot)2π(t2-t1)(IP22+QP22)---(39)]]>式中,和分別為I通道和Q通道即時支路t1時刻的采樣值,和分別為I通道和Q通道即時支路緊接著t1時刻之后的t2時刻的采樣值,t1為k-1時刻的時間,t2為k時刻的時間,sign(x)為符號函數(shù),取值如下:sign(x)=+1x≥0-1x<0]]>b)碼環(huán)鑒別器:碼鑒別器選取歸一化的非相干超前減滯后模型,得到碼相位測量值:zcode=12·IE2+QE2-IL2+QL2IE2+QE2+IL2+QL2---(40)]]>式中,IE和QE分別為I通道和Q通道超前支路采樣值,IL和QL分別為I通道和Q通道滯后支路采樣值;c)位置和速度直接根據(jù)導(dǎo)航濾波器的估計誤差進行修正:Xk=X^k+δXk---(41)]]>Vk=V^k+δVk---(42)]]>碼相位、碼頻率和載波頻率根據(jù)修正后的位置速度以及導(dǎo)航濾波器估計的鐘差、鐘漂進行修正:τk(n)=τ^k(n)+δXkThk+cT+c·tb,k---(43)]]>fcode,k(n)=f^code,k(n)+(td,k+δVkhk)fcode/c---(44)]]>fcarrier,k(n)=f^carrier,k(n)+(td,k+δVkhk)fcode/c---(45)]]>式中,式中,Xk和Vk分別為k時刻解算得到的接收機位置和速度;和分別為k時刻預(yù)測的接收機位置和速度;T為解算周期;表示位置誤差修正值,δVk表示速度誤差修正值;分別為k時刻的碼相位、碼頻率和載波頻率預(yù)測值;分別為k時刻的碼相位、碼頻率和載波頻率修正之后的值;為衛(wèi)星與用戶之間的視線矢量,c為真空中光速;T為濾波周期,tb,k為k時刻的鐘漂,td,k為k時刻的鐘漂;fcode為C/A碼的基準(zhǔn)頻率,為1.023MHz;fcarrier為載波L1的頻率,為1575.42MHz。本發(fā)明方法具有良好的導(dǎo)航精度和跟蹤穩(wěn)定性,同時當(dāng)部分通道信號出現(xiàn)被遮擋或者載噪比降低時,該方法具有很好的穩(wěn)定性,不會出現(xiàn)導(dǎo)航精度明顯變差和導(dǎo)航濾波器發(fā)散的問題。當(dāng)前第1頁1 2 3