摘要：

針對(duì)超寬帶室內(nèi)定位系統(tǒng)中的測(cè)量誤差較大問(wèn)題，提出了一種基于雙路飛行時(shí)間測(cè)距方法來(lái)減小時(shí)鐘偏移誤差的解決方案。本方案針對(duì)靜態(tài)和動(dòng)態(tài)的待測(cè)節(jié)點(diǎn)的運(yùn)動(dòng)特性，通過(guò)對(duì)待測(cè)節(jié)點(diǎn)的運(yùn)動(dòng)狀態(tài)檢測(cè)，提出了一種聯(lián)合算法，其中，靜態(tài)定位采用了最小二乘估算法來(lái)得出節(jié)點(diǎn)坐標(biāo)，并通過(guò)MATLAB仿真顯示該節(jié)點(diǎn)坐標(biāo)位置；對(duì)于動(dòng)態(tài)跟蹤問(wèn)題，由于物體遮擋和運(yùn)動(dòng)軌跡等因素導(dǎo)致最小二乘法無(wú)法準(zhǔn)確估計(jì)節(jié)點(diǎn)位置，針對(duì)這一問(wèn)題，利用擴(kuò)展卡爾曼濾波算法跟蹤預(yù)測(cè)待測(cè)節(jié)點(diǎn)位置，從而改善定位效果。仿真結(jié)果表明，靜態(tài)定位誤差可以控制在6%以內(nèi)，對(duì)比靜態(tài)定位采樣，動(dòng)態(tài)跟蹤算法精度提高了20%。實(shí)驗(yàn)證明，該算法可靠有效，滿足室內(nèi)定位的基本需求。

0 引言

隨著現(xiàn)代科學(xué)技術(shù)的發(fā)展，通信和導(dǎo)航技術(shù)已經(jīng)在各個(gè)領(lǐng)域發(fā)揮巨大的作用，人們對(duì)位置服務(wù)的需求也越來(lái)越大［1］。在室外環(huán)境下，全球定位系統(tǒng)（Global Positioning System，GPS）是在室外定位采用的解決方案，定位精度一般為10 m以上［2］。在室內(nèi)環(huán)境下，GPS無(wú)法解決定位問(wèn)題，超寬帶（Ultra-Wide Band，UWB）技術(shù)作為室內(nèi)定位技術(shù)之一，有精度高、功耗低、抗干擾能力強(qiáng)等特點(diǎn)，被廣泛應(yīng)用于室內(nèi)定位領(lǐng)域［3］。基于測(cè)距的定位方法有以下幾種：到達(dá)時(shí)間差法（Time Difference 0f Arrival，TDOA）、飛行時(shí)間法（Time Of Flight，TOF）、到達(dá)角度法（Angle Of Arrival，AOA）［4］，測(cè)量節(jié)點(diǎn)間的距離或角度信息，然后使用三邊測(cè)量法、三角測(cè)量法或最小二乘法計(jì)算節(jié)點(diǎn)位置［5］。在以往的研究中，多通過(guò)優(yōu)化算法計(jì)算過(guò)程（例如Taylor算法、Fang算法、Chan算法［6］）來(lái)提高定位的精度。本文通過(guò)運(yùn)動(dòng)檢測(cè)的方式來(lái)區(qū)分待測(cè)節(jié)點(diǎn)的運(yùn)動(dòng)狀態(tài)，提出了一種聯(lián)合算法，采用雙路飛行時(shí)間的測(cè)距方法提高測(cè)距精度，最小二乘法（Least Square，LSE）來(lái)求解最佳節(jié)點(diǎn)位置，最后聯(lián)合擴(kuò)展卡爾曼濾波器（Extended Kalman Filter，EFK）來(lái)跟蹤節(jié)點(diǎn)的運(yùn)動(dòng)軌跡，并通過(guò)MATLAB仿真來(lái)驗(yàn)證算法的可靠性。

1 UWB測(cè)距原理

UWB系統(tǒng)發(fā)射的信號(hào)采用的是無(wú)載波窄脈沖，該窄脈沖的寬度一般僅為數(shù)個(gè)納秒甚至數(shù)十皮秒的數(shù)量級(jí)，這使得UWB信號(hào)具有很寬的頻譜［7］，可高達(dá)幾GHz，所以功率譜密度會(huì)很小，以上特征使得它與傳統(tǒng)連續(xù)無(wú)線電信號(hào)的系統(tǒng)不同［8］。UWB定位原理：首先測(cè)量基站與標(biāo)簽之間的距離，再由算法去求解標(biāo)簽的位置坐標(biāo)。本文利用雙路飛行時(shí)間法（Two Way-Time Of Flight，TW-TOF）進(jìn)行測(cè)距，并提出一種聯(lián)合算法求解標(biāo)簽位置。

1.1 飛行時(shí)間測(cè)距法

TOF測(cè)距方法需兩節(jié)點(diǎn)間的協(xié)作通信，記錄UWB信號(hào)在節(jié)點(diǎn)間的往返時(shí)間［9］，其測(cè)距過(guò)程如圖1所示。

標(biāo)簽向基站發(fā)送一個(gè)輪詢信息POLL，當(dāng)標(biāo)簽收到時(shí)，會(huì)立即回復(fù)一個(gè)確認(rèn)RESP來(lái)響應(yīng)這個(gè)數(shù)據(jù)包。標(biāo)簽記錄從發(fā)送數(shù)據(jù)包到接收確認(rèn)的時(shí)間T1；基站記錄了從收到數(shù)據(jù)包到回應(yīng)的時(shí)間T2，那么基站到標(biāo)簽的距離d就可以表示為：

式（1）是理想狀態(tài)下的測(cè)距公式。在TOF測(cè)距中節(jié)點(diǎn)用各自的時(shí)鐘計(jì)時(shí)，兩個(gè)節(jié)點(diǎn)的時(shí)鐘無(wú)法完全同步，導(dǎo)致兩節(jié)點(diǎn)間出現(xiàn)時(shí)鐘偏移，會(huì)造成比較嚴(yán)重的測(cè)距誤差。設(shè)基站的時(shí)鐘偏差為FA，標(biāo)簽的時(shí)鐘偏差為FB，那么可以得到信號(hào)傳輸時(shí)間誤差：

雖然可以選用高穩(wěn)定性的時(shí)鐘來(lái)減小這種誤差的影響，但價(jià)格都比較昂貴，會(huì)提高成本。

1.2 雙路飛行時(shí)間測(cè)距

對(duì)于TOF測(cè)距法中節(jié)點(diǎn)間的時(shí)鐘偏移而使測(cè)距誤差增大的問(wèn)題，本文利用的雙路飛行時(shí)間測(cè)距法是在其基礎(chǔ)上進(jìn)行改進(jìn)的一種測(cè)距方法。其具體過(guò)程如圖2所示。

第一階段：與TOF測(cè)距法相同，得到兩個(gè)時(shí)間T1、T2。第二階段：標(biāo)簽接收到RESP后，再向基站發(fā)出一個(gè)FINAL消息，設(shè)從接收到RESP到發(fā)出FINAL的時(shí)間為T(mén)3。第三階段：基站接收到FINAL后，就可以得到從發(fā)出RESP到接收到FINAL消息所用時(shí)間T4。如果按照理想TOF測(cè)距法，基站和標(biāo)簽時(shí)鐘完全同步，T1=T4，T3=T2，可得：

但實(shí)際上節(jié)點(diǎn)之間的時(shí)鐘無(wú)法同步，所以利用TW-TOF的測(cè)距方法可以更好地控制測(cè)距帶來(lái)的誤差，從而提高定位精度。根據(jù)上述得到的幾個(gè)時(shí)間，由式（8）可以求出UWB信號(hào)的單程飛行時(shí)間T。

由此看出，TW-TOF測(cè)距大大降低了時(shí)鐘頻率偏差引起的測(cè)距誤差。如果選用低頻偏的晶體如±10×10-6Hz，可進(jìn)一步降低測(cè)距理論誤差至±0.2 m。

2 最小二乘法靜態(tài)定位

2.1 最小二乘法

經(jīng)過(guò)TW-TOF測(cè)距得到各個(gè)基站與標(biāo)簽的距離di，同時(shí)各個(gè)基站A0、A1、A2、A3的坐標(biāo)是已知的（Xi，Yi），那么就可以得到方程組：

其中：

2.2 最小二乘法靜態(tài)定位仿真

本次仿真使用的仿真軟件為MATLAB 2014b，計(jì)算機(jī)配置如下：系統(tǒng)環(huán)境為Windows 7 64位，CPU采用Intel CoreTMi3-3220，CPU頻率為3.30 GHz，內(nèi)存為DDR3 8 GB。4個(gè)參與定位的基站坐標(biāo)分別為（0，0）、（0，100）、（100，0）、（100，100），標(biāo)簽的真實(shí)坐標(biāo)為（44，37），最小二乘法得到的仿真結(jié)果用圓圈來(lái)表示，在測(cè)試中加入了隨機(jī)測(cè)距誤差。算法仿真運(yùn)行100次以后的結(jié)果如圖3所示。

由圖3中的仿真結(jié)果可以看到，最小二乘法得到的定位結(jié)果基本上分布在標(biāo)簽的真實(shí)坐標(biāo)周圍，說(shuō)明此定位算法有效。為了更加直觀地評(píng)價(jià)算法的表現(xiàn)，使用均方根誤差（Root Mean Square Error，RMSE）來(lái)分析定位誤差，如式（15）所示：

由圖4可以看出，利用最小二乘法來(lái)進(jìn)行待測(cè)節(jié)點(diǎn)的靜態(tài)定位，定位結(jié)果誤差控制在6%以內(nèi)，精確度符合預(yù)期結(jié)果，說(shuō)明此算法是有效的。

3 擴(kuò)展卡爾曼濾波動(dòng)態(tài)定位

最小二乘法可以估測(cè)到靜態(tài)節(jié)點(diǎn)的位置，但是對(duì)于移動(dòng)的標(biāo)簽，在實(shí)際測(cè)量中由于物體或人體造成多徑傳播和非視距誤差，會(huì)導(dǎo)致定位誤差較大。卡爾曼濾波器利用上一時(shí)刻的位置信息和這一時(shí)刻的觀測(cè)信息對(duì)位置進(jìn)行估計(jì)［10］。

3.1 擴(kuò)展卡爾曼濾波器

傳統(tǒng)卡爾曼濾波器只能在線性高斯模型中使用，而運(yùn)動(dòng)標(biāo)簽定位問(wèn)題的模型是非線性的，那么可以使用擴(kuò)展卡爾曼濾波器，用非線性函數(shù)的局部線性特征來(lái)近似非線性。其過(guò)程包括兩個(gè)階段：預(yù)測(cè)和更新［11］。

上式兩個(gè)方程分別是對(duì)k時(shí)刻狀態(tài)向量和估計(jì)誤差的協(xié)方差的預(yù)測(cè)，Q（k）是系統(tǒng)誤差的協(xié)方差。由于h（s（k））是非線性函數(shù)，在使用能解決非線性問(wèn)題的擴(kuò)展卡爾曼濾波時(shí)，使用泰勒級(jí)數(shù)展開(kāi)來(lái)簡(jiǎn)化表達(dá)式。在k-1時(shí)刻向k時(shí)刻轉(zhuǎn)換時(shí)，H（k）是h（s（k））的雅克比矩陣，即求導(dǎo)化簡(jiǎn)后得：

設(shè)ω（k）和v（k）的協(xié)方差陣分別為Q（k）和R（k），都為對(duì)角矩陣，列數(shù)為2，維數(shù)是基站個(gè)數(shù)，對(duì)角線上各元素取值在仿真中分別設(shè)為定位坐標(biāo)最大誤差的一半和其相應(yīng)的測(cè)量方差。

3.2 動(dòng)態(tài)定位仿真

為了體現(xiàn)擴(kuò)展卡爾曼濾波在動(dòng)態(tài)定位中的特性，在仿真中把它與靜態(tài)定位算法經(jīng)過(guò)時(shí)間采樣后的結(jié)果進(jìn)行對(duì)比，用MATLAB進(jìn)行仿真，設(shè)置4個(gè)基站，分布在（0，0）、（0，1000）、（1000，0）、（1000，1000）處，本文測(cè)試的動(dòng)態(tài)過(guò)程是從（0，500）處開(kāi)始，來(lái)測(cè)試對(duì)動(dòng)態(tài)節(jié)點(diǎn)的跟蹤效果。時(shí)間采樣周期為0.1 s，根據(jù)擴(kuò)展卡爾曼濾波器原理，只需知道初值即可，仿真結(jié)果如圖5所示。

由圖5中可見(jiàn)，最小二乘法靜態(tài)采樣測(cè)量出的位置和經(jīng)過(guò)EFK估測(cè)后形成的軌跡相比，經(jīng)過(guò)EKF估測(cè)后的移動(dòng)節(jié)點(diǎn)的坐標(biāo)更接近真實(shí)的運(yùn)動(dòng)軌跡，說(shuō)明了擴(kuò)展卡爾曼濾波能更好地對(duì)運(yùn)動(dòng)中的標(biāo)簽進(jìn)行位置估計(jì)。接下來(lái)為了更直觀地顯示擴(kuò)展卡爾曼濾波的動(dòng)態(tài)定位效果，對(duì)比分析仿真結(jié)果的 RMSE，結(jié)果如圖6所示。

圖6中靜態(tài)算法采樣后的RMSE為波動(dòng)較大的虛線，經(jīng)過(guò)EFK算法估測(cè)后的RMSE為波動(dòng)較小的實(shí)線，誤差從25%以內(nèi)穩(wěn)定到了5%左右，比較直觀地體現(xiàn)出了擴(kuò)展卡爾曼濾波算法的有效性。

4 系統(tǒng)工作流程

4.1 運(yùn)動(dòng)狀態(tài)檢測(cè)

聯(lián)合算法的應(yīng)用需要了解標(biāo)簽的運(yùn)動(dòng)狀態(tài)，所以設(shè)計(jì)了運(yùn)動(dòng)狀態(tài)檢測(cè)方法。設(shè)（xt，yt）為t時(shí)刻最小二乘法靜態(tài)定位采樣所得的位置，假設(shè)系統(tǒng)最大誤差為σ，那么就可以通過(guò)以下的公式來(lái)檢測(cè)運(yùn)動(dòng)狀態(tài)：

式中，k是每個(gè)測(cè)量序列的時(shí)刻，開(kāi)始定位后每個(gè)測(cè)量時(shí)刻都與t時(shí)刻的位置比較。當(dāng)式（24）成立時(shí)，檢測(cè)到標(biāo)簽的運(yùn)動(dòng)狀態(tài)為靜止，相反，當(dāng)式（25）成立時(shí)，也就是說(shuō)在t+k時(shí)刻與t時(shí)刻的距離超過(guò)了最大誤差一定的倍數(shù)，那么就判斷標(biāo)簽是運(yùn)動(dòng)的。經(jīng)過(guò)多次仿真，最后仿真中所取的最大誤差為σ=0.2 m，誤差系數(shù)λ=2。為了防止標(biāo)簽在臨界狀態(tài)下頻繁波動(dòng)的情況，定義一個(gè)變量N來(lái)記錄檢測(cè)次數(shù)，只有當(dāng)N》3時(shí)，也就是連續(xù)3次檢測(cè)為運(yùn)動(dòng)時(shí)，才能真正判定為運(yùn)動(dòng)狀態(tài)。用此時(shí)刻的最小二乘靜態(tài)定位的結(jié)果作為擴(kuò)展卡爾曼濾波算法的初值，來(lái)繼續(xù)對(duì)標(biāo)簽進(jìn)行動(dòng)態(tài)跟蹤定位。

4.2 聯(lián)合算法工作流程

由上文所述的聯(lián)合算法的推導(dǎo)和仿真，最小二乘法聯(lián)合擴(kuò)展卡爾曼濾波定位算法的運(yùn)算流程如圖7所示。

5 結(jié)論

本文介紹了雙路飛行時(shí)間（TW-TOF）測(cè)距法，能夠降低時(shí)鐘偏移所帶來(lái)的測(cè)距誤差，并提出了一種最小二乘靜態(tài)定位聯(lián)合擴(kuò)展卡爾曼濾波動(dòng)態(tài)定位算法。對(duì)于靜態(tài)定位采用了最小二乘估算法進(jìn)行對(duì)待測(cè)節(jié)點(diǎn)定位，仿真結(jié)果表明，此算法的定位誤差能控制在6%以內(nèi)；對(duì)移動(dòng)的待測(cè)節(jié)點(diǎn)進(jìn)行動(dòng)態(tài)定位時(shí)，采用了擴(kuò)展卡爾曼濾波器，對(duì)運(yùn)動(dòng)軌跡進(jìn)行預(yù)測(cè)跟蹤，并通過(guò)MATLAB仿真對(duì)比表明，相對(duì)于靜態(tài)定位采樣，擴(kuò)展卡爾曼濾波算法的誤差降低了20%，定位效果更好。接下來(lái)還可以通過(guò)增加基站數(shù)量和合理的基站分布位置來(lái)提高定位精度。在算法上也有優(yōu)化的可能，把非線性問(wèn)題轉(zhuǎn)化為線性問(wèn)題的過(guò)程中，泰勒展開(kāi)式中取更多的項(xiàng)，通過(guò)提高算法復(fù)雜度來(lái)提高精度。

參考文獻(xiàn)

［1］ 董家志。基于UWB的室內(nèi)定位與跟蹤算法的研究［D］。成都：電子科技大學(xué)，2015.

［2］ 盧靖宇，余文濤，趙新，等。基于超寬帶的移動(dòng)機(jī)器人室內(nèi)定位系統(tǒng)設(shè)計(jì)［J］。電子技術(shù)應(yīng)用，2017，43（5）：25-28.

［3］ MATTEO R，SAMUEL V D，V，HEIDI S，et al.Analysis of the scalability of UWB indoor localization solutions for high user densities［J］.Sensors，2018，18（6）：1875-1880.

［4］ 孫鳳，施偉斌，黃靈鳳。基于無(wú)線傳感器網(wǎng)絡(luò)的室內(nèi)定位技術(shù)的研究［J］。電子技術(shù)應(yīng)用，2013，39（10）：80-83.

［5］ MCELROY C，NEIRYNCK D，MCLAUGHLIN M.Comparison of wireless clock synchronization algorithms for indoor location systems［C］.Proceedings of 2014 IEEE International Conference on Communications Workshops.Sydney.IEEE，2014：157-162.

［6］ LI X，HE D，JIANG L，et al.A method indoor multi-path IR-UWB location based on multi-task compressive sensing［C］.Fourth International Conference on Ubiquitous Positioning.IEEE，2017：64-68.

［7］ HEWISH M.Ultra-wideband technology opens up new horizons［J］.Janes International Defense Review Idr，1999，2：20-22.

［8］ 梁久禎。無(wú)線定位系統(tǒng)［M］。北京：電子工業(yè)出版社，2013.

［9］ 魯旭陽(yáng)。基于TOF的WSN節(jié)點(diǎn)定位技術(shù)研究［D］。鄭州：解放軍信息工程大學(xué)，2012.

［10］ 嵇茂祥.UWB室內(nèi)定位系統(tǒng)研究與實(shí)現(xiàn)［D］。上海：華東師范大學(xué)，2017.

［11］ 師延山，李道本，范躍祖。無(wú)線定位擴(kuò)展卡爾曼濾波算法的優(yōu)化［J］。北京航空航天大學(xué)學(xué)報(bào)，2003，29（4）：308-311.