李 婕，王榮文，楊權啟

（1.河南工學院 電氣工程與自動化學院，河南 新鄉(xiāng) 453003；2.新鄉(xiāng)市機械設備運行狀態(tài)智能監(jiān)測工程技術研究中心，河南 新鄉(xiāng) 453003）

0 引言

在室內(nèi)定位技術研究中，定位的精度和坐標實時性問題一直都是熱點和難點。近幾年來，中國北斗與美國GPS 相繼研發(fā)出了一些定位方法，但這些方法在室內(nèi)環(huán)境中都無法獲取較為精準的位置信息，因此研究一種針對室內(nèi)的高精度定位系統(tǒng)是十分有必要的[1]。

超寬帶(Ultra Wide Band，UWB)定位技術是一種采用納秒級別的非正弦窄脈沖作為介質(zhì)的無線載波通訊技術，因為其具有對信道衰落不敏感、截獲能力低等特點而成為室內(nèi)定位的優(yōu)選方法。UWB 定位技術采用定位基站-被定位標簽式的定位方法，在被定位標簽附近放置3 個以上已知絕對坐標的基站，由每個基站測量出本基站和定位標簽的絕對距離，通過幾何解算得到標簽的坐標[2]。

本文研究的主要內(nèi)容是利用UWB 技術通過最小二乘法優(yōu)化的卡爾曼濾波算法來對被定位系統(tǒng)的位置進行跟隨和預測。首先通過UWB 技術對目標進行定位，獲取被定位目標的平面坐標，并將定位的數(shù)據(jù)保存起來。該位置坐標是通過傳感器直接獲得的數(shù)據(jù)，故存在一定量的噪聲和誤差，所以需要用濾波算法對該數(shù)據(jù)進行處理[3]。假定被定位者在一個時間段內(nèi)作勻速直線運動，由于卡爾曼濾波算法線性回歸的特性，會使濾波器輸出的坐標滯后于實際坐標。假定采用的傳感器速度和控制器計算坐標的速度足夠快，可以將被定位者視為在某一時間段內(nèi)做直線運動。利用最小二乘法對已保存數(shù)據(jù)做最小誤差估計，并對誤差數(shù)據(jù)做二階擬合，將擬合的結果用卡爾曼濾波算法進行校正，從而得到最優(yōu)坐標信息。通過試驗驗證，相比于傳統(tǒng)的卡爾曼濾波算法，校正后的算法對坐標位置的跟隨具有更好的實時性和快速收斂性[4]。本文將以仿真和實際模型驗證改進算法的可行性，并通過對比表明改進算法能有效地提高定位的精度。

1 UWB 技術定位模型

1.1 UWB 定位原理

UWB 定位技術的基礎主要是采用ToF 測距方法，即飛行時間測距方法。這種方法通過電磁波在空中飛行的時間解算出發(fā)射點和接收點的絕對距離。直接采用這種方法來解算距離會因為各自晶振不同步，導致解算出來的數(shù)據(jù)精度較低[5]。為了解決這一問題，本文采用每次做完通訊后都發(fā)送一個專門用于同步信號的方法，具體通訊時序見圖1。

圖1 通訊時序圖

圖1 中，由設備A 發(fā)送測距請求信號給設備B，當設備B 接收到測距請求信號后經(jīng)過處理在t3 時間段對設備A 進行響應反饋。為了使下次測距時兩個設備的時鐘同步，設備A 在t5 時刻發(fā)送一幀數(shù)據(jù)到設備B 作為下次通訊時的時鐘同步信號。TF為信號的飛行時間，Delay為設備B 接收到信號所需要的處理時間。最后由設備A 計算并得出兩個設備的絕對距離為：

其中distance為兩個設備之間的距離，c為光速常數(shù)，t1—t6 為設備A 發(fā)送的時間戳。

1.2 定位模型

在獲得各個基站與被定位標簽距離之后，需要進一步解算才可以得到坐標數(shù)據(jù)。假設定位基站與被定位標簽的絕對距離為d，利用測量學中的空間距離后方交會法可以得出，被定位的標簽一定是在以基站為圓心、d為半徑的圓上[6]。但在實際中，無法保證基站和被定位者永遠在同一平面內(nèi)，并且被定位者在定位空間中的高度也不一定一致，因此如果采用傳統(tǒng)的三點定位法而不加以矯正會向系統(tǒng)引入更多的噪聲，從而導致誤差增大。為了解決這一問題，本文采用以基站為圓心、以d為半徑的空間球體進行位置解算，當定位基站存在三個或三個以上時，就可以確定空間中一條弧線段，該弧線段的上下兩個端點坐標就是被定位者可能存在的兩個坐標，對得出的兩個坐標，可以根據(jù)工程實際情況舍去一個坐標，或者直接使用第四個定位基站確定另一個坐標。

當方程式大于等于4 個時，求解上述方程組即可獲得被定位標簽的空間唯一解。但是實際中存在的距離誤差和傳輸干擾會產(chǎn)生一個距離誤差ρ。所以在計算的過程中考慮到ρ的存在可以提前舍去誤差外的坐標。

2 卡爾曼濾波算法的定位實現(xiàn)

卡爾曼濾波器是一種基于時序的最佳線性濾波器，它可以根據(jù)上一時刻的系統(tǒng)狀態(tài)和系統(tǒng)推測狀態(tài)來預測下一時刻的系統(tǒng)真實狀態(tài)?？柭鼮V波算法可以簡單地由以下5 個方程組成：

（1）狀態(tài)預測方程：該方程根據(jù)狀態(tài)轉(zhuǎn)移矩陣F把上一時刻的狀態(tài)轉(zhuǎn)換為當前時刻的狀態(tài)。

（2）預測協(xié)方差矩陣：

（3）計算卡爾曼濾波增益：

（4）輸出目標狀態(tài)：

其中，z為傳感器的測量向量，在本系統(tǒng)中z就是通過UWB 技術經(jīng)過幾何計算得到的坐標值。

（5）優(yōu)化協(xié)方差矩陣：

根據(jù)UWB 室內(nèi)定位系統(tǒng)的特性，假設定位基站有n個，故可以獲取的坐標就為f(n)個。令基站位置坐標為(i可取值1,2,3,…,f(n)，目標的坐標為(x,y)。故被定位標簽到基站的距離d為：

系統(tǒng)狀態(tài)空間模型為：

其中公式(9),(10)分別為狀態(tài)方程和測量方程。X(k)為狀態(tài)向量，A(k,k-1)為系統(tǒng)矩陣，W(k-1)為過程噪聲向量，Z(k)為觀測向量，C(k)為測量矩陣，V(k)為測量噪聲向量。

由式(8)即可以構建一個關于x和y的方程

令狀態(tài)變量X的初始值為式(17)，其中和為獲取UWB 定位的坐標初始值：

該系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣為：

過程噪聲方差矩陣為：

最后帶入式(3)—(7)即可計算出當前目標的真實狀態(tài)。綜上所述，卡爾曼濾波算法的工作順序和遞歸原理可以由圖2 簡易地表達出來。

圖2 卡爾曼濾波算法流程圖

傳統(tǒng)的卡爾曼濾波算法雖然會根據(jù)卡爾曼增益不斷地調(diào)整系統(tǒng)觀測和系統(tǒng)預測兩個數(shù)據(jù)的權值，使得最終值逐漸逼近系統(tǒng)的真實值，但是在位置跟隨等需要結果快速收斂的場合，傳統(tǒng)的卡爾曼濾波算法會導致位置更新滯后，具有較大的穩(wěn)態(tài)誤差[7]。故本文針對UWB 室內(nèi)定位與跟隨狀態(tài)停滯問題，在傳統(tǒng)的卡爾曼濾波算法中加入最小二乘法對其進行優(yōu)化。

3 優(yōu)化的卡爾曼濾波算法

優(yōu)化的卡爾曼濾波算法是通過最小二乘法與卡爾曼濾波算法相結合的計算方法。最小二乘法也叫最小平方法，它可以通過最小化誤差來尋找與數(shù)據(jù)匹配的最佳函數(shù)[8]。它在工程中通常會用于傳感器的標定和后續(xù)數(shù)據(jù)的預測，其原理就是通過已知數(shù)據(jù)和理想結果的誤差平方和，并使其最小化，從而得到該組數(shù)據(jù)的回歸函數(shù)，然后使用該函數(shù)對系統(tǒng)進行預測。

令t為最小二乘法的擬合長度，系統(tǒng)輸入為，系統(tǒng)輸出為，i=(1,2,3…n)。所以總誤差的平方和為：

當最小二乘法為二階擬合時，上述誤差可以寫為：

此時上式中不同的a，b對應不同的。所以當：

根據(jù)g(x)就可以對系統(tǒng)t+1 時刻的狀態(tài)進行預測。在最小二乘法和卡爾曼濾波算法的融合中，將t+1 時刻的系統(tǒng)狀態(tài)作為卡爾曼濾波算法中系統(tǒng)預測向量的，此時系統(tǒng)預測值的誤差協(xié)方差為：

系統(tǒng)通過式(3),(27),(28),(4)—(7)即可計算或預測目標的狀態(tài)值。綜上，優(yōu)化的卡爾曼濾波算法工作順序和遞歸原理可由圖3 簡化得出。

圖3 優(yōu)化的卡爾曼濾波算法流程圖

由上圖可得優(yōu)化的卡爾曼濾波算法，系統(tǒng)預測值不再使用t-1 時刻卡爾曼濾波算法輸出值取而代之的是對傳感器觀測的歷史數(shù)據(jù)擬合后再預測的值。由于上文中提到了在時間內(nèi)，假定系統(tǒng)的真實狀態(tài)是在做勻速直線運動，故可以通過最小二乘法對歷史數(shù)據(jù)進行二次擬合后得到作為卡爾曼濾波算法的系統(tǒng)預測值，最小二乘法擬合的誤差作為卡爾曼濾波算法的預測方差。所以不同于傳統(tǒng)的卡爾曼濾波算法中系統(tǒng)預測值的是上一時刻的遞歸。通過最小二乘法擬合預測的數(shù)據(jù)更具有預測的參考價值，并且可以有效地抑制在幾何解算過程中所出現(xiàn)的誤差[10]。

4 仿真結果與對比

4.1 傳統(tǒng)卡爾曼濾波算法與優(yōu)化卡爾曼濾波算法的比較

通過MATLAB 對本文提出的采用最小二乘法優(yōu)化的卡爾曼濾波算法進行仿真驗證與經(jīng)典算法作對比。假定一個20×200 的單位場地，目標在場地內(nèi)從（0,0）點到（200,20）點做勻速直線運動。因在工程實踐中Z軸坐標一般是為了求解平面坐標的輔助解，所以為方便計算不考慮Z軸的變化。采樣周期為1，如圖4 所示。

圖4 目標實際運動軌跡

從 UWB 定位設備中獲取的位置坐標精度為10cm 即1dm，UWB 定位設備作為系統(tǒng)觀測數(shù)據(jù)的來源，故Q取單位1。

在MATLAB 中對原始信號加入1db 的高斯噪聲來模擬UWB 定位設備的數(shù)據(jù)，這里為了觀察更直觀，加入噪聲后的定位曲線的橫坐標為被定位者的X坐標，縱坐標為被定位者的Y坐標，如圖5 所示。

圖5 UWB 定位曲線

在實際應用中，由于傳感器本身器件特性都會存在一定量的噪聲，并且這些噪聲都遵循高斯分布，因此我們通過MATLAB 對假定的真實狀態(tài)的X,Y坐標分別加入高斯白噪聲，即可模擬出真實傳感器測量的曲線情況。

圖6 中，星號虛線的曲線代表使用未優(yōu)化的卡爾曼濾波算法做濾波所得到的狀態(tài)曲線。實線曲線代表使用最小二乘法優(yōu)化的卡爾曼濾波算法所得到的狀態(tài)曲線。由對比圖不難看出，使用最小二乘法優(yōu)化的卡爾曼濾波算法的性能優(yōu)于原始的卡爾曼濾波算法。

圖6 最小二乘法優(yōu)化的卡爾曼濾波算法與傳統(tǒng)卡爾曼濾波算法的對比

4.2 粒子濾波算法與優(yōu)化的卡爾曼濾波算法的比較

粒子濾波算法是一種以貝葉斯推理和重要性采樣為基本框架的算法，粒子濾波結構實際上就是加一層重要性采樣思想的蒙特卡洛方法[11]。該思想的基本結構是用一組樣本來近似表示系統(tǒng)的概率分布，然后使用這樣的近似表達來估計非線性系統(tǒng)的實際狀態(tài)，因此利用這個思想，粒子濾波算法可以處理任意分布形式分布的概率，這一點是優(yōu)于傳統(tǒng)卡爾曼濾波算法只能處理線性高斯分布的問題。但是由于粒子濾波算法的特性，其相比于優(yōu)化的卡爾曼濾波算法還是缺少一定的快速性和準確性[12]。

對比圖7 和圖8 不難看出，粒子濾波算法誤差在0—0.3 范圍內(nèi)，優(yōu)化的卡爾曼濾波算法最開始誤差達到了0.6，但是收斂之后，誤差均在0.1 以下。雖然粒子濾波算法可以將具有高斯白噪聲的信號與系統(tǒng)真實狀態(tài)的誤差控制在一定范圍內(nèi)，但是優(yōu)化的卡爾曼濾波算法對噪聲的抑制是優(yōu)于傳統(tǒng)的粒子濾波算法的。

為了更直觀地看出優(yōu)化的卡爾曼濾波算法在室內(nèi)定位系統(tǒng)中相對于其他算法和傳統(tǒng)的卡爾曼濾波算法的優(yōu)勢，表1 列出了三種算法在同樣的環(huán)境下誤差絕對值的均值。

由表1 可見，在UWB 室內(nèi)定位系統(tǒng)中，優(yōu)化的卡爾曼濾波算法明顯優(yōu)于傳統(tǒng)的卡爾曼濾波算法和粒子濾波算法。所以測試表明，本文提出的采用最小二乘法優(yōu)化的卡爾曼濾波算法針對UWB 室內(nèi)定位系統(tǒng)，可以使定位精度和性能有明顯的提升。

圖7 粒子濾波算法誤差曲線

圖8 最小二乘法優(yōu)化的卡爾曼濾波算法誤差曲線

表1 傳統(tǒng)卡爾曼濾波算法、優(yōu)化卡爾曼濾波算法和粒子濾波算法的數(shù)據(jù)對比

5 試驗數(shù)據(jù)與分析

在實際獲取數(shù)據(jù)的過程中，采用DWM1000 超寬帶定位模塊，該模塊具有“基站”和“標簽”兩種工作模式。在測試中，使用STM32 系列單片機作為主控制器，由于使用空間三個球體定位計算量過大，因此用四個模塊作為基站，一個模塊作為被定位的標簽，采用平面三點定位的方式，利用另一個基站（其余三個基站不在同一平面）作空間補償。由于UWB 測距的特性，距離數(shù)據(jù)最終只在基站上產(chǎn)生，因此在每個基站上還要額外添加一個無線通訊裝置，用來保證距離數(shù)據(jù)不會混淆并可進行高速的數(shù)據(jù)傳輸。各個基站與主控的通訊采用無線MODBUS 通訊協(xié)議，由于該通訊協(xié)議的靈活性和簡便性，后期測試中可以隨機添加或者減少一定量的定位基站和被定位者，方便測試。

測試中被定位者以小車代替，小車的真實坐標從小車的驅(qū)動輪的編碼器做積分運算得出。測試過程中小車從相對坐標點（100,10）勻速走向坐標點（200,20）。在被定位者（小車）勻速直線運動中，每間隔50ms 分別對小車實際坐標、UWB傳感器計算出的原始坐標和經(jīng)過優(yōu)化的卡爾曼濾波算法計算得到的坐標各采集一次并上傳到計算機，最后對上傳的數(shù)據(jù)收集、分類后導入MATLAB并繪制曲線如圖9 所示。

圖9 中，original 曲線為被定位者真實的坐標，由于被定位者在測試中以小車代替，小車的速度和行進角度都是由內(nèi)部控制算法控制。由于算法和傳感器本身帶有誤差，所以被定位者（小車）行進曲線是一個含有一定噪聲的曲線。UWB 曲線為傳感器原始數(shù)據(jù)，least squares-Kalman 曲線為濾波后的數(shù)據(jù)。由圖9 可以看出，優(yōu)化的曲線明顯比原始數(shù)據(jù)曲線平滑很多，并且在位置更新上并沒有出現(xiàn)滯后的現(xiàn)象。然后再將原始數(shù)據(jù)和濾波后的數(shù)據(jù)取X軸數(shù)據(jù)求出誤差，誤差比較如圖10 所示。

圖10 中original 曲線為原始數(shù)據(jù)誤差曲線，Kalman-Min 曲線為濾波后的數(shù)據(jù)誤差曲線。由圖可知，由最小二乘法優(yōu)化的卡爾曼濾波算法可以有效抑制系統(tǒng)的噪聲，濾波算法的誤差有明顯降低，且收斂速度快。

圖9 測試數(shù)據(jù)曲線

圖10 測試誤差曲線

6 結束語

本文介紹了一種不同于傳統(tǒng)的目標跟隨算法的復合式濾波與預測算法，針對定位和動態(tài)位置跟蹤系統(tǒng)的特性，通過最小二乘法特有的數(shù)據(jù)誤差最小化的理念與卡爾曼濾波算法相結合，利用兩種算法的優(yōu)勢對UWB 室內(nèi)定位技術產(chǎn)生的過程噪聲所導致的定位跟隨誤差做進一步處理，相對于傳統(tǒng)的室內(nèi)坐標跟隨濾波算法，該算法可有效提高UWB 室內(nèi)定位技術的精度和目標位置跟隨的實時性和快速性。

通過測試，把小車作為被定位對象，采用四個DWM1000 超寬帶測距（定位）模塊作為定位基站，在被定位者以勻速直線運動的環(huán)境下，對本文提出的算法進行驗證。通過試驗得出的數(shù)據(jù)證明，本文提出的采用最小二乘法優(yōu)化的卡爾曼濾波算法與傳統(tǒng)的目標跟隨算法相比，在保證準確性的同時具有較好跟隨性和快速性。