基于IMU和兩信標(biāo)節(jié)點的高精度概率推測定位系統(tǒng)

宋述鵬　陳雪晨　王大強(qiáng)　畢　揚　馮　崢　王鵬宇

(1.中山大學(xué)電子與信息工程學(xué)院　廣東廣州　510275；2.華東瑯琊山抽水蓄能有限責(zé)任公司　安徽滁州　239000)

?

基于IMU和兩信標(biāo)節(jié)點的高精度概率推測定位系統(tǒng)

宋述鵬1陳雪晨1王大強(qiáng)2畢揚2馮崢2王鵬宇2

(1.中山大學(xué)電子與信息工程學(xué)院廣東廣州510275；2.華東瑯琊山抽水蓄能有限責(zé)任公司安徽滁州239000)

摘要：傳統(tǒng)的定位系統(tǒng)需要3個或3個以上的環(huán)境節(jié)點到未知節(jié)點的距離信息幫助定位，而在很多惡劣多變的環(huán)境中，這一前提往往難以得到滿足。提出運用一個慣性測量單元和兩個環(huán)境節(jié)點傳感器(信標(biāo)節(jié)點)對未知的移動傳感器節(jié)點實現(xiàn)定位。兩個環(huán)境節(jié)點與移動節(jié)點共同屬于Cricket系統(tǒng)，該系統(tǒng)利用射頻信號和超聲波信號在空氣中傳輸速度的不同，通過測量兩種信號到達(dá)時間的差值去獲得環(huán)境節(jié)點和位置節(jié)點之間的距離估計。提出首先根據(jù)固定在未知節(jié)點上的慣性測量單元的測量輸出和上一時刻的最優(yōu)估計位置，估計出未知節(jié)點當(dāng)前時刻的位置，根據(jù)慣性測量系統(tǒng)測量噪聲模型，可以推斷出未知節(jié)點當(dāng)前時刻的真實位置在此估計位置為中心的某區(qū)域內(nèi)。再根據(jù)未知節(jié)點與兩個環(huán)境節(jié)點的估計距離，通過仿真測量噪聲模型，利用最小均方估計和最大后驗估計算法，最終分別得到未知節(jié)點的最優(yōu)估計位置和次優(yōu)估計位置。仿真和實驗表明了本算法的有效性和魯棒性。

關(guān)鍵詞：定位慣性測量單元到達(dá)時間差最小均方估計最大后驗估計

在無線傳感網(wǎng)中實現(xiàn)節(jié)點定位是一個重要的研究課題，因為位置信息在很多應(yīng)用場景中扮演了一個重要角色，例如無線膠囊內(nèi)窺鏡跟蹤、環(huán)境監(jiān)測、室內(nèi)人員跟蹤、航海導(dǎo)航等。無線傳感網(wǎng)中的定位算法分為基于測距的定位(range-based)和非測距的定位(range-free)。在基于測距的定位中，有TDOA(Time Difference Of Arrival), TOA(Time Of Arrival), RSSI(Received Signal Strength Indicator) , AOA(Angle Of Arrival)等?；诜菧y距(range-free)的定位中，有DV-Hop(Distance Vector-Hop)算法、凸規(guī)劃(Convex Optimization)算法、質(zhì)心算法(Centroid Algorithm APIT(Approximate Point-In-Triangulation Test)等。相比于非測距定位，測距定位具有更高的精度。

在國內(nèi)外的研究中，文獻(xiàn)[1]在Non-Line-of-Sight(NLOS)干擾導(dǎo)致的TOA測量誤差下，提出了半定規(guī)劃SDP的節(jié)點定位算法，避免了非視距傳播對定位精度的影響。文獻(xiàn)[2]運用3個慣性測量單元IMU測量的加速度值，通過加速度誤差消除算法對加速度值的處理，能夠得到人員前進(jìn)的準(zhǔn)確速度和位置。文獻(xiàn)[3]提出了一種偏差減少的方法，并結(jié)合泰勒級數(shù)和極大似然估計得到一個偏差表達(dá)式，分析表達(dá)式降低偏差，從而提高定位精度。文獻(xiàn)[4]在無線傳感網(wǎng)中使用概率論的方法和基于RSSI的概率分布算法來估計未知節(jié)點的位置。美國麻省理工學(xué)院研發(fā)了室內(nèi)定位系統(tǒng)，此系統(tǒng)的定位精度可達(dá)到10 cm,方向精度可達(dá)到3°。文獻(xiàn)[5]利用Cricket定位系統(tǒng)并結(jié)合randomize算法,能很好地克服多徑干擾，減少定位誤差。文獻(xiàn)[6]首次提出了水電站地下室內(nèi)定位系統(tǒng)的項目設(shè)計，但并未真正地完全實施其方案。

傳統(tǒng)的定位系統(tǒng)需要3個或3個以上的環(huán)境節(jié)點到未知節(jié)點的距離信息幫助定位，而在很多惡劣多變的環(huán)境中，這一前提往往難以得到滿足。因此本文提出運用一個慣性測量單元和兩個環(huán)境節(jié)點傳感器，對未知的移動傳感器節(jié)點實現(xiàn)定位。其中基于測距定位的TDOA技術(shù)是基于移動臺到達(dá)不同基站的時間差來定位的，而不是通過移動目標(biāo)到達(dá)基站的絕對時間來對目標(biāo)定位。其優(yōu)勢在于，無需對移動臺進(jìn)行改動，與TOA相比并不需要精確的時間同步，與AOA相比不需要天線陣列，同時也能夠消除NLOS所引起的測量誤差，定位精度比較高，但需要額外的硬件支持。攜帶在人員身上的慣性測量單元包括加速度計、陀螺儀和磁力計，其能夠獲得比較準(zhǔn)確的前進(jìn)方向和距離估計。陀螺儀和磁力計的結(jié)合已經(jīng)廣泛應(yīng)用于獲得航向的方面[7-8]，我們對通過加速度計獲得的加速度值進(jìn)行兩次積分可以得到前進(jìn)的距離。然而，由于測量會有噪聲和偏差，因此產(chǎn)生了很大的漂移需要去糾正。

本文用移動節(jié)點到環(huán)境傳感器的TDOA測量值結(jié)合從慣性測量單元獲得的前進(jìn)方向和距離，采用最小均方估計和最大后驗估計去獲得位置估計。TDOA定位比較適合被動式定位，并不需要信號時間戳。我們充分利用TDOA定位的優(yōu)勢去減少加速度計、陀螺儀和磁力計產(chǎn)生的漂移，從而提高定位精度。

1背景和坐標(biāo)系統(tǒng)

本文采用的是歐幾里得坐標(biāo)系統(tǒng)。設(shè)(xk,yk)是未知節(jié)點在時刻tk的位置坐標(biāo)，(xi,yi)是信標(biāo)節(jié)點i的位置坐標(biāo)，Zi代表未知節(jié)點和信標(biāo)節(jié)點i之間的歐幾里得距離。

(1)

(2)

其中Wi表示標(biāo)準(zhǔn)差為σi的加性高斯白噪聲。

為了讓TDOA測量的標(biāo)準(zhǔn)差更接近實際情況，其方差應(yīng)該和傳播距離有關(guān)。根據(jù)文獻(xiàn)[5]得到噪聲方差和信標(biāo)節(jié)點與未知節(jié)點距離之間的關(guān)系式：

(3)

其中a是一個根據(jù)實際驗證得到的比例因子，一般取1～2，SNR0=SNR(Z0)是信標(biāo)節(jié)點與未知節(jié)點距離為Z0處相應(yīng)的信噪比SNR值。

圖1描述了IMU-TDOA場景和坐標(biāo)系統(tǒng)。航位推算是基于慣性測量單元在一段時間內(nèi)估計的距離和方向，在已知先前的絕對位置來對目標(biāo)進(jìn)行位置跟蹤的一種方法。首先，在前一時刻目標(biāo)位于兩綠色虛線交點X1=(30，40)，隨后在當(dāng)前時刻目標(biāo)移動到位置X2。在這段時間間隔內(nèi)我們使用慣性測量單元的加速度計和陀螺儀來估計位置X2的橫坐標(biāo)值x2和縱坐標(biāo)值y2：

(4)

同理對于航向角，有：

圖1　IMU-TDOA系統(tǒng)下當(dāng)前時刻未知節(jié)點區(qū)域

2最小均方估計MMSE和最大后驗估計MAP

在數(shù)理統(tǒng)計和信號處理中，一個最小均方估計是對一個獨立變量的擬合值進(jìn)行最小化均方誤差的估計方法，也是比較常見的估計手段。MMSE是在已知的測量值條件下對真實值的條件期望。

(5)

根據(jù)貝葉斯概率理論可以進(jìn)一步推導(dǎo)后驗概率得到：

(6)

(6)式中，分母可以展開為：

(7)

根據(jù)高斯噪聲模型，有：

(8)

假定在交界區(qū)域內(nèi)，(x,y)的分布是均勻的，即p((x,y))為一定值。接下來將(8)代入(7)之后得到的結(jié)果再代入(6)，最終結(jié)果再代入(5)式中，就得到了最優(yōu)均方誤差估計的結(jié)果。

在貝葉斯統(tǒng)計中，最大后驗概率估計MAP盡管精度沒有MMSE方法高，但由于其計算復(fù)雜性大為降低，計算時長減少，因此這里也采用MAP方法進(jìn)行估計和比較。MAP方法的輸出為：

(9)

只需將(7) 式的計算代入就可得到最大后驗概率估計的輸出。

3定位精度

x+δx=

G(Z1+W1,Z2+W2,Z3+W3)≈

(10)

因為測量誤差是已知的獨立的零均值高斯白噪聲，對于測量的距離和方向近似的偏差表達(dá)式為：

(11)

(12)

4仿真和實驗結(jié)果

在本節(jié)中，首先用MATLAB測試了這兩種算法對于測量誤差的魯棒性，接著基于MEMSIC的CricketMote(MCS-KIT410CA)和慣性測量單元IMU380ZA-409進(jìn)行了實驗。比較對象為用3個MCS-KIT410CA進(jìn)行傳統(tǒng)TDOA測距定位的方案。

圖2針對慣性測量單元的測量值前進(jìn)距離和航向角的不同大小情況驗證了IMU-TDOA系統(tǒng)在MMSE算法和MAP算法下的不同定位精度。通過TDOA的100次隨機(jī)測量值能計算出平均的測量精度的大小。測量精度在本文中由均方距離誤差，即RMSE(rangemeansquareerror) 來衡量。由圖2可以看出：當(dāng)航向角的大小一定時，MMSE算法的性能高于MAP算法，前者的RMSE也比較小。隨著航向角的增大，兩種算法的RMSE將會一直變大，其主要原因是在不同航向角的范圍TDOA的測量距離值是變化的，它能影響兩種算法的性能。

圖2　不同航向角大小和前進(jìn)距離誤差的RMSE

接下來考慮僅TDOA的情況來做比較，即利用3個MCS-KIT410CA獲得100次隨機(jī)的TDOA測量值去計算移動目標(biāo)的位置。在圖3中明顯看出IMU-TDOA系統(tǒng)下的兩種算法的性能都要比僅TDOA好。

圖3　不同定位方案的RMSE

為了提高IMU-TDOA系統(tǒng)的性能，我們還提出一種迭代算法，IMMSE(iterative MMSE)和IMAP(iterative MAP)。該算法中，TDOA的測量值作為迭代估計算法的初始值，以此初始值計算出來的位置估計作為第一次迭代的估計值，第一次迭代的估計值和環(huán)境傳感器的距離值再作為第二次迭代的初始值，以此類推。

圖4展示了當(dāng)航向角A=1(π/180)和前進(jìn)距離誤差L=0.6 m時,對于IMMSE算法在5個不同初始測量誤差情況下，IMU-TDOA的估計誤差和迭代次數(shù)之間的關(guān)系。5個關(guān)系曲線都是逐漸趨近于一固定值，其在不同的迭代次數(shù)點達(dá)到相同的最小值0.16。IMU-TDOA IMMSE算法的定位誤差有16%，其性能相比于IMU-TDOA IMAP算法比較差。因此在航向角A和前進(jìn)距離誤差L的大小一定時，不同的初始測量誤差下，IMU-TDOA IMMSE都有相同的誤差最小值。

圖4　不同迭代次數(shù)的IMU-TDOA IMMSE的估計誤差

圖5展示了當(dāng)航向角A=1(π/180)和前進(jìn)距離誤差L=0.6 m時,IMAP算法在5個不同初始測量誤差情況下，IMU-TDOA的估計誤差和迭代次數(shù)之間的關(guān)系。5個關(guān)系曲線都是凸函數(shù)，在不同的迭代次數(shù)點有相同的最小值0.009。這說明IMU-TDOA IMAP算法的定位誤差僅有0.9%。無論航向角A和前進(jìn)距離誤差L有多大，關(guān)系曲線一直是凸函數(shù)，所以對不同的初始誤差都有相同的最小值，只是其迭代次數(shù)不一樣而已。

圖5　不同迭代次數(shù)的IMU-TDOA IMAP的估計誤差

以上是基于MATLAB的仿真結(jié)果。我們還將算法應(yīng)用到了實際的無線傳感器網(wǎng)絡(luò)中，進(jìn)行了外場試驗以驗證算法的有效性和實用性。

我們采用的是MIT開發(fā)的Cricket系統(tǒng)。MCS410CA,Cricket Mote是著名的MICA2低功率處理器/無線模塊的定位設(shè)備，Cricket Mote模塊能夠配置為信標(biāo)節(jié)點或移動節(jié)點。由于能夠接受到移動節(jié)點發(fā)出的RF射頻信號，信標(biāo)節(jié)點可以監(jiān)聽到相應(yīng)的超聲波脈沖信號。當(dāng)脈沖信號到達(dá)時，信標(biāo)節(jié)點能夠利用RF射頻信號和超聲波信號的傳播速度這一特點獲得相應(yīng)的移動節(jié)點的距離估計。

圖6描述了MCS410CA節(jié)點之間的距離估計值，在IMU-TDOA場景中僅僅用兩個距離估計值，但是在僅TDOA場景用3個距離估計值。

圖6　信標(biāo)節(jié)點獲得相應(yīng)移動節(jié)點的距離估計

圖7展示了隨著實驗者的移動，利用距離移動節(jié)點最近的2個信標(biāo)節(jié)點的測量值和IMU測量值去實現(xiàn)定位的效果圖。圖中可以看出我們的實驗用10個節(jié)點(9個信標(biāo)節(jié)點和一個移動節(jié)點)分布在70 m×70 m沒有任何障礙物的室內(nèi)環(huán)境，這9個信標(biāo)節(jié)點均勻地分布，實驗者攜帶一個移動節(jié)點在實驗環(huán)境內(nèi)持續(xù)地移動。紅色線表示真實的移動路線，紅色加號和藍(lán)色星號分別表示使用hybrid系統(tǒng)的MMAP迭代算法和MAP算法的估計位置。由圖可見，使用MMSE算法的估計路線非常接近于真實路線，MAP算法次之，都實現(xiàn)了高精度的定位。

圖7　室內(nèi)環(huán)境的定位實驗

5結(jié)論

考慮當(dāng)三點定位法不能適用的情況下，即未知節(jié)點只能與兩個信標(biāo)節(jié)點通信時，用從Cricket Mote測量的兩個距離值和慣性測量單元IMU測量的前進(jìn)航向角、前進(jìn)距離誤差，利用MMSE和MAP算法對移動節(jié)點進(jìn)行定位。通過比較兩種算法的性能，證實了MMSE算法的定位精度比MAP算法好，但前者的算法復(fù)雜度高?？紤]了兩種不同方案即IMU-

TDOA結(jié)合和僅TDOA的方法來實現(xiàn)定位，在室內(nèi)環(huán)境下IMU-TDOA的性能比較好。

參考文獻(xiàn)

[1]KIM J W, JANG H J, HWANG D H, et al. A step, stride and heading determination for the pedestrian navigation system[J]. Journal of Global Positioning Systems, 2004,(3):273-279.

[2]GOYAL P, RIBEIRO V J, SARAN H, et al. Strap-down Pedestrian Dead-reckoning System[C]. Proceedings of the 14th International Conference on IEEE Indoor Positioning and Indoor Navigation(IPIN), 2011.1-7.

[3]FOY W H, Position-location solutions by Taylor-series estimation[J]. IEEE Trans. Aerosp. Electron. Syst., 1976,AES-12:187-194.

[4]BECK A, STOICA P, LI J. Exact and approximate solutions of source localization problems[J]. IEEE Trans. Signal Process, 2008,56:1770-1778.

[5]KAUNE R, HORST J, KOCH W. Accuracy Analysis for TDOA Localization in Sensor Networks[C].2011 Proceedings of the 14th IEEE International Conference on Information Fusion (FUSION), 2011.1-8.

[6]JI Y, YU C, A B D O. Systematic bias correction in source localization[J]. IEEE Transactions on Aerospace and Electronic Systems, 2013,49:1692-1709.

[7]JI Y, YU C, WEI J, A B. Localization bias reduction in wireless sensor networks[J]. IEEE Transactions on Industrial Electronics, 2015,62:3004-3016.

[8]DIAZ E M, MULLER F de P, JIMENEZ A R. Evaluation of AHRS Algorithms for Inertial Personal Localization in Industrial Environments[C]. IEEE International Conference on Industrial Technology (ICIT), 2015.3412-3417.

A Highly Accurate Localization System Based on Hybrid IMU and Environmental Sensors system

SONG Shu-peng1, CHEN Xue-chen1, WANG Da-qiang2, BI Yang2,FENG Zheng2, WANG Peng-yu2

(1.SchoolofElectronicsandinformationTechnology,SunYat-senUniversity,Guangzhou510275,Guangdong,China;2.LangyashanPumpedStroragePowerStation,Chuzhou239000,Anhui,China)

Abstract:Traditional localization system asks for at least three environmental anchor nodes to assist the unknown nodes to identify their positions. However, in the real world, due to many kinds of uncontrollable factors，which have impact on the communications between the anchor node and unknown node, the requisite can not be always be satisfied. Because of that, in this paper, we propose a platform, which consists of one IMU (Inertial Measurement Unit) and two anchor nodes to achieve the positioning goal. The anchor nodes and unknown node are all constructed on cricket system, which is based on TDOA (time difference of arrival) technology. In the paper, firstly, we estimate the area the unknown node is locating at the current time with the measurements of IMU and the optimal position estimation of last time. Then combing with the ranges measurements among between the unknown node and anchor node, we obtain the optimal and sub-optimal estimations of the current positioning information by MMSE (minimum mean squared error) estimator and MAP (maximum a posterior) estimator. Experimental results show that our scheme can improve the accuracy of the positioning compared to pure inertial solution.

Key words:Localization, Inertial Measurement Unit (IMU), Time Difference Of Arrival (TDOA), Minimum Mean Squared Error(MMSE), Maximum a Posterior Estimation (MAP)

中圖分類號：TP18

文獻(xiàn)標(biāo)志碼：A

文章編號：1671-8755(2016)01-0061-06

作者簡介:宋述鵬(1991—)，碩士研究生，研究方向為無線室內(nèi)定位。E-mail:945599193@qq.com.通信作者：陳雪辰(1984—)，講師，研究方向為無線通信、定位算法、信源信道編碼、分布式壓縮感知。E-mail:chenxch8@mail.sysu.edu.cn

基金項目:國家自然科學(xué)基金項目(61301181)；教育部留學(xué)回國人員項目(教外司留[2014]1685號)；廣東省自然科學(xué)基金項目(2014A030313117)。

收稿日期：2015-12-30