高精度室內(nèi)融合定位算法研究

黃 健，楊國(guó)偉*，胡起立，畢美華，李 晶，李 娜

(1.杭州電子科技大學(xué) 通信工程學(xué)院，浙江 杭州 310018；2.杭州海關(guān)技術(shù)中心 環(huán)境與化學(xué)安全檢測(cè)研究所，浙江 杭州 311200；3.中國(guó)電子科技集團(tuán)公司航天信息應(yīng)用技術(shù)重點(diǎn)實(shí)驗(yàn)室，河北 石家莊 050081)

0 引言

近年來(lái)，隨著無(wú)線通信技術(shù)的迅速發(fā)展，基于位置的服務(wù)在各個(gè)領(lǐng)域中都爆發(fā)出急速增長(zhǎng)的需求，精確定位技術(shù)開(kāi)始廣泛地應(yīng)用于各個(gè)領(lǐng)域。室內(nèi)定位作為導(dǎo)航定位的“最后一公里”，一直是研究機(jī)構(gòu)和企業(yè)關(guān)注的熱點(diǎn)[1]。近十多年來(lái)，大量的新興技術(shù)如地磁、WiFi、藍(lán)牙、射頻識(shí)別、可見(jiàn)光[2-3]和超聲波等相繼應(yīng)用于室內(nèi)定位，但是這些技術(shù)都有各自的局限性，很難實(shí)現(xiàn)在實(shí)際復(fù)雜多變場(chǎng)景中的高精度定位。近年來(lái)，超寬帶(Ultra-Wideband，UWB)技術(shù)因具有很高的分辨率和很強(qiáng)的抗多徑能力能實(shí)現(xiàn)高精度測(cè)距而受到廣泛關(guān)注。

事實(shí)上，在非視距(Non-Line-of-Sight,NLOS)室內(nèi)環(huán)境中，由于障礙物的遮擋會(huì)導(dǎo)致UWB定位精度急速下降，因此單一的室內(nèi)定位方式仍然難以在復(fù)雜的室內(nèi)環(huán)境中實(shí)現(xiàn)穩(wěn)定且高精度的定位。慣性導(dǎo)航技術(shù)作為自主導(dǎo)航技術(shù)[4]，可以輔助UWB技術(shù)渡過(guò)NLOS的室內(nèi)環(huán)境。又由于低成本的微機(jī)電系統(tǒng)(MEMS)慣性傳感器的精度難以匹配傳統(tǒng)慣性導(dǎo)航算法，所以需要采用行人航位推算(Pedestrian Dead Reckoning，PDR)算法來(lái)代替?zhèn)鹘y(tǒng)的積分方法。但MEMS慣性傳感器的誤差隨著系統(tǒng)運(yùn)行時(shí)間增加，會(huì)出現(xiàn)較大的誤差累積情況。為解決單一定位技術(shù)在室內(nèi)定位時(shí)的不足，眾多學(xué)者提出采用融合定位方式完成室內(nèi)定位。文獻(xiàn)[5]實(shí)現(xiàn)了WiFi與慣導(dǎo)結(jié)合的室內(nèi)定位系統(tǒng)，但定位精度仍相對(duì)較低。文獻(xiàn)[6]將慣導(dǎo)定位與地圖結(jié)合實(shí)現(xiàn)手機(jī)端的室內(nèi)定位，但是要求正常持握手機(jī)姿態(tài)，實(shí)用性不好。文獻(xiàn)[7]對(duì)WiFi和PDR室內(nèi)組合定位進(jìn)行了研究，通過(guò)無(wú)跡卡爾曼濾波算法融合二者定位數(shù)據(jù),但定位精度仍受限于WiFi定位精度。文獻(xiàn)[8]使用擴(kuò)展卡爾曼濾波融合UWB和PDR數(shù)據(jù)，但無(wú)法校準(zhǔn)航向角誤差。

本文基于融合定位的思想，充分利用UWB和PDR各自的優(yōu)勢(shì)，提出一種基于UWB與PDR的新型融合定位方法。在NLOS環(huán)境中，UWB定位產(chǎn)生了較大誤差甚至無(wú)法定位時(shí)，借助PDR減少因?yàn)镹LOS而產(chǎn)生的定位誤差，同時(shí)可以借助UWB高精度的定位信息來(lái)消除PDR誤差累積。本文重點(diǎn)聚焦了二者融合定位過(guò)程中的主輔切換方法和補(bǔ)償方法，實(shí)驗(yàn)證明本文方法的定位軌跡更加貼近真實(shí)軌跡，有效地降低了NLOS定位誤差，提高了室內(nèi)定位的性能。

1 UWB定位模型

1.1 UWB測(cè)距原理

本文UWB測(cè)距采用簡(jiǎn)單高效的TOA技術(shù)方案[9]。由于基于TOA的測(cè)距通常需要基站與標(biāo)簽之間時(shí)間同步，而精確的時(shí)間同步實(shí)現(xiàn)難度大。UWB測(cè)距首先采用了單側(cè)雙向測(cè)距(SS-TWR)，該方法通過(guò)信號(hào)在標(biāo)簽與基站之間的來(lái)回傳輸，避免了標(biāo)簽與基站之間的時(shí)間同步問(wèn)題，測(cè)距流程如圖1所示。

(1)

該SS-TWR方法沒(méi)有標(biāo)簽與基站的時(shí)間同步問(wèn)題。但是，由于標(biāo)簽與基站之間存在微小的時(shí)鐘偏移，如果標(biāo)簽和基站的時(shí)鐘偏移分別為eA和eB，則飛行時(shí)間誤差為：

(2)

由式(2)可以看出，飛行時(shí)間的誤差隨著Treply增加而增加，并且同設(shè)備的時(shí)鐘偏移也有關(guān)系。所以SS-TWR的精度也不夠高，因此提出了一種改進(jìn)雙向測(cè)距法——雙邊雙向測(cè)距(ADS-TWR)[10]。

ADS-TWR方法是SS-TWR的升級(jí)改進(jìn)版本，測(cè)距流程如圖2所示。

圖2 ADS-TWR測(cè)距流程Fig.2 ADS-TWR ranging process

(3)

誤差分析如下：

(4)

由式(4)可以看出，ADS-TWR的誤差與延遲時(shí)間Treply無(wú)關(guān)。同樣的時(shí)鐘差，ADS-TWR比SS-TWR的誤差小很多，可以得到更高的測(cè)距精度。獲得測(cè)距距離后，由于UWB模塊出廠時(shí)天線就存在接收信號(hào)延遲的問(wèn)題，對(duì)UWB模塊進(jìn)行測(cè)距修正，以提高實(shí)際使用中的測(cè)距精度。

1.2 UWB定位與天牛須搜索算法

在無(wú)線傳感器網(wǎng)絡(luò)定位算法中，全局搜索算法越來(lái)越受到關(guān)注。全局搜索算法將尋常三維定位的非線性方程組求解問(wèn)題轉(zhuǎn)化為最優(yōu)化問(wèn)題，在很多領(lǐng)域得到成功應(yīng)用[11]，本文嘗試將天牛須搜索(Beetle Antennae Search，BAS)算法應(yīng)用到UWB定位算法中。

(5)

(6)

st=cδt，

(7)

δt=ηδt-1,

(8)

下標(biāo)r，l表示右側(cè)和左側(cè)；上標(biāo)t表示第t次迭代；δ為天牛的步長(zhǎng)；c為質(zhì)心到須的距離與步長(zhǎng)之間的系數(shù)；η為步長(zhǎng)衰減系數(shù)。天牛在t時(shí)刻的坐標(biāo)為：

(9)

式中，sign(·)為符號(hào)函數(shù)。BAS算法的整體流程如下：

① 設(shè)置算法的迭代次數(shù)、初始步長(zhǎng)和步長(zhǎng)的衰減系數(shù)，并隨機(jī)生成天牛的初始位置。

② 隨機(jī)生成單位向量b，根據(jù)式(5)和式(6)計(jì)算天牛左須和右須的坐標(biāo)和適應(yīng)度函數(shù)，沿著較好方向前進(jìn)，并更新下一次迭代使用的搜索范圍和步長(zhǎng)。

③ 根據(jù)式(9)，經(jīng)過(guò)迭代不斷更新天牛的空間坐標(biāo)并更新最佳值。

④ 判斷是否達(dá)到最大迭代次數(shù)或全局最優(yōu)解的適應(yīng)值是否滿足最小界限，若未達(dá)到轉(zhuǎn)到步驟②。

將BAS算法應(yīng)用到UWB定位的關(guān)鍵就是設(shè)置適應(yīng)度函數(shù)(食物氣味強(qiáng)度)。f(x)為本文設(shè)計(jì)的適應(yīng)度函數(shù)，表示為：

(10)

2 PDR模型

PDR算法是慣性導(dǎo)航的一種簡(jiǎn)化算法，在慣性傳感器精度不高時(shí)，利用人走動(dòng)產(chǎn)生周期性變化的特點(diǎn)，估計(jì)出人行走的步數(shù)和步長(zhǎng)，再獲得每一步的航向角即可以求解出人行走前后的相對(duì)位置。PDR定位原理如圖3所示。

圖3 PDR定位原理Fig.3 PDR positioning principle

若(xk,yk)為人的當(dāng)前位置，Lk為估計(jì)的步長(zhǎng)，ψk為行走的航向角，則行人下一步的位置(xk+1,yk+1)可以通過(guò)下式求出：

(11)

PDR算法需要知道開(kāi)始第一步的具體位置和方向，由此推算出后面行走的相對(duì)位置。所以步態(tài)檢測(cè)、步長(zhǎng)估計(jì)和航向估計(jì)是PDR算法的3個(gè)核心步驟。

2.1 步態(tài)檢測(cè)

因?yàn)閭尾ǚ宓拇嬖冢瑐鹘y(tǒng)的基于閾值峰值檢測(cè)法會(huì)出現(xiàn)漏檢或誤檢的情況，本文采用了一種基于時(shí)間周期性的峰值檢測(cè)法作為步態(tài)檢測(cè)的方法[13]。當(dāng)人在行走時(shí)，每一步都可以看成一個(gè)周期，并且每一步相隔的時(shí)間都應(yīng)該差不多，所以可以利用該特點(diǎn)并結(jié)合峰值檢測(cè)法來(lái)進(jìn)行步態(tài)檢測(cè)。

首先，通過(guò)傳統(tǒng)的波峰檢測(cè)法求得第1個(gè)波峰與第2個(gè)波峰相隔的時(shí)間，記為t1，根據(jù)加速度的周期性變化原理，第2個(gè)波峰與第3個(gè)波峰之間的相隔時(shí)間也大概為t1，因此可以在第2個(gè)波峰之后的t1時(shí)間附近尋找第3個(gè)波峰，找到第3個(gè)波峰之后更新相隔時(shí)間，以此類推找到所有波峰點(diǎn)。計(jì)算如下：

(12)

(13)

式中，δ為波動(dòng)參數(shù)，可由實(shí)驗(yàn)測(cè)試進(jìn)行估計(jì)。為了保證每一步的周期具有強(qiáng)的相關(guān)性，每一步后要對(duì)Ti進(jìn)行更新。最后，由于偽波峰的影響，經(jīng)過(guò)篩選，在波動(dòng)范圍內(nèi)產(chǎn)生多個(gè)極值點(diǎn)，取這幾個(gè)極值點(diǎn)中波峰最大的點(diǎn)作為真正的波峰點(diǎn)。

2.2 步長(zhǎng)估計(jì)

獲得步數(shù)后，要估計(jì)對(duì)應(yīng)每一步的長(zhǎng)度。因?yàn)槊總€(gè)人的生理狀況不一樣，步長(zhǎng)也因人而異。當(dāng)然，同一個(gè)人在行走過(guò)程中，每一步長(zhǎng)度也有所不同，所以基于常數(shù)模型的步長(zhǎng)估計(jì)很難適應(yīng)實(shí)際情況。本文的自適應(yīng)算法結(jié)合了基于加速度幅值變化非線性步長(zhǎng)估計(jì)[14]和基于步頻的線性步長(zhǎng)估計(jì)[15]，計(jì)算如下：

(14)

式中，α，β與人的生理因素有關(guān)，可由大量實(shí)驗(yàn)得出，α取值在0.4～0.5，β取值在0～0.1；amax，amin分別表示該步周期內(nèi)加速度的最大值和最小值；fk為對(duì)應(yīng)的每一步的行走頻率；φ是一個(gè)常數(shù)。

由于噪聲的影響，加速度數(shù)據(jù)會(huì)引入噪聲誤差，所以要對(duì)采集的加速度數(shù)據(jù)進(jìn)行濾波來(lái)消除噪聲，提高步態(tài)檢測(cè)的準(zhǔn)確率。本文使用滑動(dòng)平均濾波來(lái)降低噪聲信息。采用上述的步態(tài)檢測(cè)法和步長(zhǎng)模型，并使用陀螺儀計(jì)算得到前進(jìn)的航向角改變量，根據(jù)UWB前期準(zhǔn)確定位的結(jié)果得到初始值，再通過(guò)式(11)就可以得到航位推算位置。

3 UWB和PDR融合定位

3.1 UWB NLOS定位場(chǎng)景判定和優(yōu)化

(15)

由于濾波器的遞推作用，受NLOS污染嚴(yán)重而偏差較大的坐標(biāo)值不僅使當(dāng)前時(shí)刻的坐標(biāo)值估計(jì)發(fā)生較大偏差，并且由于卡爾曼濾波器的記憶性，使后續(xù)的坐標(biāo)估計(jì)都受到嚴(yán)重影響，對(duì)于這些不準(zhǔn)確的坐標(biāo)點(diǎn)，應(yīng)當(dāng)舍棄。在被舍棄的點(diǎn)上，用狀態(tài)預(yù)測(cè)值代替狀態(tài)估計(jì)值，所以本文引入偏移卡爾曼濾波器消除NLOS誤差[17]。當(dāng)濾波過(guò)程中沒(méi)有進(jìn)入NLOS場(chǎng)景，卡爾曼濾波則正常迭代；若進(jìn)入NLOS場(chǎng)景，通過(guò)把卡爾曼增益設(shè)為0來(lái)舍去這個(gè)測(cè)量值，并用狀態(tài)預(yù)測(cè)值來(lái)代替狀態(tài)估計(jì)值。x軸定位數(shù)據(jù)的卡爾曼濾波如圖4所示，當(dāng)沿著y軸方向運(yùn)動(dòng)時(shí)，坐標(biāo)x軸數(shù)據(jù)變化較小，門(mén)限值選取合理，該方法對(duì)x軸坐標(biāo)NLOS誤差抑制明顯。

圖4 x軸定位數(shù)據(jù)的卡爾曼濾波Fig.4 Kalman filter of x-axis positioning data

但是當(dāng)運(yùn)動(dòng)方向改變，沿著x軸方向運(yùn)動(dòng)時(shí)，濾波模型發(fā)生變化，若濾波器的參數(shù)保持不變，x軸數(shù)據(jù)變化過(guò)快時(shí)，將影響正常濾波，嚴(yán)重時(shí)甚至造成濾波發(fā)散，這時(shí)必須改變?yōu)V波器參數(shù)增強(qiáng)濾波收斂效果。數(shù)據(jù)急速變化時(shí)，可以通過(guò)增加過(guò)程噪聲Q值，Q值越大，代表越信任實(shí)際測(cè)量值，以提高此段的跟蹤能力，從而促使該段估計(jì)值能較快收斂，但Q值也不能太大，當(dāng)Q值太大時(shí)濾波數(shù)據(jù)將基本與實(shí)際測(cè)量值相等，無(wú)法做殘差判斷。由圖4可以看出，改變了Q值的偏移卡爾曼濾波能更好地適應(yīng)數(shù)據(jù)急速變化的情況。只通過(guò)x軸的殘差數(shù)據(jù)Δdx來(lái)判斷NLOS場(chǎng)景是不夠的，對(duì)y軸數(shù)據(jù)也做相同處理得到殘差數(shù)據(jù)Δdy，對(duì)二者做聯(lián)合判斷，2個(gè)殘差值只要有一個(gè)大于閾值ε就可以判定進(jìn)入NLOS場(chǎng)景。

通過(guò)濾波器來(lái)更好地獲取運(yùn)動(dòng)方向的變化，其關(guān)鍵是需要實(shí)現(xiàn)動(dòng)態(tài)地取Q值?？梢岳肞DR的航向角來(lái)輔助濾波器動(dòng)態(tài)地取Q值。PDR定位是一種相對(duì)定位方法，開(kāi)始定位時(shí)需要獲得PDR的初始坐標(biāo)點(diǎn)和初始航向角。PDR的初始位置可由UWB定位的坐標(biāo)獲得，PDR航向角初始值由初始時(shí)刻UWB的N個(gè)準(zhǔn)確定位結(jié)果的位置共同決定。如果只選用UWB的2個(gè)定位結(jié)果獲得的初始航向角，初始航向角有時(shí)會(huì)因?yàn)槎ㄎ坏牟▌?dòng)產(chǎn)生較大的誤差。因?yàn)閁WB數(shù)據(jù)獲取頻率通常是大于PDR每一步的頻率，所以在PDR進(jìn)行第一步定位前有N個(gè)UWB定位數(shù)據(jù)。對(duì)這N個(gè)數(shù)據(jù)做線性擬合[18]，得到的直線斜率作為PDR初始航向角，減少定位波動(dòng)對(duì)初始航向角的影響。N的值由實(shí)際UWB的采樣頻率和實(shí)際場(chǎng)景決定，本文N值取4。獲得PDR的初始坐標(biāo)和初始航向角后，就可以獲得PDR每一步的坐標(biāo)和航向角，知道大概的運(yùn)動(dòng)方向?？紤]x軸坐標(biāo)為濾波對(duì)象，當(dāng)PDR的航向角與x軸的夾角在90°附近時(shí)，數(shù)據(jù)變動(dòng)較小，這時(shí)Q值可以取較小的值。當(dāng)PDR的航向角與x軸的夾角在45°附近時(shí)，數(shù)據(jù)變動(dòng)較平緩，這時(shí)Q值可以取適中的值；當(dāng)PDR的航向角與x軸的夾角在0°附近時(shí)，數(shù)據(jù)變動(dòng)較大，這時(shí)Q值可以取較大的值。當(dāng)濾波對(duì)象為y軸坐標(biāo)時(shí)，方法同上。所以，可以根據(jù)PDR的航向角值θ來(lái)動(dòng)態(tài)地對(duì)Q賦值。本文通過(guò)給定的3個(gè)Q值：Q1，Q2和Q3，來(lái)覆蓋整個(gè)角度范圍(0°,360°)。Q1，Q2，Q3值根據(jù)實(shí)際數(shù)據(jù)和濾波情況來(lái)確定，本文設(shè)置的濾波器R=0.542，Q1=1，Q2=0.4，Q3=0.05，則有：

(16)

式中，QX為x軸坐標(biāo)濾波器Q值；QY為y軸坐標(biāo)濾波器Q值。

3.2 PDR誤差累積修正和融合定位

由于PDR定位存在累積誤差，特別是較大角度的拐彎會(huì)對(duì)PDR航向角造成較大的影響。UWB是一種絕對(duì)定位方式，可以通過(guò)UWB定位對(duì)PDR的航向角進(jìn)行修正[19-20]。首先，設(shè)定航向角變化閾值H，當(dāng)某一步相較于初始航向角的航向角變化超過(guò)閾值H時(shí)，認(rèn)為PDR航向角累積誤差過(guò)大，對(duì)這一步的PDR定位進(jìn)行UWB定位替代，用UWB估計(jì)的方向取代PDR的當(dāng)前航向，用UWB定位坐標(biāo)作為PDR的初始坐標(biāo)。若UWB定位剛好處于NLOS場(chǎng)景，先不修正，等到UWB定位離開(kāi)NLOS場(chǎng)景時(shí)，再對(duì)PDR定位進(jìn)行修正。

獲得UWB優(yōu)化后的定位數(shù)據(jù)、NLOS情況和誤差累積修正的PDR數(shù)據(jù)后，要對(duì)二者數(shù)據(jù)進(jìn)行融合。在LOS場(chǎng)景，直接以UWB濾波優(yōu)化后的定位數(shù)據(jù)作為定位結(jié)果；在NLOS場(chǎng)景，UWB定位數(shù)據(jù)會(huì)有較大的誤差，因此舍棄UWB定位數(shù)據(jù)，以誤差累積修正后的PDR的定位數(shù)據(jù)代替在NLOS場(chǎng)景中UWB的定位數(shù)據(jù)，將PDR定位數(shù)據(jù)作為NLOS場(chǎng)景中的定位結(jié)果。

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

4.1 UWB定位測(cè)試

測(cè)試在2個(gè)場(chǎng)景進(jìn)行：一個(gè)是杭州電子科技大學(xué)1教428教室，該場(chǎng)景無(wú)遮擋，不存在NLOS情況；另一個(gè)是杭州電子科技大學(xué)1教架空層，遮擋場(chǎng)景多，存在NLOS情況。測(cè)試設(shè)備包括4個(gè)UWB基站和1個(gè)自主設(shè)計(jì)的高精度定位系統(tǒng)，該系統(tǒng)包括用于UWB定位的DW1000模塊和用于PDR定位的MPU9250慣導(dǎo)模塊，實(shí)驗(yàn)測(cè)試數(shù)據(jù)通過(guò)4G模塊上傳到云端。測(cè)試設(shè)備及測(cè)試環(huán)境如圖5所示。

(a) 定位裝置

(b) UWB基站

(c) 教室場(chǎng)景

(d) 架空層車庫(kù)場(chǎng)景

進(jìn)行定位實(shí)驗(yàn)前，首先對(duì)UWB模塊進(jìn)行測(cè)距修正。在LOS環(huán)境下測(cè)距誤差可以通過(guò)調(diào)整UWB模塊的天線延時(shí)參數(shù)來(lái)降低。在LOS環(huán)境下，以0.6 m的間隔獲得各個(gè)點(diǎn)的測(cè)距值，再利用最小二乘法求得一個(gè)修正值,使各個(gè)點(diǎn)修正后的測(cè)距值與真實(shí)距離值的差值平方和最小，最后將該修正值除以光速轉(zhuǎn)換成天線延時(shí)參數(shù)輸入到UWB模塊中。測(cè)距修正前后數(shù)據(jù)如表1所示。測(cè)距修正前，平均測(cè)距誤差為21.19 cm；測(cè)距修正后，平均測(cè)距誤差約為3.9 cm。

表1 UWB測(cè)距值

在教室場(chǎng)景中布置的4個(gè)UWB基站坐標(biāo)分別為(0,0,2.4)，(9.25,0,2.4),(8.95,11.7,2.4)，(-0.1,11.5,2.4)，在車庫(kù)場(chǎng)景中布置的UWB基站坐標(biāo)分別為(0,0,1.8)，(16.8,0,1.8)，(16.8,25.2,1.8)，(0,25.2,1.8)，接著選定幾個(gè)測(cè)試坐標(biāo)進(jìn)行自主設(shè)計(jì)定位系統(tǒng)的UWB定位測(cè)試，分別用測(cè)距未修正最小二乘法算法、測(cè)距修正最小二乘法算法和測(cè)距修正BAS算法進(jìn)行定位并對(duì)結(jié)果進(jìn)行比較。因?yàn)闇y(cè)距精度的限制，UWB定位的z軸定位誤差較大，所以通常UWB定位只關(guān)注x軸和y軸的定位精度，本實(shí)驗(yàn)UWB定位也只考慮了x軸和y軸的二維平面定位精度，并以定位坐標(biāo)與真實(shí)坐標(biāo)的歐式距離來(lái)衡量定位精度，定位結(jié)果如圖6所示。

(a) 教室定位示例

(b) 架空層定位示例

(c) 定位誤差累積分布函數(shù)(CDF)

在教室場(chǎng)景中，測(cè)距未修正最小二乘法的平均定位誤差約為13.79 cm，測(cè)距修正最小二乘法的平均定位誤差約為10.96 cm，而測(cè)距修正BAS算法的平均定位誤差約為9.65 cm。在地下車庫(kù)場(chǎng)景中，測(cè)距未修正最小二乘法的平均定位誤差約為22.05 cm，測(cè)距修正最小二乘法的平均定位誤差約為19.63 cm，而測(cè)距修正BAS算法的平均定位誤差約為16.65 cm?？梢?jiàn)，在場(chǎng)景遮擋情況少、定位結(jié)果較好的情況下，BAS算法雖然也提升了定位精度，但是提升不大。而在場(chǎng)景遮擋情況嚴(yán)重、定位結(jié)果較差的情況下，該優(yōu)化算法的提升效果較好。

4.2 PDR定位測(cè)試

首先對(duì)本文PDR算法的步態(tài)檢測(cè)和步長(zhǎng)估計(jì)進(jìn)行評(píng)估。進(jìn)行步態(tài)檢測(cè)評(píng)估時(shí)，分別走了20，50，100步，再用基于閾值的峰值檢測(cè)法和基于時(shí)間周期性的峰值檢測(cè)法進(jìn)行比較，實(shí)驗(yàn)結(jié)果如圖7所示。由圖7可以看出，基于時(shí)間周期性的峰值檢測(cè)法相較于基于閾值的峰值檢測(cè)法精度更高，提升了約5%的精度。進(jìn)行步長(zhǎng)估計(jì)評(píng)估時(shí)，控制每一步的步長(zhǎng)都保持在60 cm，再用本文的自適應(yīng)步長(zhǎng)估計(jì)算法同經(jīng)典的基于加速度幅值變化非線性步長(zhǎng)估計(jì)進(jìn)行對(duì)比，實(shí)驗(yàn)結(jié)果如圖7(d)所示，可以看出本文的自適應(yīng)步長(zhǎng)估計(jì)算法精度更高。

(a) 真實(shí)步數(shù)為20

(b) 真實(shí)步數(shù)為50

(c) 真實(shí)步數(shù)為100

(d) 步長(zhǎng)誤差分布

PDR實(shí)驗(yàn)在杭州電子科技大學(xué)1教的架空層車庫(kù)進(jìn)行測(cè)試，設(shè)定起點(diǎn)坐標(biāo)為(0,0)，沿著(0,0)，(0,25.3)，(-8.3,25.5)，(-8.3,0)，(0,0)的路線行走，通過(guò)定位系統(tǒng)上慣性傳感器模塊的數(shù)據(jù)采集，對(duì)數(shù)據(jù)進(jìn)行處理，再用本文的PDR算法進(jìn)行軌跡計(jì)算，實(shí)驗(yàn)結(jié)果如圖8所示?？梢钥闯?，在開(kāi)始階段PDR定位能達(dá)到較高的精度，定位誤差能保持在20 cm以內(nèi)。隨著時(shí)間的推移，由于誤差累積，特別是拐角的影響，定位誤差逐漸增大，最后的定位點(diǎn)與實(shí)際點(diǎn)的誤差達(dá)到74 cm，所以PDR單獨(dú)定位時(shí)適合短時(shí)間且拐彎較少的場(chǎng)景。

圖8 PDR定位實(shí)驗(yàn)結(jié)果Fig.8 Diagram of PDR positioning test

4.3 UWB和PDR融合定位

融合定位實(shí)驗(yàn)在杭州電子科技大學(xué)1教的架空層車庫(kù)進(jìn)行測(cè)試。測(cè)試場(chǎng)景如圖9所示，中間有2根柱子作為遮擋來(lái)創(chuàng)造NLOS環(huán)境。

圖9 融合定位測(cè)試場(chǎng)景Fig.9 Environment of fusion positioning test

首先布置4個(gè)UWB基站，其坐標(biāo)分別為(0,0,1.8)，(0.4,16.9,1.8)，(9.3,0.05,1.8)，(8,16.25,1.8)，并設(shè)定起點(diǎn)坐標(biāo)為(1,1)。沿著(1,1)，(1,14)，(9.3,14)，(9.3,1)，(1,1)的路線行走，同時(shí)開(kāi)啟UWB和PDR定位，UWB定位采用上述的BAS算法，PDR定位采用上述的PDR算法，并對(duì)二者的定位進(jìn)行數(shù)據(jù)融合。本文的融合算法針對(duì)UWB NLOS區(qū)域，在LOS區(qū)域仍采用UWB定位，定位結(jié)果如圖10所示。由圖10可以看出，由于柱子遮擋的影響，UWB定位在2個(gè)NLOS處產(chǎn)生了較大的定位誤差，PDR定位在剛開(kāi)始時(shí)誤差也較小，但是經(jīng)過(guò)了2次90°拐彎，航向角產(chǎn)生了約5%的偏移，本文融合定位算法的定位軌跡誤差更小，也更接近實(shí)際真實(shí)軌跡。本文各種定位方式的誤差如下：在NLOS場(chǎng)景中，UWB的平均定位誤差為42.8 cm，PDR的平均定位誤差為17.2 cm，融合定位的誤差為13.7 cm。本文的融合算法較好地利用了2個(gè)系統(tǒng)各自的特點(diǎn)，有效地抑制了NLOS干擾，提高室內(nèi)定位整體準(zhǔn)確性和可靠性。

圖10 UWB和PDR融合定位實(shí)驗(yàn)結(jié)果Fig.10 UWB and PDR fusion positioning experiment

5 結(jié)束語(yǔ)

本文對(duì)UWB和PDR室內(nèi)定位算法進(jìn)行獨(dú)立和融合的改進(jìn)研究。首先將BAS算法應(yīng)用到UWB定位中，在無(wú)遮擋和有遮擋情況下開(kāi)展定位實(shí)驗(yàn)測(cè)試，精度分別提升至9.65，16.65 cm。然后將基于時(shí)間的周期性的峰值檢測(cè)法和自適應(yīng)步長(zhǎng)估計(jì)應(yīng)用到PDR定位算法中，實(shí)驗(yàn)測(cè)試結(jié)果顯示，在最初30 m以內(nèi)和一個(gè)拐彎的情況，精度在20 cm左右，但隨著距離增加和拐彎增多，精度下降較快。盡管上述2種改進(jìn)方法都在一定情況下提升了各自定位精度，但仍無(wú)法解決UWB定位NLOS和PDR定位累計(jì)誤差的問(wèn)題。針對(duì)這2個(gè)問(wèn)題，提出了一種基于UWB和PDR技術(shù)的融合定位算法，根據(jù)基于PDR航向角動(dòng)態(tài)改變過(guò)程噪聲Q值的偏移卡爾曼濾波法來(lái)判別NLOS情況，利用了UWB定位修正PDR累積誤差，同時(shí)利用了PDR定位修正UWB在NLOS下的定位誤差。實(shí)驗(yàn)測(cè)試結(jié)果顯示，所提出的融合定位算法有效地降低了NLOS情況下UWB的定位誤差，平均定位誤差降至0.137 m，比單一UWB定位精度提升了約68%，對(duì)于地下停車場(chǎng)、倉(cāng)庫(kù)和商場(chǎng)等大型室內(nèi)場(chǎng)所的長(zhǎng)時(shí)間、高精度定位具有一定的參考價(jià)值。