一種融合卡爾曼濾波的室內(nèi)組合定位算法*

徐淑萍，熊小墩，郭 宇，蘇小會，張玉西

(西安工業(yè)大學 計算機科學與工程學院，西安 710021)

移動機器人獲取自身位置是最基本的要求，如何在室內(nèi)復雜多變的環(huán)境下完成高精度、快速實時定位任務(wù)成為一個需要解決的難題[1]。目前，在室內(nèi)環(huán)境下常用的定位技術(shù)有Wi-Fi、超寬帶(Ultra Wide Band,UWB)、超聲波等。其中，基于UWB技術(shù)的定位系統(tǒng)，具有測距精度高、發(fā)射功率低、通信穩(wěn)定性好、抗干擾能力強等特點，適用于室內(nèi)定位[2]。而在室內(nèi)無線定位方法中，基于測距的到達時間差(The Time Differences of Arrival,TDOA)具有測量簡單、成本低、高定位準確度等優(yōu)點[3]。

在室內(nèi)定位方法方面，文獻[4]使用三邊定位法構(gòu)建UWB定位系統(tǒng)，針對帶有噪聲干擾的定位數(shù)據(jù)，采用IMM卡爾曼濾波方法進行平滑處理，顯著抑制了噪聲帶來的干擾。文獻[5]提出了一種基于UWB技術(shù)的掘進機位姿檢測方法，考慮環(huán)境變化、測距范圍等影響因素的情況下進行測距實驗并進行定位精度的分析。由于系統(tǒng)和測量噪聲誤差的統(tǒng)計特征很難被準確估計，所以常見的卡爾曼濾波效果并不理想[6]。文獻[7]采用模糊自適應卡爾曼濾波，雖然可以實現(xiàn)測量噪聲方差陣的在線自適應調(diào)整，定位精度有所提高，但是實時定位效果卻不佳;文獻[8]在系統(tǒng)噪聲的估計模型中引入了極大似然準則，對模型優(yōu)化過程中加入滾動時域策略[9]，最后基于二次規(guī)劃方法對噪聲統(tǒng)計的估計值進行獲取，但是該方法復雜度較高。

綜合上述分析，文中基于TDOA定位方法，并結(jié)合陳-泰勒(Chan-Taylor,C-T)協(xié)同定位算法，引入權(quán)重因子，再融合卡爾曼濾波進行平滑處理，設(shè)計一種陳-泰勒-卡爾曼(Chan-Taylor-Kalman,CTK)組合定位算法，減少實際定位過程中因隨機誤差和環(huán)境干擾引起的的測量誤差而產(chǎn)生的噪聲數(shù)據(jù)對定位的影響，提升定位的精度和穩(wěn)定性，并在移動機器人平臺上進行動靜態(tài)實驗，相較于C-T協(xié)同算法取得了更好的定位效果。

1 TDOA定位算法描述

TDOA即信號到達時間差測距，通過測量兩個不同基站接收信號的時間差值，乘以信號的傳播速度c得到定位標簽到基站的距離差，求解距離差方程組得出定位標簽的位置坐標,如圖1所示。

圖1 TDOA定位原理圖

TDOA二維定位為雙曲線模型，選定BS1為主基站，BS2、BS3為參考基站，BS1、BS2、BS3接收到信號的時間分別為t1、t2、t3，MS(x,y)為定位標簽的位置坐標，固定基站BSi的坐標為(xi,yi)，ri(i=1,2,3)為固定基站到定位標簽的距離，ri1為定位標簽到第i個基站的距離和到主基站BS1的距離的差值。由定位標簽到3個固定基站的時間差可得2個TDOA值，結(jié)合ri1=c·(ti-t1)得到距離差的方程組為

(1)

由式(1)得到雙曲線的方程組為

(2)

對式(2)的方程組求解可得出定位標簽的位置坐標。

1.1 Chan 算法

Chan算法基于TDOA技術(shù)，是求解解析表達式雙曲線方程組的定位算法[10]，具有非遞歸性。求解過程中，對式(2) 進行線性化處理，兩次利用最小二乘法加權(quán)估計結(jié)果。Chan定位算法在假定測量噪聲分布符合高斯分布并且測量誤差較小時，定位性能較好，但在實際多徑環(huán)境中，信道環(huán)境較為復雜，定位精度將變得很差[11]。

1.2 Taylor 算法

Taylor算法是一種需要初始值的遞歸算法[12]，其利用上一次的計算結(jié)果作為本次迭代過程的初始值，通過最小二乘法求出誤差解來對迭代值不斷的進行修正，直到誤差滿足設(shè)定的閾值為止。該算法最大優(yōu)點是精度高，但是在解算之前，需要選擇合適的初值，初值選取將直接影響定位結(jié)果的精度[13]，因此一般會與其他算法協(xié)同定位。

2 基于UWB的CTK組合定位算法

2.1 C-T協(xié)同定位算法

為了減小運算的復雜程度并提高運算效率，將 Chan算法作為Taylor級數(shù)展開法的初始算法，既可以改善在非高斯噪聲的干擾下Chan算法精度下降，Taylor算法結(jié)果出現(xiàn)不收斂的現(xiàn)象[14]，得到一個比較準確的初始估計坐標，又能反映未知節(jié)點位置和測量值之間的關(guān)系。所以將Chan算法和Taylor算法進行融合優(yōu)化為C-T協(xié)同定位算法，算法的具體步驟如下：

① 利用UWB基站的分布位置和TDOA測量值構(gòu)建TDOA方程，然后利用Chan算法對TDOA中的定位方程組進行求解，解算出標簽的位置估計值。

② 若①中得到的初始值均方根誤差滿足應用，則直接作為最終的標簽位置估計值。

③ 當①若中得到的初始值均方根誤差不滿足應用時，則利用Taylor 算法對Chan算法解算出的估計值進行迭代修正，得出一個優(yōu)化的定位結(jié)果，然后判斷其誤差是否滿足設(shè)定的閾值。

④ 若③中的誤差不滿足設(shè)定的閾值，則轉(zhuǎn)回到Taylor算法中繼續(xù)迭代，直到滿足設(shè)定的閾值為止。

⑤ 當③中的誤差滿足設(shè)定的閾值時，則輸出其定位結(jié)果，作為最終的標簽位置估計值。

2.2 C-T協(xié)同加權(quán)定位算法

雖然C-T協(xié)同定位算法相對于Taylor算法能更收斂，但是仍可能出現(xiàn)定位結(jié)果發(fā)散而不收斂的情況，所以通過把C-T協(xié)同算法的結(jié)果進行加權(quán)的計算，根據(jù)合理設(shè)置的加權(quán)系數(shù)，得出目標最終的坐標估計，獲得更精確的位置，算法的具體步驟如下：

① 利用UWB基站的分布位置和TDOA測量值構(gòu)建TDOA方程，然后利用Chan算法對TDOA中的定位方程組進行求解，解算出標簽的位置估計值。

② 若①中得到的初始值均方根誤差滿足應用，則直接計算其加權(quán)系數(shù)獲取最終標簽定位估計值。

③ 當①中得到的初始值均方根誤差不滿足應用時，則利用Taylor 算法對Chan算法解算出的估計值進行迭代修正，得出一個優(yōu)化的定位結(jié)果，然后判斷其誤差是否滿足設(shè)定的閾值。

④ 若③中的誤差不滿足設(shè)定的閾值，則轉(zhuǎn)回到Taylor算法中繼續(xù)迭代，直到滿足設(shè)定的閾值為止。

⑤ 當③中的誤差滿足設(shè)定的閾值時，則輸出其定位結(jié)果，并計算其加權(quán)系數(shù)，獲取最終標簽定位估計值。

為了計算加權(quán)系數(shù)，定義定位結(jié)果與測量值之間的殘差為

(3)

如果對同一組TDOA測量值分別采用k種定位算法，則第k種算法的加權(quán)系數(shù)Rk的計算方法[15]為

(4)

(5)

2.3 改進Kalman濾波剔除異常值

卡爾曼濾波算法分兩個過程：預測和更新。預測階段卡爾曼濾波算法利用前個狀態(tài)的估計做出對當前狀態(tài)的估計[16-17]。然后更新階段利用當前狀態(tài)的觀測值優(yōu)化預測值，進而得到最佳估計新值。卡爾曼濾波的狀態(tài)方程為

xk=Fxk-1+wk-1。

(6)

觀測方程為

zk=Hxk-1+vk。

(7)

狀態(tài)預測為

(8)

狀態(tài)估計值為

(9)

濾波增益為

(10)

協(xié)方差為

(11)

更新協(xié)方差為

Pk|k=(I-KkHk)Pk|k-1。

(12)

當系統(tǒng)在k時刻進行更新時，異常的測量值會影響最優(yōu)估計的結(jié)果，但若在式(9)狀態(tài)估計時，進行修正計算的殘差，然后再最優(yōu)估計，則會減輕異常值的影響。其中Δ(k)為殘差，D(k)為方差矩陣，表示為

(13)

D(k)=E[Δ(k)Δ(k)T]

=HP(k|k-1)HT+R。

(14)

由3σ準則和式(13)、式(14)可以得出異常值的一個判定公式，表示為

(15)

2.4 融合卡爾曼濾波的CTK組合定位算法

由于在實際定位中會有隨機誤差和環(huán)境干擾引起的測量誤差，因這些誤差產(chǎn)生的噪聲數(shù)據(jù)會使定位產(chǎn)生波動，影響定位的穩(wěn)定性和精度，而卡爾曼濾波能較好的抑制這些噪聲，故進一步優(yōu)化算法將C-T協(xié)同定位算法與卡爾曼濾波相結(jié)合構(gòu)成CTK組合定位算法，以改善定位的穩(wěn)定性同時提升定位的精度。該算法將上述C-T協(xié)同加權(quán)定位算法輸出的估計位置坐標作為改進卡爾曼濾波的觀測狀態(tài)，對移動機器人標簽坐標位置進行二次估計，即對初步優(yōu)化估計坐標進行濾波處理，算法的具體步驟如下：

① 利用UWB基站的分布位置和TDOA測量值構(gòu)建TDOA方程，然后利用Chan算法對TDOA中的定位方程組進行求解，解算出標簽的位置估計值。

② 若①中得到的初始值均方根誤差滿足應用，則計算其加權(quán)系數(shù)獲取標簽定位估計值并作為改進Kalman濾波的觀測狀態(tài)，然后對標簽的位置進行二次估計得到最終的定位結(jié)果。

③ 當①中得到的初始值均方根誤差不滿足應用時，則利用Taylor 算法對Chan算法解算出的估計值進行迭代修正，得出一個優(yōu)化的定位結(jié)果，然后判斷其誤差是否滿足設(shè)定的閾值。

④ 若③中的誤差不滿足設(shè)定的閾值，則轉(zhuǎn)回到Taylor算法中繼續(xù)迭代，直到滿足設(shè)定的閾值為止。

⑤ 當③中的誤差滿足設(shè)定的閾值時，則輸出其定位結(jié)果，并計算其加權(quán)系數(shù)，獲取標簽定位估計值。

⑥ 將⑤中獲取的定位估計值作為改進Kalman濾波的觀測狀態(tài)，然后對標簽的位置進行二次估計得到最終的定位結(jié)果。

3 實驗驗證與分析

3.1 定位性能評價指標

文中采用均方根誤差(Root Mean Square Error,RMSE)作為定位性能的評價指標。

RMSE表示為標簽的估計值和真實值偏差的平方與觀測數(shù)n的比值的平方根[20]，其值越小，表明估計值越靠近真實值，表示為

(16)

式中:n為觀測次數(shù);x,y為機器人的實際位置坐標;xi,yi為第i次估計的標簽位置坐標。

3.2 靜態(tài)定位分析

實驗采用一標簽四基站的方式，其中UWB標簽T0固定在移動機器人身上，用筆記本電腦遠程控制機器人運動。四個UWB基站分別為A0、A1、A2和A3，其中以主基站A0的坐標(0,0)為原點建立坐標系，A1、A2、A3的坐標為各自布置的距離。各基站固定在距離地面1.5 m高的相同三腳架的頂端，基站和標簽分別配備充電寶供電，其中主基站A0通過USB串口和筆記本電腦連接通信。測試場景如圖2所示的10 m×10 m大小的室內(nèi)視距環(huán)境，不考慮行人的干擾。

圖2 場景布置示意圖Fig.2 Layout of the test site

為分析算法在靜態(tài)環(huán)境下的定位精度，在測試區(qū)域選取趨近于正方形的16個點用作移動機器人的定位分析，記為T1-T16，測量數(shù)據(jù)見表1。

表1 基站靜態(tài)測量數(shù)據(jù)Tab.1 Static measurement data of base station 單位：m

由于UWB測距定位數(shù)據(jù)是實時刷新的，表1中的數(shù)據(jù)為該定位點上截取的一小段時間內(nèi)測量結(jié)果的平均值。為了方便觀察，將其中部分實時刷新的100次數(shù)據(jù)記錄下來，分別計算其定位均方根誤差，并用三維圖進行表示，如圖3所示。

通過圖3和圖2中基站的分布位置可以明顯看出，由于基站的安放位置靠近墻邊，靠近墻體的定位點精度會受到墻體的影響導致定位的RMSE增大，越靠近四個基站的中心位置，定位的誤差越小。同時對比圖3(a)和圖3(b)可以看出，同樣是靠近墻體定位受到干擾的情況下CTK組合定位相比于C-T協(xié)同定位的均方根誤差更小，即定位的精度更高。

圖3 兩種算法定位均方根誤差三維圖Fig.3 Three dimensional maps of the root mean square error of the two algorithms

對表1中16個點的定位結(jié)果進行分析，比較CTK組合算法和C-T協(xié)同算法的定位效果，如圖4所示。從圖4(a)中可以觀察出CTK組合定位算法的位置估計值相比于C-T協(xié)同算法更靠近實際值，定位結(jié)果更加準確。從圖4(b)可以觀察到，CTK組合算法在絕大多數(shù)的定位RMSE都要明顯低于C-T協(xié)同算法。

圖4 C-T和CTK兩種算法定位結(jié)果對比Fig.8 Comparison of the positioning results obtained by C-T and CTK algorithms

結(jié)合表1中兩種算法的平均RMSE表明：靜態(tài)情況下CTK組合算法相較于C-T協(xié)同算法定位RMSE下降了約13.2%，進一步降低了噪聲對定位的影響，提高了定位的精度。

3.3 動態(tài)定位分析

圖2為實驗環(huán)境的視距場地，機器人從坐標(1，5)勻速直線運動到坐標(11，5)，速度設(shè)置為v=0.5 m·s-1，如圖5所示。

圖5 兩種算法動態(tài)定位結(jié)果Fig.5 Dynamic positioning results obtained by two algorithms

由圖5(a)可以看出，C-T協(xié)同算法定位的坐標點比較分散，沒有集中靠近在真實運動軌跡附近，而CTK組合算法的定位軌跡與真實運動軌跡則比較吻合，整體靠近在真實運動軌跡附近。從圖5(b)中兩種算法定位軌跡的均方根誤差中可以觀察到CTK組合算法定位RMSE相對比較平穩(wěn)，波動幅度較小，大部分集中在9 cm，而C-T協(xié)同算法雖然有小部分RMSE接近于0，但整體波動幅度相對較大，定位性能不穩(wěn)定。

綜上所述，動態(tài)環(huán)境下CTK組合算法相比于C-T協(xié)同算法的定位性能更穩(wěn)定，整體來看定位的RMSE更低，定位精度更高，更好的抑制了噪聲的干擾影響。

4 結(jié) 論

通過對UWB定位算法進行分析和優(yōu)化，在C-T協(xié)同算法的基礎(chǔ)上，引入權(quán)重因子，再融合卡爾曼濾波算法進行平滑處理，提出一種抑制定位誤差的CTK組合算法。實驗結(jié)果表明CTK組合算法相比于C-T協(xié)同算法靜態(tài)環(huán)境下的定位均方根誤差為7.54 cm，下降了約13.2%，提升了定位的精度；動態(tài)環(huán)境下的定位均方根誤差波動幅度更小，集中在9 cm，定位性能更穩(wěn)定。在室內(nèi)對高精度定位有要求的場景下，將具有較高的應用性和實用價值。