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

賈時蕾，王潛心

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

0 引言

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

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

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

1 NLoS 誤差對TOA 定位的影響

假定LoS 環(huán)境下移動站在k時刻與第i個基站之間的TOA 測距值為ri(k)，其通?？杀硎緸?/p>

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

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

圖1 NLoS 誤差對TOA 定位的影響

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

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

2.1 Kalman 濾波基本原理

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

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

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

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

2.2 NLoS 誤差識別與抑制

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

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

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

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

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

圖2 算法流程圖

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

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

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

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

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

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

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

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

圖4 NLoS 誤差估計性能

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

圖5 仿真NLoS 誤差與估計NLoS 誤差對比

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

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

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

圖7 使用本算法前后定位性能對比

4 總結(jié)

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