基于移動節(jié)點輔助定位的UWB室內定位方法

徐建華，張雨霖，韓勇強

（北京理工大學 自動化學院，北京 100081）

超寬帶（Ultra Wide-band,UWB）技術是一種無線載波通信技術，它不采用正弦載波，而是利用納秒級的非正弦波窄脈沖傳輸數據，具有數據傳輸速率高、抗多徑干擾強、功耗低等特點[1]。視距條件下，其理論定位精度可達厘米級，近年來成為室內定位的研究熱點[2]。采用UWB 技術定位，是通過UWB 設備之間的通信來獲取相對位置信息，從而可以解算出待定位設備的位置信息。常用的UWB 定位方法包括到達時間法（Time of Arrival,TOA）、到達時間差法（Time Difference of Arrival,TDOA）、到達角度法（Angle of Arrival,AOA）、接收信號強度指示（Received Signal Strength Indication,RSSI）等[3]。其中TOA 和TDOA是基于測距的定位方法，具有穩(wěn)定、易實現的特點，得到了大量應用。目前基于測距的方法大致分為三個步驟：獲取各節(jié)點間距離，使用位置解算方法得到坐標初值，對坐標初值做濾波處理實現定位功能。從上述任一環(huán)節(jié)對定位過程進行優(yōu)化，都能提升最終的定位效果。

UWB 設備通過檢測信號傳輸時間計算節(jié)點間距離，傳輸時間測量越準確，測距精度越高。從硬件上提高UWB 設備的性能，是優(yōu)化定位結果最直觀的方法[4]。然而，UWB 設備的技術更新受限于通信原理的發(fā)展速度，使得大量UWB 定位研究著重于優(yōu)化位置解算方法。常用的解算方法包括基于TOA 的最小二乘法、基于TDOA 的Chan 算法和Fang 算法等[5]，以及使用狀態(tài)估計的各類概率模型方法，如卡爾曼濾波、粒子濾波等[6,7]。此外，為減小室內環(huán)境帶來的誤差，一些研究也針對不同環(huán)境提出了不同的方案。文獻[8]通過理論分析誤差帶，使用最大絕對定位誤差分析拓撲結構對定位結果精度的影響，提出了對拓撲結構的優(yōu)化原則，能夠根據環(huán)境特點，得出節(jié)點分布的最優(yōu)拓撲結構。文獻[9]使用誤差圖表示一個靜態(tài)空間的分布定位誤差，提高了室內定位精度。此外，行人的室內活動也對UWB 定位精度存在影響[10]。實際應用中，將UWB 與慣性測量單元（Inertial Measurement Unit，IMU）組合使用可以提高定位精度[11,12]，但是存在數據融合以及成本增加的問題。文獻[13]在無線傳感網的定位過程中運用了相似的輔助節(jié)點定位方法，文獻[14]著重于解決輔助節(jié)點移動時的路徑規(guī)劃問題。

本文通過對傳統解析方法的研究，針對具有基站-移動節(jié)點方式部署的場景，通過對低速及靜止節(jié)點的判斷，引入“退化節(jié)點”作為新基站，增加視距條件下基站數，提高UWB室內定位精度。確定可使用基站個數之后，通過設置增量隊列篩選測距值，使用最小二乘法求解定位初值，然后使用擴展卡爾曼濾波器（Extended Kalman Filter,EKF）進行狀態(tài)估計，得到較為準確的定位結果。

1 基于移動節(jié)點輔助定位的方法

基于移動節(jié)點輔助定位的方法分為判斷可使用基站數、定位初值解算和使用EKF 進行狀態(tài)估計三個階段。首先在系統中設置分布節(jié)點標志位，當移動節(jié)點在分布式節(jié)點網絡中被判斷為靜止（或速度很低）時，將該節(jié)點標志位置位，其當前坐標可作為輔助基站使用。將該移動節(jié)點加入到定位解算過程中，實現輔助定位的效果。分布式節(jié)點網絡中，動態(tài)節(jié)點不斷地加入和退出節(jié)點網絡，參與定位的節(jié)點數目呈現動態(tài)變化。固定基站節(jié)點的坐標保持不變，該節(jié)點始終可用。在解算定位初值時，首先對待定位節(jié)點到各基站的測距值進行篩選，建立一個增量隊列去除誤差較大的測距值，然后使用最小二乘法解算出定位初值，將該定位初值作為EKF 的觀測值。在EKF 算法中，建立合適的狀態(tài)方程和觀測方程，最后輸出待定位節(jié)點的優(yōu)化定位結果。

2 UWB 定位過程

2.1 測距值增量隊列

本文設置一個增量隊列來剔除測距值增量過大的異常點，從而提高定位精度。由于采樣周期很短，節(jié)點低速移動時，理論上獲取的相鄰采樣值的增量較小。如果其增量過大，則視作誤差較大的異常點。基站為參與定位解算的各個固定節(jié)點，設Xi(i= 0,1…N)為移動節(jié)點到各個基站的距離，ΔXi(i= 0,1…N)為移動節(jié)點在一個采樣周期內沿其與各個基站連線方向的位移增量分量，即移動節(jié)點到各個基站距離的增量：

其中，v為速度，Δt為采樣周期。當采樣頻率為20 Hz時，Δt=0.05s。由于室內移動節(jié)點速度常低于2 m/s，一個采樣周期內其移動距離小于0.1 m。相較于室內定位UWB 設備測距誤差可達亞米級誤差，可忽略一個采樣周期內室內移動節(jié)點移動造成的定位誤差。

設Xi(t)、Xi(t-1)(i= 0,1…N)分別表示移動節(jié)點到各個基站當前時刻t及上一時刻t- 1的距離，則：

記ΔX(t+1)為最新計算的增量矩陣，即：

計算增量矩陣時，前n個時刻增量隊列形式如圖2所示，設置增量隊列長度為n=5，將其作為循環(huán)隊列使用，隊列中始終存儲最近5 次的增量數據。每次采樣結束后，求出ΔX(t+1)。

圖2 增量隊列示意圖Fig.2 Diagram of the increment queue

記Y為增量判斷矩陣：

其中，ζ為比例系數，用于控制接受增量的范圍。得到增量判斷矩陣Y之后，如果Y中所有元素均大于0，即：

式(5)表示使用當前采樣測距值所計算的增量符合條件，這時將該增量放入隊列，同時該增量將覆蓋(t-4) 時刻的增量數據。系統接受該組測距值，并將其用于定位解算。

2.2 定位初值解算

使用解析方法求解式(6)所示的非線性方程組得到坐標值，通常的做法是將非線性方程組改寫成線性方程組再進行計算。由于存在測距值誤差、基站數冗余等原因，一般需要求解線性回歸方程。多元線性回歸方程的求解可以使用最小二乘法、梯度下降法、穩(wěn)健回歸等，其中最小二乘法由于不需要迭代求解且計算量小，是最常見的求解方法。

使用最小二乘法求解定位結果，記各個基站坐標為(xi,yi)，移動節(jié)點坐標為(x,y)，其到各基站的距離為di，可以得到：

將式(6)中各式兩端平方，并用第一式減去后N式。對含有x、y項進行合并，將得到的結果寫成矩陣形式：

并記作AX = B，用最小二乘法解式(7)：

2.3 擴展卡爾曼濾波器

卡爾曼濾波是線性高斯系統應用貝葉斯濾波方程而得到的顯式解，將其應用在非線性系統時可能會出現結果不收斂的情況。EKF 的基本思想是利用泰勒級數展開將非線性系統線性化，然后使用卡爾曼濾波框架對信號進行濾波，其應用分為預測（predict）和校正（correct）兩個步驟。

將室內移動節(jié)點運動模型近似為恒定轉速和速度模型（ConstantTurnRateandVelocity,CTRV），并使用2.2節(jié)中計算得到的結果作為觀測量，狀態(tài)變量為待定位節(jié)點的坐標、速度、轉向角、角速度?？紤]以下離散時間非線性系統：

其中，第一行的狀態(tài)方程為非線性，第二行的觀測方程為線性，觀測矩陣為系統噪聲、為量測噪聲。令：

離散時間步驟k與連續(xù)時間值t相關，可以得到當前時刻狀態(tài)估計值，即：

計算式(11)積分的結果，可以得到非線性狀態(tài)方程的表達式：

那么預測過程可表示為：

然后進行狀態(tài)校正，首先計算卡爾曼增益：

其中，R為測量噪聲協方差矩陣；Kk為卡爾曼增益。

其次求狀態(tài)校正量，即：

最后更新誤差矩陣：

輸出校正后的狀態(tài)量xk中的T[x′y′]，即為最終定位結果。

3 移動節(jié)點輔助定位算法

當一個移動節(jié)點處于靜止或低速狀態(tài)時，其定位精度高。將該節(jié)點解算得到的坐標加入到節(jié)點網絡中，可以補充待定位節(jié)點的可利用基站個數。當視距范圍內基站數量少時，對移動節(jié)點連續(xù)定位結果進行分析，在幾個連續(xù)采樣周期內，其二維坐標的離散度小，如圖3所示。

圖3 單節(jié)點連續(xù)采樣坐標圖Fig.3 Single node continuous sampling coordinate diagram

由于UWB 設備定位誤差不呈正態(tài)分布[6]，對坐標兩個維度分別進行分析，從而判斷其是否可以被認定為“退化節(jié)點”，即其定位結果能否被其他節(jié)點當作基站坐標使用。同樣地，定義一個長度為m的隊列，存儲m個連續(xù)采樣坐標(xj,yj),j=0,1…m-1，得到x、y方向的均值μx、μy：

于是，可以得到x、y方向的方差：

當方差滿足式(22)的條件時，接受該均值坐標(μx,μy)，并將其作為節(jié)點坐標。

其中，ηx、ηy為閾值，根據實驗環(huán)境及精度要求確定。

本文所采用的UWB 定位系統并不從硬件上區(qū)分基站和標簽，分布式節(jié)點網絡可以得到任何其他節(jié)點到本節(jié)點的距離。任何節(jié)點在進行上述狀態(tài)判斷之后，都將坐標和標志位更新，并發(fā)布到節(jié)點網絡上。為了避免重復計算，連續(xù)采樣應該具有一段時間間隔。

單個節(jié)點定位流程如圖4所示。系統啟動之后，首先判斷可使用的基站個數，解算得到節(jié)點定位結果。只有當系統處于連續(xù)采樣周期內時，才進行移動節(jié)點的位置判斷，觀察其是否可以作為基站使用。當連續(xù)采樣計算得到的坐標值個數達到隊列長度，判斷隊列中的數據在兩個方向上的方差是否小于閾值，若滿足條件則將標志位置位，并將其發(fā)布到節(jié)點網絡上。

圖4 單節(jié)點一個定位周期的流程圖Fig.4 Flow chart of a positioning cycle for a single node

4 實驗驗證

4.1 實驗環(huán)境

在室內環(huán)境中選取大小約為3 m×9 m 的長方形區(qū)域，布置三個固定節(jié)點作為基站，其坐標分別為（0，0）、（0，8.28）、（1.6，8.28），單位：m。設置兩個移動節(jié)點，一個節(jié)點放在實驗者身上，通過該節(jié)點的USB端口輸出數據到電腦端。另一個節(jié)點放在小車平臺上，移動速度較低，作為輔助節(jié)點使用。

圖5 節(jié)點分布示意圖Fig.5 Node distribution diagram

實驗 UWB 設備采用 Nooploop 公司設計的LinkTrack S 模塊，該模塊數據幀輸出頻率可達50 Hz。實驗過程中，使用模塊的分布式節(jié)點網絡模式進行系統搭建，使網絡中的每個節(jié)點都可以輸出它到任何其他節(jié)點的距離值。

圖6 移動節(jié)點Fig.6 Mobile nodes

4.2 測距值對定位結果的影響

基于測距的定位方法對于節(jié)點間測距值十分敏感，很小的單個測距誤差也可能導致定位結果的較大偏差?？紤]到該UWB 設備測距誤差可達亞米級，對測距值加以隨機誤差進行仿真實驗，由3σ原則取σ=0.33。隨機取一個定位點，進行100 次定位計算，得到結果如圖7所示。由于定位結果隨測距值變化很大，對測距值的篩選是很有必要的。

圖7 測距誤差對定位結果的影響Fig.7 The influence of ranging error on positioning

常規(guī)異常值剔除方法較多，如3σ原則、卡方檢驗、箱線圖法等。與常規(guī)方法中對完整數據進行異常值剔除有所不同，本文實驗中對異常值的剔除需要滿足實時性，即實時判斷數據是否為異常值。將本文提到的增量隊列篩選方法與卡方檢驗、3σ原則進行比較，在保證采樣點足夠及坐標相對連續(xù)的情況下，比較三種方法處理后的定位均方根誤差、最大定位絕對誤差和計算時間。其中，計算時間為單次實驗使用某一方法的計算總時間，卡方檢驗的卡方分布因子閾值設置為5。

由表1 可知，卡方檢驗的定位誤差較小，但是計算時間太長，大量的矩陣計算無法保證定位的實時性。3σ原則計算時間較短，但是定位誤差稍大，且需要存儲大量數據用于計算。增量隊列篩選方法計算時間最短，可以滿足實時定位的需求，且每次計算所需數據量少。

表1 三種異常值剔除方法性能對比Tab.1 Performance comparison of three outlier removal methods

4.3 無輔助定位實驗效果

實驗開始前，先對UWB 設備進行標定。無輔助定位實驗指在不加入移動節(jié)點的情況下進行定位測試，即不更新第3 節(jié)中所述標志位，只依靠現有固定節(jié)點進行定位解算。實驗前已經畫出待定位節(jié)點移動的真實軌跡，實驗中對最小二乘法、使用增量隊列篩選和加入增量隊列后使用EKF 的方法進行對比。

室內環(huán)境下，多徑效應和金屬物體對信號衰減影響明顯。當用于定位的節(jié)點較少時，可能會出現較大的定位誤差。由于增量隊列剔除了異常點，所以采用EKF方法得到的采樣點會減少。按照采樣點采樣時的順序進行繪圖比較，從圖8-9 和表2 可以看出，采用改進算法得到的定位結果，誤差明顯減小。與最小二乘法相比，使用增量隊列篩選后定位均方根誤差減小了7.14%，最大定位絕對誤差減小了26.95%。加入增量隊列后使用EKF 方法，相比于最小二乘法其定位均方根誤差減小了10.02%，最大定位絕對誤差減小了64.47%；相比于增量隊列篩選方法，其定位均方根誤差減小了3.1%，最大定位絕對誤差減小了51.37%。異常點的剔除使得定位結果的最大誤差顯著降低，消除了曲線上的突變，使得定位結果更加平滑。EKF 方法中狀態(tài)變量估計曲線如圖10所示，v為速度、θ為角度、ω為角速度。

圖1 基于移動節(jié)點輔助定位的流程圖Fig.1 Flow chart based on mobile node auxiliary positioning

表2 三種算法下的定位誤差對比Tab.2 Positioning error comparison of three algorithms

圖8 無輔助定位實驗軌跡圖Fig.8 Experimental trajectory diagram of non-auxiliary positioning experiment

圖10 無輔助定位實驗EKF 方法中狀態(tài)變量估計曲線Fig.10 State variable estimation curves in EKF method of nonauxiliary positioning experiment

圖9 無輔助定位實驗定位誤差Fig.9 Positioning error in non-auxiliary positioning experiment

4.4 移動節(jié)點輔助定位實驗效果

將第3 節(jié)所述標志位使能，加入移動節(jié)點輔助定位功能。配置系統參數，采用第3 節(jié)中單節(jié)點定位周期流程進行實驗。分別使用最小二乘法、Chan 算法和移動節(jié)點輔助定位的方法進行實驗，對實驗采樣數據進行分析處理，得到其定位結果及誤差。

當移動節(jié)點（小車）進入低速（靜止）狀態(tài)時，其標志位翻轉。圖11 給出了某個時刻加入到節(jié)點網絡的輔助節(jié)點的實際位置，將解算出來的坐標用于待定位節(jié)點的定位過程，得到采用不同算法進行實驗的結果。從圖11-12 和表3 可知，基于移動節(jié)點輔助的定位解算方法相比于最小二乘法和Chan 算法，其均方根誤差分別減小了15.89%和16.45%，最大絕對誤差分別減小了60.99%和62.77%。圖13 給出了EKF 方法中各狀態(tài)變量估計值的變化情況。

表3 不同算法下的定位誤差Tab.3 Positioning errors under different algorithms

圖11 輔助定位實驗軌跡圖Fig.11 Experimental trajectory diagram of auxiliary positioning experiment

圖12 輔助定位實驗定位誤差Fig.12 Positioning error of auxiliary positioning experiment

圖13 輔助定位實驗EKF 方法中狀態(tài)變量估計曲線Fig.13 State variable estimation curves in EKF method of auxiliary positioning experiment

對比4.3 節(jié)和4.4 節(jié)的實驗結果，可見使用移動節(jié)點輔助定位的方法能夠使得定位誤差更小。在加入增量隊列篩選后，對是否有移動節(jié)點輔助定位進行多次對比實驗，得到如表4所示的結果。

表4 有無輔助定位時的定位誤差Tab.4 Positioning error with or without auxiliary positioning

對比表中數據可以得到，加入移動節(jié)點輔助定位可以有效降低定位誤差。由于輔助節(jié)點可以動態(tài)加入，能夠增加視距范圍內有效基站數，提高定位解算冗余度，從而減小定位誤差，提高室內定位的精度。另一方面，移動節(jié)點輔助定位的方法一定程度上降低了對固定節(jié)點的依賴性，可以減少固定基站部署，從而降低成本。

5 結論

本文設計了一種基于移動節(jié)點輔助定位的UWB室內定位方法，通過引入輔助節(jié)點，增加參與定位的基站數量，從而提高待定位節(jié)點的定位精度。該方法首先判斷可使用基站數，設置一個增量隊列，用于對各個測距值進行篩選，剔除異常點。使用最小二乘法解算坐標，得到定位初值。利用該定位初值，通過EKF進行狀態(tài)估計，得到最終的定位結果。實驗結果表明，本文方法可以減小定位誤差，消除定位曲線異常突變，提高定位精度，具有一定的工程應用價值。