一種抗非視距誤差的組合定位算法

徐淑萍， 王 雙， 郭 宇， 蘇小會， 張玉西

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

移動機器人定位技術作為控制機器人達到標定位置的關鍵環節，其目標在于提升定位的實時性與精度，通常被用來確定移動機器人在工作環境中的位姿。然而如紅外定位，藍牙定位、超聲波定位以及射頻識別等傳統室內定位技術，已不能滿足室內移動機器人對定位精度以及設備功耗方面的需求[1]。超寬帶(ultra wide band,UWB)技術在進行數據傳輸時，由于采用極低功率譜密度與極窄脈沖寬度的脈沖信號，使得UWB技術的時間分辨率與障礙物穿透力方面比其他定位技術更高，特別是針對少量障礙物或視距(line of sight，LOS)傳輸下的定位性能有顯著提升。但遇到目標與探測器之間障礙物有較多障礙物的非視距(non-line of sight，NLOS)時，定位性能明顯下降[2-3]。因此在定位過程中有效避免非視距誤差的影響，提高定位的精確性已然成為了提升室內定位技術的重要挑戰之一[4]。

為了降低 NLOS 誤差的影響，中外學者做了大量研究。文獻[5]通過分析大量的視距與非視距環境下的UWB信號波形特征，對NLOS狀態通過支持向量機對其進行甄別，并降低誤差的影響，雖能有效降低NLOS 誤差的影響，但前期大量的數據統計使整個方法的工作量較大；文獻[6]引入均方根延遲拓展的統計模型與最小二乘法求目標位置的最優解，來提高定位精度和算法的收斂速度，該方法一般作用在室內環境； 文獻[7]通過UWB技術結合慣性導航進行定位，通過先驗閾值對NLOS進行甄別，通過慣性導航的短期高精度結果來鑒別NLOS誤差并消除其影響，但該方法的缺點在于算法復雜，硬件成本高；文獻[8]通過采用最小二加數乘法與泰勒級別計數法技術進行結合定位,有效解決防止泰勒級數的算法不精確收斂的定位問題,卻無法有效消除針對NLOS進行定位時的誤差；文獻[9]利用無跡卡爾曼濾波及粒子濾波法對復雜環境下的目標進行定位、測速,實驗結果表明在避免NLOS誤差、測距精度方面有明顯提升,而缺點在于算法復雜,不易實現。

綜上，本文所提的N-CTK(NLOS Chan-Taylor-Kalman，N-CTK)組合定位算法中，抗NLOS誤差的卡爾曼濾波不用鑒別測距值中是否存在NLOS傳播信號路徑所帶來的偏差，對于NLOS誤差消除也不是使用卡爾曼增益進行的，而是在擴展卡爾曼濾波算法待估計的狀態向量中引入一個NLOS誤差轉化因子，然后在不斷迭代的過程中把定位估計的NLOS誤差數據漸漸剔除，進而增加定位標簽的定位準確率。

1 CTK組合定位算法

1.1 C-T協同定位算法

針對測量噪聲分布為高斯分布與測量誤差較小的情況，Chan定位算法的定位性能較好，但在實際多徑環境中，信道環境較為復雜，定位精度將變得很差[10]。Talyor定位算法簡單實用，但是對于初始坐標的依賴性較強，在信道環境中比較差的情況定位精度會下降，只有當給定的初始值接近真實坐標時才能得到精確的解[11]。通過分析兩種算法的優劣勢，為了減小運算的復雜程度并提高運算效率，使用Chan算法作為Taylor級數展開法的初始算法，既能得到一個比較準確的初始估計坐標，又能反映未知節點位置和測量值之間的關系，且有利于Taylor算法收斂[12]。同時為了提高算法精度，對C-T(Chan-Taylor)協同定位算法進行加權優化，即通過對C-T協同算法的定位結果進行加權，并設定合理的加權系數，進而得出目標的估計值，最終獲得更精確的位置。算法流程圖如圖1所示。

圖1 C-T協同加權定位算法過程

首先硬件校正及環境搭建，然后基于計算TDOA(time difference of arrival)得到的測量值構建雙曲線方程，利用Chan算法得出待測標簽的初始值，之后計算均方根誤差，若滿足實際應用，則結合計算得到殘差加權系得到最終估計值，否則將該初始值用作Taylor算法的開始得初始值，進行多次遞歸迭代并判斷定位誤差是否超過門限大小，直到滿足迭代門限，計算相應得加權系數，按加權系數得出移動標簽的位置估計坐標。

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

C-T協同定位算法不僅穩定性好，而且定位精度比單一的定位算法要高，但也有隨機性誤差存在，卡爾曼濾波能較好地抑制隨機性誤差，故將C-T協同定位算法與卡爾曼濾波相結合構成CTK(Chan Taylor Kalman)組合定位算法。該算法首先通過C-T協同定位對移動機器人標簽初次估計坐標位置，引入加權系數，再次按加權系數計算初次位置坐標得到精度更高的估計位置坐標，最后，將輸出初步優化的估計坐標位置作為卡爾曼濾波的觀測狀態，對移動機器人標簽坐標位置進行二次估計，即對初步優化估計坐標進行濾波處理。算法具體實現步驟如下。

Step1設定定位區域的大小、基站的分布、位置的存儲等。

Step2利用TDOA法進行測量，得到測量數據，將初始值數據整理并建立TDOA方程。

Step3利用Chan算法計算TDOA方程獲得標簽初始估計值，并判斷初始值的RMSE是否滿足應用，若滿足則跳轉至Step 7，否則進行下一步。

Step4把上一步驟Chan定位求解得到的初始值用作Taylor算法開始的初始值。

Step5Taylor多次迭代后的估計值判斷是否滿足閾值門限，以便定位誤差最小，定位結果最優。

Step6若滿足閾值門限則輸出定位估計值結果，否則將跳轉到步驟Step 4不斷更新迭代，直到滿足閾值門限為止。

Step7依據這時的位置估計值計算基于殘差的加權系數。

Step8將計算得到的加權系數與此時位置的定位結果融合，就可得到待測標簽的最終位置。

Step9將輸出初步優化的估計位置坐標用作改進卡爾曼濾波的觀測狀態，再對標簽節點坐標位置進行二次估計，從而輸出定位結果。

為分析室內環境下定位系統中移動機器人的C-T協同定位與CTK組合定位算法效果，如圖2所示，在西安工業大學教4樓道室內環境的8 m×10 m場地實驗區域，以A0基站為原點，根據各基站到A0的實際距離建立坐標系，假設在坐標系中A0坐標為(0 m，0 m)，其他三個基站的坐標為各自布置的距離建立A1、A2、A3坐標，隨機選取實驗區域內16個點用作移動機器人的定位分析位置，隨機取其中一部分進行分析，并記錄其定位精度，如圖3所示。

圖2 機器人室內環境下定位實景

圖3 C-T和CTK兩種算法定位結果對比

從圖3中可以觀察出，相比C-T協同定位算法，CTK組合的定位算法位置估計與真實誤差更小，均方根誤差(root mean squard error，RMSE)更低，可見CTK組合算法不僅使定位結果更加準確，并且其定位準確度有了明顯的提高。但在室內復雜環境下，信號會存在很多NLOS路徑，為了抑制NLOS誤差，需要對此算法進行優化以提高定位精度。

2 N-CTK組合算法的優化

為了消除NLOS誤差對定位系統造成的影響，首先對NLOS傳播路徑進行鑒別分類，再引入一個NLOS轉化因子，使之加入到EKF中，將得到的初步估計值整理并建立TDOA方程，通過CTK組合算法解析TDOA方程并獲得標簽位置的最終估計值。

2.1 NLOS誤差鑒別

NLOS環境下在tk時刻，定位標簽與基站距離的誤差模型可以表示為

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

(1)

式(1)中：rm(tk)為基站BSm與待測標簽在tk時刻的距離測量值；Lm(tk)為LOS環境中基站BSm與待測目標在tk時刻的真實距離；vm(tk)為待測標簽的移動速度；nm(tk)為基站的系統測量誤差，也稱標準測量誤差。

根據多項式擬合，平滑處理定位標簽到各個基站的測量距離值表示為

(2)

(3)

定位基站BSm與定位標簽的真實距離以sm(tk)用作參考值；rm(tk)的樣本標準差可以計算出來表示為

(4)

式(4)中：K表示測距樣本數量。由式(1)可以看出由于系統測量值數據中既有NLOS信號傳播的誤差也有測量噪聲的誤差，因此，測量值的標準差要大于平滑值的標準差。

(5)

式(5)中：假設檢驗H0成立，可以大致確定此時刻為LOS環境下的測量值；假設檢驗H1成立，可以大致確定此時刻為NLOS環境下的測量值，就可以重構LOS測量數據，之后利用重構的數據估計標簽位置。

2.2 引入轉化因子誤差模型

如果待測標簽估計位置的測量值中包含較多NLOS路徑，可能獲取不了定位標簽準確位置的最大似然估計值，進而導致定位標簽的位置估計偏差較大。為解決上述問題引入一個NLOS誤差轉化因子，即r(k)，也就是把NLOS信號傳播路徑重構成LOS信號傳播路徑，即相當于重構LOS，表示為

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

(6)

由式(6)可知NLOS誤差nm(tk)所占真實測量距離Lm(tk)的比例相當于轉化因子r(k)。式(6)與式(1)聯立就可以獲得一個誤差模型，其中含有NLOS 轉化因子，表達式為

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

(7)

(8)

由式(8)可知，假設r(k)=0，此時刻為LOS環境下的測量值；假設r(k)>0，則該時刻為NLOS環境下的測量值，說明此時出現了NLOS誤差。若r(k)=0.5，也就是說NLOS誤差占真實測量距離的0.3，則真實測量距離就是以前的1.3倍。

2.3 融合轉化因子的卡爾曼濾波

因為NLOS的電波傳播方式使得測量值產生較大的正偏差，引起卡爾曼定位算法性能下降[13-15]。為了消除NLOS誤差對定位估計的影響，將前面提出的NLOS轉換因子r(k)加入到擴展卡爾曼濾波待估計的狀態向量中，改進擴展卡爾曼濾波(extended Kalman filter，EKF)的迭代過程。

新的狀態矢量為

(9)

同樣，可以得到新的測量矢量，即

Z′k=[1+r(k)]d(Lk)+Vk

(10)

式(10)中：Vk為過程噪聲，d(Lk)為定位基站接收與發送之間的距離。

那么，基于引入NLOS轉換因子，擴展卡爾曼濾波態轉換和測量模型函數變為

S′k=f′k(S′k-1,W′k)=F′kS′k-1+Γ′kW′k

(11)

Z′k=h′k(S′k-1,Vk)=[1+r(k)]d(Lk)+Vk

(12)

2.4 N-CTK組合定位算法流程

結合CTK算法提出一種抗NLOS誤差的改進CTK組合定位算法，即N-CTK組合定位算法，流程如圖4所示。算法實現過程如下。

圖4 N-CTK算法流程圖

Step1部署定位區域的大小、校正硬件的相關參數、設置NLOS與LOS環境等。

Step2建立NLOS誤差模型，引入一個NLOS誤差轉化因子r(k)。

Step3把NLOS 轉化因子融合到 EKF 算法的待估計狀態向量中，然后進行多次迭代，使NLOS信號傳播路徑所帶來的異常值進行剔除，利用EKF優化處理NLOS誤差的測距估計值。

Step4將定位初步估計值整理并建立TDOA方程。

Step5CTK組合算法解析TDOA方程并獲得標簽位置的最終估計值。

3 定位實驗與分析

在西安工業大學教4樓走廊廣場8 m×10 m和計算機科學與工程學院實驗室7 m×10 m的場地，如圖5所示，假設噪聲服從高斯分布，在運動狀態下對定位數據進行分析，假設機器人移動的速度為勻速為0.5 m/s，還要保持盡量避免人員走動的遮擋所造成不必要的誤差，隨機選取一個標簽用作機器人的定位標簽節點，采用三個固定基站進行搭建，主基站坐標為(0 m，0 m)、參考基站1坐標為(5 m，5 m)、參考基站2坐標為(10 m，0 m)，在規定的路線移動運行一段時間，選取前25 s對收集的定位坐標數據進行MATLAB仿真分析，分別在室內LOS環境與室內NLOS環境下進行測試。

圖5 機器人定位實景環境

3.1 LOS環境下定位分析

在室內LOS環境下，如圖5(a)所示，移動機器人沿著規定的路線運動，選取前25 s對收集的位置坐標數據進行MATLAB仿真，仿真實驗結果如圖6和圖7所示，其中改進CTK算法就是N-CTK組合算法。

圖6 LOS環境下CTK改進前后定位結果

從圖6(a)中可以看出LOS環境下CTK組合算法改前后定位軌跡差別較小，CTK組合算法改進前后機器人定位軌跡都更加趨近真實軌跡；由圖6(b)中可以看出，在位置誤差為6 cm時，CTK組合算法改進前后的CDF(cumulative density function)相差較小，且定位誤差都比較小，但改進后的CTK組合算法還是比沒改進的CTK組合算法在定位誤差方面略微較小，在不考慮輕微誤差的情況下說明LOS環境中，兩種組合算法都適用。

從圖7中可以看出，除圖7(b)中機器人與第二個定位基站的距離在沒改進前的CTK組合算法下與真實值有輕微的誤差外，其他兩個定位基站的估計值與真實值基本吻合，表明兩種定位算法在LOS環境下定位誤差較小，定位精度較高。

圖7 LOS環境下CTK改進前后機器人與基站的距離對比

3.2 NLOS環境下定位分析

在室內NLOS環境下，如圖5(b)所示，在規定的路線移動運行一段時間，不考慮人的干擾，選取前25 s對收集的定位坐標數據進行MATLAB仿真分析，仿真實驗結果如圖8和圖9所示，圖中的改進CTK算法就是N-CTK組合算法。

圖8 NLOS環境下CTK改進前后定位結果

從圖8(a)中可以看出，在NLOS環境下CTK組合算法改前后定位軌跡差別較大，改進前的CTK組合算法機器人定位軌跡與真實軌跡偏差較大，而改進后的N-CTK組合算法機器人定位軌跡更加趨近真實軌跡；由圖8(b)中可以看出，在位置誤差為10 cm時，CTK組合算法改進前后的CDF相差較大，N-CTK組合算法定位誤差明顯較小，精度更高，由此表明：在NLOS環境下，改進后的N-CTK組合算法更加適用。

從圖9中可以看出，改進CTK組合算法前機器人與第一、二、三基站的距離誤差明顯較大，而改進后的N-CTK組合算法機器人與第一、二、三基站的距離誤差相對較小，定位軌跡較重合，更加說明在NLOS環境下N-CTK組合算法的定位算法更適應。

圖9 NLOS環境下CTK改進前后定位機器人與基站的距離對比

4 結論

基于UWB定位技術，在室內NLOS環境下進行系統定位，為消除NLOS誤差，引入一個NLOS誤差轉化因子，之后加入到擴展卡爾曼濾波待估計的狀態量中，通過與CTK組合定位算法聯合，提出一種N-CTK組合算法，最后在LOS和NLOS環境下進行動態定位實驗，并利用MATLAB工具對本文算法定位結果能進行了仿真分析，使用定位性能評價指標對本文算法定位結果進行了優越性分析分析，得出以下結論。

(1)實驗結果證明，相比傳統的C-T協同室內定位算法，CTK組合的定位算法位置估計與真實誤差更小，均方根誤差(RMSE)更低，定位結果更加準確，定位精度有了明顯的提高。

(2)實驗結果證明，在 LOS環境下誤差為6 cm時，N-CTK組合算法相比CTK組合算法的累積分布函數提高了13.5%，NLOS環境下誤差為15 cm時，N-CTK組合算法相比CTK組合算法的累積分布函數提高了55%，定位精度明顯提高。

(3)文中提出的一種抗NLOS的N-CTK組合算法可以有效抑制并且改善UWB測距中的NLOS誤差和異常測距數據值,提升 UWB定位的準確性和可靠度。