混沌粒子群優(yōu)化擴(kuò)展卡爾曼濾波北斗偽距定位

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

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

0 引 言

北斗衛(wèi)星導(dǎo)航系統(tǒng)是我國(guó)自行研制、獨(dú)立運(yùn)行的全球衛(wèi)星導(dǎo)航系統(tǒng)。和美國(guó)的GPS、俄羅斯的“格洛納斯”(GLO-NASS)以及歐洲的“伽利略”系統(tǒng)并列為全球四大衛(wèi)星定位系統(tǒng)[1]。目前,北斗衛(wèi)星導(dǎo)航系統(tǒng)已具備亞太區(qū)域?qū)Ш椒?wù)能力。

偽距定位具有易于實(shí)現(xiàn)、速度快、不存在整周模糊度等優(yōu)點(diǎn)，具有很大的應(yīng)用價(jià)值。目前,偽距定位算法普遍采用最小二乘算法、卡爾曼濾波算法(Kalman filtering)等。在偽距定位算法中，最小二乘算法是一種相對(duì)簡(jiǎn)單且廣泛的算法。但最小二乘算法在定位中無法處理數(shù)據(jù)中的粗差且計(jì)算量大，所以,利用最小二乘定位算法使得定位精度下降[2]。卡爾曼濾波算法通過預(yù)測(cè)方程和更新方程對(duì)目標(biāo)進(jìn)行最優(yōu)狀態(tài)估計(jì)[3-4]，由于觀測(cè)數(shù)據(jù)中的量測(cè)噪聲和系統(tǒng)噪聲對(duì)解算存在干擾，所以卡爾曼濾波解算過程也是濾波過程?？柭鼮V波算法在導(dǎo)航及高精度衛(wèi)星定位領(lǐng)域也得到廣泛應(yīng)用。文獻(xiàn)[5]提出基于卡爾曼濾波方法的BDS偽距單點(diǎn)定位算法，相較于最小二乘算法，卡爾曼濾波算法的定位精度有明顯提高。針對(duì)卡爾曼濾波算法在狀態(tài)轉(zhuǎn)移、觀測(cè)等非線性條件下無法保持正態(tài)性問題，文獻(xiàn)[6-7]提出擴(kuò)展卡爾曼濾波定位算法。擴(kuò)展卡爾曼濾波算法使用雅克比矩陣代替卡爾曼濾波中的線性矩陣，使擴(kuò)展卡爾曼濾波在非線性條件下可以保持正態(tài)性，進(jìn)而提高定位精度，文獻(xiàn)[8-9]提出粒子群優(yōu)化擴(kuò)展卡爾曼濾波算法，首先采用具有最小均方誤差估計(jì)效果的 EKF 濾波算法，其次加入粒子群算法來優(yōu)化EKF 噪聲矩陣,進(jìn)而提高定位精度。

但粒算子群算法(Particle Swarm Optimization, PSO)在粒子尋優(yōu)過程中具有隨機(jī)性，導(dǎo)致粒子無法收斂到全局的最佳位置且易于陷入局部極值。該缺陷使算法不能實(shí)現(xiàn)最優(yōu)化并影響算法精度，不能滿足高精度的定位要求。因此,提出混沌粒子群優(yōu)化擴(kuò)展卡爾曼濾波算法，在傳統(tǒng)粒子群的基礎(chǔ)上添加混沌擾動(dòng)，擴(kuò)大粒子群的應(yīng)用范圍，擺脫局部最優(yōu)，進(jìn)而提高收斂速度和定位精度。

1 偽距定位算法

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

i=1,2,…,n,

(3)

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

2 EKF 濾波算法

北斗導(dǎo)航系統(tǒng)的觀測(cè)模型和狀態(tài)模型為：

狀態(tài)方程

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

(4)

觀測(cè)方程

yk=Bkxk+vk,

(5)

式中：xk----歷元k的系統(tǒng)狀態(tài)向量;

xk-1----歷元k-1的系統(tǒng)狀態(tài)向量；

Ak,k-1----歷元k-1 到歷元k的狀態(tài)轉(zhuǎn)移矩陣；

yk----歷元k的觀測(cè)量；

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

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

EKF濾波過程公式如下：

預(yù)測(cè)方程

(6)

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

(7)

濾波增益矩陣

(8)

狀態(tài)更新方程

(9)

協(xié)方差陣更新方程

(10)

3 混沌粒子群算法

3.1 粒子群算法

粒子群算法是1995年由Eberhart和Kennedy開發(fā)的一種進(jìn)化優(yōu)化算法，其靈感來自對(duì)鳥群覓食行為的研究[11]。粒子群算法用一個(gè)隨機(jī)的個(gè)體群體進(jìn)行初始化，其中每個(gè)個(gè)體被稱為具有位置和速度的粒子。當(dāng)已知粒子群的初始位置和速度值時(shí)，種群中粒子的速度和位置通過式(1)和式(2)不斷地迭代更改,進(jìn)而找到最優(yōu)解。

Vij(t+1)=ωvij(t)+c1*r1j*(Pbest(t)-

Xij(t))+c2*r2j*(Pgbest(t+1)-

Xij(t)),

Xij(t+1)=Xij(t)+Vij(t),

(11)

式中:Vij(t)----第i個(gè)粒子第j維上的速度向量；

Xij(t)----第i個(gè)粒子在第j維上的位置向量；

Pbest(t)----第i個(gè)粒子當(dāng)前搜索到的最佳位置向量；

Pgbest(t)----種群中迄今為止搜索到的最佳位置向量；

t----當(dāng)前時(shí)間；

c1,c2----粒子的加速度系數(shù)；

r1j,r2j----兩個(gè)獨(dú)立均勻分布的隨機(jī)變量，目的是給迭代計(jì)算加入隨機(jī)性，范圍為[0，1]；

ω----進(jìn)行自適應(yīng)變化的慣性權(quán)重，表示粒子上一時(shí)刻速度對(duì)下一時(shí)刻運(yùn)動(dòng)的影響。

(12)

式中：ωmin----初始權(quán)重;

ωmax----最終權(quán)重;

Tmax----最大迭代數(shù);

t----當(dāng)前迭代數(shù)。

(13)

N----迭代次數(shù)；

PSO算法的運(yùn)算過程如圖1所示。

圖1 PSO算法流程圖

3.2 混沌粒子群

針對(duì)傳統(tǒng)粒子群算法在迭代過程中易過早陷入局部極值點(diǎn)收斂的缺點(diǎn)，提出混沌粒子群算法[13-14]，目的是增加粒子變異，擴(kuò)大算法的迭代范圍，能夠保證粒子可以收斂到全局的最佳位置，減少局部最優(yōu)解的存在[15]。混沌理論是科學(xué)家在1975年提出的[16]，在電路系統(tǒng)、保密通信及工程研究領(lǐng)域都得到了廣泛應(yīng)用。在混沌粒子群中，粒子具有隨機(jī)性和遍歷性，不會(huì)重復(fù)經(jīng)歷空間內(nèi)全部狀態(tài)?；煦缌Ｗ尤核惴朔肆Ｗ酉萑刖植繕O值的弱點(diǎn)，提高了優(yōu)化算法效率，得到了最佳優(yōu)化結(jié)果。

混沌點(diǎn)列的生成過程：

1)假設(shè)粒子m維位置向量

xi=(xi1,xi2,…,xim)。

映射向量

yi=(yi1,yi2,…,yim),

其中

式中：ak----xik的上界；

bk----xik的下界。

混沌粒子群算法的執(zhí)行過程如下：

1)設(shè)置卡爾曼濾波算法的狀態(tài)變量和噪聲協(xié)方差的初始值；

2)初始化種群中粒子的位置和速度；

3)通過式(13)得到粒子適應(yīng)度，確定每個(gè)粒子的極值和全局極值；

4)通過式(11)的迭代計(jì)算更新粒子的當(dāng)前位置和速度進(jìn)行尋優(yōu)；

7)更新粒子最優(yōu)值和全局最優(yōu)值，滿足終止條件，則輸出混沌粒子群算法的最優(yōu)解,即最優(yōu)解R、Q。

3.3 混沌粒子群優(yōu)化 EKF 結(jié)構(gòu)

基于混沌粒子群優(yōu)化EKF 結(jié)構(gòu)如圖 2 所示。

圖2 混沌粒子群優(yōu)化的 EKF 結(jié)構(gòu)

圖中:R----系統(tǒng)協(xié)方差陣；

Q----系統(tǒng)測(cè)量噪聲協(xié)方差陣；

xk----輸入值;

zk----測(cè)量值。

通過各時(shí)刻的目標(biāo)函數(shù)值和濾波參數(shù)值,以及混沌粒子群算法對(duì)系統(tǒng)噪聲R和測(cè)量噪聲Q尋找最優(yōu)解，將優(yōu)化后的系統(tǒng)噪聲和測(cè)量噪聲代入擴(kuò)展卡爾曼濾波后的運(yùn)算過程，再通過式(6)～式(10)可以算出k時(shí)刻的最優(yōu)估計(jì)濾波值xk。

4 仿 真

文中定位解算中使用的是CGCS2000坐標(biāo)系，對(duì) RINEX文件數(shù)據(jù)進(jìn)行解算。分別采用粒子群優(yōu)化EKF算法和混沌粒子群優(yōu)化EKF算法對(duì)X、Y、Z三個(gè)方向進(jìn)行定位解算，然后再將定位結(jié)果進(jìn)行對(duì)比分析。

誤差對(duì)比如圖3所示。

(a) 粒子群優(yōu)化擴(kuò)展卡爾曼濾波算法

(b) 混沌粒子群優(yōu)化擴(kuò)展卡爾曼濾波算法圖3 X、Y、Z三個(gè)方向的誤差結(jié)果對(duì)比

圖3明顯可以看出,混沌粒子群優(yōu)化擴(kuò)展卡爾曼濾波算法的定位誤差更小，濾波結(jié)果也更加平滑。混沌粒子群優(yōu)化擴(kuò)展卡爾曼濾波算法的定位解算結(jié)果相較于傳統(tǒng)粒子群優(yōu)化擴(kuò)展卡爾曼濾波的定位精度有明顯提高，相較于圖3(a)的收斂過程也明顯縮短，實(shí)現(xiàn)了更加精確的定位。

粒子群優(yōu)化EKF和混沌粒子群優(yōu)化EKF三個(gè)方向的誤差對(duì)比結(jié)果見表1。

表1 誤差對(duì)比 m

表1可以看出，混沌粒子群優(yōu)化EKF算法的定位精度較傳統(tǒng)粒子群優(yōu)化EKF算法提高約70%，濾波結(jié)果也更加平滑。故混沌粒子群優(yōu)化EKF算法極好地改進(jìn)了定位誤差。

5 結(jié) 語

提出混沌粒子群優(yōu)化擴(kuò)展卡爾曼濾波定位解算算法，利用混沌粒子群優(yōu)化擴(kuò)展卡爾曼濾波算法的系統(tǒng)噪聲和測(cè)量噪聲來抑制濾波發(fā)散情況,進(jìn)而提高定位精度。在混沌特性的作用下，粒子算法搜索的遍歷性得到完善。不再局限于幾個(gè)局部的最優(yōu)值，而是產(chǎn)生更多的局部最優(yōu)鄰域點(diǎn)，由此對(duì)粒子去除局部極值起到關(guān)鍵作用，進(jìn)而提高精度,保證效率和時(shí)效性。

通過 Matlab處理數(shù)據(jù)的實(shí)驗(yàn)結(jié)果表明,混沌粒子群優(yōu)化EKF算法的收斂速度與定位精度均明顯提高。