曹學(xué)瑤， 胡黃水， 韓 博

(長春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院，吉林 長春 130012)

0 引 言

北斗衛(wèi)星導(dǎo)航系統(tǒng)(以下稱為北斗系統(tǒng))是我國自主創(chuàng)建研發(fā)的衛(wèi)星導(dǎo)航系統(tǒng)，也是全球第三個(gè)成熟的導(dǎo)航系統(tǒng)。在交通運(yùn)輸、農(nóng)業(yè)林業(yè)、水力監(jiān)測等領(lǐng)域都得到了普遍應(yīng)用[1-2]。由于北斗系統(tǒng)可以為用戶端提供全天候、全天時(shí)的高精度定位服務(wù)，所以提高北斗定位精度極其重要。

根據(jù)觀測量的不同，北斗定位可以分為偽距定位和載波相位定位[3]。相比載波相位定位來說,偽距定位不要求高強(qiáng)度信號(hào)，也不存在整周模糊度的問題,并且偽距定位速度較快[4]。隨著定位技術(shù)的日漸成熟，國內(nèi)外專家對(duì)偽距定位算法展開了深入研究，目前的主流算法有極大似然估計(jì)、最小二乘法、卡爾曼濾波和牛頓迭代法等。文獻(xiàn)[5]提出加權(quán)最小二乘法，對(duì)可信度高的觀測值分配大的權(quán)值。但加權(quán)最小二乘法對(duì)觀測數(shù)據(jù)中的粗差不具備處理能力，所以定位誤差大。文獻(xiàn)[6]采用抗差M估計(jì)算法，將最小二乘算法的解作為抗差初值，然后,根據(jù)偽距殘差矢量和等價(jià)權(quán)矩陣進(jìn)行計(jì)算位置坐標(biāo)。和最小二乘法相比，抗差M估計(jì)定位精度更高。文獻(xiàn)[7]采用卡爾曼濾波進(jìn)行定位，直接將位置與鐘差的信息作為狀態(tài)變量求解,忽略了卡爾曼濾波對(duì)定位的初始位置敏感的特點(diǎn)，導(dǎo)致了定位精度不高。文獻(xiàn)[8]提出在擴(kuò)展卡爾曼濾波的基礎(chǔ)上加入加權(quán)最小二乘法。首先,利用加權(quán)最小二乘法對(duì)系統(tǒng)進(jìn)行線性化處理后,用擴(kuò)展卡爾曼濾波對(duì)用戶端位置進(jìn)行預(yù)測。該算法相對(duì)于文獻(xiàn)[7]提升了線性化的精度，但由于觀測量中粗差和卡爾曼濾波中量測噪聲固定不變的影響，難以滿足高精度定位要求。

為了解決最小二乘法對(duì)觀測數(shù)據(jù)中的粗差無法進(jìn)行處理,以及卡爾曼濾波對(duì)初始位置敏感且量測噪聲方差不變的問題，提出融合抗差M估計(jì)和模糊卡爾曼濾波算法進(jìn)行偽距定位。首先,利用抗差M估計(jì)解算出用戶端的位置，然后將解算結(jié)果作為卡爾曼濾波的初始狀態(tài)位置,根據(jù)模糊控制系統(tǒng)不斷調(diào)節(jié)濾波增益Kk值進(jìn)行定位解算，從而提高定位精度。

1 偽距定位原理

偽距定位原理是通過北斗導(dǎo)航系統(tǒng)中導(dǎo)航衛(wèi)星的三維位置坐標(biāo)信息,以及衛(wèi)星到接收端的距離得到用戶端的三維位置坐標(biāo)信息的過程[9]。其中,導(dǎo)航衛(wèi)星的三維位置坐標(biāo)信息可以通過導(dǎo)航電文中的星歷數(shù)據(jù)獲得，由于衛(wèi)星發(fā)射的信號(hào)會(huì)受到各種誤差的干擾,導(dǎo)致用戶端接收到的觀測距離并不準(zhǔn)確。將帶有誤差的觀測距離稱為偽距[10],公式如下：

ρi=ri+δρ1+δρ2+c(δtu-δt1)，

(1)

(2)

式中:ri——第i顆衛(wèi)星與用戶端的真實(shí)距離：

ρi——衛(wèi)星與用戶設(shè)備之間的真實(shí)距離；

δtu——電流層的時(shí)延距離；

δρ1——對(duì)流層的時(shí)延距離；

δt1——用戶設(shè)備的鐘差；

δρ2——衛(wèi)星的鐘差；

c——信號(hào)的傳播速度,c=2.997 924 58 m/s;

(xi,yi,zi)——第i顆衛(wèi)星的三維坐標(biāo)；

(x,y,z)——用戶接收機(jī)的位置坐標(biāo)。

忽略可修正項(xiàng)，偽距定位公式為

ρi=ri+c(δtu-δt1)=

cδt,

(3)

i=1,2,…,n，

式中：δt——接收機(jī)的鐘差。

2 算法設(shè)計(jì)

利用抗差M估計(jì)構(gòu)造等價(jià)權(quán)函數(shù)，避免粗差影響未知參數(shù)的估值。將該估值作為模糊卡爾曼濾波器的初始值，并對(duì)量測噪聲方差實(shí)時(shí)進(jìn)行調(diào)整，進(jìn)而提高系統(tǒng)的定位精度。

2.1 抗差M估計(jì)計(jì)算初值

在北斗導(dǎo)航系統(tǒng)中，最小二乘法是偽距定位系統(tǒng)的觀測方程。初始值

x0=(x0,y0,z0,δt0)

處線性化后得到的系統(tǒng)觀測模型為

Δy=H·Δx+e,

(4)

式中：Δx——m×1的矢量，表示狀態(tài)殘差，即真實(shí)位置與初始化后得到的位置之間的差值;

m——當(dāng)前歷元下的衛(wèi)星個(gè)數(shù);

Δy——m×1的矢量，表示偽距殘差,即偽距的觀測值與估計(jì)值之間的差值；

H——m×4的觀測矩陣；

e——m×1的誤差矢量，其元素一般假設(shè)為相互獨(dú)立的高斯隨機(jī)過程。

首先用最小二乘法解算此線性方程，目標(biāo)函數(shù)為

(5)

初始值處的最小二乘解為

(6)

偽距殘差矢量為

Δr=Δy-HΔx=(In-H(HTH)-1HT)e。

(7)

當(dāng)測量噪聲為高斯分布時(shí)，最小二乘估計(jì)為最優(yōu)估計(jì)[11-13]。但在北斗導(dǎo)航系統(tǒng)中，觀測量中含有粗差會(huì)直接影響最小二乘定位精度，因此采用M估計(jì)抑制粗差[14]，其目標(biāo)函數(shù)為

(8)

(9)

采用Huber[15]函數(shù)給含有粗差的觀測值賦予小的權(quán)值

(10)

式中:k——根據(jù)M估計(jì)方差性能確定的常數(shù)。

抗差M估計(jì)表達(dá)式為

(11)

式中：Wk+1——等價(jià)權(quán)矩陣，公式為

(12)

抗差M估計(jì)定位的具體步驟為：

4)返回2)，直到兩相鄰步驟的回歸系數(shù)差值小于設(shè)定閾值時(shí)，迭代結(jié)束，即

max|xk+1-xk|<ξ。

2.2 模糊自適應(yīng)Kalmen濾波定位

擴(kuò)展卡爾曼濾波有兩個(gè)系統(tǒng)模型[16]：系統(tǒng)觀測模型以及系統(tǒng)狀態(tài)模型。

狀態(tài)方程

xk=Ak,k-1xk-1+ωk-1,

(13)

式中：xk——?dú)v元k下的系統(tǒng)狀態(tài)向量；

xk-1——?dú)v元k-1下的系統(tǒng)狀態(tài)向量;

Ak,k-1——?dú)v元k-1到歷元k的狀態(tài)轉(zhuǎn)移矩陣;

ωk-1——高斯白噪聲，均值為零，且相互獨(dú)立。

觀測方程

yk=Bkxk+vk,

(14)

式中：yk——?dú)v元k的觀測量；

Bk——狀態(tài)量和觀測量之間的關(guān)系矩陣；

vk——高斯白噪聲，均值為零，且相互獨(dú)立。

擴(kuò)展卡爾曼濾波包含預(yù)測和更新兩個(gè)過程[16-17]，其預(yù)測方程為：

狀態(tài)先驗(yàn)估計(jì)值

(15)

求解先驗(yàn)估計(jì)值的協(xié)方差

(16)

式中：Qk——系統(tǒng)噪聲序列的方差陣。

其更新方程為：

濾波增益矩陣

(17)

式中：Rk——測量噪聲序列的方差陣。

對(duì)狀態(tài)更新

(18)

對(duì)協(xié)方差陣更新

(19)

量測噪聲具有隨機(jī)性[18],但擴(kuò)展卡爾曼濾波中量測噪聲的方差始終都采用初始計(jì)算時(shí)的先驗(yàn)值[19],因此,采用模糊卡爾曼濾波[17]調(diào)整量測噪聲的協(xié)方差矩陣[20]來減小濾波發(fā)散。

新息(rk)是觀測量估計(jì)值與觀測量真實(shí)值的差，公式為

rk=yk-Bkxk,k-1。

(20)

式(18)可以寫為

(21)

(22)

(23)

由模糊推理系統(tǒng)(FIS)可以計(jì)算出調(diào)整因子tk。tk可以對(duì)量測噪聲方差進(jìn)行調(diào)整。模糊推理系統(tǒng)[21]采用單輸入、單輸出Mamdani模糊邏輯控制器[22]，將每一步的殘差實(shí)測方差與理論方差的比值作為輸入，輸出為tk。采用的隸屬度函數(shù)如圖1所示。

圖1 Ck與tk的隸屬度函數(shù)

模糊規(guī)則為：

If1>,then1>

If,then

If,then

Fk殘差的實(shí)測方差陣受新息影響,

(24)

式中：N——統(tǒng)計(jì)數(shù)。

Dk是殘差的理論方差陣，

(25)

Ck是殘差的實(shí)測方差矩陣和殘差的理論方差矩陣的商,

(26)

每進(jìn)行一次濾波FIS更新一次輸出值,即tk，再將tk代入式(22)、式(23)中,對(duì)量測噪聲方差矩陣進(jìn)行在線調(diào)整。

因此,基于M估計(jì)的模糊卡爾曼濾波定位解算步驟如下:

1)將M估計(jì)解算位置和鐘差信息作為擴(kuò)展卡爾曼濾波的狀態(tài)初始值，然后初始化協(xié)方差陣;

2)建立模糊卡爾曼濾波系統(tǒng)方程，對(duì)狀態(tài)向量和觀測向量協(xié)方差陣進(jìn)行估計(jì)。通過FIS計(jì)算調(diào)整因子tk，然后進(jìn)行預(yù)測和更新過程;

3)判斷歷元是否解算完畢，未完成返回2),繼續(xù)解算。

3 仿真分析

采用Matlab軟件對(duì)RINEX文件數(shù)據(jù)[23]進(jìn)行解算。在觀測文件為 450個(gè)歷元數(shù)據(jù)的情況下，分別采用擴(kuò)展卡爾曼濾波,以及模糊自適應(yīng)卡爾曼濾波方法對(duì)X、Y、Z三 個(gè)方向進(jìn)行解算，然后再將定位結(jié)果進(jìn)行對(duì)比分析。誤差對(duì)比如圖2所示。

(a)擴(kuò)展卡爾曼濾波算法在X、Y、Z三個(gè)方向的誤差結(jié)果

圖2中,X、Y、Z三個(gè)方向的平均誤差分別為-3.96、-8.96、-13.47 m，明顯可以看出，模糊卡爾曼濾波算法的誤差更小，濾波結(jié)果也更加平滑。模糊卡爾曼濾波的定位解算結(jié)果相較于擴(kuò)展卡爾曼濾波的定位精度有明顯提高，相較于圖2(a)的收斂過程也明顯縮短，實(shí)現(xiàn)了更加精確的定位。

利用Matlab軟件將擴(kuò)展卡爾曼濾波與模糊卡爾曼濾波的運(yùn)動(dòng)軌跡與接收機(jī)真實(shí)軌跡進(jìn)行模擬。

將擴(kuò)展卡爾曼濾波、真實(shí)軌跡以及模糊卡爾曼濾波進(jìn)行誤差對(duì)比，如圖3所示。

圖3 擴(kuò)展卡爾曼濾波、真實(shí)軌跡以及模糊卡爾曼濾波誤差對(duì)比

由圖3可以看出，擴(kuò)展卡爾曼濾波與真實(shí)值之間存在的誤差較大,且擬合度不高; 而模糊卡爾曼濾波運(yùn)動(dòng)軌跡更加集中，而且均勻分布在真實(shí)軌跡四周，擬合程度較好。故模糊卡爾曼濾波對(duì)定位誤差具有極好的改進(jìn)。

4 結(jié) 語

結(jié)合M估計(jì)和模糊卡爾曼濾波的定位解算算法，將抗差M估計(jì)的定位結(jié)果作為擴(kuò)展卡爾曼濾波算法的初值，通過模糊推理系統(tǒng)得到的調(diào)整因子來調(diào)整量測噪聲抑制濾波發(fā)散情況，進(jìn)而提高定位精度。解決了傳統(tǒng)最小二乘算法對(duì)粗差不具抵抗力，以及卡爾曼濾波收斂過程慢且對(duì)初值敏感的缺點(diǎn)。

通過 Matlab 處理數(shù)據(jù)的實(shí)驗(yàn)結(jié)果也表明，模糊卡爾曼濾波算法的收斂速度與定位精度均明顯提高。