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

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

0 引言

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

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

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

1 UWB定位模型

1.1 UWB測距原理

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

(1)

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

(2)

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

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

圖2 ADS-TWR測距流程Fig.2 ADS-TWR ranging process

(3)

誤差分析如下：

(4)

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

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

在無線傳感器網(wǎng)絡(luò)定位算法中，全局搜索算法越來越受到關(guān)注。全局搜索算法將尋常三維定位的非線性方程組求解問題轉(zhuǎn)化為最優(yōu)化問題，在很多領(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次迭代；δ為天牛的步長；c為質(zhì)心到須的距離與步長之間的系數(shù)；η為步長衰減系數(shù)。天牛在t時(shí)刻的坐標(biāo)為：

(9)

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

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

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

③ 根據(jù)式(9)，經(jīng)過迭代不斷更新天牛的空間坐標(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)航的一種簡化算法，在慣性傳感器精度不高時(shí)，利用人走動產(chǎn)生周期性變化的特點(diǎn)，估計(jì)出人行走的步數(shù)和步長，再獲得每一步的航向角即可以求解出人行走前后的相對位置。PDR定位原理如圖3所示。

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

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

(11)

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

2.1 步態(tài)檢測

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

首先，通過傳統(tǒng)的波峰檢測法求得第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)

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

2.2 步長估計(jì)

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

(14)

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

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

3 UWB和PDR融合定位

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

(15)

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

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

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

通過濾波器來更好地獲取運(yùn)動方向的變化，其關(guān)鍵是需要實(shí)現(xiàn)動態(tài)地取Q值?？梢岳肞DR的航向角來輔助濾波器動態(tài)地取Q值。PDR定位是一種相對定位方法，開始定位時(shí)需要獲得PDR的初始坐標(biāo)點(diǎn)和初始航向角。PDR的初始位置可由UWB定位的坐標(biāo)獲得，PDR航向角初始值由初始時(shí)刻UWB的N個(gè)準(zhǔn)確定位結(jié)果的位置共同決定。如果只選用UWB的2個(gè)定位結(jié)果獲得的初始航向角，初始航向角有時(shí)會因?yàn)槎ㄎ坏牟▌赢a(chǎn)生較大的誤差。因?yàn)閁WB數(shù)據(jù)獲取頻率通常是大于PDR每一步的頻率，所以在PDR進(jìn)行第一步定位前有N個(gè)UWB定位數(shù)據(jù)。對這N個(gè)數(shù)據(jù)做線性擬合[18]，得到的直線斜率作為PDR初始航向角，減少定位波動對初始航向角的影響。N的值由實(shí)際UWB的采樣頻率和實(shí)際場景決定，本文N值取4。獲得PDR的初始坐標(biāo)和初始航向角后，就可以獲得PDR每一步的坐標(biāo)和航向角，知道大概的運(yùn)動方向?？紤]x軸坐標(biāo)為濾波對象，當(dāng)PDR的航向角與x軸的夾角在90°附近時(shí)，數(shù)據(jù)變動較小，這時(shí)Q值可以取較小的值。當(dāng)PDR的航向角與x軸的夾角在45°附近時(shí)，數(shù)據(jù)變動較平緩，這時(shí)Q值可以取適中的值；當(dāng)PDR的航向角與x軸的夾角在0°附近時(shí)，數(shù)據(jù)變動較大，這時(shí)Q值可以取較大的值。當(dāng)濾波對象為y軸坐標(biāo)時(shí)，方法同上。所以，可以根據(jù)PDR的航向角值θ來動態(tài)地對Q賦值。本文通過給定的3個(gè)Q值：Q1，Q2和Q3，來覆蓋整個(gè)角度范圍(0°,360°)。Q1，Q2，Q3值根據(jù)實(shí)際數(shù)據(jù)和濾波情況來確定，本文設(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定位存在累積誤差，特別是較大角度的拐彎會對PDR航向角造成較大的影響。UWB是一種絕對定位方式，可以通過UWB定位對PDR的航向角進(jìn)行修正[19-20]。首先，設(shè)定航向角變化閾值H，當(dāng)某一步相較于初始航向角的航向角變化超過閾值H時(shí)，認(rèn)為PDR航向角累積誤差過大，對這一步的PDR定位進(jìn)行UWB定位替代，用UWB估計(jì)的方向取代PDR的當(dāng)前航向，用UWB定位坐標(biāo)作為PDR的初始坐標(biāo)。若UWB定位剛好處于NLOS場景，先不修正，等到UWB定位離開NLOS場景時(shí)，再對PDR定位進(jìn)行修正。

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

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

4.1 UWB定位測試

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

(a) 定位裝置

(b) UWB基站

(c) 教室場景

(d) 架空層車庫場景

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

表1 UWB測距值

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

(a) 教室定位示例

(b) 架空層定位示例

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

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

4.2 PDR定位測試

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

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

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

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

(d) 步長誤差分布

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

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

4.3 UWB和PDR融合定位

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

圖9 融合定位測試場景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í)開啟UWB和PDR定位，UWB定位采用上述的BAS算法，PDR定位采用上述的PDR算法，并對二者的定位進(jìn)行數(shù)據(jù)融合。本文的融合算法針對UWB NLOS區(qū)域，在LOS區(qū)域仍采用UWB定位，定位結(jié)果如圖10所示。由圖10可以看出，由于柱子遮擋的影響，UWB定位在2個(gè)NLOS處產(chǎn)生了較大的定位誤差，PDR定位在剛開始時(shí)誤差也較小，但是經(jīng)過了2次90°拐彎，航向角產(chǎn)生了約5%的偏移，本文融合定位算法的定位軌跡誤差更小，也更接近實(shí)際真實(shí)軌跡。本文各種定位方式的誤差如下：在NLOS場景中，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é)束語

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