基于卡爾曼濾波的UWB室內定位技術研究

楊 會，沈微微，李傳輝，楊 鵬，常智超

（宿遷學院 信息工程學院，江蘇 宿遷 223800）

0 引 言

當今社會人們對位置服務的需求越來越迫切，越來越普遍地利用全球導航衛星系統進行室外定位，而在室內環境中存在多徑效應、非視距傳播和人員流動等因素會導致全球導航衛星系統對室內定位的效果不太理想[1]。目前，常用的室內定位技術主要有紅外線定位[2]、超聲波定位[3]、RFID近距離定位[4]、WiFi定位[5]、藍牙定位[6]等，但這些定位技術仍存在穩定性較弱、安全性較差、成本較高、定位精度不夠等問題。

超寬帶定位技術（UWB）具有穿透力強、功耗低、多徑分辨率高、安全性高、定位精度高等優點，在室內靜止或者移動的物體和人的定位跟蹤與導航的應用上有很大的優勢[7-8]；應用場景非常廣泛，可用于醫院、養老院對醫護人員、醫療設備、特殊病人以及老人等進行實時定位跟蹤，用于智慧工廠、機場、司法監獄對重要的物品以及人員進行實時定位跟蹤，還可用于對礦井下礦工、火災現場消防員的定位等[9]。

基于UWB技術的室內定位，定位基站數量越多，定位精度越高，但是當定位基站達到一定數量時，無論再增加多少基站，定位精度的提高微乎其微[10]。因此，在選定基站數量并合理布局基站后，選取合適的定位測距方法及定位解算算法是UWB室內定位技術的關鍵。本文采用了TOF（Time Of Flight）雙邊雙向三消息模式的測距方法，并針對非視距環境下誤差較大的問題，提出了采用卡爾曼濾波算法進行優化，從而一定程度上提高了系統的定位精度。

1 系統結構設計

1.1 系統整體設計

基于卡爾曼濾波的UWB室內定位系統主要由可以移動的若干標簽以及固定于周圍環境中的不少于3個的基站組成，標簽和基站之間采用TOF算法來實現系統的定位功能，并采用卡爾曼濾波算法對其進行優化，從而一定程度上提高了系統的定位精度。系統的工作原理如圖1所示。

圖1 系統的工作原理

系統運行時，首先由標簽發送測距開始指令，當各個基站接收到測距指令后，開始進行測距并進行數據傳輸；最后將全部數據匯總至主基站，經主基站將數據上傳至上位機，上位機通過位置解算算法，將標簽坐標精確解算并顯示出來。

1.2 系統硬件設計

定位系統中標簽和基站的硬件設計是一致的，主要由STM32控制模塊、UWB定位模塊以及電源模塊三部分組成，其硬件設計原理如圖2所示。其中，STM32控制模塊由單片機STM32F103C8T6實現，主要負責數據采集、存儲并處理收發的數據；UWB模塊主要負責標簽和基站、基站和基站之間的數據通信，采用Decawave公司針對UWB信號設計的DW1000信號收發器，該信號收發器可以準確記錄UWB信號的到達時間，時間精度達到15.6 ps，可以滿足厘米級的定位要求；電源模塊主要是為電路中各個模塊實現供電管理。

圖2 硬件設計

另外，可以通過軟件設置的方式將系統配置成標簽或者基站，標簽以輪詢形式與各個基站進行測距通信，最后將測得的各個數據匯總至主基站，由主基站上傳至上位機進行顯示。主基站還配有藍牙模塊，支持主基站到上位機的無線傳輸。

1.3 系統軟件設計

1.3.1 TOF定位算法

本系統采用TOF定位算法，TOF算法又稱飛行時間法，是一種雙邊雙向測距技術，它通過測量UWB信號在基站與標簽之間往返的飛行時間來計算距離。信號由標簽發送出去時，如果有3個及以上基站接收到，要對每個接收器的到達時間進行測量，將發射點、接收點間距計算出來。然后以距離為半徑進行圓形測量，三個圓形的交叉點即為標簽位置。TOF定位算法如圖3所示。

圖3 TOF定位算法

圖3中，基站A、B、C測得與標簽的距離分別為d1、d2、d3，以各個基站間距作圓，3個圓的交點就是標簽位置，設定標簽的位置坐標為（x，y），N個基站的位置坐標為（xi，yi）。根據圓的幾何意義，它們之間滿足的關系為：

接著將公式展開并化簡可得：

得到Y=AX后，結合實際需要，計算得出標簽位置，即X值，以線性代數規則變換公式，可得

1.3.2 TOF測距原理及流程

TOF測距的過程就是Device A（標簽）與Device B（基站）之間進行消息傳送的過程，測距原理如圖4所示。

圖4 測距原理

以四消息為基礎，應用三消息模式，能將Device B發送時間戳環節省略，當Device A接收到數據以后，會立刻返回響應，所得時間差分別有Tround1、Treply1、Tround2、Treply2。

整個測距流程大致為：Device A是測距指令發送起點，由Device B來響應。首先由標簽發送測距指令，并記錄發送時間TX；當基站接收到測距指令時，將測距信息向標簽發送，此時記錄接收時間RX，由標簽來接收響應信息RX，由此將各類最終指令發送到基站。

由上述測距過程，可以測得飛行時間Tprop，函數關系整理為：

利用上述原理設計軟件工作流程，如圖5所示。第一步，將MCU、DWM1000等初始化處理，讀取工作角色后進行測距，需要判斷是否為測距目標，當否認時則要返回MCU外設。重新配置以后再判斷是否為測距目標，這是單標簽、多基站輪詢形式的工作方式。當獲取時間戳后，計算出來距離傳送給卡爾曼濾波函數進行處理，最終將經過卡爾曼濾波算法處理過的數據傳到上位機進行坐標解算，將標簽的位置解算出來。

圖5 軟件設計流程

2 卡爾曼濾波算法對定位算法的改進

2.1 定位誤差分析

TOF測距相對于其他定位算法精度較高，但是仍存在很大的誤差，原因主要有以下三個方面：

（1）時鐘同步精度

時間測量其實就是TOF測距時所依賴的重要數據，光速本身約為3×105km/s，所以即便是存在異常微小的誤差，在后面計算時都會引起很大的誤差，要確定精確的距離，就必須提高收發雙方計時系統的標準。

（2）時間延遲

延遲類型主要有發送、接收、天線等。根據測距信號，以數字、模擬信號進行處理及調制時，傳輸過程中會通過無線電收發形成延遲誤差。這就是天線的收發信號均存在顯著延遲的原因。不僅如此，延遲還出現非均勻場景，是由同一輻射方向所產生的延遲與另一個輻射方向不一致引起的。

（3）多路徑非視距傳播

多路徑非視距傳播是提高信號特征測量值精度的重要基礎。兩點間無直接傳播路徑，信號通過信號反射、衍生等會流動到接收端，但第一個到達脈沖時間不一定能反映出真實值結果，因此非視距誤差是明顯存在的。

2.2 定位算法的改進

由于采用TOF算法定位存在上述誤差，所以為了改進TOF算法的缺陷，選取Fang算法、Chan算法、卡爾曼濾波等常用的改進算法，對比這幾種算法的優缺點。卡爾曼濾波算法在解決多址路徑、時鐘遷移問題方面有顯著成效，最終確定選取它解決TOF算法的誤差問題。

很多情況下都對測量數據有高度精確需求，這是得出狀態最優成果的前提。卡爾曼濾波算法是一種系統性的狀態估計法，它具有計算量小、容易實現和適應性強等特點，被廣泛應用在工程中。可將其劃分為兩個步驟：（1）預測：將前時刻數據信息作為參考，需要考慮到每個時刻的計算精度；（2）更新：對前面已經估計出的結果，可將精度進一步提升。具體流程函數關系如下。

公式（5）屬于狀態預測，根據上一時刻的狀態對當前時刻的狀態進行預測，還要考慮外界環境對系統的作用。公式（6）屬于誤差矩陣預測，實施預測操作時除了有不確定性預測噪聲增加因子Q，還要再考慮原有的不確定性。公式（7）是Kalman增益計算，根據預測結果的不確定性P和觀測結果的不確定性R，計算卡爾曼增益（權重）。公式（8）屬于狀態校正，以加權平均方式處理預測、觀測等結果，所得結果是當前時刻狀態估計。輸出結果表示Kalman濾波最終結果，公式（9）代表誤差矩陣更新。式中各變量的含義見表1所列。

表1 各變量說明

由于卡爾曼濾波算法可以解決非視距信道傳輸的誤差，還可以解決時鐘延遲的問題，不受基站個數限制，因此卡爾曼濾波算法是最適合改進TOF定位算法的。

2.3 卡爾曼濾波算法的流程設計

利用三消息方式發送接收所得的四個時間差：Tround1、Treply1、Tround2、Treply2，計算出來飛行距離，當每個基站計算出來距離后，將距離參數傳遞給卡爾曼濾波函數，對數據進行卡爾曼濾波處理。卡爾曼濾波是根據上一時刻的狀態和系統誤差來預測下一時刻的狀態與系統誤差，然后根據系統的最優狀態來計算，流程設計如圖6所示。經過卡爾曼濾波后，將數據匯總至主基站，再上傳至上位機進行位置解算。

圖6 卡爾曼濾波流程

3 定位精度測試

3.1 卡爾曼濾波的MATLAB仿真

利用卡爾曼濾波算法對定位算法進行優化，將定位誤差盡可能降低，為了進一步驗證其可行性，采用MATLAB對該算法進行了仿真。選取三基站一標簽定位系統，由該系統測得標簽和各基站的距離，經MATLAB對所測得的數據進行處理，得到濾波前后的誤差對比，如圖7所示。

圖7 濾波前后對比

圖中灰色曲線是所測得的數據直接經TOA定位算法解算出來的位置與實際位置的誤差，黑色曲線則是所測得的數據首先經卡爾曼濾波處理后再經TOA解算得到的位置與實際位置的誤差。由圖中可以看出，濾波前的誤差較大，而經過卡爾曼濾波算法處理過的數據曲線更加平滑，誤差較小。因此，卡爾曼濾波算法確實可以彌補定位算法所出現的不足，從而提高定位精度。

3.2 卡爾曼濾波前后實測系統數據的比較

3.2.1 測試系統搭建

選取宿遷學院現教中心一樓進行定位系統測試環境的搭建，如圖8所示，該系統由三個基站、一個標簽構成。此時基站坐標各自對應 A0（0，0）、A1（5，0）、A2（0，5），其中A0作為主基站與PC機相連，標簽是可移動的，并在上位機軟件中設定好基站的坐標。

圖8 各節點的實際布局

靜態標簽的測試是為了展示加入卡爾曼濾波算法后定位的效果。按上一節方法布置好場景后，將標簽依次放置在（0.5，0.5）、（1.5，1.5）、（2.0，2.0）、（2.5，2.5）、（3.0，3.0）、（3.5，3.5）、（4.0，4.0）、（4.5，4.5）、（5.0，5.0）進行定位測試。平面布局如圖9所示。

圖9 靜態標簽定位布局

3.2.2 數據比較

首先將未加入卡爾曼濾波算法的程序下載到標簽和基站中，標簽放置到指定位置，此時將上位機界面顯示的坐標記錄下來；采用同樣的方法，再將加入卡爾曼濾波算法后的程序下載到標簽和基站中，將上位機界面顯示的坐標記錄下來，其測量結果見表2所列。

表2 靜態標簽定位測試

最后將采集的數據導入MATLAB畫出卡爾曼濾波前后數據對比圖，如圖10所示，可以明顯看出濾波后的數據與實際軌跡更加一致。由此得出結論，卡爾曼濾波對TOF測距算法的改進是有效果的，該算法能夠使得測算的坐標更加精確。

圖10 卡爾曼濾波前后數據對比

4 結 語

本文設計的基于UWB室內定位系統實現了對室內人員及物品的靜態定位和動態定位。為了進一步提高該室內定位系統的定位精度，采用了卡爾曼濾波算法對其定位算法進行優化。一方面，用MATLAB對卡爾曼濾波算法進行仿真，通過仿真結果可以明顯看出卡爾曼濾波算法對定位精度的提高是非常明顯的；另一方面，將卡爾曼濾波算法嵌入到定位系統的硬件程序中，進行了卡爾曼濾波前后的實驗，并對實驗數據進行比對。可以看出，加入卡爾曼濾波算法在一定程度上對 定位精度是有所改善的，對于提高基于UWB室內定位系統的定位精度有一定的參考價值。