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

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

0 引 言

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

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

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

1 系統(tǒng)結(jié)構(gòu)設(shè)計

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

基于卡爾曼濾波的UWB室內(nèi)定位系統(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)硬件設(shè)計

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

圖2 硬件設(shè)計

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

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

1.3.1 TOF定位算法

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

圖3 TOF定位算法

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

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

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

1.3.2 TOF測距原理及流程

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

圖4 測距原理

以四消息為基礎(chǔ)，應用三消息模式，能將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ù)關(guān)系整理為：

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

圖5 軟件設(shè)計流程

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

2.1 定位誤差分析

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

（1）時鐘同步精度

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

（2）時間延遲

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

（3）多路徑非視距傳播

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

2.2 定位算法的改進

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

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

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

表1 各變量說明

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

2.3 卡爾曼濾波算法的流程設(shè)計

利用三消息方式發(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)來計算，流程設(shè)計如圖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)由三個基站、一個標簽構(gòu)成。此時基站坐標各自對應 A0（0，0）、A1（5，0）、A2（0，5），其中A0作為主基站與PC機相連，標簽是可移動的，并在上位機軟件中設(shè)定好基站的坐標。

圖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ù)比較

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

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

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

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

4 結(jié) 語

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