一種非視距環(huán)境下改進的組合定位算法

徐淑萍，楊 帆，蘇小會，王 雙

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

隨著我國對機器人技術(shù)的重視程度不斷加深，對移動機器人的使用已經(jīng)逐漸滲透到社會生產(chǎn)生活的方方面面。在移動機器人技術(shù)當中，如何提升移動機器人在室內(nèi)環(huán)境里的高精度定位要求已成為目前一個不可忽視的問題[1]。而傳統(tǒng)的室內(nèi)定位技術(shù)例如紅外線、超聲波、WIFI、藍牙、射頻識別等等，已經(jīng)不能滿足移動機器人對于在精度上的要求。文中所使用的超寬帶(Ultra Wide Band，UWB)技術(shù)，由于使用UWB信號的功率譜密度極低，脈沖寬度極窄的脈沖信號進行數(shù)據(jù)傳輸，使得UWB定位技術(shù)在傳輸速度，功率消耗，穿透能力、定位精度相比于其他定位技術(shù)效果更好，特別是在視距(Line of Sight，LOS)傳輸下的定位性能的效果顯著。但真實環(huán)境下往往非視距(Non-Line of Sight，NLOS)的情況較多，此時會導(dǎo)致定位精度明顯下降[2-4]。因此研究在非視距環(huán)境下，如何提高定位精度，已然成為了提升室內(nèi)定位技術(shù)的重要挑戰(zhàn)之一[5]。

為了抑制NLOS環(huán)境對定位精度的影響，國內(nèi)外已提出多種解決方案。文獻[6]通過分析視距以及非視距環(huán)境下的UWB信號，通過SVM算法來辨別和減少非視距環(huán)境對定位精度的影響，該方法雖可以有效降低非視距誤差，但工作量較大；文獻[7]將校正因子帶入最小二乘法中，通過逐步迭代降低非視距的影響，但室內(nèi)定位精度較低；文獻[8]以慣性導(dǎo)航結(jié)合UWB技術(shù)共同定位，利用先驗閾值對非視距進行判斷，然后使用慣性導(dǎo)航的短期高精度結(jié)果來判斷和消除非視距誤差，但硬件成本較高；文獻[9]利用殘差加權(quán)法來減少非視距誤差，但由于需要較多的基站參與定位，可實施性低。文獻[10]通過最小二乘法以及泰勒級數(shù)算法對移動目標進行定位，可解決泰勒級數(shù)算法不收斂的問題，但無法消弱非視距誤差對定位的影響；文獻[11]利用無跡卡爾曼濾波和粒子濾波方法來計算移動目標的位置以及速度，可消弱非視距誤差，提升定位精度，但算法復(fù)雜，不易實現(xiàn)。

基于上述分析，通過在擴展卡爾曼濾波器中的待估計狀態(tài)向量中引入非視距誤差轉(zhuǎn)換因子判別和消除非視距誤差，并在基于Chan和Taylor算法的協(xié)同定位算法基礎(chǔ)上，融合卡爾曼濾波算法，計算移動機器人的位置，提升整體算法的定位精度。

1 CTK組合定位算法

1.1 Chan-Taylor協(xié)同定位算法

針對測量噪聲分布為高斯分布與測量誤差較小的情況，Chan定位算法的定位性能較好，但在實際多徑環(huán)境中，信道環(huán)境較為復(fù)雜，定位精度將變得很差[12]。Talyor定位算法簡單實用，但是對于初始坐標的依賴性較強，在信道環(huán)境中比較差的情況定位精度會下降，只有當給定的初始值接近真實坐標時才能得到精確的解[13]。由于兩種算法都有自己的局限性，為了減小運算的復(fù)雜程度并提高運算效率，使用Chan算法作為Taylor級數(shù)展開法的初始算法，既能得到一個比較準確的初始估計坐標，又能反映未知節(jié)點位置和測量值之間的關(guān)系，有利于Taylor算法收斂[14]。同時為了提高算法精度，利用Chan和Taylor級數(shù)展開算法的結(jié)果進行加權(quán)系數(shù)的計算，根據(jù)合理設(shè)置的加權(quán)系數(shù)，對兩種算法的位置估計進行處理，得出目標最終的坐標估計，獲得更精確的位置。

首先把基站布置好，相應(yīng)的基站坐標保存，然后計算TDOA測量值，有了TDOA測量值以后，就可以構(gòu)建雙曲線方程，解該方程時，用上述的Chan算法得到待定位標簽的初始值，然后將該初始值作為Taylor算法的起始點，接著計算均方根誤差，并判斷誤差是否超過迭代門限，直到滿足迭代門限，使誤差最低，即得出待定位標簽的位置坐標。

具體的操作步驟[15]如下：

① 利用 Chan 算法解算出待測物體的位置估計值并將其作為初值。

② 利用 Taylor 算法經(jīng)過多步運算，得出一個定位結(jié)果。

③ 由②處理后的位置估計往往精度高于第一步的初值，但為處理Taylor 算法結(jié)果不收斂的情形，計算得到基于殘差的加權(quán)系數(shù)。

④ 將此加權(quán)系數(shù)與①②得到的位置估計結(jié)果相結(jié)合，得出Chan-Taylor(簡稱C-T)協(xié)同定位算法的最終位置估計。

1.2 融合Kalman的CTK組合定位算法

C-T協(xié)同定位算法不僅穩(wěn)定性好，而且定位精度比單一的定位算法要高，但也有隨機性誤差存在。卡爾曼濾波能較好的抑制隨機性誤差，雖然C-T協(xié)同定位算法引入了加權(quán)系數(shù)，但在不同環(huán)境下還是不可避免的存在異常值或野值或離群簇，導(dǎo)致定位算法結(jié)算的位置離實際位置偏離較大。本文將C-T協(xié)同定位優(yōu)化算法與Kalman濾波相結(jié)合構(gòu)成C-T-Kalman(簡稱CTK)組合定位算法，在室內(nèi)定位中，C-T協(xié)同定位算法的輸出坐標作為Kalman濾波的初始值，輸出移動定位標簽的估計位置。

首先C-T協(xié)同定位算法對移動機器人標簽初次估計坐標位置，引入加權(quán)系數(shù)；其次按加權(quán)系數(shù)計算初次位置坐標得到精度更高的估計位置坐標；最后，將輸出初步優(yōu)化的估計坐標位置作為卡爾曼濾波的觀測狀態(tài)，對移動機器人標簽坐標位置進行二次估計，即對初步優(yōu)化估計坐標進行濾波處理。

2 改進的CTK組合定位算法

室內(nèi)定位環(huán)境中，由于障礙物導(dǎo)致信號的非視距傳播路徑，進而造成的UWB定位估計誤差是導(dǎo)致定位偏差的主要來源之一[16]。首先，非視距環(huán)境引發(fā)信號傳播的多徑效應(yīng)使得信號到達接收節(jié)點的直達分量被錯誤判別；其次，由于UWB信號傳播過程中穿過障礙物時使得信號到達時間附帶時延，因此非視距誤差不可避免會受到一定的影響，進而產(chǎn)生基站 (Base Station，BS)與移動臺(Mobile Station，MS)也就是本文所說的移動標簽的測量距離明顯大于真實距離，導(dǎo)致UWB定位估計值大于真實值。鑒于上述非視距的影響，若實際定位中直接采用信號在非視距環(huán)境中得到的UWB定位估計值進行標簽位置估計，那么估計值會大大偏離真實值，因此對非視距傳播路徑進行鑒別分類，再通過CTK算法處理相關(guān)數(shù)據(jù)可以提高室內(nèi)定位性能[17]。

2.1 非視距誤差鑒別

(1)

式(1)中,當假設(shè)檢驗H0成立，信號在NLOS環(huán)境傳播;當假設(shè)檢驗H1成立，信號在NLOS環(huán)境傳播。確定測量誤差來自NLOS環(huán)境影響后，用卡爾曼濾波對測量數(shù)據(jù)進行重構(gòu)，將重構(gòu)后的數(shù)據(jù)代入定位算法，計算出待測目標的位置坐標。

為了更好的說明傳統(tǒng)定位算法(Chan算法，Taylor算法)，C-T算法以及CTK算法在定位精度上的差別，利用Matlab對這四種算法在理想的高斯噪聲環(huán)境下，只考慮基站設(shè)備自身的測量誤差，分別測量當TDOA測量值誤差為10 ns,20 ns,30 ns,40 ns,50 ns，標準差分別為3 m,6 m,9 m,12 m,15 m時這四種算法對目標信號的位置估計值，設(shè)定參與定位的基站為4個，用基站來定位機器人標簽發(fā)射源。主基站設(shè)在A號監(jiān)測點并用均方根誤差來評價仿真結(jié)果，如圖1所示。

圖1 四種算法定位誤差仿真結(jié)果

由圖1可知，C-T算法的均方根誤差值比Chan算法、Taylor級數(shù)定位算法的低，其定位性能優(yōu)于傳統(tǒng)定位算法，但是CTK算法比C-T算法定位性更高。可見CTK提高了經(jīng)典TDOA算法的定位精度，是一種可行有效的新型定位算法。

2.2 引入轉(zhuǎn)化因子誤差模型

在非視距環(huán)境下誤差模型可表示為

rm(tk)=Lm(tk)+vm(tk)+nm(tk),

(2)

式中:rm(tk)是tk時刻在NLOS環(huán)境中BSm基站與待測目標的距離值，Lm(tk)表示tk時刻在視距環(huán)境中基站BSm與待測目標在tk時刻的真實距離值，vm(tk)為基站的系統(tǒng)測量誤差，nm(tk)是NLOS環(huán)境下在tk時刻產(chǎn)生的測量誤差。

如果直接利用較大非視距誤差的非高斯測量值對移動臺進行定位計算，會導(dǎo)致移動臺的預(yù)測位置與真實距離的誤差增加[16]。為減少這種情況帶來的誤差，文中引入非視距轉(zhuǎn)換因子r(k)，利用式(3)將非視距誤差轉(zhuǎn)化為視距誤差，如下所示。

Lm(tk)+nm(tk)=[1+r(k)]Lm(tk),

(3)

轉(zhuǎn)換因子r(k)等于非視距誤差nm(tk)所占真實距離的比率Lm(tk)。綜合式(2)與式(3)可以得出引入非視距因子的誤差模型公式為

rm(tk)=[1+r(k)]Lm(tk)+nm(tk)。

(4)

(5)

由式(5)可知，當r(k)=0時，表示tk時刻的測量值為視距環(huán)境下的測量值；當r(k)>0時，則表示tk時刻的測量值為非視距環(huán)境下的測量值。

2.3 抗非視距誤差Kalman濾波

由于在非視距環(huán)境下，UWB信號會在穿過障礙物時會附帶時延而導(dǎo)致測量值產(chǎn)生誤差，使得Kalman算法性能下降[18-20]。為了消弱非視距誤差對位置預(yù)測的影響，將上一小節(jié)提出的非視距轉(zhuǎn)換因子r(k)引入到擴展Kalman濾波的待估計狀態(tài)向量中來改進其迭代過程。

新的狀態(tài)矢量表示為

(6)

其中[x(k)y(k)]T為水平和垂直方向的移動臺坐標，可以得到新的測量模型為

(7)

其中，改進的擴展Kalman濾波的狀態(tài)轉(zhuǎn)換和測量模型可用下面(可微)函數(shù)表示

(8)

(9)

2.4 非視距CTK組合定位算法流程

為了抑制非視距誤差，文中結(jié)合CTK算法給出了一種適用于室內(nèi)非視距環(huán)境的改進組合定位算法，算法實現(xiàn)步驟如下。

① 設(shè)置定位環(huán)境參數(shù)包括定位區(qū)域、基站部署、視距與非視距設(shè)置，誤差模型；

② 引入非視距轉(zhuǎn)換因子進行誤差識別，當為視距路徑時保存測距值轉(zhuǎn)到④，當為非視距路徑時轉(zhuǎn)到③；

③ 利用改進卡爾曼濾波器優(yōu)化處理非視距路徑的測距值，通過多次測距將非視距誤差轉(zhuǎn)化近似服從正態(tài)分布；

④ 將測距值整理并建立TDOA方程；

⑤ Chan算法解析TDOA方程并獲得標簽位置的第一次估計值；

⑥ 將估計值代入Taylor算法中獲得第二次估計值；

⑦ 計算第二次與第一次位置估計值的RMSE；

⑧ 判斷RMSE是否超過預(yù)設(shè)的門限值，若超過門限值，則跳轉(zhuǎn)至④；若沒有超過門限值，則輸出位置估計值；

⑨ 根據(jù)此時位置估計值計算加權(quán)系數(shù)；

⑩ 根據(jù)加權(quán)系數(shù)計算最終標簽位置估計值；

3 定位實驗與分析

3.1 視距環(huán)境下定位分析

實驗場地設(shè)定為4號教學(xué)樓的一塊6 m×10 m的場地，如圖2所示。假設(shè)噪聲服從高斯分布，測定移動機器人在運動時的定位數(shù)據(jù)并進行分析。

圖2 機器人定位部署環(huán)境

本實驗選用人進行代替移動機器人。假設(shè)人是類似機器人移動，為了減少由于人身體對定位標簽的遮擋，讓人在測量期間一直手持定位標簽并舉過頭頂。機器人在地面移動，而人手持定位標簽，定位標簽在大約離地2米的空中移動，對此所造成的輕微影響本實驗不考慮在內(nèi)。任意選取四個基站中的其中一個作為定位標簽，其他三個為固定基站。固定基站的坐標分別為(0,0)，(5,5)，(10,0)，在規(guī)定的路線移動運行一段時間，并且保持周邊環(huán)境不變，例如沒有人員走動，選取前25秒對收集的位置坐標數(shù)據(jù)使用Matlab仿真分析。首先視距環(huán)境下測試，之后在非視距環(huán)境下測試，測試數(shù)據(jù)圖如圖3所示。如圖3(a)所示，其中CDF也稱為累積分布函數(shù)，為概率密度函數(shù)的積分，用來說明待測標簽定位誤差的概率分布。累積分布函數(shù)圖用來描述定位誤差滿足門限之下定位點數(shù)量的概率分布情況，從圖中可以清晰的看到不同算法的定位效果。視距環(huán)境下的改進前后的CTK組合算法對移動臺的移動軌跡的測量差距較小，更趨近于真實軌跡；如圖3(b)所示。當位置誤差為60 mm時，雖然改進后的CTK組合定位算法較改進前的CDF相差不大，定位誤差相差也比較小，但整體上還是略優(yōu)于改進前的CTK算法。實驗結(jié)果表明在不考慮輕微誤差時的視距環(huán)境下，兩種組合算法都適用。

圖3 視距環(huán)境下CTK改進前后定位結(jié)果對比圖

如圖4所示，改進前后的CTK算法在測定視距環(huán)境下移動臺與三個固定基站之間的距離時，在測定第二個基站的定位值與真實值有輕微的誤差，其他兩個基站的測量值與真實值也基本吻合。同樣表明改進前后的CTK定位算法在視距環(huán)境下的定位誤差都較小，精度較高，都適用于對視距環(huán)境下移動機器人的測量。

圖4 視距環(huán)境下改進前后的CTK算法測定移動臺與基站的距離值對比圖

3.2 非視距環(huán)境下定位分析

實驗場地設(shè)定為學(xué)院實驗室的一塊6 m×10 m的場地，如圖5所示。假設(shè)噪聲服從高斯分布，測定移動機器人在運動時的定位數(shù)據(jù)并進行分析。任意選取四個基站中的其中一個作為定位標簽，其他三個為固定基站，固定基站的坐標分別為(0,0)，(5,5)，(10,0)。在非視距環(huán)境下的規(guī)定的路線移動運行一段時間，并且保持周邊環(huán)境不變，例如沒有人員走動，選取前25秒對收集的位置坐標數(shù)據(jù)使用Matlab仿真分析，其結(jié)果如圖6所示。

圖5 非視距環(huán)境下CTK改進前后定位結(jié)果對比圖

圖6 非視距環(huán)境下改進前后的CTK算法測定移動臺與基站的距離值對比圖

如圖5(a)所示，在非視距環(huán)境下改進前后的CTK組合算法定位軌跡相差較大，改進后的CTK組合定位算法更加貼近于真實值；如圖5(b)所示，以位置誤差為100 mm時為例，改進后的CTK組合算法相較于改進前的CDF值有了明顯的提升，也說明改進后的CTK算法的定位精度有了明顯的提升，因此表明在非視距情況下，改進后的CTK算法在定位精度上有了一定的提升。如圖6所示，改進前的CTK組合算法測量的移動機器人與第一、第二、第三基站的距離誤差明顯較大，而改進后的N-CTK組合算法機器人與第一、第二、第三基站的距離誤差相對較小，定位軌跡較重合，充分說明在非視距環(huán)境下，改進后的CTK算法的性能要優(yōu)于改進前的CTK算法。

3.3 機器人方型軌跡分析

選取室內(nèi)環(huán)境6 m×7 m的范圍內(nèi)，采用四個基站節(jié)點一個標機器人簽節(jié)點進行搭建，移動機器人從坐標(1 m,1 m)開始，一次移動到(5 m,1 m)，(5 m,5 m)，(1 m,5 m)這三個坐標點，并往返兩次進行測試。設(shè)定移動機器人速度為0.5 m·s-1，可知移動機器人起始位置會有一個滯后差的距離。測距、數(shù)據(jù)傳輸以及算法處理所耗費的時間大約為100 ms，可知機器人在非視距環(huán)境移動狀態(tài)下的滯后距離約為0.05 m。在更為復(fù)雜的環(huán)境下可能會造成更大的滯后差，CTK改進前后定位方型軌跡和CDF圖如圖7～8所示。其中圖7的橫縱坐標分別為選取的室內(nèi)環(huán)境的長和寬，(Y距離，X距離)為機器人相對于選取的室內(nèi)環(huán)境位置坐標。

圖7 CTK改進前后定位方型軌跡

文中用的是普通方法繪制CDF圖，而其他方法繪制的CDF圖，可能會和文中的實驗結(jié)果有差異，呈現(xiàn)的精度也可能發(fā)生細微的改變。取一次定位軌跡數(shù)據(jù)分析，如圖8所示?？梢钥闯?，改進CTK聯(lián)合定位算法大約95%的定位結(jié)果誤差在0.04 m左右，CTK定位算法大約82%的定位結(jié)果誤差在0.04 m左右，因此改進CTK聯(lián)合算法定位結(jié)果更加準確，定位效果提升了大約13%左右。另外本實驗采集的數(shù)據(jù)有限，所有實驗結(jié)果圖都是基于本實驗數(shù)據(jù)量為某一定值的前提下得出的，當采集數(shù)據(jù)量增多或減少時，實驗結(jié)果可能會和本文結(jié)果有偏差，文中的研究為提高室內(nèi)定位精度提供了一種方法。

圖8 CTK改進前后定位方型軌跡CDF圖

4 結(jié) 論

基于UWB定位技術(shù)，在室內(nèi)非視距環(huán)境下進行系統(tǒng)定位，為消除非視距誤差，引入一個非視距誤差轉(zhuǎn)化因子，加入到擴展卡爾曼濾波待估計的狀態(tài)量中。通過與CTK組合定位算法聯(lián)合，提出一種改進的CTK組合算法，最后在視距環(huán)境和非視距環(huán)境下進行動態(tài)定位實驗，并利用Matlab工具對文中算法定位結(jié)果能進行了仿真分析，使用定位性能評價指標對本文算法定位結(jié)果進行了優(yōu)越性分析。分析結(jié)果表明，文中算法針對非視距環(huán)境在約95%的定位結(jié)果中的誤差僅為0.04m左右，相比于CTK算法，定位精度效果提升了13%左右，定位結(jié)果更加準確，能夠有效消除視距與非視距環(huán)境下的使用UWB技術(shù)進行測距時產(chǎn)生的非視距誤差，從而提高了UWB 定位的精度和可靠性，滿足室內(nèi)移動機器人的定位要求。