優(yōu)化的卡爾曼濾波算法在UWB 定位系統(tǒng)中的仿真研究

李 婕，王榮文，楊權(quán)啟

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

0 引言

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

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

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

1 UWB 技術(shù)定位模型

1.1 UWB 定位原理

UWB 定位技術(shù)的基礎(chǔ)主要是采用ToF 測(cè)距方法，即飛行時(shí)間測(cè)距方法。這種方法通過(guò)電磁波在空中飛行的時(shí)間解算出發(fā)射點(diǎn)和接收點(diǎn)的絕對(duì)距離。直接采用這種方法來(lái)解算距離會(huì)因?yàn)楦髯跃д癫煌剑瑢?dǎo)致解算出來(lái)的數(shù)據(jù)精度較低[5]。為了解決這一問(wèn)題，本文采用每次做完通訊后都發(fā)送一個(gè)專門用于同步信號(hào)的方法，具體通訊時(shí)序見圖1。

圖1 通訊時(shí)序圖

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

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

1.2 定位模型

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

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

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

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

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

（2）預(yù)測(cè)協(xié)方差矩陣：

（3）計(jì)算卡爾曼濾波增益：

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

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

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

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

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

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

由式(8)即可以構(gòu)建一個(gè)關(guān)于x和y的方程

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

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

過(guò)程噪聲方差矩陣為：

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

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

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

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

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

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

當(dāng)最小二乘法為二階擬合時(shí)，上述誤差可以寫為：

此時(shí)上式中不同的a，b對(duì)應(yīng)不同的。所以當(dāng)：

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

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

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

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

4 仿真結(jié)果與對(duì)比

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

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

圖4 目標(biāo)實(shí)際運(yùn)動(dòng)軌跡

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

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

圖5 UWB 定位曲線

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖9 中，original 曲線為被定位者真實(shí)的坐標(biāo)，由于被定位者在測(cè)試中以小車代替，小車的速度和行進(jìn)角度都是由內(nèi)部控制算法控制。由于算法和傳感器本身帶有誤差，所以被定位者（小車）行進(jìn)曲線是一個(gè)含有一定噪聲的曲線。UWB 曲線為傳感器原始數(shù)據(jù)，least squares-Kalman 曲線為濾波后的數(shù)據(jù)。由圖9 可以看出，優(yōu)化的曲線明顯比原始數(shù)據(jù)曲線平滑很多，并且在位置更新上并沒(méi)有出現(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 測(cè)試數(shù)據(jù)曲線

圖10 測(cè)試誤差曲線

6 結(jié)束語(yǔ)

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

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