基于TOF和自適應抗差卡爾曼濾波的UWB室內定位算法

方賢寶，林 勇，蘇羿安，鐘樂天

（合肥工業大學電氣與自動化學院，安徽 合肥 230009）

0 引 言

隨著移動通信、人工智能（AI）的蓬勃發展，定位技術早已走進人們的日常生活中，如室外定位技術中，全球定位系統（global positioning system，GPS）幾乎覆蓋人類的生產生活。與此同時，人們對室內定位的需求也與日俱增，工廠的智能生產、醫院的高效救治、商場的貨品搜尋等都需要準確的位置定位信息［1］。因此，將位置服務拓展到室內是一個急需解決的問題。超寬帶脈沖（impulse radio of ultrawideband，IR-UWB）技術具有高帶寬、高分辨率、高無線傳輸速率、超強魯棒性、低能耗等顯著優點，現已成為未來最具開發前景的室內無線定位解決方案之一［2］。常見的超寬帶（UWB）定位技術分為4 類：基于飛行時間（time of flight，TOF）的［3］、基于到達時間差（time difference of arrival，TDOA）的［4］，基于到達角（angle of arrival，AOA）的［5］和基于接收信號強度（received signal strength，RSS）的［6］。但在復雜的室內環境下，UWB 信號傳播極易受到影響，從而造成定位精度降低。文獻［7］使用Chan算法獲取初始定位，比較殘差與門限值鑒別與剔除非視距（non-line-of-sight，NLOS）誤差，再利用改進無跡卡爾曼濾波（unscented Kalman fitering，UKF）算法得到精確的位置坐標；文獻［8］提出對原始定位數據進行2 次粒子濾波，通過模型存儲與權重調節，從而實現精準高效的動態目標定位；文獻［9］利用抗差因子鑒別NLOS 誤差，以削弱其對測距器造成的噪聲干擾及影響，同時利用Sage-Husa 濾波算法進行估計和校正，最終使用卡爾曼濾波（Kalman filtering，KF）算法實現高精度UWB定位與解算。

造成UWB室內定位算法的誤差為標準時間誤差和多徑效應，即在NLOS 環境下將大大降低定位誤差。而上述文獻只關注于消除NLOS誤差，而忽略了標準時間誤差，所以為了抑制標準誤差和NLOS誤差，提高定位算法的精度，本文提出一種基于TOF 和自適應抗差KF（adaptive robust KF，ARKF）的UWB 室內定位算法--IKF 算法。首先對UWB設備進行標定，實現對測距誤差信息的快速修正；針對標準誤差，使用改進的測距算法，優化定位；其次采用自適應抗差濾波算法抑制NLOS誤差，提高定位精度。

1 UWB／TOF測距原理

1.1 UWB定位誤差模型

UWB室內定位系統如圖1（a）所示，主要由3個主從基站，1個標簽以及路由器和服務器組成。室內定位系統中，基站與標簽之間形成的信號直射傳輸路徑往往容易受到室內裝飾物或人員等障礙物阻擋，對此產生的信號誤差一般可稱為NLOS誤差，信號的傳播可以稱為NLOS 傳播，路徑軌跡如圖1（b）所示。

圖1 UWB定位模型

假設在視距（line of sight，LOS）環境下，基站k與標簽在ti時刻測量距離值為rk（ti），具體可表示為

式中Lk（ti）為基站k與標簽在ti時刻的真實距離，nk（ti）為標準時間誤差，由標準時間偏差帶來的測量誤差，包括脈沖信號在基站與標簽之間的固定延時誤差、器件誤差，還與信號傳播距離、室內溫度等有關［10］。該類誤差通常認為是均值誤差為0，標準差較小的高斯隨機噪聲，可使用測距標定法消除部分，同時本文使用改進的測距算法進一步剔除，增加定位精度。在NLOS環境下，存在隨機測量誤差，誤差模型表示為

式中 NLOSk（ti）為NLOS誤差，服從指數、均勻、高斯或者δ分布，且與系統定位環境有關，可通過濾波算法降低其誤差。

1.2 TOF測距原理

基于TOF 的測距原理為設備間互發信號，通過信號TOF與傳播速度測量兩者之間的距離。其方式主要有2種：單邊雙向測距（single-sided two-way ranging，SS-TWR）算法［11］與雙邊雙向測距（double-sided two-way ranging，DS-TWR）算法［12］。

SS-TWR算法原理如圖2（a）所示，先由設備A 向設備B發送測距請求，當設備B 接收該請求后，延遲一段時間Treply向設備A發送確認消息，經過Tprop設備A 接收。從設備A發送信息至接收，整個過程時間記為Tround。每個設備都對信號傳輸和接收的時間進行精確記錄，最終計算出Tround和Treply，估算出最終的TOF，測量值與誤差表達式如下

圖2 TOF測距原理

其中，eA與eB為設備A和設備B 的時鐘漂移，單位為10-6。該算法雖僅需一次信息交換即可得到測距信息從而降低功耗，但是TOF 估計誤差隨著Treply的增加而增加，令測距值存在較大誤差，故SS-TWR 算法在實際情形下并不適用。

DS-TWR算法原理如圖2（b）所示。設備A 發送UWB信號，經過Tprop后，設備B 接收并延遲一段時間Treply1后發送確認消息幀，設備A在接收后延后時間Treply2再次發送測距信息。最終TOF與估計誤差如下

由式（5）可知，只有令設備的回復時間間隔一致，才可以降低由時鐘漂移引起的標準誤差。該算法雖降低了標準時間誤差，但設備延遲時間必須盡可能相等，若不等，則將增大測距誤差，進一步降低定位精度。

2 算法改進

2.1 基于對稱DS-TWR方法的優化

由以上分析可知，SS-TWR算法僅需一次信息交換即可得TOF，但是誤差大，DS-TWR 算法簡單，僅需簡單數學運算即可得到測距結果，但要求設備高精度，否則將引起較大誤差。對此，本文提出一種基于DS-TWR 的改進算法，如圖3所示。

圖3 DS-TWR優化算法示意

首先由設備A 發送測距請求信號，在等待時間Tround0后，再次發送測距請求，經過延遲時間Treply1與信號Tprop后，設備A接收到確認消息幀，其中，Tround0與Treply1為預先設定好的時間間隔。設備B第一次與第二次接收消息的時間間隔設為Treply0，且令其等于Tround0。Tround0和Tround1通過設備計時器得到，通過簡單運算，有

真實值為

由以上公式可得該算法中測量值與真實值Tprop的誤差為

由上式可知，該算法中測距誤差僅與飛行實際時間Tprop和eB有關。由于SS-TWR算法誤差較大，并不適用，僅對比DS-TWR算法與本文改進算法。例如，對于DS-TWR算法，假設當設備A與設備B應答消息延遲時間相差1 ms、時鐘漂移之差為2 ×10-5，真實距離為300 m 時，計算出標準誤差為5 ns，對此產生的測距誤差為1.5 m；而本文測距算法中，信號傳播時間的實際值為1 000 ns，當設備B 的時鐘漂移eB最大為20 ×10-6，TOF誤差僅為0.04 ns，測距誤差為1.2 cm。相比DS-TWR 算法，該算法降低了標準時間誤差，提升了測距精度。

2.2 基于KF算法的優化

KF是一種線性濾波器，可以根據上一時刻的系統狀態和預測狀態，對系統下一時刻的真實狀態進行推測的最優估計算法［13］。KF 本身是一種遞歸過程，由以下5 個遞歸方程組成

式中Ak-1與Bk-1分別為狀態轉移矩陣和輸入控制矩陣，Qk-1為過程噪聲協方差矩陣，Uk-1為系統控制量，Hk為觀測模型矩陣，Rk為過程噪聲協方差矩陣。Kk為卡爾曼增益，對最優估計＾Xk進行比例調節，其大小反映了在最優化狀態值估計過程中“更相信”觀測值Zk還是估計值。

KF算法能夠對數據進行線性濾波，結合觀測值與測量值，最終得到系統的最佳估計值。但在復雜的室內環境下，室內裝飾與人員等都會遮擋UWB 信號，形成多徑效應，帶來NLOS誤差，使得定位估計值出現偏差。因此，本文提出抗NLOS KF算法，通過比較殘差與3 倍信息的方差來判斷LOS與NLOS情形，若存在NLOS則對增益進行縮放以減小測距誤差。

測量的部分異常值是在k時刻的最優估計時首次被引入，若能預先將計算所得的殘差進行修正，再繼續進行濾波最優估計，則可以有效消除部分異常值，將殘差矩陣Δk和方差矩陣Dk定義為

依據3倍方差理論和上式，得到如下判斷公式

式中為Dk矩陣對角線上的第i個元素，為Δk的第i個元素，當上式不滿足時，則受到NLOS誤差，進行修正

式中α為修正系數，決定了對NLOS誤差的抑制效果。在實際情形中，UWB使用場景受到的NLOS誤差不是唯一的，因此對α取固定值不符合實際。所以，為進一步提高抗NLOS誤差的效果，需要依據殘差與方差的對應關系來調節α的值

其中，b為調制指數，控制著修正指數的放大與縮小。當處于移動情形下，定位精度會受到NLOS 誤差而發生變化，相對應的殘差也會改變，所以需要調節修正系數α在1 -b至1不斷變化，達到自適應抗NLOS 誤差的效果。在本文中，最終經過實驗驗證，修正系數α設為0.8，最大縮小比例b設置為0.4可達到最佳實驗效果。

3 實驗與驗證結果

實驗布置如圖4所示。圖4中，標簽位于Pi處，基站分布四周，分別使用KF算法和本文算法進行濾波，最后使用三邊定位法進行定位，使用式（16）計算定位誤差，比較定位精度

圖4 實驗環境示意

其中，（x，y）為定位算法解算得到的標簽坐標，（xt，yt）為標簽實際坐標。

3.1 UWB測距標定

為進一步消除定位誤差，本文實驗依次對實驗設備進行測距標定［14］。實驗在逸夫樓4 樓一個空實驗室內進行，大小為6.5 m×8 m，實驗環境中基站與標簽之間無遮擋物，無實驗人員穿行，為相對意義下的LOS環境。首先對基站A1 進行測距實驗標定：基站A1 不動，標簽由近及遠選擇10個待測點，通過測距儀器測得的距離作為真實值，標簽與基站A1測得的數據在使用3σ原則剔除較大誤差數據后取平均值作為量測值，對2種測量數據進行一次線性擬合，確定誤差模型，完成基站的標定，其余基站同理。表1 為UWB各基站的校正參數。其中，擬合優度的數值越接近1，表明擬合程度越好。

表1 基站校正參數

3.2 UWB靜態定位實驗

表2列出了LOS環境下，2 種算法在靜態時定位誤差的數據對比?？梢钥闯?，在觀測環境較好的情形下，2 種算法的精度相當，但IKF 算法精度仍有較小的提高。因為本文使用了改進的TOF算法和測距標定，剔除了標準誤差，一定程度上提高了定位精度。

表2 靜態測試點LOS環境定位誤差比較

表3為在NLOS環境下的靜態定位實驗結果?？梢钥吹剑篕F算法的最小誤差達到約30 cm，而IKF算法最大誤差僅為20 cm左右，平均定位誤差大幅度降低。整體上，本文提出的算法可以在NLOS環境下有效且準確地對系統進行靜態定位。

表3 靜態測試點NLOS環境定位誤差比較

3.3 UWB動態定位實驗

在本文實驗中，實驗人員攜帶標簽從P1 出發，依次經過P2，P3，P4，P5，直至點P6，整體呈現5 字型，進行LOS與NLOS環境下的動態定位實驗，人員以正常勻速移動。如圖5 所示，ARKF 算法與KF 算法在2 種環境下的定位軌跡。

圖5 2 種算法在LOS環境下的動態定位比較

從圖5中的2幅圖對比看出，在LOS環境下，2 種算法的動態定位結果相差不大，但KF算法仍有較少部分的斷點與軌跡偏移，而本文算法基本貼合原路徑。在NLOS 環境下，如圖6 所示，KF 與ARKF 算法皆有一定的誤差，但KF算法卻有部分斷點，偏離原路徑，最大誤差達到50 cm，而本文算法動態定位較連續，無較大偏差，可以達到20 cm以內，最大誤差僅25 cm，定位效果較好，定位精度較高。

圖6 2 種算法在NLOS環境下的動態定位比較

4 結束語

在UWB測距過程中，易受包括器件誤差、延時誤差在內的標準誤差以及NLOS 測距誤差等因素的影響，導致測距精度大幅度降低。針對前者，本文提出一種改進的TOF算法，并對設備進行測距標定，利用一次線性擬合剔除測距過程中的標準誤差；通過比較殘差與3 倍信息的方差判斷是否有NLOS存在，以此減小NLOS 誤差。實驗結果表明：本文提出的改進算法能夠有效剔除標準時間誤差與NLOS誤差，提高定位系統在復雜室內環境下的定位精度。