匡 兵，陳鳳冉，孫毛毛，曾憲鋒，景 暉

（桂林電子科技大學(xué) 機電工程學(xué)院，廣西 桂林 541000）

超寬帶（ultra wide band，UWB）作為一種室內(nèi)無線定位技術(shù)，因其信號隱蔽性強、數(shù)據(jù)傳輸效率高、多徑分辨率好等優(yōu)點［1］，越來越多地應(yīng)用在室內(nèi)移動機器人的導(dǎo)航定位中。目標跟蹤技術(shù)作為UWB室內(nèi)導(dǎo)航定位領(lǐng)域中的一項重要關(guān)鍵技術(shù)，是目前相關(guān)高校及企業(yè)關(guān)注的難題之一。經(jīng)典卡爾曼濾波器（kalman filter，KF）在線性系統(tǒng)中要求狀態(tài)空間變量和觀察到的變量是連續(xù)的、高斯分布［2］。但在UWB室內(nèi)目標跟蹤過程中，由于周圍環(huán)境存在噪聲和測量誤差干擾，并非理想中的線性高斯環(huán)境，因此KF已經(jīng)不再適用非線性室內(nèi)目標跟蹤的研究中［3］。

非線性室內(nèi)目標跟蹤的研究中，處理非線性目標跟蹤問題盡管存在很大的改進空間，但作為基礎(chǔ)研究方法，粒子濾波（particle filter，PF）仍具有很大的研究價值［4］。PF作為非線性貝葉斯估計問題的一種數(shù)值逼近方法，目前已有較為成熟的理論和實際應(yīng)用［5］。2014年，Li X等［6］對傳統(tǒng)PF目標跟蹤過程中存在的粒子退化問題，研究引入了重采樣技術(shù)，實驗結(jié)果得出PF具有更好的實時跟蹤性能及跟蹤精度。2017年，Peter Rohal等［7］創(chuàng)建了一個粒子濾波器，可以觀察二維空間中隨機飛行物體的軌跡，實驗結(jié)果得出：與經(jīng)典卡爾曼濾波跟蹤相比，粒子濾波對雷達實測軌跡跟蹤效果更好。

針對非視距室內(nèi)超寬帶目標動態(tài)定位跟蹤不精確的問題，本文利用適用于線性模型的卡爾曼濾波對測距部分進行濾波降噪處理，有效降低了測距誤差。在此基礎(chǔ)上采用適用于非線性模型的PF對目標小車進行動態(tài)定位跟蹤研究，并與EKF跟蹤結(jié)果進行對比分析。通過實驗得出，PF對于小車的定位跟蹤效果更好，跟蹤軌跡更加接近真實軌跡。

1 UWB測距定位算法

1.1 UWB測距原理

本文選用的UWB傳感器主要定位模塊，是Decawave公司于2012年推出的基于DW1000定位芯片的DWM1000模塊。由于DWM1000模塊抗多徑衰落能力強、支持高數(shù)據(jù)速率通信、功耗低等優(yōu)點，在室內(nèi)UWB定位領(lǐng)域被廣泛應(yīng)用。為了減小超寬帶在測距過程中因時鐘偏移導(dǎo)致不必要的過大測距誤差，采用DW1000推出的DS測距算法（double-sided two-way ranging，DS）。

為了彌補因時鐘偏移而導(dǎo)致的誤差，DS測距算法在SS測距（single-sided two-way ranging，SS）原理基礎(chǔ)上增加了一次通訊。其測距原理如圖1所示。

DS算法的基本測距過程為：標簽A首先向基站B發(fā)送一個無線電信息并記錄下發(fā)送時間T1；基站B需要提前準備接收此信號，并在接收到信號之后記錄下時間T2；基站B接收到信號在一個特定的延遲時間Tr1后發(fā)送回復(fù)信號給標簽A，此時基站B記錄發(fā)送時間T3；標簽A收到回復(fù)信號并記下接收時間T4；標簽A在一個特定延遲時間Tr2后發(fā)送最終信號給基站B，并記錄下時間T5；基站B接收到最終信號，記錄時刻T6。

由標簽A與基站B之間的測距過程，可計算出信號飛行時間為：

式（1）中：TR1＝T4-T1；TR2＝T6-T3；Tr1＝T3-T2；Tr2＝T5-T4。

由式（1）可知，DS測距得出電磁波在A與B之間的飛行時間，假設(shè)電磁波在空氣中的傳播速度與光速相近為c，則兩節(jié)點之間的距離為：

1.2 UWB定位原理

無線傳感器網(wǎng)絡(luò)的位置定位算法可分為基于距離（range-based）的定位算法和基于無距離（range-free）的定位算法2種。前者能夠利用節(jié)點之間的距離等測量值來計算出標簽的運動位置，對傳感器節(jié)點依賴低、定位精度高；后者對傳感器設(shè)備的要求較小，不需要節(jié)點之間的測量信息，但定位誤差精確度不高［8］。通過對比分析，基于距離的定位算法更能滿足實驗要求。

現(xiàn)有技術(shù)中，因不同的數(shù)學(xué)處理方法使得基于測距的定位技術(shù)略有不同，如三角測量、三邊定位算法及多重網(wǎng)格［9］。其中，三邊定位算法由于能減小不必要的計算且容易實現(xiàn)，是無線傳感器網(wǎng)絡(luò)定位的基本測距方法［10］，其定位原理如圖2所示。

由圖2可知，3個已知基站P1、P2、P3的坐標分別為（x1，y1，z1）、（x2，y2，z2）、（x3，y3，z3）；未知的標簽坐標設(shè)為（x0，y0，z0）；標簽到三基站的距離為各圓半徑，分別為di（i＝1，2，3）。

則由三邊定位原理得到標簽的距離公式為：

將式（3）方程組中各個方程平方之后得到一組非線性方程組，利用數(shù)學(xué)原理得到其線性化方程為：

利用最小二乘法，通過解算計算出標簽的坐標：

由式（3）～式（5）可知，由于無線傳感器網(wǎng)絡(luò)三邊定位算法得到的測量定位方程并不是線性的，因此在超寬帶定位下，為了得到更加準確的定位跟蹤信息，本文采用適用于非線性系統(tǒng)下的PF對目標進行跟蹤研究分析。

2 基于UWB室內(nèi)小車跟蹤方法

針對超寬帶在非視距室內(nèi)目標動態(tài)跟蹤不精確的問題，本文提出一種基于超寬帶的室內(nèi)小車動態(tài)跟蹤方法。該方法利用KF對超寬帶測距部分進行降噪處理，有效降低了因非視距及信號干擾等原因造成的測距誤差影響。利用適用于非線性非高斯情況下PF對室內(nèi)小車進行定位跟蹤研究，有效提高了跟蹤精度。

2.1 卡爾曼濾波

本文實驗場景為小面積室內(nèi)多障礙物復(fù)雜條件，采集到的UWB測距值主要受非視距誤差影響嚴重，波動性較大，通過實際測量得出小車動態(tài)情況下的定位均方根誤差為18.586 cm。為了得到小車更加精準的位置信息，針對非視距等因素的影響，有必要對UWB測距部分進行降噪處理。

2019年，王川陽等［11］針對UWB定位系統(tǒng)中由非視距等因素導(dǎo)致的測距誤差，分別采用小波變換、Vondrak濾波及卡爾曼濾波3種方法對UWB原始觀測數(shù)據(jù)和定位結(jié)果2種方案進行降噪處理，實驗結(jié)果得出卡爾曼濾波方法的降噪效果最優(yōu)。作為線性模型中的一種經(jīng)典濾波降噪方法，KF具有處理傳感器噪聲能力強及計算復(fù)雜度較低等優(yōu)點，能很大程度上降低超寬帶室內(nèi)非視距等影響，減小測距誤差。

綜上所述，超寬帶在室內(nèi)測距過程中受非視距影響，使得測距數(shù)據(jù)精度嚴重受到影響，產(chǎn)生過大的誤差，對后續(xù)目標跟蹤研究極為不利，因此考慮對超寬帶測距數(shù)據(jù)首先進行KF濾波。短時間內(nèi)，標簽與基站之間的距離可認為是均勻變化［12］，距離公式為：.

則基于超寬帶測距的KF的狀態(tài)方程與觀測方程離散形式為：

式（6）～（7）中：F、H是系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；T0為數(shù)據(jù)采樣周期；xk為狀態(tài)向量；wk，vk分別為由協(xié)方差矩陣Q，R組成的高斯白噪聲。

KF預(yù)測方程及狀態(tài)更新方程定義為：

KF采用線性遞推的方式，通過最小均方差來獲得最佳線性估計或濾波［13］，并在對多個參數(shù)與數(shù)據(jù)進行處理時，能夠?qū)崿F(xiàn)實時狀態(tài)更新。由于KF是在已知方差的情況下估計系統(tǒng)狀態(tài)［11］，因此設(shè)

2.2 粒子濾波

超寬帶定位過程并不是線性分布，傳統(tǒng)的線性跟蹤方法在非線性系統(tǒng)中局限性較大，使跟蹤精度受到較大影響。PF打破了傳統(tǒng)的基于KF延伸的非線性跟蹤定位思想，對系統(tǒng)的觀測及狀態(tài)方程不再有更多的線性需求，因此其在非線性系統(tǒng)的跟蹤性及適應(yīng)性更強，跟蹤精度及收斂速度更加滿足工程需求。

PF實質(zhì)上是蒙特卡洛方法里增加了一層重要性采樣，其依然屬于貝葉斯估計的理論范疇［14］。該方法的基本思想是用同一組隨機變量（粒子）來遞歸近似系統(tǒng)狀態(tài)的后驗概率分布，使用近似估計來逼近非線性系統(tǒng)的狀態(tài)，大數(shù)定律表明這個近似估計收斂于真實分布［15］。用大量隨機樣本建立的系統(tǒng)，基本的后驗概率分布為：

式中：N為獨立分布的樣本數(shù)；ωk為k時刻的粒子權(quán)重，滿足并采用重要性采樣（importance sampling，IS）進行選取；σk為Delta函數(shù)。

PF預(yù)測部分中，粒子通過系統(tǒng)模型在傳播過程中添加了噪聲，導(dǎo)致傳播中的粒子出現(xiàn)了不確定性，從而影響系統(tǒng)模型的準確性。為了解決因噪聲問題導(dǎo)致的不準確性，權(quán)重函數(shù)被用來給每一個粒子分配一個權(quán)重值，即ωk。其中，IS的基本原則就是給每個粒子分配權(quán)重，即通過當(dāng)前的分布提取新的粒子，使得重量較輕的粒子消失，重量較大的粒子會存活下來并可能取代較輕的粒子，這實際上就是PF的更新部分［15］。PF的跟蹤過程如下：

PF預(yù)測過程為：

PF更新過程為：

PF需要大量的粒子才能對非線性系統(tǒng)進行正確的建模，但隨著粒子數(shù)量的增加，計算復(fù)雜度與運算耗費時間也隨之增加［16］。如果減少粒子的數(shù)目，則會導(dǎo)致有效粒子的數(shù)目越小，使得粒子之間權(quán)重值差距增大，出現(xiàn)粒子退化現(xiàn)象。針對PF跟蹤過程中出現(xiàn)的這種問題，研究在重要性采樣之后采用重采樣方法。即在保持粒子數(shù)目情況下，用一組新的由權(quán)重較大的粒子，重新根據(jù)自身的權(quán)重值按比例分配所得的粒子代替權(quán)重小的粒子。

重采樣舍棄了權(quán)重較小的粒子，并代之以權(quán)重較大的粒子，降低了重要性采樣算法中存在的權(quán)值退化現(xiàn)象。重采樣過程在滿足條件下，將初始粒子集合更新為即：

初始粒子集經(jīng)過重采樣后，權(quán)重較大的粒子代替了較小的粒子，增加了粒子集的可靠性。同時粒子的權(quán)重被重新分配為最終PF后驗概率分布更新為：

綜上所述，超寬帶室內(nèi)導(dǎo)航定位經(jīng)過PF算法重采樣及狀態(tài)更新后，計算出k時刻系統(tǒng)狀態(tài)估計值為：

3 實驗與結(jié)果對比分析

3.1 UWB測距卡爾曼濾波

本文實驗場地選用的是500 cm×600 cm室內(nèi)非空曠場地，3個定位基站的坐標分別為A（0，0，0），B（0，300，0），C（450，550，0），動態(tài)運動下目標標簽的速度為20 cm／s。為了減小UWB因信號干擾等因素導(dǎo)致的測距數(shù)據(jù)精度低的問題，在其測距部分首先采用KF進行濾波處理，效果如圖3所示。

由于實驗所用到的場地并非視距環(huán)境下，因此標簽在運動過程中，與基站之間的信號通訊隨著環(huán)境變化而呈現(xiàn)不同頻率的波動，導(dǎo)致原始測量得到的數(shù)據(jù)并非理想中的平滑曲線。由圖3可以看出，原始測量數(shù)據(jù)d1、d2、d3曲線并不光滑，某些采樣點的測量數(shù)據(jù)偏離較為嚴重。經(jīng)過KF處理后，曲線kfd1、kfd2、kfd3能有效地減弱原始數(shù)據(jù)的偏離度，使得采樣曲線更加接近于理想平滑曲線，有效降低測距帶來的誤差影響。

3.2 EKF與PF的動態(tài)目標跟蹤性能對比

由于實驗過程中處于非視距小面積的室內(nèi)場地，面對復(fù)雜的室內(nèi)環(huán)境，實驗得出PF的計算復(fù)雜度及計算時間，盡管隨著粒子數(shù)目的增多而增加，但其定位跟蹤精度卻并非隨之嚴格單調(diào)遞增。為了得到更加直觀的跟蹤精度變化，本文定位指標的性能評定標準選用均方根誤差（root mean square error，RMSE）。圖4為在同一條件下，實驗分別采用粒子數(shù)目為i（i＝100，500∶500∶5 000）進行PF跟蹤時所得到的系統(tǒng)RMSE。

圖5為Matlab對PF運算時間擬合結(jié)果，其中，PF運算時長為對所采用的不同粒子總數(shù)i分別進行10次運算所求取的PF定位跟蹤平均運算的時長；圖6為EKF跟蹤運算時間。

由圖4可得，當(dāng)i＝2 000時，PF在對標簽進行動態(tài)定位跟蹤時RMSE相對較?。挥蓤D5、圖6可得，通過Matlab對PF跟蹤運算時間擬合結(jié)果得出，PF的跟蹤運算時間隨著粒子總數(shù)的增多近似呈指數(shù)增長。且當(dāng)i＝2 000時，PF定位跟蹤的運算時間為199.189 ms，盡管PF跟蹤速度相對EKF較慢，但在非視距室內(nèi)實驗過程中，目標運動速度為20 cm／s情況下，其由重采樣技術(shù)等引起的運算復(fù)雜度所導(dǎo)致的跟蹤性誤差為3.984 cm，遠小于UWB因非視距等因素導(dǎo)致的測量誤差，滿足實驗實時性跟蹤需求。

因此，以下PF實驗均采用粒子數(shù)i＝2 000時所得到的實驗結(jié)果與EKF進行對比分析。圖7與圖8為EKF與PF的動態(tài)目標跟蹤軌跡，從圖7、圖8可以看出：與原始測量軌跡相比，EKF與PF的跟蹤軌跡更加接近真實軌跡。

為了更加清晰地顯現(xiàn)出EKF與PF兩者之間的跟蹤差異性，繪制了兩者在X方向與Y方向的跟蹤波動偏差對比及濾波軌跡與真實軌跡的RMSE曲線圖，如圖9～圖11所示，其定位跟蹤精度統(tǒng)計計算如表1所示。

表1 X、Y方向波動及定位跟蹤精度 cm

從表1可以看出，與UWB原始測量定位相比，EKF及PF對原始測量軌跡進行跟蹤后，其定位跟蹤精度均有不同程度地提高。通過實際計算可以看出，EKF跟蹤后的X方向波動性與測量相比降低了9.772%，Y方向波動性降低了7.346%，定位精度提高了6.968%；PF跟蹤后的X方向波動性與測量相比降低了15.447%，Y方向波動性降低了22.736%，定位精度提高了20.074%；PF跟蹤后的X方向波動性與EKF相比降低了6.290%，Y方向波動性降低了16.610%，定位精度提高了14.088%。

由實驗最終定位跟蹤精度得出，PF的跟蹤效果更好，跟蹤實時性滿足實驗需求。與EKF跟蹤相比，其在X方向與Y方向的跟蹤波動更小，定位跟蹤精度相比提高了14.088%，與真實軌跡更加接近。

4 結(jié)論

通過對UWB測距原理進行分析，采用適用于線性模型的KF對測距部分進行了濾波降噪處理，有效克服了UWB因室內(nèi)非視距、信號干擾等因素導(dǎo)致的部分測量數(shù)據(jù)誤差較大的情況。利用PF對標簽?zāi)繕诉M行動態(tài)跟蹤，建立了超寬帶室內(nèi)小車非線性動態(tài)跟蹤系統(tǒng)模型。通過實驗對比分析了EKF與PF兩者基于超寬帶室內(nèi)目標動態(tài)定位跟蹤效果。實驗結(jié)果得出：PF的跟蹤效果更好，在X方向與Y方向的跟蹤波動更小，與真實軌跡更加接近，可為室內(nèi)非線性系統(tǒng)模型下超寬帶目標定位跟蹤提供參考。