劉 唯，尹 磊，沈子陽

（南京理工大學(xué) 機(jī)械工程學(xué)院，南京 210094）

UWB 定位技術(shù)[1]是一種基于超寬帶無線電波傳輸?shù)亩ㄎ患夹g(shù)，具有較高的定位精度和抗干擾能力，適用于室內(nèi)移動(dòng)設(shè)備定位、室外無人空曠地帶定位、多路徑等復(fù)雜環(huán)境下的高精度定位應(yīng)用。文獻(xiàn)[2]利用UWB 技術(shù)進(jìn)行煤礦井下定位研究，在復(fù)雜環(huán)境下，其定位誤差控制在了0.5 m 以內(nèi)。文獻(xiàn)[3]通過UWB 測(cè)距技術(shù)設(shè)計(jì)出用于醫(yī)院內(nèi)的三維定位系統(tǒng)，最大誤差為0.244 m，最小誤差達(dá)0.01 m。由于本次應(yīng)用的定位目標(biāo)較大，預(yù)計(jì)為2 m×2 m 的占地面積的障礙物，故將采用多個(gè)標(biāo)簽對(duì)應(yīng)1 個(gè)目標(biāo)的方式。

常用的UWB 定位方法有基于到達(dá)時(shí)間（TOA）、基于到達(dá)時(shí)間差（TDOA）、基于到達(dá)角度（AOA）和基于接收信號(hào)強(qiáng)度（RSS）等。TDOA 標(biāo)簽容量大、精度高，但需要所有基站保持時(shí)間同步，較難實(shí)現(xiàn)[4]。TOA 不需要基站的時(shí)鐘嚴(yán)格同步，大大降低應(yīng)用難度，在成本和可靠性上更具優(yōu)勢(shì)[5]?？紤]到精度問題，衍生出了雙邊測(cè)距（TWR）法，從精度的角度出發(fā)，DS-TWR 是最優(yōu)選擇，能夠有效減少晶振偏移導(dǎo)致的測(cè)距誤差。

出于對(duì)實(shí)際應(yīng)用場(chǎng)景的考慮，基于常規(guī)DS-TWR定位系統(tǒng)，作出如下改進(jìn)：基站對(duì)標(biāo)簽的應(yīng)答不再是一對(duì)一及時(shí)回復(fù)，而是延時(shí)的廣播回復(fù)，同時(shí)引入動(dòng)態(tài)查詢機(jī)制保證系統(tǒng)的性能和穩(wěn)定性；考慮到系統(tǒng)的容量問題，本文引入了Hash 算法，對(duì)標(biāo)簽進(jìn)行標(biāo)記和管理。此外，系統(tǒng)加入一種改進(jìn)卡爾曼濾波，對(duì)DS-TWR 算法得到的測(cè)距值的精確度進(jìn)一步提高。

1 DS-TWR 測(cè)距原理

SS-TWR 的信息交互過程如圖1 所示。節(jié)點(diǎn)A、B 每接收或發(fā)送信息時(shí)都將記錄下當(dāng)時(shí)時(shí)間戳[6]，節(jié)點(diǎn)A、B 間通信飛行時(shí)間可表示為

圖1 SS-TWR 信息交互圖Fig.1 SS-TWR information interaction diagram

式中：Tround表示節(jié)點(diǎn)A 發(fā)送消息到節(jié)點(diǎn)B 和接收到回復(fù)消息的時(shí)間間隔；Treply表示節(jié)點(diǎn)B 接收到消息并發(fā)送回復(fù)消息的延時(shí)；Tf表示通信飛行時(shí)間。

受設(shè)備晶振偏移影響，測(cè)量的飛行時(shí)間和其誤差函數(shù)可表示為

式中：eA、eB是節(jié)點(diǎn)A、B 的晶振偏移（單位：ppm）。易看出，Treply將會(huì)引起系統(tǒng)誤差[7]。

為解決此問題，DS-TWR 將重點(diǎn)放在減少晶振時(shí)鐘偏移的影響[8]。在SS-TWR 的基礎(chǔ)上，DS-TWR多了一輪節(jié)點(diǎn)間的通信，額外獲得2 個(gè)時(shí)間戳，即多獲得1 次測(cè)距結(jié)果。DS-TWR 的信息交互過程如圖2 所示。在實(shí)際應(yīng)用中，一般將延時(shí)Treply1和Treply2設(shè)置一樣，故DS-TWR 的飛行時(shí)間、測(cè)量的飛行時(shí)間和誤差函數(shù)可表示為

圖2 DS-TWR 信息交互圖Fig.2 DS-TWR information interaction diagram

從式（6）可看出，測(cè)距的誤差主要來源于Treply1和Treply2之間的差值[9]，且呈線性關(guān)系，考慮到時(shí)鐘偏差，故二者差值會(huì)非常?。ㄔ谠O(shè)置一樣的情況下）。

對(duì)DS-TWR 的誤差函數(shù)進(jìn)行數(shù)學(xué)建模，如圖3所示，得出在不同延時(shí)差值和時(shí)鐘偏差的情況下，測(cè)量飛行時(shí)間的誤差。在所取的最極端情況下，即延時(shí)差值100 μs，時(shí)鐘偏差20 ppm 時(shí)，測(cè)量的飛行時(shí)間飛行誤差約為0.0005 μs，此時(shí)的測(cè)距誤差大約為0.15 m，滿足精度要求。

圖3 DS-TWR 信號(hào)飛行時(shí)間測(cè)量誤差Fig.3 DS-TWR signal time-of-flight measurement error

2 改進(jìn)方案的實(shí)現(xiàn)

2.1 通信方法的改進(jìn)

常規(guī)的DS-TWR 算法中，以較多的通信次數(shù)為代價(jià)來減少測(cè)距誤差，但是單次測(cè)距的通信次數(shù)變多會(huì)需要更多的信道容量，從而系統(tǒng)的定位標(biāo)簽的容納量將會(huì)減少[10]。因此，本文提出一種改進(jìn)的通信流程，如圖4 所示。由標(biāo)簽發(fā)起測(cè)距消息，基站在接收到消息后延時(shí)Δt，再對(duì)所有的標(biāo)簽消息統(tǒng)一回復(fù)。每次測(cè)距的步驟如下：

圖4 改進(jìn)通信流程Fig.4 Improved communication process

步驟1標(biāo)簽1 向基站發(fā)送RNG_Tag1 消息，并記錄此時(shí)的時(shí)間戳TRNG_Tag1_tx。

步驟2基站接收到RNG_Tag1 消息時(shí)，記錄時(shí)間戳TRNG_Tag1_rx，并在延時(shí)的Δt 時(shí)間內(nèi)保持接收狀態(tài)，在此時(shí)間內(nèi)，基站會(huì)依次記錄接收到其他標(biāo)簽的消息時(shí)的時(shí)間戳TRNG_TagN_rx。

步驟3Δt 延時(shí)后，基站開始向所有已發(fā)送RNG_TagN 消息的標(biāo)簽發(fā)送RES 消息，并記錄時(shí)間戳TRES_tx。

步驟4標(biāo)簽接收到RES 消息后，記錄接收時(shí)的時(shí)間戳TRES_TagN_rx，依次在延時(shí)ΔtN后，將收到的其他時(shí)間戳打包進(jìn)FIN_TagN 消息并發(fā)送給基站，記錄下發(fā)送FIN_TagN 消息的時(shí)間戳TFIN_TagN_tx。

步驟5基站接收到FIN_TagN 消息后，記錄時(shí)間戳TFIN_TagN_rx，解析消息獲得各個(gè)時(shí)間戳，再通過DS-TWR 算法計(jì)算距離。

在此通信方式下，基站多數(shù)時(shí)間將處于接收狀態(tài)，大大降低在發(fā)送狀態(tài)時(shí)無法接收標(biāo)簽消息而導(dǎo)致的測(cè)距沖突的發(fā)生，從而增加了系統(tǒng)的標(biāo)簽容納量。實(shí)際應(yīng)用中，將設(shè)定延時(shí)Treply都為Δt 以降低通信產(chǎn)生的誤差[11]。

2.2 Hash 算法

本次Hash 算法采用的是取模算法，方法如下：首先針對(duì)一個(gè)標(biāo)簽的通信地址，即一個(gè)與之對(duì)應(yīng)的ID，使其乘上一個(gè)很大的質(zhì)數(shù)2654435761（232與黃金比例相乘的近似整數(shù)，以確保Hash 函數(shù)的結(jié)果在模運(yùn)算下的均勻性和散列性能），并加上Hash 算法迭代次數(shù)的平方，這里的迭代次數(shù)用于解決Hash沖突，表示每發(fā)現(xiàn)一個(gè)被占用的Hash 值，將進(jìn)行重新計(jì)算并迭代次數(shù)加1，然后對(duì)此結(jié)果用基站（本文應(yīng)用場(chǎng)景為4 基站）的總數(shù)取模并加1，得到最終的Hash 值。同時(shí)，為解決Hash 沖突問題，本文采用了開放地址法中的線性探測(cè)法。如果一個(gè)標(biāo)簽的Hash值產(chǎn)生沖突，則在線性探測(cè)法下會(huì)逐個(gè)檢查后續(xù)的Hash 值，直到找到未被占用的Hash 值為止[12]。如果所有的Hash 值都被占用了，那么這個(gè)標(biāo)簽就無法和該基站進(jìn)行通信。

此外，為了保證系統(tǒng)的性能和穩(wěn)定性，本文加入了動(dòng)態(tài)查詢機(jī)制，當(dāng)標(biāo)簽數(shù)量增加時(shí)，增加基站響應(yīng)時(shí)間來減小Hash 沖突的可能性，以處理更多的標(biāo)簽。同樣，當(dāng)標(biāo)簽數(shù)量減少時(shí)，可減少基站響應(yīng)時(shí)間以提高響應(yīng)速度。本文采用MATLAB 仿真模擬動(dòng)態(tài)查詢機(jī)制的應(yīng)用，標(biāo)簽數(shù)量的變化和響應(yīng)時(shí)間變化的關(guān)系如圖5 所示。

圖5 動(dòng)態(tài)查詢Fig.5 Dynamic query

2.3 改進(jìn)卡爾曼濾波

卡爾曼濾波算法根據(jù)系統(tǒng)的運(yùn)動(dòng)方程由上一時(shí)刻的系統(tǒng)狀態(tài)和預(yù)測(cè)狀態(tài)推測(cè)下一時(shí)刻的系統(tǒng)真實(shí)狀態(tài)[13]，解決估計(jì)值和測(cè)量值之間的權(quán)重關(guān)系，從而得到一個(gè)更接近真實(shí)值的結(jié)果。約定：以下方程中，帶有符號(hào)“^”的為估計(jì)狀態(tài)，帶有符號(hào)“-”的為先驗(yàn)狀態(tài)。故常規(guī)卡爾曼濾波的狀態(tài)預(yù)測(cè)方程可以寫成：

狀態(tài)更新方程可以寫成：

式中：Xk為系統(tǒng)狀態(tài)方程；Uk為系統(tǒng)控制輸入；A、B是狀態(tài)轉(zhuǎn)移矩陣；Pk為估計(jì)誤差的協(xié)方差矩陣；Qk為系統(tǒng)過程噪聲的協(xié)方差矩陣，會(huì)隨著預(yù)測(cè)誤差加權(quán)調(diào)整；Kk為卡爾曼增益；H 為觀測(cè)矩陣；Rk為觀測(cè)噪聲的協(xié)方差矩陣，調(diào)整過程與Qk類似；Zk為觀測(cè)變量；I 為單位矩陣。在本系統(tǒng)中可以忽略系統(tǒng)控制輸入。

在原有卡爾曼濾波的基礎(chǔ)上，為獲取更高的測(cè)距精度，本文進(jìn)行了自適應(yīng)的加權(quán)改進(jìn)，如式（12）、式（13）所示，增加加權(quán)系數(shù)wk，根據(jù)噪聲觀測(cè)值與預(yù)測(cè)值之間的偏差，不斷更新自適應(yīng)加權(quán)系數(shù)。偏差越小，加權(quán)系數(shù)越趨近于1，觀測(cè)值的置信度較高，應(yīng)獲得較高權(quán)重，反之，加權(quán)系數(shù)越趨近于0，加權(quán)系數(shù)的置信度較低，應(yīng)給予較低的權(quán)重。

使用MATLAB 仿真該算法的優(yōu)化程度，仿真結(jié)果如圖6 所示。

圖6 濾波算法仿真Fig.6 Filter algorithm simulation

3 實(shí)驗(yàn)結(jié)果

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

實(shí)驗(yàn)硬件平臺(tái)采用藍(lán)點(diǎn)無限公司的BP-400模塊。在模塊中，使用STM32F401CC 為主控芯片，DW1000 為射頻芯片，采用38.4 MHz 的晶體振蕩器作為外部晶振，天線為200 mm 的3000～7000 MHz棒狀全向天線。采用DS-TWR 測(cè)距方式，在空曠環(huán)境下采集定位標(biāo)簽與這4 個(gè)基站間的距離信息。設(shè)定1 號(hào)基站與上位機(jī)連接，并通過串口將信息傳輸至上位機(jī)，在上位機(jī)通過本文的改進(jìn)卡爾曼濾波算法對(duì)得到的距離信息進(jìn)行處理，從而得到更為精確的測(cè)距值。

實(shí)驗(yàn)場(chǎng)景選取一個(gè)空曠開闊的100 m×100 m 的戶外環(huán)境，在實(shí)驗(yàn)區(qū)域內(nèi)建立直角坐標(biāo)系，5 個(gè)標(biāo)簽為一組，模擬一個(gè)需定位的障礙物，在上位機(jī)上顯示出標(biāo)簽和基站之間的距離測(cè)量值，再與真實(shí)距離進(jìn)行誤差的比對(duì)分析。

3.2 沖突率對(duì)比

由于測(cè)試通信沖突的實(shí)驗(yàn)需要大量的定位標(biāo)簽，從成本方面考慮，本文采用MATLAB 仿真模擬多標(biāo)簽通信。模擬通信規(guī)定：設(shè)定固定的4 個(gè)基站，編號(hào)為1～4 號(hào)，每個(gè)標(biāo)簽和4 個(gè)基站按照編號(hào)順序依次通信，若標(biāo)簽發(fā)起通信時(shí)，基站正與其他標(biāo)簽通信，則標(biāo)簽進(jìn)入1 s 的等待時(shí)間，等待時(shí)間結(jié)束，記錄為1 次通信沖突并繼續(xù)與下一個(gè)基站通信，以此類推，最終統(tǒng)計(jì)通信沖突次數(shù)并計(jì)算沖突率。

改進(jìn)前，在500 個(gè)標(biāo)簽中隨機(jī)選擇80 個(gè)標(biāo)簽與4 個(gè)基站進(jìn)行通信[14]，單次通信2 ms，測(cè)距6 ms，總計(jì)8 ms，在周期400 ms 內(nèi)進(jìn)行仿真，改進(jìn)前結(jié)果如圖7 所示。改進(jìn)后，在500 個(gè)標(biāo)簽中隨機(jī)選擇80 個(gè)標(biāo)簽與4 個(gè)基站進(jìn)行通信，單次通信2 ms，3～20 ms內(nèi)基站處于接收狀態(tài)，在400 ms 內(nèi)進(jìn)行仿真，改進(jìn)后結(jié)果如圖8 所示。改進(jìn)前的通信沖突率達(dá)到47.5%，改進(jìn)后的通信沖突率下降至11.25%，結(jié)果表明，經(jīng)過改進(jìn)，通信沖突率大幅降低，能夠有效緩解通信失敗的問題。

圖7 改進(jìn)前通信次數(shù)Fig.7 Number of communications before improvement

圖8 改進(jìn)后通信次數(shù)Fig.8 Number of communications after improvement

3.3 改進(jìn)卡爾曼濾波測(cè)距實(shí)驗(yàn)

在無卡爾曼濾波、標(biāo)準(zhǔn)卡爾曼濾波、改進(jìn)卡爾曼濾波3 種情況下進(jìn)行測(cè)距實(shí)驗(yàn)，分別以20 m、40 m、60 m、80 m 這4 個(gè)距離進(jìn)行多組測(cè)距對(duì)比，使用平均絕對(duì)誤差（MAE）、均方誤差（MSE）和均方根誤差（RMSE）進(jìn)行分析[15]，結(jié)果保留3 位小數(shù)，如表1～表4 所示。

表1 20 m 測(cè)距誤差分析Tab.1 Error analysis of 20 m range measurement

表2 40 m 測(cè)距誤差分析Tab.2 Error analysis of 40 m range measurement

表3 60 m 測(cè)距誤差分析Tab.3 Error analysis of 60 m range measurement

表4 80 m 測(cè)距誤差分析Tab.4 Error analysis of 80 m range measurement

由表中數(shù)據(jù)可見，距離對(duì)測(cè)距的精度存在較大的影響。在無濾波的情況下，測(cè)距值與實(shí)際值相比，存在較大誤差，而在使用標(biāo)準(zhǔn)卡爾曼濾波算法后，測(cè)距值相比在無濾波的情況下更接近實(shí)際值。同時(shí)，使用改進(jìn)的自適應(yīng)加權(quán)卡爾曼濾波算法后，測(cè)距的精度又有了進(jìn)一步的提升，與無濾波的情況相比，誤差減小了約76%。

4 結(jié)語

本文提出了一種基于DS-TWR 算法的室外遠(yuǎn)距離定位的算法優(yōu)化方案，該方案在通信方面能有效減少系統(tǒng)完成測(cè)距所需要的通信次數(shù)，從而降低沖突發(fā)生。同時(shí)引入Hash 算法和動(dòng)態(tài)查詢機(jī)制，保證系統(tǒng)的標(biāo)簽容納量和測(cè)距成功率。在測(cè)距算法方面加入了自適應(yīng)加權(quán)卡爾曼濾波算法，提高測(cè)距精度以確保定位坐標(biāo)的準(zhǔn)確性，通過實(shí)驗(yàn)，證明了該算法的有效性和準(zhǔn)確性。