郭 崴，張軒雄

(上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海 200093)

隨著通信技術(shù)的發(fā)展與無線網(wǎng)絡(luò)的普及，基于位置的定位服務(wù)(Location Based Services，LBS)受到廣泛關(guān)注。室外定位技術(shù)以全球定位系統(tǒng)為基礎(chǔ)，包括美國(guó)GPS定位系統(tǒng)、俄羅斯GLONASS定位系統(tǒng)、我國(guó)自主研制的北斗定位系統(tǒng)等。然而，衛(wèi)星信號(hào)難以穿透墻體，且全球定位系統(tǒng)在復(fù)雜環(huán)境下精度較差，導(dǎo)致衛(wèi)星定位系統(tǒng)難以應(yīng)用于室內(nèi)[1]。針對(duì)這一問題，各種室內(nèi)定位技術(shù)應(yīng)運(yùn)而生。

室內(nèi)定位技術(shù)在室內(nèi)行人導(dǎo)航[2]、室內(nèi)機(jī)器人定位[3]、室內(nèi)無人機(jī)定位[4]等領(lǐng)域有著廣泛的應(yīng)用前景。由于室內(nèi)環(huán)境的復(fù)雜性，室內(nèi)定位技術(shù)也多種多樣。用于室內(nèi)定位的感測(cè)信號(hào)有WiFi[5]、ZigBee[6]、RFID[7]、超寬帶(Ultra Wide Band，UWB)、藍(lán)牙[8]、超聲波、慣性導(dǎo)航等[9]。

UWB定位系統(tǒng)作為一種無線定位系統(tǒng)具有精度高、功耗低等優(yōu)點(diǎn)[10]，能夠提供厘米級(jí)的定位精度。但是，UWB定位系統(tǒng)定位性能常受到信號(hào)非視距(None Line of Sight，NLOS)的影響。嚴(yán)重的NLOS將導(dǎo)致定位精度下降，甚至丟失位置信息。為了抑制NLOS對(duì)UWB定位系統(tǒng)的影響，需要將其他定位信息進(jìn)行融合。文獻(xiàn)[11]使用UWB融合視覺里程計(jì)來提高定位精度，但該方法計(jì)算復(fù)雜，難以滿足實(shí)時(shí)性需求。

基于微機(jī)電慣性導(dǎo)航單元(Micro-Electro-Mechanical-System Inertial Measurement Unit，MEMS-IMU)的行人航跡推算(Pedestrian Dead Reckoning，PDR)是一種自主定位導(dǎo)航系統(tǒng)，具有短期精度高的優(yōu)點(diǎn)，但其長(zhǎng)期定位精度存在較大的累積誤差。PDR算法使用慣性傳感器，通過對(duì)行人的步態(tài)進(jìn)行檢測(cè)來估計(jì)行人的步長(zhǎng)，并可結(jié)合方向等信息對(duì)行人位置進(jìn)行累加推算，且不受NLOS影響。文獻(xiàn)[2]利用磁力計(jì)修正行人航向，并結(jié)合零速更新(Zero Velocity Update Procedure，ZUPT)來修正行人位置，但是其總體定位精度依然在米級(jí)，無法滿足高精度定位要求。

室內(nèi)單源定位導(dǎo)航系統(tǒng)無法滿足復(fù)雜室內(nèi)環(huán)境定位需求，因此多源信息組合定位逐漸成為當(dāng)前研究的熱點(diǎn)。文獻(xiàn)[12]利用采集的室內(nèi)環(huán)境地圖信息，提出利用地標(biāo)修正PDR，對(duì)定位精度有一定提升，但地圖環(huán)境數(shù)據(jù)庫(kù)的建立及維護(hù)工作量較大。文獻(xiàn)[13]采用卡爾曼濾波對(duì)WiFi和IMU進(jìn)行融合，但是由于兩種方式的定位精度都在米級(jí)，整體定位精度仍不高。根據(jù)UWB和PDR系統(tǒng)的優(yōu)缺點(diǎn)，采取適當(dāng)?shù)娜诤戏绞綄煞N定位方法進(jìn)行融合，可以有效解決復(fù)雜室內(nèi)場(chǎng)景下的定位問題。

為了提高定位系統(tǒng)的穩(wěn)定性和精度，本文提出了基于行人航跡推算與UWB組合的室內(nèi)定位方法。PDR算法使用加速度傳感器數(shù)據(jù)進(jìn)行步態(tài)檢測(cè)以及步長(zhǎng)估計(jì)，然后計(jì)算出行人的相對(duì)位置變化。UWB通過測(cè)量移動(dòng)節(jié)點(diǎn)與參考節(jié)點(diǎn)之間的距離來計(jì)算行人的絕對(duì)位置。最后，本文利用卡爾曼濾波算法對(duì)多源信息作濾波修正。實(shí)驗(yàn)結(jié)果表明，所提方法的定位精度和系統(tǒng)穩(wěn)定性都有明顯改善。

1 UWB定位

UWB技術(shù)是一種無載波通信技術(shù)，具有穿透性好、功耗低、測(cè)距精度高等優(yōu)點(diǎn)。UWB定位系統(tǒng)基于無線測(cè)距技術(shù)[14]，其定位過程可以分成兩步：距離測(cè)量和位置解算。

本文采用基于到達(dá)時(shí)間(Time of Arrival，TOA)的測(cè)距方法，使用雙邊雙向測(cè)距方法(Double-Sided Two-Way Ranging，DS-TWR)[15]，測(cè)距流程如圖1所示。

圖1 雙邊雙向測(cè)距流程Figure 1. Double side two-way ranging

該方法能有效抑制設(shè)備時(shí)鐘漂移引起的測(cè)距誤差，且不需要設(shè)備間時(shí)鐘同步，簡(jiǎn)化了系統(tǒng)實(shí)現(xiàn)過程。實(shí)現(xiàn)完整的1次DS-TWR測(cè)距需要進(jìn)行3次通信：首先設(shè)備A向設(shè)備B發(fā)送測(cè)距數(shù)據(jù)包，發(fā)起1次測(cè)距請(qǐng)求；然后設(shè)備B接收到數(shù)據(jù)包后發(fā)送響應(yīng)數(shù)據(jù)包；最后設(shè)備A發(fā)送結(jié)束數(shù)據(jù)包，完成1次完整的測(cè)距過程。所有數(shù)據(jù)包的收發(fā)時(shí)間戳?xí)辉O(shè)備內(nèi)部的高精度時(shí)鐘記錄下來：troundA為設(shè)備A發(fā)送請(qǐng)求數(shù)據(jù)包到接收到響應(yīng)數(shù)據(jù)包的時(shí)間；treplyB為設(shè)備B接收到請(qǐng)求數(shù)據(jù)包到發(fā)送響應(yīng)數(shù)據(jù)包的時(shí)間；treplyA為設(shè)備A接收到響應(yīng)數(shù)據(jù)包到發(fā)送結(jié)束數(shù)據(jù)包的時(shí)間；troundB為設(shè)備B發(fā)送響應(yīng)數(shù)據(jù)包到收到結(jié)束數(shù)據(jù)包的時(shí)間；TOF為電磁波飛行時(shí)間(Time of Flight，TOF)。

根據(jù)圖1，設(shè)備A與設(shè)備B之間電磁波飛行時(shí)間tTOF由式(1)計(jì)算得到

(1)

使用式(2)計(jì)算出設(shè)備A(移動(dòng)節(jié)點(diǎn))與設(shè)備B(參考節(jié)點(diǎn))之間的距離d

d=c×tTOF

(2)

其中，c為電磁波在空氣中的飛行速度。

(3)

(4)

將式(4)寫成如下矩陣形式

HX=b

(5)

其中，

X為行人的估計(jì)位置。式(5)的最小二乘解為式(6)。

X=(HTH)-1HTb

(6)

2 行人航跡推算

室內(nèi)環(huán)境中，高精度慣性器件體積大，不利于攜帶，而MEMS-IMU體積小，功耗低，方便集成，易于攜帶。但是，傳統(tǒng)MEMS-IMU精度低，如果采用傳統(tǒng)的基于積分的捷聯(lián)慣導(dǎo)[16]定位方式，會(huì)導(dǎo)致定位結(jié)果迅速發(fā)散。行人航跡推算通過檢測(cè)行人步態(tài)可以有效抑制累積誤差迅速增大的問題。該方法基于確定的起始位置，利用步長(zhǎng)和航向角推算下一時(shí)刻行人的相對(duì)位置。行人航跡推算算法計(jì)算行人相對(duì)位置包括3個(gè)方面：步態(tài)檢測(cè)、步長(zhǎng)估計(jì)以及航向推算。

步態(tài)檢測(cè)方法包括基于角速度和基于加速度兩種方法?；诮撬俣鹊牟綉B(tài)檢測(cè)使用安裝在行人腳部或手部的角速度傳感器，通過檢測(cè)角速度周期變化來檢測(cè)行人行走步數(shù)。基于加速度的步態(tài)檢測(cè)使用加速度傳感器，相較于角速度步態(tài)檢測(cè)，傳感器安裝位置更加靈活。本文采用基于加速度的步態(tài)檢測(cè)方法。此外，為了避免傳感器安裝位置對(duì)步態(tài)檢測(cè)的影響，使用三軸加速度模值進(jìn)行步態(tài)檢測(cè)。加速度模值可以使用式(7)進(jìn)行計(jì)算。

(7)

其中，axi、ayi、azi為各樣點(diǎn)三軸加速度的值。

圖2 基于加速度的步態(tài)檢測(cè)Figure 2. Step detection based on acceleration

如圖2所示，行人在正常行走時(shí)，加速度數(shù)據(jù)會(huì)呈現(xiàn)周期性變化，加速度峰值點(diǎn)為行人腳落地的時(shí)刻，即一步步伐。本文采用常用的峰值檢測(cè)方法來檢測(cè)行人狀態(tài)，并通過設(shè)定適當(dāng)?shù)募铀俣纫约爸芷陂撝祦硖岣邫z測(cè)精度。根據(jù)文獻(xiàn)[17]，峰值檢測(cè)方法的檢測(cè)精度可以達(dá)到98%以上。

步長(zhǎng)可以通過加速度數(shù)據(jù)進(jìn)行估計(jì)，本文應(yīng)用文獻(xiàn)[18～19]中的Weinberg模型進(jìn)行步長(zhǎng)估計(jì)

(8)

其中，S(k)為第k步長(zhǎng)；Amax(k)和Amin(k)是第k步中的最大和最小加速度值；K為常數(shù)，可以通過離線數(shù)據(jù)訓(xùn)練得到。

PDR中的航向可以使用下面的模型進(jìn)行推算

ψ(k)=ψ(k-1)+ΔT×∑ω

(9)

其中，ψ(k)和ψ(k-1)分別為k與k-1時(shí)刻的航向，初始航向在初始時(shí)進(jìn)行校準(zhǔn)；ΔT為陀螺儀采樣數(shù)據(jù)間隔；∑ω為k-1到k時(shí)刻的陀螺儀輸出值之和。

根據(jù)上文得到的步長(zhǎng)和航向信息，并結(jié)合當(dāng)前已知的位置，可以出推算當(dāng)前行人的位置為式(10)。

(10)

3 基于卡爾曼濾波的數(shù)據(jù)融合算法

本文提出的UWB/PDR組合室內(nèi)定位系統(tǒng)框圖如圖3所示。定位系統(tǒng)由慣性器件和UWB模塊組成，其中慣性器件包含三軸加速度計(jì)和三軸陀螺儀，UWB模塊包含3個(gè)參考節(jié)點(diǎn)和1個(gè)移動(dòng)節(jié)點(diǎn)。

圖3 UWB/PDR組合定位系統(tǒng)框圖Figure 3. Block diagram of the UWB/PDR integrated system

根據(jù)圖3，PDR系統(tǒng)通過讀取慣性器件數(shù)據(jù)預(yù)測(cè)行人每一步的步長(zhǎng)和航向，而UWB系統(tǒng)通過TOA測(cè)距得到節(jié)點(diǎn)間距離，經(jīng)過NLOS檢測(cè)剔除異常測(cè)距值；然后，使用最小二乘法得到行人位置；最終，使用卡爾曼濾波進(jìn)行融合，輸出融合后的位置。

3.1 非視距檢測(cè)

UWB系統(tǒng)的測(cè)距精度易受非視距影響，室內(nèi)的各種障礙物都容易產(chǎn)生NLOS誤差。這種誤差不是隨機(jī)噪聲造成的，使用卡爾曼濾波時(shí)，可能導(dǎo)致濾波器發(fā)散。為了消除NLOS影響，本文提出一種非視距檢測(cè)方法，在融合濾波前過濾掉受到NLOS干擾的UWB測(cè)距數(shù)據(jù)，以便提高系統(tǒng)穩(wěn)定性。

通常行人行走速度有限，UWB系統(tǒng)可以以較高的速率進(jìn)行位置更新。在UWB兩次測(cè)量間隔時(shí)間(40 ms)內(nèi)，行人移動(dòng)距離較小。假設(shè)UWB測(cè)距值不會(huì)發(fā)生突變，如果滿足如下條件，則認(rèn)為存在NLOS影響

min(αi，αi+1，αi+2)>η

(11)

其中，αi=TOFi-(TOFi-1+TOFi-2+TOFi-3)/3為第i次測(cè)量值與其前3次測(cè)量均值的差。根據(jù)式(11)，如果檢測(cè)到測(cè)量結(jié)果大于η則認(rèn)為進(jìn)入NLOS區(qū)域；如果滿足式(12)，則認(rèn)為離開NLOS域。

max(αi，αi+1，αi+2)<ρ

(12)

3.2 卡爾曼濾波

在排除NLOS帶來的影響后，本文采用基于卡爾曼濾波的數(shù)據(jù)融合算法對(duì)UWB和PDR信息進(jìn)行融合，以便得到更優(yōu)的定位結(jié)果。根據(jù)卡爾曼濾波準(zhǔn)則，定義系統(tǒng)的狀態(tài)轉(zhuǎn)移方程為

X(k)=AX(k-1)+ω(k-1)

(13)

(14)

系統(tǒng)的測(cè)量方程定義為

Z(k)=HX(k)+v(k)

(15)

卡爾曼濾波可以分為預(yù)測(cè)和更新兩步。根據(jù)k-1步狀態(tài)以及PDR系統(tǒng)信息，建立如下卡爾曼預(yù)測(cè)方程

(16)

其中，Xk|k-1為先驗(yàn)狀態(tài)估計(jì)矩陣；Mk|k-1為先驗(yàn)協(xié)方差矩陣；Q為過程噪聲矩陣。

基于UWB系統(tǒng)測(cè)量值，建立如下卡爾曼更新方程

(17)

其中，Xk|k為后驗(yàn)狀態(tài)矩陣；Mk|k為后驗(yàn)協(xié)方差矩陣；Kk為卡爾曼增益。

4 實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證算法的定位性能，搭建了基于UWB與PDR組合的定位系統(tǒng)，該系統(tǒng)由1個(gè)移動(dòng)節(jié)點(diǎn)與3個(gè)參考節(jié)點(diǎn)組成。其中移動(dòng)節(jié)點(diǎn)由行人佩戴且集成有慣性導(dǎo)航模塊(MPU9250)以及Decawave公司生產(chǎn)的DWM1000模塊；參考節(jié)點(diǎn)僅集成DWM1000模塊。實(shí)驗(yàn)中，移動(dòng)節(jié)點(diǎn)安裝在行人頭頂，參考節(jié)點(diǎn)安裝在室內(nèi)固定位置且安裝在同一高度(2 m)。行人沿著固定路徑行走，通過USB或者無線將采集到的數(shù)據(jù)上傳到PC端并進(jìn)行位置解算。

4.1 PDR定位測(cè)試

為了驗(yàn)證PDR定位的性能，行人將慣性傳感器佩戴在頭頂并沿著圖3中虛線路徑行走3圈。

圖4 行人航跡推算結(jié)果Figure 4.Results of pedestrian dead reckoning

從圖4可以看出，消費(fèi)級(jí)MEMS-IMU精度較差，使用PDR算法對(duì)行人步態(tài)進(jìn)行識(shí)別能一定程度上彌補(bǔ)傳統(tǒng)慣導(dǎo)算法累積誤差迅速增大的缺陷。然而，PDR算法在步長(zhǎng)估計(jì)和航向估計(jì)方面均存在誤差，雖然該方法在短時(shí)間內(nèi)精度較高，但是累積誤差同樣會(huì)隨著時(shí)間越來越大。從圖中可以看出，經(jīng)過1圈行走過后，PDR算法得到的軌跡已偏離實(shí)際軌跡，因此單一PDR算法難以滿足長(zhǎng)時(shí)間、高精度定位的需求。

4.2 UWB定位測(cè)試

為了測(cè)試UWB定位系統(tǒng)的定位性能，行人頭頂配戴UWB移動(dòng)節(jié)點(diǎn)沿著圖5中虛線路徑行走1圈，實(shí)驗(yàn)中UWB系統(tǒng)位置更新速率約為25 Hz。從圖中可以看出，UWB系統(tǒng)不存在累積誤差，但是其測(cè)距易受NLOS及其本身隨機(jī)誤差的影響，導(dǎo)致定位結(jié)果波動(dòng)較大。

圖5 UWB定位結(jié)果Figure 5. Results of UWB positioning

根據(jù)UWB和PDR定位結(jié)果，兩種定位方法各有優(yōu)缺點(diǎn)，使用卡爾曼濾波可以有效融合兩種定位系統(tǒng)的結(jié)果。

4.3 UWB/PDR組合定位測(cè)試

為了實(shí)現(xiàn)更加穩(wěn)定、精度更高的定位，本文使用卡爾曼濾波進(jìn)行融合。首先測(cè)試非視距影響下的定位性能，實(shí)驗(yàn)中，行人沿著圖5中虛線路徑行走1圈，且參考節(jié)點(diǎn)與行人之間沒有障礙物阻擋。

圖6 定位結(jié)果比較Figure 6. Comparison of positioning results

從圖6可以看出，UWB/PDR組合定位算法能有效修正PDR系統(tǒng)定位產(chǎn)生的積累誤差，并減輕UWB系統(tǒng)定位抖動(dòng)的缺點(diǎn)，與真實(shí)行走路徑更加接近?？柭诤辖馑惴椒ㄔ谝欢ǔ潭壬贤ㄟ^PDR方法中的距離約束剔除了精度較差的定位結(jié)果，減小了UWB定位結(jié)果的波動(dòng)，提高了整體的定位精度。如表1 所示為3種定位方式的誤差結(jié)果比較。

表1 定位誤差比較Table 1. Position error comparison /m

為了驗(yàn)證非視距情況下定位系統(tǒng)的性能，在室內(nèi)放置書架模擬非視距場(chǎng)景，得到如圖7所示的定位軌跡圖。實(shí)驗(yàn)中，在非NLOS影響下，UWB測(cè)距誤差通常在10 cm左右。而在NLOS情況下，UWB信號(hào)由于障礙物遮擋發(fā)生反射、多徑等，導(dǎo)致測(cè)距距離通常大于正常值。因此，根據(jù)實(shí)際實(shí)驗(yàn)，將式(11)、式(12)中的閾值η、ρ分別設(shè)為經(jīng)驗(yàn)值20 cm和-15 cm。

根據(jù)圖7，由于書架的遮擋，UWB測(cè)距誤差增大，導(dǎo)致靠近書架處UWB計(jì)算的坐標(biāo)偏離參考路徑較大。使用本文的NLOS檢測(cè)方法剔除異常測(cè)距值并與PDR進(jìn)行融合后，得到的軌跡更加符合參考路徑，表明定位系統(tǒng)在短暫NLOS影響下依然有較高的精度。

圖7 NLOS定位結(jié)果Figure 7. Positioning results in NLOS scene

5 結(jié)束語

本文提出了一種UWB與PDR組合的室內(nèi)行人定位算法。該算法通過卡爾曼濾波器將UWB定位系統(tǒng)與PDR系統(tǒng)相結(jié)合，解決了PDR累積誤差大的問題，并改善了UWB易受NLOS影響的缺陷。為了驗(yàn)證組合定位系統(tǒng)的性能，本文搭建了實(shí)驗(yàn)平臺(tái)。實(shí)驗(yàn)結(jié)果表明，本文提出的組合定位系統(tǒng)具有較好的精度和穩(wěn)定性。下一步還需要研究在NLOS影響下，在不直接剔除誤差較大值時(shí)對(duì)UWB測(cè)距誤差進(jìn)行修正的方法，以及利用定位結(jié)果對(duì)步長(zhǎng)預(yù)測(cè)進(jìn)行修正的方法，以便實(shí)現(xiàn)更高精度的定位。