樊龍江，陳安升，陳 帥，韓 林

（1.上海航天電子技術(shù)研究所，上海201109；2.北京自動化控制設(shè)備研究所，北京100074；3.南京理工大學自動化學院，南京210094）

0 引言

北斗導航系統(tǒng)近年來發(fā)展日益成熟,在國民生活中的應用越來越廣泛。定位算法是衛(wèi)星導航的核心算法之一,好的定位解算算法直接決定了定位導航服務的質(zhì)量。

傳統(tǒng)接收機使用的定位算法多為最小二乘法（Least Squares,LS）或擴展 Kalman 濾波（Extended Kalman Filter,EKF）。LS算法在含有噪聲和誤差的量測值中找到一個最優(yōu)值,使所有量測值的殘差平方和最小,但是由于不同時刻的量測誤差與噪聲經(jīng)LS計算后轉(zhuǎn)化為相應時刻的定位誤差和噪聲,且各個時刻的定位解算互相獨立,因而LS的定位結(jié)果顯得既粗糙又雜亂。EKF具有預測功能,Kalman濾波器對于不同歷元的信息共享,其定位結(jié)果的誤差表現(xiàn)為有色噪聲分布,體現(xiàn)在時間上為定位誤差緩慢變化,定位結(jié)果比LS誤差小。但是,EKF對非線性模型進行線性化近似,使得誤差通過線性傳遞導致精度下降,同時需要計算Jacobi矩陣,計算負擔大。為了不增加計算量和算法復雜度,同時改善線性化帶來的高階項誤差,無跡變換被引入Kalman濾波算法中。無跡Kalman濾波（Unscented Kalman Filter,UKF）通過確定的規(guī)則選取采樣點來近似隨機變量的概率分布,從原理上避免了對方程線性化帶來的誤差,并且不需要計算復雜的Jacobi矩陣,與EKF算法相比性能有一定的提升。但UKF算法受初值誤差和系統(tǒng)噪聲影響較大,易出現(xiàn)濾波收斂速度慢、定位精度差等問題。

針對以上問題,本文提出了一種自適應UKF定位算法。該算法利用觀測殘差信息構(gòu)建了自適應漸消矩陣,消除了量測噪聲異常帶來的影響,同時提高了濾波精度。仿真結(jié)果表明,在同等噪聲和狀態(tài)偏差條件下,與傳統(tǒng)的EKF算法和UKF算法相比,AUKF算法在定位精度和魯棒性等方面都有較好的改善。

1 北斗定位原理與模型建立

1.1 北斗定位原理

北斗接收機提供定位導航服務的過程大致如下：接收機通過天線接收衛(wèi)星播發(fā)的無線電信號,射頻模塊將衛(wèi)星信號下變頻至離散數(shù)字中頻信號,基帶處理模塊對中頻信號進行載波剝離和偽碼剝離,解調(diào)出衛(wèi)星信號中的導航電文,最后導航處理器提取導航電文中的星歷信息,捕獲衛(wèi)星大于等于4顆時即可進行定位解算,實時為用戶提供定位導航服務。北斗接收機的系統(tǒng)框圖如圖1所示。

圖1 北斗接收機系統(tǒng)框圖Fig.1 System chart of Beidou receiver

北斗系統(tǒng)與GPS系統(tǒng)定位算法的差異主要體現(xiàn)在衛(wèi)星軌道的分布。不同于GPS系統(tǒng)6個軌道平面均勻分布4～6顆衛(wèi)星,北斗系統(tǒng)由地球靜止軌道衛(wèi)星（GEO）、地球同步軌道衛(wèi)星（IGSO）和中軌衛(wèi)星（MEO）組成。求解衛(wèi)星位置時,北斗MEO/IGSO衛(wèi)星與GPS衛(wèi)星計算方式相同,北斗GEO衛(wèi)星雖然也遵循Kepler定律,但如果按照相同的計算方式,會出現(xiàn)位置無法求解的情況。圖2展示了兩種不同軌道傾角下衛(wèi)星軌道面與赤道面的相交情況。

圖2 不同傾角情況下軌道面相交情況Fig.2 Intersecting condition of track surfaces at different inclination angles

從圖2（b）可以看出,當軌道傾角i較小時,軌道交界線不再是一條明確的連線,而變?yōu)橐粭l模糊帶。極端情況下,當軌道傾角為0°時,衛(wèi)星軌道面和赤道面平行或接近平行,升交點和降交點位置出現(xiàn)奇異性,也就無法給出正確的衛(wèi)星信息。

從GEO衛(wèi)星的問題根源來看,并不是衛(wèi)星軌道有問題,而是軌道的參考面選擇不合理。只需讓衛(wèi)星軌道面不和赤道面平行,就可解決這個問題。為此,只需在產(chǎn)生北斗GEO星歷時,把GEO衛(wèi)星軌道人為地偏轉(zhuǎn)一個角度β,即可避免這個問題。在北斗接收機解算時,針對GEO衛(wèi)星軌道旋轉(zhuǎn)的處理,只需在最后計算衛(wèi)星位置時多乘一個旋轉(zhuǎn)矩陣。計算方式如下

其中,PGEO為最終求解的衛(wèi)星位置,φ（t）為旋轉(zhuǎn)矩陣,Prot為旋轉(zhuǎn)后的衛(wèi)星位置,ωa為地球自轉(zhuǎn)角速度,β為人為軌道偏轉(zhuǎn)角。

解算出衛(wèi)星的位置后,依據(jù)圓-圓定位原理,只需三個未知數(shù)方程即可實現(xiàn)方程求解。又因為接收機解算時存在時間偏差問題,通常將時鐘偏差也作為一個未知量一同求解。構(gòu)建四元非線性方程組,北斗導航定位、定時的本質(zhì)就是求解如下四元非線性方程組

式（3）中,X=[xyz]T為待求接收機的位置向量,X=[xnynzn]T（n∈｛1,2…,N｝）為衛(wèi)星N的位置坐標向量,C為光速,Tu為時鐘偏差,為偽矩測量值。

1.2 模型建立

北斗定位解算系統(tǒng)模型主要由狀態(tài)模型和觀測模型組成。狀態(tài)模型為線性模型,觀測模型需要求解衛(wèi)星的偽矩、偽距率故為非線性模型。為了減少計算,選用系統(tǒng)誤差作為系統(tǒng)狀態(tài)向量,系統(tǒng)模型如下：

（1）狀態(tài)模型

式（4）中,Xk為狀態(tài)向量,F為狀態(tài)轉(zhuǎn)移矩陣,Vk為系統(tǒng)狀態(tài)噪聲。Xk主要由三部分組成,即位置誤差[δxδyδz]、速度誤差[δvxδvyδvz]和北斗時鐘誤差[δtbδt′b]。

（2）觀測模型

本文選取偽矩及偽距率作為系統(tǒng)觀測量,觀測模型如下

式（7）中,H（Xk）為系統(tǒng)觀測非線性函數(shù),Wk為觀測噪聲。

2 AUKF算法分析

2.1 基本UKF算法

在EKF的基礎(chǔ)上,為了不增加系統(tǒng)的計算負擔,同時改進線性化帶來的高階項截斷誤差,UKF算法應運而生。EKF算法通過一階Taylor展開式對系統(tǒng)進行線性化近似,而UKF算法無需對系統(tǒng)求導,不必線性化近似,其算法核心在于無跡變換（Unscented Transformation,UT）原則。 UT 的原理如圖3所示。

圖3 UT原理圖Fig.3 Principle of UT

UT算法首先依據(jù)狀態(tài)向量維數(shù)n選取2n+1個近似Gauss分布、離散分布的Sigma點,其均值為,方差為P。每個Sigma點經(jīng)非線性函數(shù)變換得到非線性函數(shù)點集,經(jīng)加權(quán)處理后得到非線性系統(tǒng)的均值和協(xié)方差。這樣得到的均值和協(xié)方差最少具有2階精度（Taylor展開）。

考慮離散非線性系統(tǒng)

式（8）中,xk為狀態(tài)向量,zk+1為量測向量,f（·）為系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù),h（·）為系統(tǒng)非線性量測函數(shù),wk為系統(tǒng)狀態(tài) Gauss白噪聲,vk為量測Gauss白噪聲,并且滿足下列關(guān)系

式（9）中,Qk、Rk分別為wk、vk的協(xié)方差陣,且對稱、非負定。

UKF算法的具體流程如下：

（1）初始化

設(shè)定系統(tǒng)初始狀態(tài)向量為x0,系統(tǒng)初始狀態(tài)協(xié)方差為P0,初始化條件如下

（2）計算采樣點及采樣權(quán)重

計算2n+1個采樣點,n為狀態(tài)向量的維數(shù)。n≥1時,考慮輸入變量的均值和方差,將和Pk由Sigma點近似。

采樣點權(quán)重計算為

式（13）中,α取值控制采樣點的分布狀態(tài),β在正態(tài)分布時最優(yōu)值為2。

（3）預測方程（4）計算一步預測Sigma點及觀測量

（5）更新方程

2.2 AUKF算法

UKF算法是一種遞推算法,初始值的準確度對UKF算法影響較大,UKF算法運用在非線性系統(tǒng)時,需要獲得系統(tǒng)模型及系統(tǒng)噪聲統(tǒng)計信息。如若系統(tǒng)模型或噪聲統(tǒng)計特性不準確,則可能導致系統(tǒng)估計誤差偏大甚至濾波發(fā)散。真實環(huán)境下,衛(wèi)星信號脆弱,多路徑現(xiàn)象嚴重,難以獲得系統(tǒng)噪聲的統(tǒng)計特性,致使濾波輸出發(fā)散不準確。

針對以上問題,本文根據(jù)自適應濾波原理提出了一種改進的自適應UKF濾波算法。通過采集M步歷元的觀測殘差信息,構(gòu)建了自適應漸消矩陣,實時調(diào)整增益矩陣,從而對系統(tǒng)狀態(tài)估計進行自適應調(diào)節(jié),削弱了系統(tǒng)噪聲偏差帶來的影響。

由實測量測值與一步預測值計算量測殘差

式（17）中,Zk+1為k+1時刻的真實量測值,由BDS提供；為量測估計值。

若系統(tǒng)量測的真實誤差統(tǒng)計特性與濾波遞推誤差模型一致時,有

式（18）中,M為滑動窗口的寬度,表示量測殘差值的采集個數(shù)。

當系統(tǒng)量測出現(xiàn)異常時,量測噪聲與濾波遞推誤差模型不符,需引入自適應矩陣Ak+1

則Ak+1可推導如下

綜合式（19）和式（20）可知, 量測噪聲正常時,Ak+1=I,為單位矩陣,對濾波結(jié)果無影響。

考慮到計算誤差等因素影響,矩陣Ak+1的對角線元素可能小于1。因此,還需對自適應矩陣進一步處理

式（22）中, （Ak+1）ii表示Ak+1矩陣的第i個主對角元素。當量測噪聲突變時,會對濾波增益矩陣產(chǎn)生影響。

當系統(tǒng)某一維量測噪聲突變時,自適應矩陣Ak對應項將增大,濾波增益相應減小,使得異常的量測值對系統(tǒng)影響較小。因此,即使量測向量中出現(xiàn)多個異常值,系統(tǒng)仍能做出相應的調(diào)節(jié),減小異常量測帶來的影響,獲得相對準確的濾波結(jié)果。

3 仿真試驗

為了驗證AUKF算法的性能,利用實驗室自主研制的北斗原理樣機進行模擬器仿真試驗,試驗地點為南京理工大學導航實驗室,模擬器仿真軌跡為速度400m/s、加速度4g、高度800m的低軌圓周運動。將原理樣機采集到的觀測數(shù)據(jù)分別使用EKF算法、UKF算法和AUKF算法進行處理,計算結(jié)果與模擬器標準軌跡進行對比,三個方向上的位置誤差曲線如圖4～圖6所示。

圖4 X方向位置誤差Fig.4 Position error of X-direction

圖5 Y方向位置誤差Fig.5 Position error of Y-direction

圖6 Z方向位置誤差Fig.6 Position error of Z-direction

從圖4～圖6可以看出,AUKF算法定位精度明顯高于UKF算法和EKF算法。為了更直觀地比較三種算法的性能,采用均方根誤差（Root Mean Square Error,RMSE）進行定量分析,結(jié)果如表1所示。

表1 三種濾波算法的均方根誤差Table 1 RMSE of EKF，UKF and AUKF

由表1可知,各項誤差指標中,AUKF的精度比EKF和UKF的精度都高。

為了驗證AUKF算法的魯棒性,在以上試驗的基礎(chǔ)上,在仿真過程60s～70s處,量測信息加入均值為0、標準差為50的Gauss白噪聲。比較EKF算法、UKF算法、AUKF算法在相同條件下抑制量測噪聲的能力,如圖7～圖9所示。

針對60s～70s處進行濾波性能分析,仿真結(jié)果表明,與EKF算法、UKF算法相比,所設(shè)計的AUKF算法收斂速度更快,在系統(tǒng)量測噪聲突變時,能夠快速地消除系統(tǒng)量測噪聲突變帶來的影響,具有較好的魯棒性。

圖7 X方向位置誤差Fig.7 Position error of X-direction

圖8 Y方向位置誤差Fig.8 Position error of Y-direction

圖9 Z方向位置誤差Fig.9 Position error of Z-direction

4 結(jié)論

本文介紹了一種自適應UKF算法在北斗定位解算中的運用。該算法依據(jù)自適應原理構(gòu)建自適應漸消修正矩陣,提高了UKF算法的定位精度和收斂速度,增強了系統(tǒng)的魯棒性。仿真結(jié)果表明,與傳統(tǒng)EKF算法和UKF算法相比,AUKF算法具有較好的定位精度和收斂速度,在外界擾動噪聲較大時仍有較好的定位導航性能,為北斗接收機提供了一種更穩(wěn)定的定位解算算法。