基于TwIST的UWB精確定位算法設(shè)計(jì)及仿真

楊 柳，袁光福，趙 濤，郭子文*，吳 堅(jiān)

（1.95875部隊(duì)，甘肅 酒泉 735000；2.95859部隊(duì)，甘肅 酒泉 735000；3.航天新通科技有限公司，重慶 401332）

0 引 言

位置信息作為一個(gè)時(shí)空基礎(chǔ)，在萬(wàn)物互聯(lián)的時(shí)代，發(fā)揮著不可替代的作用。在當(dāng)今社會(huì)，位置信息不僅關(guān)系著國(guó)家的安全，也關(guān)系著國(guó)民經(jīng)濟(jì)的發(fā)展。在測(cè)控方面，在一些衛(wèi)星定位無(wú)法使用的情況，如衛(wèi)星定位受干擾的情況下，急需其他定位技術(shù)的補(bǔ)充。此外，如智能制造、車聯(lián)網(wǎng)等一些國(guó)家重點(diǎn)發(fā)展的戰(zhàn)略方向領(lǐng)域，都需要精確的位置信息。

室外定位環(huán)境中，全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS）可以在開闊地帶提供良好的定位精度，一般情況下可以達(dá)到數(shù)十米級(jí)的定位精度，如果使用實(shí)時(shí)動(dòng)態(tài)測(cè)量（Real Time Kinematic，RTK）的方式進(jìn)行定位，可以達(dá)到厘米級(jí)的定位精度[1]。但GNSS信號(hào)在遇到阻擋時(shí)會(huì)快速衰減，同時(shí)也會(huì)產(chǎn)生多路徑效應(yīng)，降低接收信號(hào)的質(zhì)量，不利于解算出距離信息，導(dǎo)致定位精度受到影響，因此GNSS在如倉(cāng)儲(chǔ)工廠、智能制造、地下停車場(chǎng)等室內(nèi)環(huán)境難以進(jìn)行精確定位。

超寬帶（Ultra Wide Band，UWB）無(wú)線電定位、藍(lán)牙定位、蜂窩定位、ZigBee定位、WiFi定位、無(wú)線射頻標(biāo)簽（Radio Frequency Identification，RFID）定位、地磁定位、偽衛(wèi)星定位、計(jì)算機(jī)視覺定位、可見光定位、紅外定位以及超聲波定位等[2-6]是較為常見的幾種室內(nèi)定位方式。其中，UWB定位技術(shù)不采用傳統(tǒng)的載波調(diào)制解調(diào)方式，而采用ns級(jí)的脈沖窄帶信號(hào)來(lái)直接傳輸U(kuò)WB信號(hào)[7]，頻帶范圍可達(dá)到3.1～10.6 GHz，在噪聲較大的環(huán)境中同樣可以正常工作。此外，UWB脈沖的功率譜密度很低，能夠有很好的時(shí)間分辨率，同時(shí)又有很強(qiáng)的抵抗多路徑干擾的能力。UWB定位系統(tǒng)的主要優(yōu)勢(shì)集中在定位精度、功耗、抗噪聲及穿透性能力等方面。

根據(jù)測(cè)距觀測(cè)量的不同，定位算法主要分為基于時(shí)間定位的到達(dá)時(shí)間法（Time of Arrival，TOA）、到達(dá)時(shí)間差法（Time Difference of Arrival，TDOA）、基于信號(hào)角度的定位方法（Angle of Arrival，AoA）以及基于信號(hào)接收強(qiáng)度的定位方法（Received Signal Strength Indication，RSSI）[8-11]。其 中，TDOA算法不需要得到絕對(duì)時(shí)間，時(shí)間同步的要求更低，系統(tǒng)復(fù)雜度更小，是目前主流定位系統(tǒng)使用的算法之一。

在使用TDOA算法進(jìn)行目標(biāo)的位置求解過(guò)程中，需要對(duì)得到的定位方程組進(jìn)行求解。Chan算法[12]采用二重最小二乘法對(duì)該方程組進(jìn)行求解，在TDOA測(cè)量誤差較小時(shí)，具有最優(yōu)的估計(jì)性能。本文提出了一種基于兩步收縮閾值（Two-step Iterative Shrinkage Thresholding，TwIST）[13]的定位方案，詳細(xì)分析對(duì)比了基于TwIST算法和基于Chan算法求解目標(biāo)位置的性能，并采用Kalman濾波算法對(duì)數(shù)據(jù)進(jìn)行了濾波處理，分別應(yīng)用到這兩種算法中，通過(guò)仿真分析，驗(yàn)證了基于TwIST的TDOA定位算法的有效性，提高了定位的精確度。

1 定位原理

1.1 TDOA定位

到達(dá)時(shí)間差法（TDOA）是通過(guò)測(cè)量信號(hào)的到達(dá)時(shí)間，而后使用到達(dá)時(shí)間之差進(jìn)行定位計(jì)算。移動(dòng)臺(tái)坐標(biāo)位置為通過(guò)達(dá)到時(shí)間差構(gòu)建的雙曲線交點(diǎn)。TDOA原理如圖1所示。

圖1 TDOA原理圖

圖1中，BS1，BS2，BS3為UWB基站位置，MS為標(biāo)簽位置，測(cè)得基站BS1、BS2、BS3到標(biāo)簽MS的距離分別為L(zhǎng)1，L2，L3。在實(shí)際的UWB定位系統(tǒng)中，通過(guò)對(duì)達(dá)到時(shí)間的測(cè)量，使用時(shí)間與距離的換算公式Δdij=c(Δti-Δtj)可以計(jì)算出未知位置的移動(dòng)臺(tái)與兩個(gè)己知坐標(biāo)位置的測(cè)距基站的距離之差，再根據(jù)這個(gè)距離差和己知基站的坐標(biāo)，可以得到多條雙曲線。而移動(dòng)臺(tái)的理論坐標(biāo)位置就位于這些雙曲線的公共交點(diǎn)上。當(dāng)出現(xiàn)n個(gè)不在一條直線上的基站時(shí)，就可以得到n-1個(gè)雙曲線方程，這些雙曲線的交點(diǎn)即為移動(dòng)臺(tái)的坐標(biāo)位置。根據(jù)TDOA定位模型，通過(guò)求解下述方程組，最終可以得到移動(dòng)臺(tái)的位置。

1.2 Two-Step IST（TwIST）算法

Bioucas D J M和FIGUEREDO M A在文獻(xiàn)[13]中提出了一種兩步迭代閾值收縮算法（TwIST）。該算法同時(shí)具有IST算法[14]良好的去噪性能以及IRS算法高效處理病態(tài)問題的優(yōu)點(diǎn)。TwIST算法的核心思想是利用前兩個(gè)迭代值去更新當(dāng)前值，也就是所謂的“兩步”迭代收縮算法。令C=I+λDt，R=I-KtK，借助兩步固定迭代算法思想A=C-R，則有A=λDt+KtK，求線性方程組Ax=b可以轉(zhuǎn)變?yōu)椋?/p>

將 式（2）中 的C-1用 去 噪 算 子ψλ()代 替，TwIST算法的迭代格式如下：

式中：α和β為TwIST中定義的參數(shù)，0≤α≤1，0＜β＜2α，實(shí)驗(yàn)中，α和β在較大范圍內(nèi)均可保證解算結(jié)果收斂；ψλ是基于正則化的去噪算子。從式（3）～式（5）可知，下一次的迭代結(jié)果xt+1不僅依賴于當(dāng)前的結(jié)果xt，也將考慮上一次的結(jié)果xt-1，也就是需要前兩步的迭代結(jié)果。關(guān)于TwIST算法的收斂性能以及參數(shù)α和β的計(jì)算方式等詳細(xì)推導(dǎo)過(guò)程請(qǐng)參考文獻(xiàn)[13]。

1.3 卡爾曼濾波

數(shù)據(jù)濾波通過(guò)去除噪聲來(lái)還原真實(shí)數(shù)據(jù)?？柭鼮V波（Kalman）是一種數(shù)據(jù)濾波方式。Kalman濾波在測(cè)量方差已知的情況下，能夠從一系列存在測(cè)量噪聲的數(shù)據(jù)中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。Kalman濾波是目前應(yīng)用最為廣泛的濾波方法，便于編程實(shí)現(xiàn)，并可做自適應(yīng)的實(shí)時(shí)參數(shù)更新，廣泛地應(yīng)用于通信、導(dǎo)航與控制等多個(gè)領(lǐng)域。

卡爾曼濾波利用系統(tǒng)模型和觀測(cè)模型的統(tǒng)計(jì)特性，通過(guò)遞推得到最優(yōu)的數(shù)據(jù)。通過(guò)使用先驗(yàn)估計(jì)值和觀測(cè)值進(jìn)行加權(quán)計(jì)算的思路進(jìn)行優(yōu)化，即：當(dāng)前狀態(tài)的估計(jì)值=基于上一時(shí)刻的預(yù)估值+當(dāng)前狀態(tài)觀測(cè)值。

假設(shè)某一隨機(jī)線性離散系統(tǒng)的狀態(tài)向量Xk，系統(tǒng)過(guò)程隨機(jī)噪聲Wk，系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣Φk,k-1，噪聲輸入矩陣Γk,k-1，系統(tǒng)觀測(cè)值Zk，觀測(cè)矩陣Hk，觀測(cè)噪聲Vk，則狀態(tài)方程和觀測(cè)方程為

系統(tǒng)過(guò)程噪聲方差Qk正定，觀測(cè)噪聲方差Rk非負(fù)定，Kk卡爾曼濾波估計(jì)過(guò)程如下。

狀態(tài)一步預(yù)測(cè)：

狀態(tài)估計(jì)：

濾波增益矩陣：

一步預(yù)測(cè)誤差方差陣：

估計(jì)誤差方差陣：

式中：為k時(shí)刻的狀態(tài)估計(jì)值，Pk為k時(shí)刻的估計(jì)誤差方差。通過(guò)系統(tǒng)的初始值X0和P0，根據(jù)在第k時(shí)刻的觀測(cè)值Zk，通過(guò)遞推的方式推斷出第k時(shí)刻的狀態(tài)估計(jì)值(k=1,2,3,…)。

2 仿真與實(shí)驗(yàn)分析

2.1 基于TwIST的定位算法設(shè)計(jì)

基于TwIST的定位算法主要為求解通過(guò)TDOA算法得到的線性方程組Ax=b，不同于Chan算法中使用改進(jìn)的最小二乘進(jìn)行方程組的求解。為獲得更高的去噪性能和更快的收斂速度，本文提出的算法使用了兩步收縮閾值的方式進(jìn)行方程組的求解。定位算法主要流程如圖2所示。

圖2 基于TwIST的定位算法框圖

基于Chan的TDOA定位算法在求解方程組的過(guò)程中則是先將初始非線性TDOA方程組轉(zhuǎn)換為線性方程組，然后采用最小二乘法得到初始解，再利用第一次得到的結(jié)果及已知約束條件進(jìn)行第二次最小二乘估計(jì)，從而得到標(biāo)簽坐標(biāo)。

2.2 基于仿真模型的定位解算

本文首先使用MATLAB軟件建立一個(gè)靜態(tài)標(biāo)簽定位仿真模型，建立了一個(gè)基站數(shù)量為6的定位小區(qū)?；疚恢靡约皹?biāo)簽位置平面如圖3所示。

圖3 仿真的基站與標(biāo)簽位置平面圖

分別使用本文提出的基于TwIST的定位算法和基于Chan的TDOA定位算法進(jìn)行位置坐標(biāo)求解。在定位解算完成后，對(duì)定位結(jié)果進(jìn)行卡爾曼濾波處理，得到均方根誤差（Root Mean Square Error，RMSE）數(shù)值對(duì)比結(jié)果，如表1所示。

表1 均方根誤差數(shù)值對(duì)比

從均方根誤差RMSE結(jié)果可得，使用基于TwIST算法進(jìn)行定位解算得到的位置結(jié)果比基于Chan的TDOA算法得到的結(jié)果精確度高50%。

2.3 基于實(shí)驗(yàn)數(shù)據(jù)的定位解算

在使用仿真模型得到的仿真數(shù)據(jù)進(jìn)行定位解算、分析并對(duì)比使用基于TwIST的定位算法和基于Chan的TDOA定位算法的定位性能后，為進(jìn)一步驗(yàn)證本文算法在UWB定位中有更優(yōu)的定位精度，本文進(jìn)行了定位實(shí)驗(yàn)測(cè)試，圖4為實(shí)際的測(cè)試場(chǎng)景。

圖4 實(shí)驗(yàn)測(cè)試場(chǎng)景

具體的基站位置以及標(biāo)簽位置如圖5所示。

圖5 實(shí)驗(yàn)的基站與標(biāo)簽位置圖

基站位置分別為（0.0，0.0），（1.8，4.2），（0.0，8.4），（1.8，12.6）。在此次定位實(shí)驗(yàn)中，共測(cè)量收集到6組實(shí)驗(yàn)數(shù)據(jù)，分別為標(biāo)簽位置在（0.6，1.2），（0.6，2.4），（0.6，5.4），（0.6，7.8），（0.6，9.6）和（0.6，12.0）時(shí)所得到的距離數(shù)據(jù)。

分別使用本文提出的基于TwIST的定位算法和基于Chan的TDOA定位算法進(jìn)行二維位置坐標(biāo)求解，在定位解算完成后對(duì)定位結(jié)果進(jìn)行卡爾曼濾波處理，得到均方根誤差數(shù)值對(duì)比結(jié)果如表2所示。

表2 均方根誤差數(shù)值對(duì)比

在使用實(shí)驗(yàn)測(cè)量數(shù)據(jù)的條件下，分別使用本文提出的算法和基于Chan的TDOA算法進(jìn)行二維的標(biāo)簽位置解算，從得到的均方根誤差RMSE表格可以得出，基于TwIST的定位解算算法效果要優(yōu)于基于Chan的TDOA算法，定位精度提高了約15%。

3 結(jié) 語(yǔ)

UWB定位作為當(dāng)前精度最高的室內(nèi)定位技術(shù)，在遙控遙測(cè)應(yīng)用對(duì)位置信息有需求、無(wú)法使用衛(wèi)星定位的情況下，可以作為輔助系統(tǒng)獲取目標(biāo)位置信息。本文設(shè)計(jì)了一種基于兩步迭代閾值收縮法（TwIST）結(jié)合卡爾曼濾波的UWB精確定位算法，充分利用了TwIST算法去噪性能好、收斂速度快的優(yōu)點(diǎn)。通過(guò)與基于Chan算法仿真和實(shí)驗(yàn)數(shù)據(jù)的定位結(jié)果對(duì)比發(fā)現(xiàn)，該算法提高了UWB系統(tǒng)的定位精度；同時(shí)，通過(guò)對(duì)比二維和三維的定位結(jié)果可得，本文提出的算法得到的位置信息在三維定位中的效果更好，具有更高的空間分辨率。