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

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

?

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

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

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

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

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

0引言

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

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

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

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

1動態(tài)場景介紹

1.1場景描述

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

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

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

表1　各個用戶可見星(S表示可見，U表示不可見)

1.2系統(tǒng)模型

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

(1)

(2)

(3)

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

1.3觀測模型

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

(4)

(5)

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

(6)

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

(7)

在t時刻，用戶與用戶之間的距離測量為

(8)

2擴展卡爾曼濾波法

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

2.1預測過程

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

(9)

(10)

2.2更新過程

(11)

(12)

求卡爾曼增益

(13)

更新狀態(tài)估計

(14)

(15)

3協(xié)同定位算法

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

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

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

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

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

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

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

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

則將估計值作為預測值返回第3步

否則 進入第7步；

7：如果 估計總時間結束

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

否則 進入下一個時隙，返回第2步并將上一時隙

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

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

4仿真驗證與分析

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

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

圖2　六個用戶x軸方向的速度仿真曲

從圖2可以看出，當估計時間在10 s之后，對其中各個用戶的x方向上的速度估計比較精確，基本能夠將誤差收斂至2 m/s之內(nèi)。對用戶三x軸，y軸，z軸方向的速度估計如圖3所示。從圖3中可以看出，各個方向上速度估計的收斂速度相同，而且同一個點各方向上的方差大小也都基本相同，都在10 s以后能夠將速度估計的誤差收斂到2 m/s以內(nèi)。

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

圖3　用戶3速度估計

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

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

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

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

5結束語

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

參考文獻

[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

中圖分類號：P228

文獻標識碼：A

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

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

收稿日期：2015-07-25

引文格式：柯明星，呂晶，童凱翔，等.基于擴展卡爾曼濾波的動態(tài)協(xié)同定位算法[J].導航定位學報，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.