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ú)線(xiàn)電信號(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è)輪詢(xún)信息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í)間T₁；基站記錄了從收到數(shù)據(jù)包到回應(yīng)的時(shí)間T₂，那么基站到標(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í)鐘偏差為F_A，標(biāo)簽的時(shí)鐘偏差為F_B，那么可以得到信號(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í)間T₁、T₂。第二階段：標(biāo)簽接收到RESP后，再向基站發(fā)出一個(gè)FINAL消息，設(shè)從接收到RESP到發(fā)出FINAL的時(shí)間為T(mén)₃。第三階段：基站接收到FINAL后，就可以得到從發(fā)出RESP到接收到FINAL消息所用時(shí)間T₄。如果按照理想TOF測(cè)距法，基站和標(biāo)簽時(shí)鐘完全同步，T₁=T₄，T₃=T₂，可得：

    但實(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^-6 Hz，可進(jìn)一步降低測(cè)距理論誤差至±0.2 m。

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

2.1 最小二乘法

    經(jīng)過(guò)TW-TOF測(cè)距得到各個(gè)基站與標(biāo)簽的距離d_i，同時(shí)各個(gè)基站A₀、A₁、A₂、A₃的坐標(biāo)是已知的(X_i，Y_i)，那么就可以得到方程組：

其中：

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

    本次仿真使用的仿真軟件為MATLAB 2014b，計(jì)算機(jī)配置如下：系統(tǒng)環(huán)境為Windows 7 64位，CPU采用Intel Core^TMi3-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)周?chē)f(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%以?xún)?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)卡爾曼濾波器只能在線(xiàn)性高斯模型中使用，而運(yùn)動(dòng)標(biāo)簽定位問(wèn)題的模型是非線(xiàn)性的，那么可以使用擴(kuò)展卡爾曼濾波器，用非線(xiàn)性函數(shù)的局部線(xiàn)性特征來(lái)近似非線(xiàn)性。其過(guò)程包括兩個(gè)階段：預(yù)測(cè)和更新^[11]。

    上式兩個(gè)方程分別是對(duì)k時(shí)刻狀態(tài)向量和估計(jì)誤差的協(xié)方差的預(yù)測(cè)，Q(k)是系統(tǒng)誤差的協(xié)方差。

    由于h(s(k))是非線(xiàn)性函數(shù)，在使用能解決非線(xiàn)性問(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ì)角線(xiàn)上各元素取值在仿真中分別設(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)較大的虛線(xiàn)，經(jīng)過(guò)EFK算法估測(cè)后的RMSE為波動(dòng)較小的實(shí)線(xiàn)，誤差從25%以?xún)?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è)(x_t，y_t)為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%以?xún)?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)化的可能，把非線(xiàn)性問(wèn)題轉(zhuǎn)化為線(xià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ú)線(xiàn)傳感器網(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ú)線(xiàn)定位系統(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ú)線(xiàn)定位擴(kuò)展卡爾曼濾波算法的優(yōu)化[J].北京航空航天大學(xué)學(xué)報(bào)，2003，29(4)：308-311.

作者信息:

李國(guó)友，宋成全，孟  巖

(燕山大學(xué) 電氣工程學(xué)院，河北 秦皇島066000)