基于兩步Kalman 濾波的NLoS 誤差抑制算法

賈時(shí)蕾，王潛心

( 中國(guó)礦業(yè)大學(xué)環(huán)境與測(cè)繪學(xué)院, 江蘇 徐州 221116 )

0 引言

隨著GPS 和北斗衛(wèi)星導(dǎo)航系統(tǒng)(BeiDou Navigation Satellite System，BDS)的迅猛發(fā)展，目前戶外定位和導(dǎo)航問(wèn)題已經(jīng)得到了很好地解決[1-2]. 而在室內(nèi)定位方面，雖然廣大學(xué)者已經(jīng)提出了各種技術(shù)手段和定位方法，但是仍未得出通用的解決方案. 現(xiàn)代人類室內(nèi)活動(dòng)時(shí)間增加以及室內(nèi)定位技術(shù)在救援、精準(zhǔn)營(yíng)銷、智能停車、智慧城市等領(lǐng)域具有的廣泛應(yīng)用價(jià)值使得人們對(duì)于高精度的室內(nèi)定位需求日益迫切[3-4].相較于當(dāng)前使用的其他定位方法，超寬帶(ultra wide band，UWB)具有大帶寬、時(shí)間分辨率高和抗多徑干擾等優(yōu)點(diǎn)，被認(rèn)為是最有前途的無(wú)線協(xié)作室內(nèi)定位解決方案之一[5-7]. 但UWB 信號(hào)在傳輸過(guò)程中受到遮擋無(wú)法直線傳播時(shí)會(huì)產(chǎn)生非視距(non line of sight,NLoS)誤差，該誤差會(huì)因在基于信號(hào)到達(dá)時(shí)間(time of arrival, TOA)距離估計(jì)算法中引入正偏差而嚴(yán)重影響定位性能[8-10]，特別是在復(fù)雜的室內(nèi)環(huán)境中. 因此，減少NLoS 誤差對(duì)定位結(jié)果的影響是UWB 定位過(guò)程中的主要目標(biāo).

針對(duì)NLoS 誤差抑制問(wèn)題，近年來(lái)各種文獻(xiàn)已經(jīng)提出了許多卓有成效的定位算法，其中最廣為流行的方法主要分為消極加權(quán)和視距(line of sight, LoS)重構(gòu)兩種[11]. 文獻(xiàn)[12]提出一種基于泰勒展開(kāi)的加權(quán)最小二乘估計(jì)方法，雖然適用性較強(qiáng)，但需要用于確定權(quán)重的統(tǒng)計(jì)信息. 文獻(xiàn)[13]提出一種殘差加權(quán)算法，雖然不需要關(guān)于權(quán)重的統(tǒng)計(jì)信息，但需要較多的基站參與定位. 文獻(xiàn)[14]利用TOA 測(cè)量值的時(shí)間歷史對(duì)LoS 環(huán)境下的TOA 測(cè)量值進(jìn)行重構(gòu)，定位誤差較小但不能實(shí)時(shí)定位. 文獻(xiàn)[15]對(duì)Kalman 濾波器進(jìn)行改進(jìn)，將 NLoS 誤差作為狀態(tài)向量的一部分引入到Kalman 濾波中，在一定程度上改善了定位精度.文獻(xiàn)[16]將測(cè)距殘差與Kalman 濾波結(jié)合，并將殘差值引入Kalman 濾波迭代過(guò)程中以修正Kalman 濾波的異常值，有效抑制了NLoS 誤差的影響. 但由于NLoS 誤差隨機(jī)性的特點(diǎn)，上述方法在LoS/NLoS 混合環(huán)境下的實(shí)際使用性能并不穩(wěn)定.

從傳統(tǒng)方法的不足與局限性出發(fā)，本文提出了一種基于兩步Kalman 濾波的NLoS 誤差抑制方法：在第一步Kalman 濾波中，將NLoS 誤差作為第三個(gè)參數(shù)加入待估計(jì)的狀態(tài)變量中，借助NLoS 誤差與測(cè)量誤差之間的相互獨(dú)立性直接估計(jì)出 NLoS 誤差，并將其與設(shè)定閾值比較以鑒別NLoS 誤差是否存在，若存在，則將初始TOA 測(cè)距值減去NLoS 誤差初步估計(jì)后的值作為第二步Kalman 濾波的輸入，第二步Kalman 濾波針對(duì)受NLoS 誤差影響較大的測(cè)距值進(jìn)行二次NLoS 誤差估計(jì)、鑒別和消除，并將減去二次NLoS 誤差估計(jì)后的輸入值作為最終的定位解算依據(jù). 通過(guò)對(duì)NLoS 誤差二次估計(jì)和二次剔除從而實(shí)現(xiàn)室內(nèi)復(fù)雜環(huán)境下的精準(zhǔn)實(shí)時(shí)定位.

1 NLoS 誤差對(duì)TOA 定位的影響

假定LoS 環(huán)境下移動(dòng)站在k時(shí)刻與第i個(gè)基站之間的TOA 測(cè)距值為ri(k)，其通常可表示為

式中：di(k) 為k時(shí)刻移動(dòng)站和第i個(gè)基站之間的真實(shí)距離；ni(k)為標(biāo)準(zhǔn)時(shí)間所造成的測(cè)量誤差，一般被認(rèn)為是服從0 均值高斯分布的高斯隨機(jī)噪聲[17-18]. 而在NLoS 環(huán)境下測(cè)距值除了包含測(cè)量誤差外，還包括NLoS 傳播環(huán)境引起的隨機(jī)測(cè)量誤差，即NLoS誤差. 此時(shí)TOA 測(cè)距值可表示為

式中，非視距誤差 NLoS(k)僅與信號(hào)傳播環(huán)境有關(guān)，在不同的信道環(huán)境中其可能服從指數(shù)分布，均勻分布或高斯分布[19]. NLoS 誤差存在的情況下，移動(dòng)站與基站之間的測(cè)距值會(huì)被引入正值的誤差分量，將該測(cè)距值用于定位解算必然會(huì)導(dǎo)致定位精度的降低，如圖1 所示.

圖1 NLoS 誤差對(duì)TOA 定位的影響

LoS 環(huán)境下的TOA 定位解算應(yīng)為三圓交點(diǎn)，但由于NLoS 誤差的存在，該點(diǎn)拓展為三圓交疊的區(qū)域. 因此想要獲取高精度的UWB 定位服務(wù)，NLoS誤差是必須解決的一個(gè)關(guān)鍵問(wèn)題.

2 基于兩步Kalman 濾波的NLoS 誤差抑制

2.1 Kalman 濾波基本原理

Kalman 濾波問(wèn)題實(shí)質(zhì)上是以某種最優(yōu)方式聯(lián)合求解未知狀態(tài)方程和測(cè)量方程的問(wèn)題[20]. 在動(dòng)態(tài)定位方面，Kalman 濾波及其變體形式能夠?yàn)橐种芅LoS傳播誤差提供可行的解決途徑，從而改善無(wú)線環(huán)境中信號(hào)TOA 的估計(jì)[21-22].

Kalman 濾波器的狀態(tài)方程和測(cè)量方程分別為：

式中：s(k)和z(k)分別為第k時(shí)刻的狀態(tài)向量和觀測(cè)向量；A和G分別為狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)向量系數(shù)矩陣；w(k)和v(k)分別是第k時(shí)刻的過(guò)程噪聲和測(cè)量噪聲.

Kalman 濾波主要包括預(yù)測(cè)和更新兩個(gè)階段：在預(yù)測(cè)階段，濾波器根據(jù)上一時(shí)刻的估計(jì)值對(duì)當(dāng)前時(shí)刻的狀態(tài)進(jìn)行估計(jì)；在更新階段，濾波器使用當(dāng)前時(shí)刻的測(cè)量值來(lái)優(yōu)化預(yù)測(cè)階段獲得的預(yù)測(cè)值[23]. Kalman濾波的具體迭代過(guò)程如下：

2.2 NLoS 誤差識(shí)別與抑制

由(2)式可知，NLoS 環(huán)境下的TOA 測(cè)距值由真實(shí)測(cè)距值、測(cè)量誤差以及NLoS 誤差三者疊加而成.鑒于標(biāo)準(zhǔn)測(cè)量誤差與隨機(jī)NLoS 誤差產(chǎn)生的原因不同，可以針對(duì)誤差獨(dú)立性對(duì)NLoS 誤差進(jìn)行分離，并重構(gòu)測(cè)距值. 在第一步Kalman 濾波過(guò)程中，將NLoS 誤差作為第三個(gè)分量加入狀態(tài)方程，此時(shí)各向量矩陣設(shè)置如下：

式中：r(k)為第k時(shí)刻基站與移動(dòng)站間的距離；r˙(k)為第k時(shí)刻移動(dòng)站相對(duì)基站的移動(dòng)速度； NLoS(k)為NLoS 誤差；T為采樣間隔；β為實(shí)驗(yàn)選取NLoS 誤差系數(shù)參數(shù)；ω為過(guò)程噪聲分量；n為測(cè)量誤差. 考慮到NLoS 誤差的非負(fù)性特點(diǎn)，在迭代過(guò)程中要保證每一時(shí)刻的NLoS 誤差估計(jì)都符合實(shí)際. 因此每得到一個(gè)狀態(tài)估計(jì)就需要對(duì)NLoS 狀態(tài)分量進(jìn)行一次符號(hào)判斷 ，當(dāng)其不符合實(shí)際情況時(shí)，需將其設(shè)置為0.

完成一次迭代過(guò)程后，將該過(guò)程估計(jì)的NLoS誤差狀態(tài)分量與給定閾值 θ進(jìn)行比較以鑒別NLoS誤差是否存在. 閾值取值對(duì)于NLoS 誤差的消除十分關(guān)鍵，若設(shè)置過(guò)大，則無(wú)法有效除去TOA 測(cè)距值中的NLoS 誤差，若設(shè)置過(guò)小則可能會(huì)導(dǎo)致Kalman 濾波器不收斂的問(wèn)題. 本文通過(guò)在多次實(shí)驗(yàn)中采用不同的門限值，考察估計(jì)結(jié)果的誤差和收斂情況以確定最終的閾值取值. NLoS 誤差鑒別過(guò)程如下：

由于NLoS 誤差的隨機(jī)性會(huì)引起NLoS 誤差的劇烈變動(dòng)，故而某些偏差較大的TOA 測(cè)距值在第一步Kalman 濾波時(shí)可能未被消除干凈，考慮在第二步Kalman 濾波時(shí)再次對(duì)NLoS 誤差進(jìn)行估計(jì)并與給定閾值 θ進(jìn)行比較以判斷是否需要對(duì)輸入值進(jìn)行修正.

圖2 展示了本文提出的基于兩步Kalman 濾波的NLoS 誤差抑制算法流程. 對(duì)每一個(gè)定位基站首先獲取當(dāng)前測(cè)距值，然后利用第一步Kalman 濾波對(duì)NLoS 誤差進(jìn)行估計(jì)并判斷該測(cè)距值是否屬于NLoS測(cè)距值，以對(duì)TOA 測(cè)距值進(jìn)行合理修正，在第二步Kalman 濾波時(shí)則利用第一步Kalman 濾波得到的輸入值對(duì)NLoS 誤差進(jìn)行二次估計(jì)、鑒別及修正以提高TOA 測(cè)距精度，最終實(shí)現(xiàn)UWB 精準(zhǔn)定位.

圖2 算法流程圖

3 仿真實(shí)驗(yàn)及結(jié)果分析

為了驗(yàn)證本文所提方法的有效性及合理性，模擬LoS/NLoS 混合室內(nèi)場(chǎng)景進(jìn)行仿真驗(yàn)證. 模擬場(chǎng)景中布設(shè)四個(gè)UWB 模塊，設(shè)定UWB 模塊采樣時(shí)間間隔為T(T=1 s)，移動(dòng)站初始位置和區(qū)域內(nèi)的三個(gè)UWB 參考基準(zhǔn)站坐標(biāo)已知，移動(dòng)站以0.2 m/s 的速度沿固定路徑勻速運(yùn)動(dòng)，路徑全長(zhǎng)20 m，前期運(yùn)動(dòng)過(guò)程中基準(zhǔn)站均處于視距狀態(tài)，后期由于移動(dòng)障礙物的出現(xiàn)致使基站1 處于長(zhǎng)達(dá)50 s 的NLoS 狀態(tài). 實(shí)驗(yàn)環(huán)境如圖3 所示.

圖3 實(shí)驗(yàn)環(huán)境示意圖

假定實(shí)驗(yàn)過(guò)程中的測(cè)量誤差服從均值為0，標(biāo)準(zhǔn)差 σ為0.2 的高斯分布，NLoS 誤差服從指數(shù)分布[24]，

式中，τRMS為均方根(root mean square，RMS)時(shí)延拓展，作為與傳播環(huán)境密切相關(guān)的隨機(jī)量，其服從對(duì)數(shù)正態(tài)分布，并且可以通過(guò)四個(gè)環(huán)境依賴變量進(jìn)一步表征為

式中：T1為 τRMS在1 km 處的中值；d為移動(dòng)站與基站之間的距離；ε為介于0.5~1.0 的指數(shù)分量；ξ為對(duì)數(shù)正態(tài)隨機(jī)變量(10logξ服從均值為0，標(biāo)準(zhǔn)差為 σξ的高斯分布). 具體參數(shù)設(shè)置如表1 所示，本文實(shí)驗(yàn)采用一般市區(qū)無(wú)線信道環(huán)境進(jìn)行NLoS 誤差模擬.

表1 不同環(huán)境下的模型參數(shù)

在進(jìn)行NLoS 誤差抑制實(shí)驗(yàn)之前，首先對(duì)本文所提方法的NLoS 誤差估計(jì)性能進(jìn)行測(cè)試. 圖4 為同一基站在不同環(huán)境下對(duì)移動(dòng)站觀測(cè)100 s 后，利用本文算法對(duì)不同環(huán)境進(jìn)行NLoS 誤差估計(jì)得到的結(jié)果. 可以看到，LoS 環(huán)境下NLoS 誤差估計(jì)值在前期由于初始給定的NLoS 誤差分量略有波動(dòng)，但隨著濾波器的穩(wěn)定，NLoS 誤差估計(jì)值逐漸平穩(wěn)地趨向于0. 同時(shí)NLoS 環(huán)境下濾波估計(jì)得到的NLoS 誤差同實(shí)驗(yàn)仿真NLoS 誤差趨勢(shì)總體一致，特別是對(duì)較大的NLoS誤差實(shí)現(xiàn)了良好的跟蹤估計(jì).

圖4 NLoS 誤差估計(jì)性能

將仿真實(shí)驗(yàn)環(huán)境下影響基站1 測(cè)距過(guò)程的仿真NLoS 誤差與利用本文方法估計(jì)得到的NLoS 誤差進(jìn)行對(duì)比，以分析驗(yàn)證濾波器是否具有環(huán)境轉(zhuǎn)變的感知能力，結(jié)果如圖5 所示. 可以看出，運(yùn)動(dòng)前期濾波估計(jì)所得NLoS 誤差整體分布較為穩(wěn)定，幅值趨近于0 且均小于給定閾值，NLoS 誤差判別效果與現(xiàn)實(shí)情況相符；在移動(dòng)站運(yùn)動(dòng)第50 s 時(shí)濾波估計(jì)所得NLoS誤差值開(kāi)始出現(xiàn)波動(dòng)，幅值相較于運(yùn)動(dòng)前期開(kāi)始逐漸增大且總體趨勢(shì)與實(shí)驗(yàn)仿真NLoS 誤差保持一致. 該結(jié)果表明濾波器不僅具有良好的跟蹤效果，對(duì)于LoS/NLoS 環(huán)境轉(zhuǎn)變的感知也具有較強(qiáng)的靈敏性.

圖5 仿真NLoS 誤差與估計(jì)NLoS 誤差對(duì)比

針對(duì)上述實(shí)驗(yàn)環(huán)境下基站1 所得測(cè)距值進(jìn)行視距重構(gòu)，重構(gòu)效果如圖6 所示. 可以看出在LoS 階段，經(jīng)本文算法濾波后測(cè)距值與原始測(cè)距值并無(wú)二異，而在NLoS 階段，經(jīng)過(guò)第一次Kalman 濾波后，NLoS 測(cè)距值相較于原始測(cè)距值整體有所降低，但一些受NLoS 誤差影響較大的測(cè)距值仍需進(jìn)一步修正；經(jīng)過(guò)第二次Kalman 濾波后，距離估計(jì)值已經(jīng)非常接近真實(shí)的測(cè)距值，相對(duì)于第一次Kalman 濾波，測(cè)距值修正結(jié)果得到了明顯的改善. 該結(jié)果表明本文所提方法對(duì)于TOA 測(cè)距值具有良好的NLoS 誤差抑制效果.

圖6 TOA 測(cè)距值重構(gòu)效果圖

將本文NLoS 誤差抑制算法與最小二乘定位算法結(jié)合以比較 NLoS 誤差抑制前后定位性能的變化，實(shí)驗(yàn)結(jié)果如圖7 所示. 可以看出，NLoS 測(cè)距值在應(yīng)用本文算法后的定位性能逼近于LoS 環(huán)境下的理想狀態(tài)，且定位收斂速度也與LoS 環(huán)境下的收斂速度相當(dāng)，定位精度較高. 其中，定位誤差小于1.25 m 的概率約為50%，定位誤差小于3 m 的概率約為90%.

圖7 使用本算法前后定位性能對(duì)比

4 總結(jié)

復(fù)雜環(huán)境下UWB 信號(hào)的NLoS 傳播嚴(yán)重影響TOA 測(cè)距精度，進(jìn)而損害UWB 定位的準(zhǔn)確性. 針對(duì)該問(wèn)題在已有NLoS 消除方法的基礎(chǔ)上，提出了一種基于兩步Kalman 濾波的NLoS 誤差抑制方法. 該方法借助NLoS 誤差與測(cè)量誤差之間的相互獨(dú)立性直接估計(jì)出 NLoS 誤差，并將其與設(shè)定閾值比較以鑒別NLoS 誤差是否存在. 通過(guò)Kalman 濾波對(duì)TOA 測(cè)距值進(jìn)行二次估計(jì)、鑒別及修正以提高TOA 測(cè)距精度，從而實(shí)現(xiàn)室內(nèi)復(fù)雜環(huán)境下的UWB 精準(zhǔn)實(shí)時(shí)定位. 仿真實(shí)驗(yàn)表明，該方法不僅能夠?qū)LoS 誤差實(shí)現(xiàn)良好的跟蹤估計(jì)，對(duì)LoS/NLoS 環(huán)境轉(zhuǎn)變也具有較強(qiáng)的靈敏感知能力，此外，NLoS 誤差測(cè)距值在應(yīng)用本文算法后的定位性能逼近于LoS 環(huán)境下的理想狀態(tài)，且定位收斂速度與無(wú)NLoS 誤差情況下的收斂速度相當(dāng).