基于IMU和兩信標節點的高精度概率推測定位系統

宋述鵬　陳雪晨　王大強　畢　揚　馮　崢　王鵬宇

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

?

基于IMU和兩信標節點的高精度概率推測定位系統

宋述鵬1陳雪晨1王大強2畢揚2馮崢2王鵬宇2

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

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

關鍵詞：定位慣性測量單元到達時間差最小均方估計最大后驗估計

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

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

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

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

1背景和坐標系統

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

(1)

(2)

其中Wi表示標準差為σi的加性高斯白噪聲。

為了讓TDOA測量的標準差更接近實際情況，其方差應該和傳播距離有關。根據文獻[5]得到噪聲方差和信標節點與未知節點距離之間的關系式：

(3)

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

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

(4)

同理對于航向角，有：

圖1　IMU-TDOA系統下當前時刻未知節點區域

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

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

(5)

根據貝葉斯概率理論可以進一步推導后驗概率得到：

(6)

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

(7)

根據高斯噪聲模型，有：

(8)

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

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

(9)

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

3定位精度

x+δx=

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

(10)

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

(11)

(12)

4仿真和實驗結果

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

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

圖2　不同航向角大小和前進距離誤差的RMSE

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

圖3　不同定位方案的RMSE

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

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

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

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

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

以上是基于MATLAB的仿真結果。我們還將算法應用到了實際的無線傳感器網絡中，進行了外場試驗以驗證算法的有效性和實用性。

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

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

圖6　信標節點獲得相應移動節點的距離估計

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

圖7　室內環境的定位實驗

5結論

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

TDOA結合和僅TDOA的方法來實現定位，在室內環境下IMU-TDOA的性能比較好。

參考文獻

[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

文獻標志碼：A

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

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

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

收稿日期：2015-12-30