面向超寬帶的室內小車動態跟蹤方法

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

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

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

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

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

1 UWB測距定位算法

1.1 UWB測距原理

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

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

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

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

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

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

1.2 UWB定位原理

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

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

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

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

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

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

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

2 基于UWB室內小車跟蹤方法

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

2.1 卡爾曼濾波

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

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

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

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

式（6）～（7）中：F、H是系統的狀態轉移矩陣；T0為數據采樣周期；xk為狀態向量；wk，vk分別為由協方差矩陣Q，R組成的高斯白噪聲。

KF預測方程及狀態更新方程定義為：

KF采用線性遞推的方式，通過最小均方差來獲得最佳線性估計或濾波［13］，并在對多個參數與數據進行處理時，能夠實現實時狀態更新。由于KF是在已知方差的情況下估計系統狀態［11］，因此設

2.2 粒子濾波

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

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

式中：N為獨立分布的樣本數；ωk為k時刻的粒子權重，滿足并采用重要性采樣（importance sampling，IS）進行選?。沪襨為Delta函數。

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

PF預測過程為：

PF更新過程為：

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

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

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

綜上所述，超寬帶室內導航定位經過PF算法重采樣及狀態更新后，計算出k時刻系統狀態估計值為：

3 實驗與結果對比分析

3.1 UWB測距卡爾曼濾波

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

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

3.2 EKF與PF的動態目標跟蹤性能對比

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

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

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

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

為了更加清晰地顯現出EKF與PF兩者之間的跟蹤差異性，繪制了兩者在X方向與Y方向的跟蹤波動偏差對比及濾波軌跡與真實軌跡的RMSE曲線圖，如圖9～圖11所示，其定位跟蹤精度統計計算如表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 結論

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