基于TOF和自適應(yīng)抗差卡爾曼濾波的UWB室內(nèi)定位算法

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

（合肥工業(yè)大學(xué)電氣與自動(dòng)化學(xué)院，安徽 合肥 230009）

0 引 言

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

造成UWB室內(nèi)定位算法的誤差為標(biāo)準(zhǔn)時(shí)間誤差和多徑效應(yīng)，即在NLOS 環(huán)境下將大大降低定位誤差。而上述文獻(xiàn)只關(guān)注于消除NLOS誤差，而忽略了標(biāo)準(zhǔn)時(shí)間誤差，所以為了抑制標(biāo)準(zhǔn)誤差和NLOS誤差，提高定位算法的精度，本文提出一種基于TOF 和自適應(yīng)抗差KF（adaptive robust KF，ARKF）的UWB 室內(nèi)定位算法--IKF 算法。首先對(duì)UWB設(shè)備進(jìn)行標(biāo)定，實(shí)現(xiàn)對(duì)測(cè)距誤差信息的快速修正；針對(duì)標(biāo)準(zhǔn)誤差，使用改進(jìn)的測(cè)距算法，優(yōu)化定位；其次采用自適應(yīng)抗差濾波算法抑制NLOS誤差，提高定位精度。

1 UWB／TOF測(cè)距原理

1.1 UWB定位誤差模型

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

圖1 UWB定位模型

假設(shè)在視距（line of sight，LOS）環(huán)境下，基站k與標(biāo)簽在ti時(shí)刻測(cè)量距離值為rk（ti），具體可表示為

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

式中 NLOSk（ti）為NLOS誤差，服從指數(shù)、均勻、高斯或者δ分布，且與系統(tǒng)定位環(huán)境有關(guān)，可通過濾波算法降低其誤差。

1.2 TOF測(cè)距原理

基于TOF 的測(cè)距原理為設(shè)備間互發(fā)信號(hào)，通過信號(hào)TOF與傳播速度測(cè)量?jī)烧咧g的距離。其方式主要有2種：?jiǎn)芜呺p向測(cè)距（single-sided two-way ranging，SS-TWR）算法［11］與雙邊雙向測(cè)距（double-sided two-way ranging，DS-TWR）算法［12］。

SS-TWR算法原理如圖2（a）所示，先由設(shè)備A 向設(shè)備B發(fā)送測(cè)距請(qǐng)求，當(dāng)設(shè)備B 接收該請(qǐng)求后，延遲一段時(shí)間Treply向設(shè)備A發(fā)送確認(rèn)消息，經(jīng)過Tprop設(shè)備A 接收。從設(shè)備A發(fā)送信息至接收，整個(gè)過程時(shí)間記為Tround。每個(gè)設(shè)備都對(duì)信號(hào)傳輸和接收的時(shí)間進(jìn)行精確記錄，最終計(jì)算出Tround和Treply，估算出最終的TOF，測(cè)量值與誤差表達(dá)式如下

圖2 TOF測(cè)距原理

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

DS-TWR算法原理如圖2（b）所示。設(shè)備A 發(fā)送UWB信號(hào)，經(jīng)過Tprop后，設(shè)備B 接收并延遲一段時(shí)間Treply1后發(fā)送確認(rèn)消息幀，設(shè)備A在接收后延后時(shí)間Treply2再次發(fā)送測(cè)距信息。最終TOF與估計(jì)誤差如下

由式（5）可知，只有令設(shè)備的回復(fù)時(shí)間間隔一致，才可以降低由時(shí)鐘漂移引起的標(biāo)準(zhǔn)誤差。該算法雖降低了標(biāo)準(zhǔn)時(shí)間誤差，但設(shè)備延遲時(shí)間必須盡可能相等，若不等，則將增大測(cè)距誤差，進(jìn)一步降低定位精度。

2 算法改進(jìn)

2.1 基于對(duì)稱DS-TWR方法的優(yōu)化

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

圖3 DS-TWR優(yōu)化算法示意

首先由設(shè)備A 發(fā)送測(cè)距請(qǐng)求信號(hào)，在等待時(shí)間Tround0后，再次發(fā)送測(cè)距請(qǐng)求，經(jīng)過延遲時(shí)間Treply1與信號(hào)Tprop后，設(shè)備A接收到確認(rèn)消息幀，其中，Tround0與Treply1為預(yù)先設(shè)定好的時(shí)間間隔。設(shè)備B第一次與第二次接收消息的時(shí)間間隔設(shè)為Treply0，且令其等于Tround0。Tround0和Tround1通過設(shè)備計(jì)時(shí)器得到，通過簡(jiǎn)單運(yùn)算，有

真實(shí)值為

由以上公式可得該算法中測(cè)量值與真實(shí)值Tprop的誤差為

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

2.2 基于KF算法的優(yōu)化

KF是一種線性濾波器，可以根據(jù)上一時(shí)刻的系統(tǒng)狀態(tài)和預(yù)測(cè)狀態(tài)，對(duì)系統(tǒng)下一時(shí)刻的真實(shí)狀態(tài)進(jìn)行推測(cè)的最優(yōu)估計(jì)算法［13］。KF 本身是一種遞歸過程，由以下5 個(gè)遞歸方程組成

式中Ak-1與Bk-1分別為狀態(tài)轉(zhuǎn)移矩陣和輸入控制矩陣，Qk-1為過程噪聲協(xié)方差矩陣，Uk-1為系統(tǒng)控制量，Hk為觀測(cè)模型矩陣，Rk為過程噪聲協(xié)方差矩陣。Kk為卡爾曼增益，對(duì)最優(yōu)估計(jì)＾Xk進(jìn)行比例調(diào)節(jié)，其大小反映了在最優(yōu)化狀態(tài)值估計(jì)過程中“更相信”觀測(cè)值Zk還是估計(jì)值。

KF算法能夠?qū)?shù)據(jù)進(jìn)行線性濾波，結(jié)合觀測(cè)值與測(cè)量值，最終得到系統(tǒng)的最佳估計(jì)值。但在復(fù)雜的室內(nèi)環(huán)境下，室內(nèi)裝飾與人員等都會(huì)遮擋UWB 信號(hào)，形成多徑效應(yīng)，帶來NLOS誤差，使得定位估計(jì)值出現(xiàn)偏差。因此，本文提出抗NLOS KF算法，通過比較殘差與3 倍信息的方差來判斷LOS與NLOS情形，若存在NLOS則對(duì)增益進(jìn)行縮放以減小測(cè)距誤差。

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

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

式中為Dk矩陣對(duì)角線上的第i個(gè)元素，為Δk的第i個(gè)元素，當(dāng)上式不滿足時(shí)，則受到NLOS誤差，進(jìn)行修正

式中α為修正系數(shù)，決定了對(duì)NLOS誤差的抑制效果。在實(shí)際情形中，UWB使用場(chǎng)景受到的NLOS誤差不是唯一的，因此對(duì)α取固定值不符合實(shí)際。所以，為進(jìn)一步提高抗NLOS誤差的效果，需要依據(jù)殘差與方差的對(duì)應(yīng)關(guān)系來調(diào)節(jié)α的值

其中，b為調(diào)制指數(shù)，控制著修正指數(shù)的放大與縮小。當(dāng)處于移動(dòng)情形下，定位精度會(huì)受到NLOS 誤差而發(fā)生變化，相對(duì)應(yīng)的殘差也會(huì)改變，所以需要調(diào)節(jié)修正系數(shù)α在1 -b至1不斷變化，達(dá)到自適應(yīng)抗NLOS 誤差的效果。在本文中，最終經(jīng)過實(shí)驗(yàn)驗(yàn)證，修正系數(shù)α設(shè)為0.8，最大縮小比例b設(shè)置為0.4可達(dá)到最佳實(shí)驗(yàn)效果。

3 實(shí)驗(yàn)與驗(yàn)證結(jié)果

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

圖4 實(shí)驗(yàn)環(huán)境示意

其中，（x，y）為定位算法解算得到的標(biāo)簽坐標(biāo)，（xt，yt）為標(biāo)簽實(shí)際坐標(biāo)。

3.1 UWB測(cè)距標(biāo)定

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

表1 基站校正參數(shù)

3.2 UWB靜態(tài)定位實(shí)驗(yàn)

表2列出了LOS環(huán)境下，2 種算法在靜態(tài)時(shí)定位誤差的數(shù)據(jù)對(duì)比?？梢钥闯?，在觀測(cè)環(huán)境較好的情形下，2 種算法的精度相當(dāng)，但I(xiàn)KF 算法精度仍有較小的提高。因?yàn)楸疚氖褂昧烁倪M(jìn)的TOF算法和測(cè)距標(biāo)定，剔除了標(biāo)準(zhǔn)誤差，一定程度上提高了定位精度。

表2 靜態(tài)測(cè)試點(diǎn)LOS環(huán)境定位誤差比較

表3為在NLOS環(huán)境下的靜態(tài)定位實(shí)驗(yàn)結(jié)果?？梢钥吹剑篕F算法的最小誤差達(dá)到約30 cm，而IKF算法最大誤差僅為20 cm左右，平均定位誤差大幅度降低。整體上，本文提出的算法可以在NLOS環(huán)境下有效且準(zhǔn)確地對(duì)系統(tǒng)進(jìn)行靜態(tài)定位。

表3 靜態(tài)測(cè)試點(diǎn)NLOS環(huán)境定位誤差比較

3.3 UWB動(dòng)態(tài)定位實(shí)驗(yàn)

在本文實(shí)驗(yàn)中，實(shí)驗(yàn)人員攜帶標(biāo)簽從P1 出發(fā)，依次經(jīng)過P2，P3，P4，P5，直至點(diǎn)P6，整體呈現(xiàn)5 字型，進(jìn)行LOS與NLOS環(huán)境下的動(dòng)態(tài)定位實(shí)驗(yàn)，人員以正常勻速移動(dòng)。如圖5 所示，ARKF 算法與KF 算法在2 種環(huán)境下的定位軌跡。

圖5 2 種算法在LOS環(huán)境下的動(dòng)態(tài)定位比較

從圖5中的2幅圖對(duì)比看出，在LOS環(huán)境下，2 種算法的動(dòng)態(tài)定位結(jié)果相差不大，但KF算法仍有較少部分的斷點(diǎn)與軌跡偏移，而本文算法基本貼合原路徑。在NLOS 環(huán)境下，如圖6 所示，KF 與ARKF 算法皆有一定的誤差，但KF算法卻有部分?jǐn)帱c(diǎn)，偏離原路徑，最大誤差達(dá)到50 cm，而本文算法動(dòng)態(tài)定位較連續(xù)，無較大偏差，可以達(dá)到20 cm以內(nèi)，最大誤差僅25 cm，定位效果較好，定位精度較高。

圖6 2 種算法在NLOS環(huán)境下的動(dòng)態(tài)定位比較

4 結(jié)束語

在UWB測(cè)距過程中，易受包括器件誤差、延時(shí)誤差在內(nèi)的標(biāo)準(zhǔn)誤差以及NLOS 測(cè)距誤差等因素的影響，導(dǎo)致測(cè)距精度大幅度降低。針對(duì)前者，本文提出一種改進(jìn)的TOF算法，并對(duì)設(shè)備進(jìn)行測(cè)距標(biāo)定，利用一次線性擬合剔除測(cè)距過程中的標(biāo)準(zhǔn)誤差；通過比較殘差與3 倍信息的方差判斷是否有NLOS存在，以此減小NLOS 誤差。實(shí)驗(yàn)結(jié)果表明：本文提出的改進(jìn)算法能夠有效剔除標(biāo)準(zhǔn)時(shí)間誤差與NLOS誤差，提高定位系統(tǒng)在復(fù)雜室內(nèi)環(huán)境下的定位精度。