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

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

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

0 引 言

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

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

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

1 系統(tǒng)結構設計

1.1 系統(tǒng)整體設計

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

圖1 系統(tǒng)的工作原理

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

1.2 系統(tǒng)硬件設計

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

圖2 硬件設計

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

1.3 系統(tǒng)軟件設計

1.3.1 TOF定位算法

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

圖3 TOF定位算法

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

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

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

1.3.2 TOF測距原理及流程

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

圖4 測距原理

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

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

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

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

圖5 軟件設計流程

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

2.1 定位誤差分析

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

（1）時鐘同步精度

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

（2）時間延遲

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

（3）多路徑非視距傳播

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

2.2 定位算法的改進

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

很多情況下都對測量數(shù)據(jù)有高度精確需求，這是得出狀態(tài)最優(yōu)成果的前提?？柭鼮V波算法是一種系統(tǒng)性的狀態(tài)估計法，它具有計算量小、容易實現(xiàn)和適應性強等特點，被廣泛應用在工程中?？蓪⑵鋭澐譃閮蓚€步驟：（1）預測：將前時刻數(shù)據(jù)信息作為參考，需要考慮到每個時刻的計算精度；（2）更新：對前面已經(jīng)估計出的結果，可將精度進一步提升。具體流程函數(shù)關系如下。

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

表1 各變量說明

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

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

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

圖6 卡爾曼濾波流程

3 定位精度測試

3.1 卡爾曼濾波的MATLAB仿真

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

圖7 濾波前后對比

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

3.2 卡爾曼濾波前后實測系統(tǒng)數(shù)據(jù)的比較

3.2.1 測試系統(tǒng)搭建

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

圖8 各節(jié)點的實際布局

靜態(tài)標簽的測試是為了展示加入卡爾曼濾波算法后定位的效果。按上一節(jié)方法布置好場景后，將標簽依次放置在（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 靜態(tài)標簽定位布局

3.2.2 數(shù)據(jù)比較

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

表2 靜態(tài)標簽定位測試

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

圖10 卡爾曼濾波前后數(shù)據(jù)對比

4 結 語

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