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

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

0 引言

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

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

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

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

1.1 北斗定位原理

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

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

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

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

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

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

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

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

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

1.2 模型建立

北斗定位解算系統(tǒng)模型主要由狀態(tài)模型和觀測(cè)模型組成。狀態(tài)模型為線性模型,觀測(cè)模型需要求解衛(wèi)星的偽矩、偽距率故為非線性模型。為了減少計(jì)算,選用系統(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]和北斗時(shí)鐘誤差[δtbδt′b]。

（2）觀測(cè)模型

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

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

2 AUKF算法分析

2.1 基本UKF算法

在EKF的基礎(chǔ)上,為了不增加系統(tǒng)的計(jì)算負(fù)擔(dān),同時(shí)改進(jìn)線性化帶來的高階項(xiàng)截?cái)嗾`差,UKF算法應(yīng)運(yùn)而生。EKF算法通過一階Taylor展開式對(duì)系統(tǒng)進(jìn)行線性化近似,而UKF算法無需對(duì)系統(tǒng)求導(dǎo),不必線性化近似,其算法核心在于無跡變換（Unscented Transformation,UT）原則。 UT 的原理如圖3所示。

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

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

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

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

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

UKF算法的具體流程如下：

（1）初始化

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

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

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

采樣點(diǎn)權(quán)重計(jì)算為

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

（3）預(yù)測(cè)方程（4）計(jì)算一步預(yù)測(cè)Sigma點(diǎn)及觀測(cè)量

（5）更新方程

2.2 AUKF算法

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

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

由實(shí)測(cè)量測(cè)值與一步預(yù)測(cè)值計(jì)算量測(cè)殘差

式（17）中,Zk+1為k+1時(shí)刻的真實(shí)量測(cè)值,由BDS提供；為量測(cè)估計(jì)值。

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

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

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

則Ak+1可推導(dǎo)如下

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

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

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

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

3 仿真試驗(yàn)

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

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

由表1可知,各項(xiàng)誤差指標(biāo)中,AUKF的精度比EKF和UKF的精度都高。

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

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

圖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é)論

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