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

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

?

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

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

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

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

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

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

在國內(nèi)外的研究中，文獻(xiàn)[1]在Non-Line-of-Sight(NLOS)干擾導(dǎo)致的TOA測(cè)量誤差下，提出了半定規(guī)劃SDP的節(jié)點(diǎn)定位算法，避免了非視距傳播對(duì)定位精度的影響。文獻(xiàn)[2]運(yùn)用3個(gè)慣性測(cè)量單元IMU測(cè)量的加速度值，通過加速度誤差消除算法對(duì)加速度值的處理，能夠得到人員前進(jìn)的準(zhǔn)確速度和位置。文獻(xiàn)[3]提出了一種偏差減少的方法，并結(jié)合泰勒級(jí)數(shù)和極大似然估計(jì)得到一個(gè)偏差表達(dá)式，分析表達(dá)式降低偏差，從而提高定位精度。文獻(xiàn)[4]在無線傳感網(wǎng)中使用概率論的方法和基于RSSI的概率分布算法來估計(jì)未知節(jié)點(diǎn)的位置。美國麻省理工學(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)的項(xiàng)目設(shè)計(jì)，但并未真正地完全實(shí)施其方案。

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

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

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

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

(1)

(2)

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

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

(3)

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

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

(4)

同理對(duì)于航向角，有：

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

2最小均方估計(jì)MMSE和最大后驗(yàn)估計(jì)MAP

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

(5)

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

(6)

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

(7)

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

(8)

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

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

(9)

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

3定位精度

x+δx=

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

(10)

因?yàn)闇y(cè)量誤差是已知的獨(dú)立的零均值高斯白噪聲，對(duì)于測(cè)量的距離和方向近似的偏差表達(dá)式為：

(11)

(12)

4仿真和實(shí)驗(yàn)結(jié)果

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

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

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

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

圖3　不同定位方案的RMSE

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

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

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

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

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

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

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

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

圖6　信標(biāo)節(jié)點(diǎn)獲得相應(yīng)移動(dòng)節(jié)點(diǎn)的距離估計(jì)

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

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

5結(jié)論

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

TDOA結(jié)合和僅TDOA的方法來實(shí)現(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)

中圖分類號(hào)：TP18

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

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

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

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

收稿日期：2015-12-30