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