朱潤(rùn)馳，王洪源，陳慕羿

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

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

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

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

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

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

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

圖1 雙邊雙向飛行時(shí)間法

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

圖2 TDOA三基站定位圖

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

(1)

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

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

(2)

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

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

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

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

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

Xt+1=f(Xt，ut)

(3)

zt=e(Xt)+vt

(4)

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

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

(5)

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

(1)初始化

(2)序貫重要性采樣

先對(duì)參考分布函數(shù)q(Xt|Xt-1，zt，ut-1)中加權(quán)的粒子進(jìn)行隨機(jī)采樣。利用貝葉斯原理，系統(tǒng)的后驗(yàn)概率密度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)

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

(8)

式中：δ表示過(guò)程中的測(cè)量噪聲方差；k表示方差維度。

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

(9)

歸一化權(quán)重為

(10)

(3)重采樣

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

(11)

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

2.2 輔助重采樣算法

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

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

(12)

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

(13)

(14)

2.3 NLOS誤差判定

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

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

(15)

具體判定步驟為

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

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

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

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

3 實(shí)驗(yàn)測(cè)試與結(jié)果分析

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

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

圖3 UWB測(cè)距模塊

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

圖4 實(shí)驗(yàn)環(huán)境

3.2 實(shí)驗(yàn)結(jié)果分析

為驗(yàn)證算法的可行性及相比于原算法的優(yōu)勢(shì)，分別進(jìn)行兩組實(shí)驗(yàn)。第一組實(shí)驗(yàn)為空曠環(huán)境中的定位效果；第二組實(shí)驗(yàn)在環(huán)境中放置障礙物來(lái)模擬遮擋信號(hào)等非視距情況。區(qū)域內(nèi)機(jī)器人的實(shí)際行進(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ù)實(shí)驗(yàn)，通過(guò)3σ準(zhǔn)則去除各次實(shí)驗(yàn)數(shù)據(jù)的較大誤差值，然后將剩余數(shù)據(jù)取平均值，作為該點(diǎn)的定位坐標(biāo)。

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

圖5 實(shí)驗(yàn)一定位軌跡對(duì)比圖

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

圖6 實(shí)驗(yàn)二定位軌跡對(duì)比圖

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

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

(16)

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

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

圖7 RMSE對(duì)比曲線

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

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

4 結(jié)論

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