徐淑萍， 王 雙， 郭 宇， 蘇小會(huì)， 張玉西

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

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

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

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

1 CTK組合定位算法

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

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

圖1 C-T協(xié)同加權(quán)定位算法過程

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

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

C-T協(xié)同定位算法不僅穩(wěn)定性好，而且定位精度比單一的定位算法要高，但也有隨機(jī)性誤差存在，卡爾曼濾波能較好地抑制隨機(jī)性誤差，故將C-T協(xié)同定位算法與卡爾曼濾波相結(jié)合構(gòu)成CTK(Chan Taylor Kalman)組合定位算法。該算法首先通過C-T協(xié)同定位對(duì)移動(dòng)機(jī)器人標(biāo)簽初次估計(jì)坐標(biāo)位置，引入加權(quán)系數(shù)，再次按加權(quán)系數(shù)計(jì)算初次位置坐標(biāo)得到精度更高的估計(jì)位置坐標(biāo)，最后，將輸出初步優(yōu)化的估計(jì)坐標(biāo)位置作為卡爾曼濾波的觀測(cè)狀態(tài)，對(duì)移動(dòng)機(jī)器人標(biāo)簽坐標(biāo)位置進(jìn)行二次估計(jì)，即對(duì)初步優(yōu)化估計(jì)坐標(biāo)進(jìn)行濾波處理。算法具體實(shí)現(xiàn)步驟如下。

Step1設(shè)定定位區(qū)域的大小、基站的分布、位置的存儲(chǔ)等。

Step2利用TDOA法進(jìn)行測(cè)量，得到測(cè)量數(shù)據(jù)，將初始值數(shù)據(jù)整理并建立TDOA方程。

Step3利用Chan算法計(jì)算TDOA方程獲得標(biāo)簽初始估計(jì)值，并判斷初始值的RMSE是否滿足應(yīng)用，若滿足則跳轉(zhuǎn)至Step 7，否則進(jìn)行下一步。

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

Step5Taylor多次迭代后的估計(jì)值判斷是否滿足閾值門限，以便定位誤差最小，定位結(jié)果最優(yōu)。

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

Step7依據(jù)這時(shí)的位置估計(jì)值計(jì)算基于殘差的加權(quán)系數(shù)。

Step8將計(jì)算得到的加權(quán)系數(shù)與此時(shí)位置的定位結(jié)果融合，就可得到待測(cè)標(biāo)簽的最終位置。

Step9將輸出初步優(yōu)化的估計(jì)位置坐標(biāo)用作改進(jìn)卡爾曼濾波的觀測(cè)狀態(tài)，再對(duì)標(biāo)簽節(jié)點(diǎn)坐標(biāo)位置進(jìn)行二次估計(jì)，從而輸出定位結(jié)果。

為分析室內(nèi)環(huán)境下定位系統(tǒng)中移動(dòng)機(jī)器人的C-T協(xié)同定位與CTK組合定位算法效果，如圖2所示，在西安工業(yè)大學(xué)教4樓道室內(nèi)環(huán)境的8 m×10 m場(chǎng)地實(shí)驗(yàn)區(qū)域，以A0基站為原點(diǎn)，根據(jù)各基站到A0的實(shí)際距離建立坐標(biāo)系，假設(shè)在坐標(biāo)系中A0坐標(biāo)為(0 m，0 m)，其他三個(gè)基站的坐標(biāo)為各自布置的距離建立A1、A2、A3坐標(biāo)，隨機(jī)選取實(shí)驗(yàn)區(qū)域內(nèi)16個(gè)點(diǎn)用作移動(dòng)機(jī)器人的定位分析位置，隨機(jī)取其中一部分進(jìn)行分析，并記錄其定位精度，如圖3所示。

圖2 機(jī)器人室內(nèi)環(huán)境下定位實(shí)景

圖3 C-T和CTK兩種算法定位結(jié)果對(duì)比

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

2 N-CTK組合算法的優(yōu)化

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

2.1 NLOS誤差鑒別

NLOS環(huán)境下在tk時(shí)刻，定位標(biāo)簽與基站距離的誤差模型可以表示為

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

(1)

式(1)中：rm(tk)為基站BSm與待測(cè)標(biāo)簽在tk時(shí)刻的距離測(cè)量值；Lm(tk)為L(zhǎng)OS環(huán)境中基站BSm與待測(cè)目標(biāo)在tk時(shí)刻的真實(shí)距離；vm(tk)為待測(cè)標(biāo)簽的移動(dòng)速度；nm(tk)為基站的系統(tǒng)測(cè)量誤差，也稱標(biāo)準(zhǔn)測(cè)量誤差。

根據(jù)多項(xiàng)式擬合，平滑處理定位標(biāo)簽到各個(gè)基站的測(cè)量距離值表示為

(2)

(3)

定位基站BSm與定位標(biāo)簽的真實(shí)距離以sm(tk)用作參考值；rm(tk)的樣本標(biāo)準(zhǔn)差可以計(jì)算出來表示為

(4)

式(4)中：K表示測(cè)距樣本數(shù)量。由式(1)可以看出由于系統(tǒng)測(cè)量值數(shù)據(jù)中既有NLOS信號(hào)傳播的誤差也有測(cè)量噪聲的誤差，因此，測(cè)量值的標(biāo)準(zhǔn)差要大于平滑值的標(biāo)準(zhǔn)差。

(5)

式(5)中：假設(shè)檢驗(yàn)H0成立，可以大致確定此時(shí)刻為L(zhǎng)OS環(huán)境下的測(cè)量值；假設(shè)檢驗(yàn)H1成立，可以大致確定此時(shí)刻為NLOS環(huán)境下的測(cè)量值，就可以重構(gòu)LOS測(cè)量數(shù)據(jù)，之后利用重構(gòu)的數(shù)據(jù)估計(jì)標(biāo)簽位置。

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

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

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

(6)

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

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

(7)

(8)

由式(8)可知，假設(shè)r(k)=0，此時(shí)刻為L(zhǎng)OS環(huán)境下的測(cè)量值；假設(shè)r(k)>0，則該時(shí)刻為NLOS環(huán)境下的測(cè)量值，說明此時(shí)出現(xiàn)了NLOS誤差。若r(k)=0.5，也就是說NLOS誤差占真實(shí)測(cè)量距離的0.3，則真實(shí)測(cè)量距離就是以前的1.3倍。

2.3 融合轉(zhuǎn)化因子的卡爾曼濾波

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

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

(9)

同樣，可以得到新的測(cè)量矢量，即

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

(10)

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

那么，基于引入NLOS轉(zhuǎn)換因子，擴(kuò)展卡爾曼濾波態(tài)轉(zhuǎn)換和測(cè)量模型函數(shù)變?yōu)?/p>

S′k=f′k(S′k-1,W′k)=F′kS′k-1+?！鋕W′k

(11)

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

(12)

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

結(jié)合CTK算法提出一種抗NLOS誤差的改進(jìn)CTK組合定位算法，即N-CTK組合定位算法，流程如圖4所示。算法實(shí)現(xiàn)過程如下。

圖4 N-CTK算法流程圖

Step1部署定位區(qū)域的大小、校正硬件的相關(guān)參數(shù)、設(shè)置NLOS與LOS環(huán)境等。

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

Step3把NLOS 轉(zhuǎn)化因子融合到 EKF 算法的待估計(jì)狀態(tài)向量中，然后進(jìn)行多次迭代，使NLOS信號(hào)傳播路徑所帶來的異常值進(jìn)行剔除，利用EKF優(yōu)化處理NLOS誤差的測(cè)距估計(jì)值。

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

Step5CTK組合算法解析TDOA方程并獲得標(biāo)簽位置的最終估計(jì)值。

3 定位實(shí)驗(yàn)與分析

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

圖5 機(jī)器人定位實(shí)景環(huán)境

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

在室內(nèi)LOS環(huán)境下，如圖5(a)所示，移動(dòng)機(jī)器人沿著規(guī)定的路線運(yùn)動(dòng)，選取前25 s對(duì)收集的位置坐標(biāo)數(shù)據(jù)進(jìn)行MATLAB仿真，仿真實(shí)驗(yàn)結(jié)果如圖6和圖7所示，其中改進(jìn)CTK算法就是N-CTK組合算法。

圖6 LOS環(huán)境下CTK改進(jìn)前后定位結(jié)果

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

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

圖7 LOS環(huán)境下CTK改進(jìn)前后機(jī)器人與基站的距離對(duì)比

3.2 NLOS環(huán)境下定位分析

在室內(nèi)NLOS環(huán)境下，如圖5(b)所示，在規(guī)定的路線移動(dòng)運(yùn)行一段時(shí)間，不考慮人的干擾，選取前25 s對(duì)收集的定位坐標(biāo)數(shù)據(jù)進(jìn)行MATLAB仿真分析，仿真實(shí)驗(yàn)結(jié)果如圖8和圖9所示，圖中的改進(jìn)CTK算法就是N-CTK組合算法。

圖8 NLOS環(huán)境下CTK改進(jìn)前后定位結(jié)果

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

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

圖9 NLOS環(huán)境下CTK改進(jìn)前后定位機(jī)器人與基站的距離對(duì)比

4 結(jié)論

基于UWB定位技術(shù)，在室內(nèi)NLOS環(huán)境下進(jìn)行系統(tǒng)定位，為消除NLOS誤差，引入一個(gè)NLOS誤差轉(zhuǎn)化因子，之后加入到擴(kuò)展卡爾曼濾波待估計(jì)的狀態(tài)量中，通過與CTK組合定位算法聯(lián)合，提出一種N-CTK組合算法，最后在LOS和NLOS環(huán)境下進(jìn)行動(dòng)態(tài)定位實(shí)驗(yàn)，并利用MATLAB工具對(duì)本文算法定位結(jié)果能進(jìn)行了仿真分析，使用定位性能評(píng)價(jià)指標(biāo)對(duì)本文算法定位結(jié)果進(jìn)行了優(yōu)越性分析分析，得出以下結(jié)論。

(1)實(shí)驗(yàn)結(jié)果證明，相比傳統(tǒng)的C-T協(xié)同室內(nèi)定位算法，CTK組合的定位算法位置估計(jì)與真實(shí)誤差更小，均方根誤差(RMSE)更低，定位結(jié)果更加準(zhǔn)確，定位精度有了明顯的提高。

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

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