基于廣義卡爾曼濾波的室內(nèi)定位技術(shù)

基于廣義卡爾曼濾波的室內(nèi)定位技術(shù)

gps高穩(wěn)定性免疫復(fù)雜模型輔助導(dǎo)航現(xiàn)代戰(zhàn)爭對武器的偽裝要求很高，因此，利用雷達監(jiān)測雷達設(shè)備的非自主導(dǎo)航受到限制，以常規(guī)導(dǎo)航和gps衛(wèi)星導(dǎo)航為代表的自主導(dǎo)航方法得到了廣泛應(yīng)用。對于攻擊距離遠,飛行時間較長的導(dǎo)彈而言,所采用的導(dǎo)航方法主要有慣導(dǎo),GPS,以及地形匹配等。匹配法主要用于攻擊末段,可以得到較高精度的導(dǎo)航信息,但會受到地形限制,當(dāng)飛行器在海面或大氣層外飛行時就無法使用;GPS是目前最受關(guān)注的導(dǎo)航系統(tǒng),精度可以達到m級,但畢竟需要接收外界的導(dǎo)航信息,在實戰(zhàn)中有可能會受到干擾;慣導(dǎo)系統(tǒng)不用接收任何外界信息,完全依靠飛行器內(nèi)部慣性器件完成導(dǎo)航,具有最強的自主性,然而受到陀螺漂移的影響,使其產(chǎn)生每小時數(shù)公里的偏差,無法實現(xiàn)長時間的導(dǎo)航。最理想的導(dǎo)航方法,應(yīng)該做到完全自主且可以根據(jù)外部條件變化進行實時修正。利用地磁場強度進行導(dǎo)航,可以完全依靠彈上設(shè)備完成,并且不會出現(xiàn)慣導(dǎo)系統(tǒng)的累積誤差。事實上,地磁場早已在用衛(wèi)星的姿態(tài)控制方面,我國的技術(shù)人員還成功運用地磁場與重力場挽救了失控的風(fēng)云一號B星。隨著地磁場模型的日趨完善以及微處理器和濾波技術(shù)的不斷發(fā)展和成熟,地磁導(dǎo)航方法于近幾年得以迅速發(fā)展,將地磁場導(dǎo)航運用在小成本衛(wèi)星上的工作正在進行中。然而,由于地磁場模型不準確,地磁測量器件精度低等缺點,導(dǎo)致地磁場導(dǎo)航在高精度要求的導(dǎo)彈導(dǎo)航方面的研究很少,尚處于探索階段。本文在之前研究的基礎(chǔ)上,采用地磁場矢量作為觀測量,獲取導(dǎo)航信息并進行分析。1地磁矢量觀測地磁導(dǎo)航,可以使用地磁強度矢量或標(biāo)量作為觀測量。如果使用標(biāo)量,導(dǎo)航系統(tǒng)比較簡單,由于測量信息少,不能保證濾波穩(wěn)定,當(dāng)飛行器在等磁線上飛行時無法作出正確估計。為了確保系統(tǒng)的穩(wěn)定性,需要使用地磁矢量作為觀測量。借鑒慣導(dǎo)的原理,將三軸磁強計穩(wěn)定在一個導(dǎo)航坐標(biāo)系下,通過測量計算得到東北天三個固定方向上面的地磁場強度,將其作為濾波觀測方程,使用廣義卡爾曼濾波來估計飛行器的當(dāng)前位置。對比慣導(dǎo)系統(tǒng),由于測量到的地磁場強度會隨時間位置變化而更新,就可以避免初值和積累誤差,而又不失其自主性。1.1航跡系3軸加速度測量這種方法需要的觀測值是地磁強度,而地磁場模型是在地心系中建立的,為了把飛行器運動信息與場模型聯(lián)系,在地心坐標(biāo)系下建立飛行器運動方程。運動方程變量選取飛行器的地心距R,東經(jīng)L,余緯λ(緯度的余角),以及飛行器的速度,航跡傾角和航跡偏角,即V,θ,ψV,根據(jù)運動關(guān)系及選取R,L,λ的意義推導(dǎo)飛行器運動方程。飛行器質(zhì)心運動方程在航跡系上投影的3個方程式為:{dVdt=aV-gsinθVdθdt=aθ-gcosθ-VcosθdψVdt=aψ(1)?????????dVdt=aV?gsinθVdθdt=aθ?gcosθ?VcosθdψVdt=aψ(1)其中,aV,aθ,aψ分別表示飛行器加速度測量裝置在航跡系3軸方向上測得的加速度。分析地面坐標(biāo)系與地心坐標(biāo)系的關(guān)系,以及地面系與航跡固連系的坐標(biāo)變換關(guān)系可以得到式(2):{˙R=VsinθR˙L=-VcosθsinψVRsinL˙λ=VcosθcosψV(2)???????R˙=VsinθRL˙=?VcosθsinψVRsinLλ˙=VcosθcosψV(2)整理以上6個方程,取方程(2)中的地心距R=r+h,其中r為參考地球半徑,其值取6738km,h為飛行器飛行高度,同時考慮到可能產(chǎn)生的噪聲加速度,可得運動模型如下:{˙h=Vsinθ˙L=-VcosθsinψVr+h˙λ=-VcosθcosψV(r+h)sinL˙V-aV-gsinθ+wV˙θ=1V(aθ-gcosθ)+1Vwθ˙ψV=-1Vcosθaψ-1Vcosθwψ(3)?????????????????????????????h˙=VsinθL˙=?VcosθsinψVr+hλ˙=?VcosθcosψV(r+h)sinLV˙?aV?gsinθ+wVθ˙=1V(aθ?gcosθ)+1Vwθψ˙V=?1Vcosθaψ?1Vcosθwψ(3)令x=[h,L,λ,V,θ,ψV]T,噪聲w=[wV,wθ,wψ]T,將u=[aV,aθ,aψ]T看作輸入,則運動模型的狀態(tài)方程形式為:1.2地磁場強度分量使用地磁場強度作為觀測方程,首先要獲得地磁場模型。高斯對地球磁場的大量測量數(shù)據(jù)進行研究后,提出用球諧波分析方法來研究,類似于引力位提出地球磁場磁位。地磁場模型可表示為:W(R,L?λ)=a∞∑n=1n∑m=0[(aR)n+1(gmncosmL+hmnsinmL)Ρmn(cosλ)](4)式中符號的意義如下:W:磁勢,矢量,無法直接測量得到;R:地心距;L:東經(jīng);λ:余緯,即緯度的余角;a:地球半徑;gmn,hmn:高斯系數(shù),由國際地磁參考場(IGRF)給出;Pmn(cosλ)是n次m階的締合Legendre多項式。磁勢是一個矢量場,并且無法測量,而導(dǎo)航需要得到東北天三個方向的地磁強度值。對W進行分解計算得到天向,北向,東向方向的地磁場強度分量:B=[BR,Bλ,BL]=[?W?R1R?W?λ-1Rsinλ?W?L]B表示地磁場強度矢量,BR,Bλ,BL是在天向,北向,東向方向的分量。代入計算可得:{BR=-∞∑n=1n∑m=0[(n+1)(aR)n+2(gmncosmL+hmnsinmL)Ρmn(cosλ)]Bλ=∞∑n=1n∑m=0[(aR)n+2(gmncosmL+hmnsinmL)?dΡmn(cosλdλ]BL=∞∑n=1n∑m=0[(aR)n+2(gmnsinmL-hmncosmL)?msinλΡmn(cosλ)](5)以上3式為地磁場強度分量表達式,用來作為濾波估計的觀測方程。在實際濾波中,可以根據(jù)需要來給出可以滿足需求的模型階數(shù)。國際地磁參考場(IGRF)的誤差有100多nT(有的地方達200多nT),此外,地磁場的長期變化對于導(dǎo)航的影響是極其微弱的,長期變化較大的中心,變化幅度為180nT/年,相對于導(dǎo)彈的飛行時間,這種變化基本可以忽略。而目前比較好的磁強計精度可以達到10nT。取狀態(tài)變量:x=[h,L,λ,V,θ,ψV]T,z=[BR,Bλ,BL]T考慮到模型與測量器件噪聲,令v=[vR,vλ,vL]T,則觀測方程為:z(t)=h(x(t),t)+v(t)2離散系統(tǒng)噪聲濾波算法根據(jù)之前的分析與推導(dǎo),濾波系統(tǒng)模型如下:{˙x(t)=f(x(t),t)+g(x(t),t)u(t)+g(x(t),t)w(t)z(t)=h(x(t),t)+v(t)對此系統(tǒng)進行廣義卡爾曼濾波,需要計算狀態(tài)轉(zhuǎn)移矩陣Φk+1,k和測量矩陣Hk:Φk+1,k=eΤF(tk)=Ι+ΤFk+Τ22!F2k+?Fk=?f(x(tk),tk)?x(tk)Ηk=?h(x(tk),tk)?x(tk)離散系統(tǒng)的噪聲項統(tǒng)計特性為:E[Wk]=0,E[WkWΤj]=QδΤjE[Vk]=0,E[VkVΤj]=RδΤj根據(jù)廣義的卡爾曼濾波算法,濾波方程如下:(Xk/k-1=(Xk-1+{f[(Xk-1?tk-1]+g[(Xk-1?tk-1]?u[tk-1]}Τ(6)(Xk=(Xk/k-1+Κk{Ζk-h[(Xk/k-1,tk]}(7)Kk=Pk/k-1HΤk[HkPk/k-1HΤk+Rk]-1(8)Pk/k-1=Φk/k-1Pk-1ΦΤk,k-1+Qk-1(9)Pk=[I-KkHk]Pk/k-1(10)濾波初始條件為:(X0=E[X0]=mX0,Ρ0=CX03仿真結(jié)果分析在之前模型的基礎(chǔ)上,使用國際地磁場模型(IGRF)2005所給出的高斯系數(shù),按照巡航導(dǎo)彈巡航段的飛行特性,對導(dǎo)航算法進行仿真驗證。假設(shè)飛行器在無機動條件下平飛,飛行高度50m,飛行速度為300m/s,初始經(jīng)度和緯度均為45°,航跡傾角為0,偏角-50°,在各狀態(tài)均有初始誤差的情況下進行濾波,觀測方程使用2階地磁場模型,模型噪聲取100nT,磁強計測量噪聲取10nT,取加速度噪聲各分量的標(biāo)準差為:[1m/s2,1m/s2,1m/s2],仿真時間為3000s,步長1s,所得估計結(jié)果如下:對仿真結(jié)果進行分析:①對狀態(tài)的估計收斂性較好;②濾波達到穩(wěn)定后,經(jīng)度估計偏差的標(biāo)準差為0.03°,緯度估計偏差的標(biāo)準差為0.04°,按照2δ(95%置信度)換算成實際距離,則經(jīng)度方向偏差約6km,緯度方向偏差約8km;③高度估計穩(wěn)定后的偏差小于2000m;該算法采用了地磁場強度矢量作為觀測量,參考信息多,算法具有良好的穩(wěn)定性,但在位置的估計上面誤差還有些大。運動方程中的加速度噪聲影響一般比較小,誤差主要是由觀測方程噪聲所引起的。由于目前地磁場模型測量精度有限,在此條件下所得的估計精度還不高。隨著地磁場模型的精度提高,該算法對位置的估計精度可以得到大幅提高。在只考慮磁強計的噪聲的條件下進行仿真,則高度估計誤差降為300m,經(jīng)緯度的估計偏差可以達到0.01°左右,換算成實際距離,則經(jīng)度和緯度方向的估計偏差約為1km～1.5km。4基于國際地磁場模型的濾波將地磁場強度作為矢量考慮,通過測量東北天三個方向上面地磁強度作為觀測量來進行濾波。由于觀測值有3個,參考信息多,因此算法的穩(wěn)定性比較好,不要求初值準確,能夠克服噪聲的突變。然而這樣需要平臺將磁強計穩(wěn)定在導(dǎo)航坐標(biāo)系中,實現(xiàn)起來比較復(fù)雜。為了簡化系統(tǒng),可以考慮使用地磁強度標(biāo)量來濾波,使用測量到的三個垂直方向的磁場強度計算得到當(dāng)?shù)氐牡卮艔姸却笮∽鳛橛^測值,但這種方法穩(wěn)定性不好,當(dāng)飛行器在等磁線上飛行時無法做出正確估計,必須使用其他參考信息來輔助導(dǎo)航。為了體現(xiàn)一般性,使研究的導(dǎo)航方法適用與全球各處,本文使用國際地磁