柯明星，呂　晶，童凱翔，朱　明

(1.解放軍理工大學(xué) 通信工程學(xué)院，南京　210007;2.衛(wèi)星通信與導(dǎo)航江蘇高校協(xié)同創(chuàng)新中心，南京　210007)

?

基于擴(kuò)展卡爾曼濾波的動(dòng)態(tài)協(xié)同定位算法

柯明星1，2，呂晶1，2，童凱翔1，2，朱明1，2

(1.解放軍理工大學(xué) 通信工程學(xué)院，南京210007;2.衛(wèi)星通信與導(dǎo)航江蘇高校協(xié)同創(chuàng)新中心，南京210007)

摘要：衛(wèi)星導(dǎo)航系統(tǒng)是目前應(yīng)用最為廣泛的一種定位與導(dǎo)航系統(tǒng)，然而在城區(qū)、峽谷、森林等復(fù)雜條件下其所提供的定位與導(dǎo)航性能會(huì)下降甚至無(wú)法提供服務(wù)。針對(duì)該問(wèn)題，已經(jīng)有多種衛(wèi)星導(dǎo)航增強(qiáng)手段被提出，其中，用戶(hù)間的協(xié)同定位由于可以減輕對(duì)基礎(chǔ)設(shè)施的依賴(lài)，具有很大發(fā)展前景。本文提出了基于擴(kuò)展卡爾曼的協(xié)同定位算法，建立了動(dòng)態(tài)條件下用戶(hù)的位置速度矢量模型的系統(tǒng)方程，利用該算法在動(dòng)態(tài)協(xié)同定位系統(tǒng)中實(shí)現(xiàn)對(duì)用戶(hù)的位置和速度的估計(jì)。文中論述了算法的詳細(xì)過(guò)程，并通過(guò)計(jì)算機(jī)軟件實(shí)時(shí)產(chǎn)生衛(wèi)星的三維位置，構(gòu)建了一個(gè)動(dòng)態(tài)用戶(hù)協(xié)同定位場(chǎng)景。數(shù)值仿真結(jié)果表明，該算法能夠有效地估計(jì)出用戶(hù)的位置和速度。

關(guān)鍵詞：擴(kuò)展卡爾曼濾波；協(xié)同定位；動(dòng)態(tài)系統(tǒng)；性能估計(jì)

0引言

目前，位置信息已經(jīng)在軍事、海事、搜救等很多領(lǐng)域起著不可或缺的作用。全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system，GNSS)是目前應(yīng)用最為廣泛的一種定位與導(dǎo)航系統(tǒng)。然而，在一些封閉場(chǎng)景，比如峽谷、森林或是存在城市建筑物遮擋情況下，GNSS就不能保證服務(wù)的質(zhì)量甚至不能提供定位服務(wù)。

針對(duì)這類(lèi)問(wèn)題，有多種導(dǎo)航增強(qiáng)手段被提出，如傳統(tǒng)的星基導(dǎo)航增強(qiáng)、基于偽衛(wèi)星的導(dǎo)航增強(qiáng)、輔助全球?qū)Ш叫l(wèi)星系統(tǒng)以及組合導(dǎo)航等[1]。協(xié)同定位是目前衛(wèi)星導(dǎo)航定位領(lǐng)域比較新穎而且有效的解決方法，其由于可以減輕對(duì)基礎(chǔ)設(shè)施的依賴(lài)而顯現(xiàn)出了很大吸引力。協(xié)同定位的概念最早是在1996年由日本學(xué)者在機(jī)器人定位領(lǐng)域提出[2]，后來(lái)在無(wú)線傳感網(wǎng)領(lǐng)域成為一個(gè)熱門(mén)的研究方向[3]。在衛(wèi)星導(dǎo)航定位領(lǐng)域，有關(guān)協(xié)同技術(shù)的研究主要集中在對(duì)一些簡(jiǎn)單模型用戶(hù)協(xié)同定位精度和覆蓋范圍等性能增強(qiáng)的驗(yàn)證，主要的的研究成果有發(fā)表在文獻(xiàn)[4-5]中。利用協(xié)同來(lái)解決實(shí)際問(wèn)題的關(guān)鍵是協(xié)同算法可以執(zhí)行分布式計(jì)算并收斂。定位問(wèn)題本質(zhì)上可以看做狀態(tài)估計(jì)的問(wèn)題。最優(yōu)狀態(tài)估計(jì)在很多領(lǐng)域都獲得了廣泛的應(yīng)用，典型的方法有最小二乘估計(jì)，卡爾曼濾波，粒子濾波等[6]。其中卡爾曼濾波作為動(dòng)態(tài)系統(tǒng)狀態(tài)估計(jì)經(jīng)典方法，一經(jīng)提出便在精確制導(dǎo)、數(shù)據(jù)通信、全球定位、目標(biāo)跟蹤等領(lǐng)域受到了廣泛關(guān)注。但是，卡爾曼濾波的局限性是要求估計(jì)系統(tǒng)為線性，而衛(wèi)星導(dǎo)航定位中相對(duì)應(yīng)的系統(tǒng)估計(jì)方程是非線性方程。對(duì)此，將卡爾曼濾波改進(jìn)得到擴(kuò)展卡爾曼濾波后才能更好的應(yīng)用于衛(wèi)星導(dǎo)航定位中。擴(kuò)展卡爾曼濾波是通過(guò)對(duì)非線性函數(shù)的泰勒級(jí)數(shù)展開(kāi)進(jìn)行一階線性化截?cái)?，忽略其余高階項(xiàng)，從而將非線性化問(wèn)題線性化。

本文通過(guò)引入基于擴(kuò)展卡爾曼濾波的協(xié)同定位算法來(lái)提高GNSS定位的精度與可用性。構(gòu)建了動(dòng)態(tài)條件下用戶(hù)的協(xié)同定位模型，實(shí)現(xiàn)了對(duì)模型中用戶(hù)的位置和速度狀態(tài)的有效估計(jì)。

文章各章節(jié)安排如下：第一部分為動(dòng)態(tài)場(chǎng)景的建立，得到系統(tǒng)模型和觀測(cè)模型；第二部分為擴(kuò)展卡爾曼濾波算法的簡(jiǎn)介；第三部分為協(xié)同定位算法的描述并將其應(yīng)用于該具體場(chǎng)景；仿真結(jié)果出現(xiàn)在第四部分；第五部分對(duì)全文進(jìn)行總結(jié)。

1動(dòng)態(tài)場(chǎng)景介紹

1.1場(chǎng)景描述

本文所關(guān)注的動(dòng)態(tài)協(xié)同定位網(wǎng)絡(luò)如圖1所示。場(chǎng)景中一共存在6個(gè)用戶(hù)，以符號(hào)m1～m6表示，對(duì)于每個(gè)用戶(hù)而言，與其他用戶(hù)協(xié)同關(guān)系的存在以連線表示，每個(gè)用戶(hù)可見(jiàn)星如表1所示。例如，對(duì)于m1而言，用戶(hù)m2、m5、m6能與其協(xié)同，可見(jiàn)星為S1、S2、S3、S4、S5、S6、S7號(hào)星。對(duì)用戶(hù)而言，其所能觀測(cè)到的衛(wèi)星集合以Sm表示，所有的協(xié)同用戶(hù)的集合以Mm表示。

圖1　協(xié)同網(wǎng)絡(luò)場(chǎng)景示意圖

通常在GNSS系統(tǒng)中，要想確定一個(gè)用戶(hù)的位置坐標(biāo)信息，起碼需要四顆可見(jiàn)衛(wèi)星，因而在上圖所示場(chǎng)景中，只有用戶(hù)m1、m2、m3可以定位。但是經(jīng)過(guò)用戶(hù)間的協(xié)同之后，可以得到所有用戶(hù)的位置信息，甚至有些用戶(hù)在一顆星都不可見(jiàn)的情況下，如用戶(hù)m6，也能夠得到其比較精確的位置速度估計(jì)信息。

表1　各個(gè)用戶(hù)可見(jiàn)星(S表示可見(jiàn)，U表示不可見(jiàn))

1.2系統(tǒng)模型

該系統(tǒng)的狀態(tài)方程為

(1)

(2)

(3)

式(2)中，I表示4×4的單位陣，Δt表示的是信息更新的時(shí)間間隔。

1.3觀測(cè)模型

用戶(hù)的觀測(cè)量為來(lái)自衛(wèi)星的偽距測(cè)量和來(lái)自協(xié)同用戶(hù)的距離測(cè)量。觀測(cè)模型為

(4)

(5)

在t時(shí)刻，用戶(hù)m與衛(wèi)星s之間的偽距為

(6)

得出擴(kuò)展卡爾曼中所需的雅克比矩陣為

(7)

在t時(shí)刻，用戶(hù)與用戶(hù)之間的距離測(cè)量為

(8)

2擴(kuò)展卡爾曼濾波法

擴(kuò)展卡爾曼濾波的思想是用線性化的方法來(lái)近似非線性化模型。它是一種迭代算法，根據(jù)噪聲的統(tǒng)計(jì)特性和當(dāng)前的測(cè)量值對(duì)用戶(hù)的狀態(tài)做出最佳的估計(jì)。濾波器主要分為兩步：預(yù)測(cè)過(guò)程和更新過(guò)程。

2.1預(yù)測(cè)過(guò)程

預(yù)測(cè)過(guò)程即利用t-1時(shí)刻的狀態(tài)量和協(xié)方差去估計(jì)t時(shí)刻的狀態(tài)量和協(xié)方差，這里用函數(shù)f(·)表示狀態(tài)預(yù)測(cè)函數(shù)為

(9)

(10)

2.2更新過(guò)程

(11)

(12)

求卡爾曼增益

(13)

更新?tīng)顟B(tài)估計(jì)

(14)

(15)

3協(xié)同定位算法

3.1協(xié)同定位基本原理

用戶(hù)之間的協(xié)同，通過(guò)用戶(hù)與用戶(hù)的通信和測(cè)距來(lái)實(shí)現(xiàn)。用戶(hù)之間相互通信，相互之間可以交互獲取所需信息。在本文的模型中，用戶(hù)之間交換的是位置坐標(biāo)信息。而通過(guò)測(cè)距進(jìn)行協(xié)同，用戶(hù)得到其與鄰近用戶(hù)的距離信息，距離信息可以加入定位方程組進(jìn)行求解，不僅能夠提高定位的精度，還能使一些原本不能定位的系統(tǒng)實(shí)現(xiàn)定位，提高了用戶(hù)的可定位性。

3.2基于擴(kuò)展卡爾曼的協(xié)同定位算法流程

基于擴(kuò)展卡爾曼濾波的協(xié)同定位算法流程

前提：衛(wèi)星的狀態(tài)是已知的，用戶(hù)運(yùn)動(dòng)狀態(tài)為勻速直線運(yùn)動(dòng)；

4：獲取觀測(cè)量，列出系統(tǒng)的狀態(tài)轉(zhuǎn)移方程和觀測(cè)方程，計(jì)算雅克比矩陣H(t)；

5：計(jì)算中間矩陣S(t)m和觀測(cè)量與預(yù)測(cè)量的偏差ΔZ，得到卡爾曼增益K；

如果 在該時(shí)隙內(nèi)迭代次數(shù)Niter

則將估計(jì)值作為預(yù)測(cè)值返回第3步

否則 進(jìn)入第7步；

7：如果 估計(jì)總時(shí)間結(jié)束

則輸出最終的估計(jì)狀態(tài)值

否則 進(jìn)入下一個(gè)時(shí)隙，返回第2步并將上一時(shí)隙

估計(jì)狀態(tài)和協(xié)方差作為初始信息。

本文中用戶(hù)之間的協(xié)同體現(xiàn)在彼此之間能相互測(cè)距并通過(guò)通信共享自己的位置信息。用戶(hù)在更新自己的位置狀態(tài)之前需要接收所有協(xié)同用戶(hù)的估計(jì)位置信息，并向協(xié)同用戶(hù)發(fā)播自己的位置信息。由于該網(wǎng)絡(luò)為動(dòng)態(tài)模型，為了更加準(zhǔn)確快速的對(duì)狀態(tài)進(jìn)行跟蹤估計(jì)，在每個(gè)更新間隔內(nèi)還要進(jìn)行n次迭代，這里取n=3。衛(wèi)星的三維坐標(biāo)通過(guò)STK軟件實(shí)時(shí)產(chǎn)生。

4仿真驗(yàn)證與分析

本節(jié)給出了基于擴(kuò)展卡爾曼的協(xié)同定位算法對(duì)圖1所示場(chǎng)景的數(shù)值仿真結(jié)果。實(shí)驗(yàn)中，假設(shè)衛(wèi)星在仿真的時(shí)間段內(nèi)其位置信息是不變的，表2給出了衛(wèi)星在地心地固坐標(biāo)系(Earth-centered Earth-fixed，ECEF)中位置具體坐標(biāo)*進(jìn)行仿真的時(shí)候需要將其轉(zhuǎn)換為站心坐標(biāo)系坐標(biāo)。表3給出了6個(gè)用戶(hù)的初始狀態(tài)，包括初始位置信息，鐘差引起的偽距測(cè)量偏差和各個(gè)方向上的速度。

表3　各個(gè)用戶(hù)的初始狀態(tài)

圖2　六個(gè)用戶(hù)x軸方向的速度仿真曲

從圖2可以看出，當(dāng)估計(jì)時(shí)間在10 s之后，對(duì)其中各個(gè)用戶(hù)的x方向上的速度估計(jì)比較精確，基本能夠?qū)⒄`差收斂至2 m/s之內(nèi)。對(duì)用戶(hù)三x軸，y軸，z軸方向的速度估計(jì)如圖3所示。從圖3中可以看出，各個(gè)方向上速度估計(jì)的收斂速度相同，而且同一個(gè)點(diǎn)各方向上的方差大小也都基本相同，都在10 s以后能夠?qū)⑺俣裙烙?jì)的誤差收斂到2 m/s以?xún)?nèi)。

在對(duì)用戶(hù)速度估計(jì)的同時(shí)，對(duì)用戶(hù)的位置也進(jìn)行估計(jì)，在20 s內(nèi)，其水平和垂直方向上的中誤差(root mean square error，RMSE)如圖4所示。相對(duì)應(yīng)的在靜態(tài)模型中，水平和垂直方向上的中誤差如圖5所示。

圖3　用戶(hù)3速度估計(jì)

圖4　動(dòng)態(tài)模型中位置的RMSE

衛(wèi)星編號(hào)S1S2S3S4S5S6S7x/m192635242612497624768710804802985438182082386-7307117y/m-13725770-57494201601307-130144371556101723437415-14002994z/m11583188-846377992557521563572196768441204814521528812

圖5　靜態(tài)模型中位置的RMSE

通過(guò)對(duì)比可以看出，動(dòng)態(tài)模型中用戶(hù)位置的中誤差波動(dòng)比較大，在10 s之后才能將水平中誤差收斂到5 m以?xún)?nèi)。而在靜態(tài)模型中，中誤差分布比較平滑，在7 s以后，就能將水平中誤差收斂到5 m。

5結(jié)束語(yǔ)

本文針對(duì)復(fù)雜環(huán)境下可見(jiàn)星不足導(dǎo)致的無(wú)法定位動(dòng)態(tài)用戶(hù)的問(wèn)題，研究了基于協(xié)同定位的導(dǎo)航增強(qiáng)方法，通過(guò)擴(kuò)展卡爾曼濾波實(shí)現(xiàn)了對(duì)動(dòng)態(tài)用戶(hù)的位置和速度的有效估計(jì)。從仿真結(jié)果可以看出，用戶(hù)的速度估計(jì)在10 s以后，其方差可以控制在2 m/s以?xún)?nèi)，對(duì)其位置估計(jì)時(shí)，在10 s以后估計(jì)誤差可收斂到5 m以?xún)?nèi)。同時(shí)，通過(guò)協(xié)同，原本可見(jiàn)衛(wèi)星不足4顆的用戶(hù)實(shí)現(xiàn)了定位，也即系統(tǒng)的可用性也得到了顯著提高。然而還應(yīng)該看到，與靜態(tài)系統(tǒng)相比，動(dòng)態(tài)系統(tǒng)內(nèi)用戶(hù)位置估計(jì)的方差會(huì)變大，位置收斂的速度也會(huì)變慢。這是在下一步的工作中需要解決的問(wèn)題。

參考文獻(xiàn)

[1]KURAZUME R，HIROSE S，NAGATA S，et al.Study on cooperative positioning system[EB/OL].(2015-03-21)[2015-06-18].http：//www.researchgate.net/publication/3631511_Study_on_Cooperative_Positioning_System.

[2]WYMEERSCH H，LIEN J，WIN M Z.Cooperative localization in wireless networks[J].Proceedings of the IEEE，2009，97(2)：23-34.DOI：10.1109/JPROC.2008.2008853.

[3]SHARP I，YU K G，HEDLEY M.On the GDOP and accuracy for indoor positioning[J].IEEE Transactions on Aerospace & Electronic Systems，2012，48(3)：2032-2051.

[4]CACERES M A，SOTTILE F，GARELLO R，et al.Hybrid GNSS-to a localization and tracking via cooperative unscented kalman filter[EB/OL].[2015-06-18].http：//www.researchgate.net/publication/224205981_Hybrid_GNSS-ToA_localization_and_tracking_via_cooperative_unscented_Kalman_filter.

[5]CACERES M A，PENNA F，WYMEERSCH H，et al.Hybrid GNSS-terrestrial cooperative positioning via distributed belief propagation[C]//The Institute of Electrical and Electronics Engineers(IEEE).Global Telecommunications Conference (GLOBECOM 2010).New York：IEEE，2010：1-5.DOI：10.1109/GLOCOM.2010.5683684.

[6]SIMON D.Optimal state estimation：Kalman，H infinity，and nonlinear approaches[M].New Jersey：John Wiley & Sons，2006：88-89.

[7]BELLANTONI J F，DODGE K W.A square root formulation of the Kalman-Schmidt filter[J].AIAA Journal，1967，5(7)：1309-1314.DOI：10.2514/3.4189.

[8]TIAN Shiwei，HUANG Boyu，LI Guangxia，et al.A perspective on Cramér-Rao bound for hybrid GNSS-terrestrial cooperative positioning[EB/OL].[2015-06-18].http：//www.cnki.net/KCMS/detail/detail.aspx?QueryID=2&CurRec=1&recid=&filename=WXDH201405009017&dbname=CPFD0914&dbcode=CPFD&pr=&urlid=&yx=&uid=WEEvREcwSlJHSldSdnQ1V1lLcDUvRkY1aHhXeENsODZxK0Jsa1IyNTlTUjF6b1U3ZDJHUEpqMFl3UzZYOWZ2eVlB PT0=$9A4hF_YAuvQ5obgVAqNKPCYcEjKensW4IQMovwHtwkF4VYPoHbKxJw!!&v=MTg3NTZackc0SDlYT XFvOUZiZXNPQ3hOS3VoZGhuajk4VG5qcXF4ZEVlTU9VS3JpZlp1TnVGaXZrVXJmTElWd1dNalhQ.

[9]PENNA F，CACERES M A，WYMEERSCH H.Cramér-Rao bound for hybrid GNSS-terrestrial cooperative positioning[J].IEEE Communications Letters，2010，14(11)：1005-1007.

A Research of Dynamic Cooperative Localization System via Extended Kalman Filter

KEMingxing1，2，LüJing1，2，TONGKaixiang1，2，ZHUMing1，2

(1.College of Communications Engineering,PLA University of Science and Technology, Nanjing 210007, China；2.Satellite Communication and Navigation Collaborative Innovative center Nanjing，210007, China)

Abstract：The global navigation satellite system (GNSS) is widely used in navigation and positioning system.While in some GNSS-challenged environments，such as urban canyons，dense foliage and building block，it will fail to guarantee the service quality or can’t provide service at all.Several satellite enhanced means were presented to solve such problem and among them，the cooperative localization has great prospective as it can reduce the dependence of infrastructure.This paper established an equation of the system based on a position-velocity-time (PVT) model and employed the extended Kalman filter algorithm to estimate the user’s location and velocity in a dynamic cooperative localization system.This paper gave out the detailed derivation process and built the model though the three-dimension position of each satellite generated by software.The simulation results show that the algorithm can track the path of dynamic users and effectively estimate the user’s velocity.

Key words：extended kalman filter；cooperative localization；dynamic system；performance estimation

中圖分類(lèi)號(hào)：P228

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：2095-4999(2016)-01-0044-06

作者簡(jiǎn)介：第一柯明星(1990—)，男，安徽安慶人，碩士生，主要從事衛(wèi)星導(dǎo)航和室內(nèi)外協(xié)同定位算法的研究。

收稿日期：2015-07-25

引文格式：柯明星，呂晶，童凱翔，等.基于擴(kuò)展卡爾曼濾波的動(dòng)態(tài)協(xié)同定位算法[J].導(dǎo)航定位學(xué)報(bào)，2016，4(1)：44-49.(KE Mingxing，Lü Jing，TONG Kaixiang，et al.A Research of Dynamic Cooperative Localization System via Extended Kalman Filter[J].Journal of Navigation and Positioning，2016，4(1)：44-49.)DOI：10.16547/j.cnki.10-1096.20160109.