自適應UKF在北斗定位算法中的應用

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

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

0 引言

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

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

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

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

1.1 北斗定位原理

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

圖1 北斗接收機系統框圖Fig.1 System chart of Beidou receiver

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

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

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

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

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

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

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

1.2 模型建立

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

（1）狀態模型

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

（2）觀測模型

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

式（7）中,H（Xk）為系統觀測非線性函數,Wk為觀測噪聲。

2 AUKF算法分析

2.1 基本UKF算法

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

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

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

考慮離散非線性系統

式（8）中,xk為狀態向量,zk+1為量測向量,f（·）為系統狀態轉移函數,h（·）為系統非線性量測函數,wk為系統狀態 Gauss白噪聲,vk為量測Gauss白噪聲,并且滿足下列關系

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

UKF算法的具體流程如下：

（1）初始化

設定系統初始狀態向量為x0,系統初始狀態協方差為P0,初始化條件如下

（2）計算采樣點及采樣權重

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

采樣點權重計算為

式（13）中,α取值控制采樣點的分布狀態,β在正態分布時最優值為2。

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

（5）更新方程

2.2 AUKF算法

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

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

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

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

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

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

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

則Ak+1可推導如下

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

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

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

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

3 仿真試驗

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

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

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

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

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

圖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 結論

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