朱潤馳，王洪源，陳慕羿

(沈陽理工大學(xué) 信息科學(xué)與工程學(xué)院，沈陽110159)

軍用機(jī)器人中最為常見的是輪式移動機(jī)器人，可代替人類進(jìn)行偵察、排爆、作戰(zhàn)等工作，其在運動方面具有移動速度快、機(jī)動能力強(qiáng)、控制方式簡易等特點[1]。考慮到軍用機(jī)器人的特殊性及室內(nèi)弱GPS環(huán)境對定位的影響較大，對其室內(nèi)精確定位穩(wěn)定性、準(zhǔn)確性及智能性均提出較高要求[2]。目前對軍用機(jī)器人精確定位的研究大多為仿真模擬，缺少實驗對比。

超寬帶(Ultra Wide Band，UWB)定位技術(shù)是一種無線載波通信技術(shù)，具有區(qū)別于其他定位技術(shù)的時間分辨率高、抗多徑能力強(qiáng)、穿透性能好等優(yōu)點，其響應(yīng)頻率能達(dá)到30Hz以上，確保了運動體實時定位的關(guān)鍵性需求。但由于其超高頻段，加之真實應(yīng)用環(huán)境復(fù)雜多變，當(dāng)系統(tǒng)所處環(huán)境中存在障礙物遮擋信號等情況出現(xiàn)非視距(Non Line of Sight，NLOS)誤差時，會使測量精度大大降低，影響定位結(jié)果[3]。

消除NLOS誤差成為目前精確定位研究的一個重要方向，許多學(xué)者針對如何改善NLOS誤差進(jìn)行了大量研究。文獻(xiàn)[4]建立了卷積神經(jīng)網(wǎng)絡(luò)(Convolutional Neural Networks，CNN)定位模型，采集數(shù)據(jù)后進(jìn)行訓(xùn)練，通過擴(kuò)展卡爾曼濾波進(jìn)行目標(biāo)位置估計。文獻(xiàn)[5]采用最小二乘法區(qū)分NLOS環(huán)境，用泰勒級數(shù)展開計算視距，采用Chan氏定位算法計算NLOS傳輸情況，然后進(jìn)行組合定位。文獻(xiàn)[6]重新定義了卡爾曼增益矩陣，使時鐘漂移得到補償，從而使主從基站保持時鐘同步，并仿真分析了帶寬與頻譜密度對于算法的影響。上述方法各有優(yōu)劣，但多為仿真分析，未考慮基于NLOS誤差建立的定位模型中也存在影響定位結(jié)果的參數(shù)。

本文設(shè)計一種改進(jìn)的粒子濾波定位模型，將標(biāo)準(zhǔn)粒子濾波算法(Particle Filter，PF)運用于UWB定位系統(tǒng)，針對原算法的重采樣環(huán)節(jié)存在樣本貧化的問題，使用馬克可夫鏈蒙特卡洛方法(Markov Chain Monte Carlo，MCMC)優(yōu)化重采樣，避免樣本貧化；通過殘差平方和(Residual Sum of Squares，RSS)算法判定當(dāng)前系統(tǒng)受到NLOS誤差影響程度，設(shè)置閾值對測量數(shù)據(jù)進(jìn)行選擇性剔除；設(shè)計UWB測距模塊進(jìn)行實驗，將實驗結(jié)果通過均方根誤差(Root Mean Squared Error，RMSE)進(jìn)行算法精度分析。

1 超寬帶定位技術(shù)原理

UWB定位技術(shù)的最大優(yōu)勢是采用了時間間隔在1ns內(nèi)的脈沖進(jìn)行通信，主要原理是通過系統(tǒng)主基站與定位標(biāo)簽之間的信號往返傳輸飛行時間(Time Of Flight，TOF)來計算距離，從而得到更準(zhǔn)確的測距結(jié)果。圖1所示為雙邊雙向飛行時間法，即采用兩個往返時間來計算飛行時間的方法，目的是消除時鐘誤差和偏差誤差[7]。

圖1 雙邊雙向飛行時間法

UWB系統(tǒng)用于標(biāo)簽定位的基礎(chǔ)算法通常使用到達(dá)時間差(Time Difference of Arrival，TDOA)方法，其原理是標(biāo)簽通過檢測脈沖信號到達(dá)兩個基站的時間差值來確定此時的位置，而非檢測信號到達(dá)目標(biāo)基站的絕對時間。TDOA的三基站定位圖如圖2所示。

圖2 TDOA三基站定位圖

將到達(dá)時間的差值乘以電磁波傳播速度c，得到距離差值，即

(1)

式中：li，j為定位標(biāo)簽到編號i基站與定位標(biāo)簽到編號j基站的距離差值，i≠j；i，j=1，2，…，k；li、lj分別為UWB標(biāo)簽到編號i和編號j基站的測距值；k為系統(tǒng)布置的基站總數(shù)；Ti、Tj分別為信號在編號i和編號j基站與標(biāo)簽之間傳輸所用時間。

將系統(tǒng)中編號為z的基站設(shè)置為系統(tǒng)中心基站，TDOA雙曲線模型可表示為

(2)

式中：li，z為標(biāo)簽到i基站與標(biāo)簽到中心基站z的距離差；(x，y)表示標(biāo)簽真實位置坐標(biāo)；(xi，yi)表示i基站的平面坐標(biāo)；(xz，yz)表示中心基站的平面坐標(biāo)。

實際應(yīng)用中，UWB系統(tǒng)會受到很多因素的干擾，影響其定位的精度。研究發(fā)現(xiàn)，影響UWB系統(tǒng)定位的最主要因素是環(huán)境存在的NLOS誤差，其產(chǎn)生的原因是標(biāo)簽在進(jìn)行信號收發(fā)的路徑上存在障礙物，導(dǎo)致系統(tǒng)傳輸信號無法進(jìn)行視距傳播，而在系統(tǒng)環(huán)境中反射或折射傳播，定位精度會受到較大影響。

2 改進(jìn)粒子濾波定位模型

2.1 標(biāo)準(zhǔn)粒子濾波算法

PF算法的核心思想是通過隨機(jī)抽取一組樣本(粒子)來代替當(dāng)前系統(tǒng)狀態(tài)的后驗概率分布。當(dāng)系統(tǒng)處于非線性環(huán)境時，相比于其他算法，使用該算法具有更好的濾波性能與處理非高斯噪聲影響的能力，故在定位系統(tǒng)中應(yīng)用越來越廣泛[8]。本文將粒子濾波算法運用至UWB系統(tǒng)，估計t時刻運動體的定位坐標(biāo)Xt，其系統(tǒng)模型由兩個非線性函數(shù)表示為

Xt+1=f(Xt，ut)

(3)

zt=e(Xt)+vt

(4)

式中：Xt+1表示系統(tǒng)的下一時刻定位坐標(biāo)；zt為當(dāng)前時刻標(biāo)簽與各基站的測量距離；ut、vt分別為系統(tǒng)存在的NLOS誤差和測量噪聲；f為運動位置轉(zhuǎn)移函數(shù)；e為觀測函數(shù)。

標(biāo)準(zhǔn)粒子濾波算法在每個時刻都會生成離散的權(quán)重粒子集，即

(5)

利用st對定位坐標(biāo)的后驗概率密度p(Xt|zt)進(jìn)行求解，過程主要分為三個階段。

(1)初始化

(2)序貫重要性采樣

先對參考分布函數(shù)q(Xt|Xt-1，zt，ut-1)中加權(quán)的粒子進(jìn)行隨機(jī)采樣。利用貝葉斯原理，系統(tǒng)的后驗概率密度p(X0∶t|zt)變換為

(6)

式中p(zt|z1∶t-1)為常數(shù)。式(6)可變換為

p(X0∶t|zt)∝p(zt|Xt)p(Xt|Xt-1)p(X0∶t-1|z1∶t-1)

(7)

觀測模型p(zt|Xt)表示在當(dāng)前定位坐標(biāo)Xt處，UWB標(biāo)簽測量信息為zt的概率，計算式為

(8)

式中：δ表示過程中的測量噪聲方差；k表示方差維度。

對每個粒子重新分配權(quán)重，即

(9)

歸一化權(quán)重為

(10)

(3)重采樣

當(dāng)UWB定位系統(tǒng)周圍環(huán)境發(fā)生變化出現(xiàn)NLOS誤差時，由于步驟(2)中概率密度的選擇未考慮當(dāng)前時刻的實測值，故先驗概率密度函數(shù)和真實的后驗密度函數(shù)的樣本會出現(xiàn)較大不同。算法結(jié)果會隨著時間不斷迭代，導(dǎo)致權(quán)重粒子集st中少數(shù)粒子的權(quán)重逐漸增大，其余粒子權(quán)重逐漸降低，該現(xiàn)象稱為粒子退化現(xiàn)象[9]。引入采樣有效閾值neff來處理該問題，即

(11)

neff越小，說明退化現(xiàn)象越嚴(yán)重。當(dāng)neff小于限定值時，通過重采樣的方法來處理退化現(xiàn)象，對加權(quán)粒子集使用“優(yōu)勝劣汰”原則去除小權(quán)重粒子。

2.2 輔助重采樣算法

重采樣能夠在一定程度上改善退化現(xiàn)象，使UWB系統(tǒng)在出現(xiàn)NLOS誤差時的定位精度不受較大影響。但由于粒子的逐漸替代更新會導(dǎo)致模型出現(xiàn)樣本貧化現(xiàn)象，使加權(quán)粒子集的多樣性逐漸減弱，UWB定位結(jié)果出現(xiàn)持續(xù)偏移與誤差累積等狀況。

針對模型樣本貧化問題，解決方式可以在重采樣階段加入MCMC方法。核心是建立穩(wěn)態(tài)分布為π的馬爾可夫鏈，從中得到π的樣本，其具有與所期望目標(biāo)密度等價的穩(wěn)態(tài)分布，能夠進(jìn)行各種統(tǒng)計推斷[10]。PF-MCMC實現(xiàn)步驟如下。

(12)

(4)狀態(tài)更新

(13)

(14)

2.3 NLOS誤差判定

實際應(yīng)用出現(xiàn)較大NLOS誤差時，會將原始測量值直接輸入到算法模型中，導(dǎo)致濾波發(fā)散，使定位誤差逐漸增大。判定系統(tǒng)的定位NLOS誤差通常有兩種方式：一是對系統(tǒng)測得的數(shù)據(jù)直接進(jìn)行處理，達(dá)到減弱NLOS誤差的目的；二是先識別系統(tǒng)信號傳輸過程中是否存在NLOS誤差，再通過優(yōu)化算法處理誤差產(chǎn)生的影響。

考慮到實際應(yīng)用中不能實時獲得真實定位坐標(biāo)等問題，本文將NLOS誤差判定方法進(jìn)行改進(jìn)，通過已知的中心基站坐標(biāo)(xz，yz)，利用RSS判斷當(dāng)前時刻的系統(tǒng)定位精度受NLOS影響程度，即

(15)

具體判定步驟為

(1)將Dt與dt帶入判定式；

(2)設(shè)置閾值Δ；

(3)當(dāng)SSR≥Δ時，表示原始定位偏移過大，剔除此時存在較大NLOS誤差的測距數(shù)據(jù)。

閾值由多次測試實驗分析后計算得出，其值過小將使系統(tǒng)失去部分有效測距數(shù)據(jù)。

3 實驗測試與結(jié)果分析

3.1 實驗設(shè)備與環(huán)境

實驗采用DecaWave公司生產(chǎn)的DWM1000芯片，設(shè)計其相關(guān)外部電路，開發(fā)了搭載于Arduino UNO核心板的超寬帶模塊。硬件模塊的具體工作流程為：上電后，基站模塊檢測周圍是否有標(biāo)簽?zāi)K；當(dāng)檢測到環(huán)境周圍有標(biāo)簽?zāi)K時啟動連接程序，與標(biāo)簽進(jìn)行連接數(shù)據(jù)傳輸，實現(xiàn)測距。本文設(shè)計UWB測距模塊的印制電路板(Printed Circuit Board，PCB)與實物設(shè)計如圖3所示。

圖3 UWB測距模塊

測試場景選取占地面積4m2的空間環(huán)境，為保證系統(tǒng)在測量時空間的統(tǒng)一性，將超寬帶模塊安裝在平臺的正中心位置。同時，為保證各模塊空間位置的準(zhǔn)確性，使用激光水平儀與激光測距儀測量場地的空間距離。實驗環(huán)境如圖4所示。通過實際環(huán)境的位置制定測試環(huán)境的相對位置坐標(biāo)，其中超寬帶基站模塊的坐標(biāo)分別為基站1(0，0，10)、基站2(200，0，10)、基站3(100，173.2，10)(坐標(biāo)單位：cm)。

圖4 實驗環(huán)境

3.2 實驗結(jié)果分析

為驗證算法的可行性及相比于原算法的優(yōu)勢，分別進(jìn)行兩組實驗。第一組實驗為空曠環(huán)境中的定位效果；第二組實驗在環(huán)境中放置障礙物來模擬遮擋信號等非視距情況。區(qū)域內(nèi)機(jī)器人的實際行進(jìn)路線設(shè)置為

A(50，0)→B(150，0)→C(150，86.6)→D(50，86.6)→A(50，0)(坐標(biāo)單位：cm)。

每組分別進(jìn)行10次重復(fù)實驗，通過3σ準(zhǔn)則去除各次實驗數(shù)據(jù)的較大誤差值，然后將剩余數(shù)據(jù)取平均值，作為該點的定位坐標(biāo)。

第一組實驗為無NLOS影響時定位效果，直接使用TDOA原始算法的定位軌跡與偏差度如圖5所示。

圖5 實驗一定位軌跡對比圖

第二組實驗為受NLOS影響時定位效果，在環(huán)境中加入體積為2500cm3的木制阻擋物，來模擬NLOS環(huán)境。TDOA原始算法定位、PF算法定位、PF-MCMC算法定位的軌跡對比結(jié)果及與真實軌跡的偏差度如圖6所示。

圖6 實驗二定位軌跡對比圖

由圖5和圖6定位結(jié)果對比可知，在無阻擋物的環(huán)境中，TDOA原始算法的定位效果良好；處于NLOS環(huán)境中時，直接使用TDOA原始算法進(jìn)行定位受NLOS干擾較大，造成定位誤差較大。

為更直觀地顯示各算法之間的差異，采用RMSE衡量各算法定位結(jié)果與真實定位結(jié)果的偏差(保留三位小數(shù))，對定位精度進(jìn)行定量分析。

(16)

式中：(xt，yt)為使用各算法時得到的相應(yīng)定位坐標(biāo)；(xturet，yturet)為該時刻直接測量得到的真實坐標(biāo)。

各算法定位結(jié)果的精度定量分析RMSE曲線如圖7所示。

圖7 RMSE對比曲線

由圖7可以看出，使用標(biāo)準(zhǔn)粒子濾波算法后，優(yōu)化了系統(tǒng)定位，但隨著運動過程中出現(xiàn)NLOS干擾，樣本逐漸貧化，誤差逐漸增大；在標(biāo)準(zhǔn)粒子濾波算法中加入MCMC步驟，能夠在系統(tǒng)出現(xiàn)

NLOS干擾時，增大部分隨機(jī)粒子的權(quán)重值，保證模型粒子的多樣性，避免局部最優(yōu)的情況出現(xiàn)，使最終的模型定位結(jié)果更加接近真實軌跡。

4 結(jié)論

針對標(biāo)準(zhǔn)粒子濾波算法存在的粒子貧化、計算量大等問題進(jìn)行了改進(jìn)，提出了一種基于MCMC的改進(jìn)粒子濾波定位模型。該定位模型能夠?qū)崟r更新隨機(jī)粒子權(quán)重，優(yōu)化在NLOS環(huán)境下出現(xiàn)的定位偏移與誤差累積問題。設(shè)計基于DWM1000的UWB硬件模塊系統(tǒng)，通過實驗對比驗證了改進(jìn)算法可以減弱粒子貧化現(xiàn)象，進(jìn)而達(dá)到精確定位的效果。結(jié)果表明，本文模型能夠在非視距的環(huán)境中有效提高定位精度，滿足軍用機(jī)器人的室內(nèi)定位需求。但文中只考慮了二維環(huán)境中的系統(tǒng)定位效果，而未考慮到Z軸方向上的位移對系統(tǒng)定位準(zhǔn)確度的影響，對三維環(huán)境中的定位是今后需要進(jìn)一步研究的重要內(nèi)容。