卡爾曼濾波在無線時(shí)鐘同步技術(shù)中的應(yīng)用研究

李小亭，張要發(fā)，韋子輝，田夢(mèng)園，郎月新

（河北大學(xué) 質(zhì)量技術(shù)監(jiān)督學(xué)院，河北 保定 071002）

0 引 言

超寬帶（Ultra?WideBand，UWB）技術(shù)是近年來新興的一項(xiàng)全新、與傳統(tǒng)通信技術(shù)有極大差異的通信無線新技術(shù)。UWB 信號(hào)具有納秒甚至亞納秒級(jí)的脈沖寬度，根據(jù)窄脈沖的特點(diǎn)，UWB 信號(hào)具有很強(qiáng)的穿透能力，所以理論上講，利用UWB 技術(shù)進(jìn)行室內(nèi)定位能夠達(dá)到厘米級(jí)的定位精度[1]。因此，在室內(nèi)定位中應(yīng)用UWB 技術(shù)具有很高的實(shí)用價(jià)值。UWB 是利用極窄的脈沖進(jìn)行信息傳輸?shù)摹Ｓ捎谄湔伎毡鹊?，在多徑的情況下可以實(shí)現(xiàn)時(shí)間上的分離，能夠充分利用發(fā)射信號(hào)的能量[2]。在現(xiàn)有的無線通信技術(shù)中，UWB 信號(hào)具有高傳輸率、低功耗、抗干擾性強(qiáng)的優(yōu)點(diǎn)，基于UWB 的精確定位可以分為同步定位與異步定位兩大類，其對(duì)應(yīng)的主流定位技術(shù)分別為基于到達(dá)時(shí)間差（Time Difference of Arrival，TDOA）和飛行時(shí)間（Time of Flight，TOF）的技術(shù)[3?4]。

TOF 方案通過測量電磁波的飛行時(shí)間，計(jì)算出標(biāo)簽與錨節(jié)點(diǎn)間的距離，再匯總計(jì)算出標(biāo)簽位置。TOF 方案中錨節(jié)點(diǎn)之間不需要同步，硬件實(shí)現(xiàn)簡單，容易部署，位置求解精度高[5]。TOF 定位算法需要測量移動(dòng)節(jié)點(diǎn)到各固定參考節(jié)點(diǎn)的距離，采用雙向雙邊測距完成到每個(gè)固定參考節(jié)點(diǎn)的測距，移動(dòng)節(jié)點(diǎn)向各個(gè)固定參考節(jié)點(diǎn)輪流發(fā)射定位請(qǐng)求信號(hào)，各固定參考節(jié)點(diǎn)分別將測距結(jié)果上傳給服務(wù)器，服務(wù)器先后收到測距結(jié)果進(jìn)而匯總到一起計(jì)算移動(dòng)節(jié)點(diǎn)的坐標(biāo)值。

TDOA 是一種經(jīng)典的無線定位技術(shù)。TDOA 定位是通過檢測移動(dòng)節(jié)點(diǎn)發(fā)射的信號(hào)到各個(gè)固定參考節(jié)點(diǎn)的時(shí)間差，進(jìn)而對(duì)移動(dòng)節(jié)點(diǎn)進(jìn)行定位[6]。比如采用2 個(gè)不同的固定參考節(jié)點(diǎn)就可以得到1 個(gè)到達(dá)時(shí)間差，對(duì)應(yīng)于1 條雙曲線，采用3 個(gè)不同的固定參考節(jié)點(diǎn)就可以得到3 個(gè)到達(dá)時(shí)間差即3 條雙曲線，移動(dòng)節(jié)點(diǎn)就位于這3 條雙曲線的交點(diǎn)上，因此要達(dá)到定位目的，至少需要3 個(gè)不同的固定參考節(jié)點(diǎn)。

TOF 定位需要多次雙向雙邊測距，存在定位速度慢的問題。TDOA 定位只需要移動(dòng)節(jié)點(diǎn)發(fā)送一次信號(hào)，定位速度快，因此TDOA 定位應(yīng)用較廣。但是，TDOA 定位方法對(duì)時(shí)鐘同步提出了非常高的要求，各個(gè)基站之間的時(shí)鐘同步要求達(dá)到亞納秒量級(jí)，時(shí)鐘同步是TDOA 定位實(shí)現(xiàn)的關(guān)鍵技術(shù)，而且高精度實(shí)時(shí)定位系統(tǒng)需要高精度、低延時(shí)的時(shí)鐘同步技術(shù)支持。

本文首先提出面向TDOA 定位應(yīng)用的無線時(shí)鐘同步算法，分析了時(shí)間差計(jì)算過程存在的時(shí)間延遲，提出采用卡爾曼濾波算法消除該時(shí)間延遲，最后通過時(shí)間差測量實(shí)驗(yàn)對(duì)這種算法進(jìn)行了實(shí)驗(yàn)驗(yàn)證。

1 無線時(shí)鐘同步算法設(shè)計(jì)

在TDOA 定位中，對(duì)時(shí)鐘同步有非常高的要求，是因?yàn)? ns 的時(shí)鐘同步誤差將會(huì)帶來30 cm 的定位誤差，因此時(shí)鐘同步在TDOA 定位中成為研究的熱點(diǎn)和難點(diǎn)。目前商用TDOA 定位系統(tǒng)時(shí)鐘同步方式包括有線傳輸和無線傳輸兩種方式。有線傳輸因?yàn)橐诂F(xiàn)場布纜，需要專用同步信號(hào)設(shè)備，所以成本比較高。無線傳輸可以彌補(bǔ)有線傳輸存在的問題，使成本大大降低，且不需要復(fù)雜的現(xiàn)場安裝[7]，無線時(shí)鐘同步方式應(yīng)用更為廣泛。

2002 年，Jeremy Elson 等人首次研究了無線傳感器時(shí)鐘同步技術(shù)的問題[8]。目前，在無線傳感器網(wǎng)絡(luò)時(shí)鐘同步協(xié)議的研究中，根據(jù)是否有參考節(jié)點(diǎn)可將時(shí)鐘同步協(xié)議分為兩種：一種是基于參考節(jié)點(diǎn)的時(shí)鐘同步協(xié)議；另一種是分布式的時(shí)鐘同步協(xié)議[9]。基于參考節(jié)點(diǎn)的時(shí)鐘同步協(xié)議剛開始會(huì)將其中的一個(gè)節(jié)點(diǎn)選作參考節(jié)點(diǎn)，剩余的節(jié)點(diǎn)會(huì)周期性地和這個(gè)參考節(jié)點(diǎn)同步，致使整個(gè)網(wǎng)絡(luò)的時(shí)鐘同步[10]。

TPSN（TimesSynchronization Protocol for Sensor Networks），其原理是選一個(gè)節(jié)點(diǎn)作為根節(jié)點(diǎn)（或參考節(jié)點(diǎn)），在這個(gè)根節(jié)點(diǎn)的基礎(chǔ)上，整個(gè)網(wǎng)絡(luò)會(huì)逐漸形成一個(gè)樹狀結(jié)構(gòu)，而網(wǎng)絡(luò)中的節(jié)點(diǎn)都與其父節(jié)點(diǎn)同步后整個(gè)網(wǎng)絡(luò) 的 時(shí) 鐘 就 能 夠 同 步[11]。FTSP（Flooding Time Synchronization Protocol）的原理是選取一個(gè)節(jié)點(diǎn)作為根節(jié)點(diǎn)之后，根節(jié)點(diǎn)會(huì)以Flooding“泛洪”傳播的方式向整個(gè)樹狀網(wǎng)絡(luò)周期性地傳播自己的時(shí)鐘信息，從而達(dá)到全網(wǎng)時(shí)鐘同步[12]。

由于每個(gè)參考節(jié)點(diǎn)都有自己獨(dú)立的時(shí)鐘源，時(shí)鐘源頻率受電壓和溫度變化影響，每個(gè)時(shí)鐘的起始運(yùn)行計(jì)數(shù)存在差值，因此需要不斷進(jìn)行時(shí)鐘校準(zhǔn)去修正時(shí)鐘起始運(yùn)行偏差和頻率偏差。

本文中提出了類似TPSN 的時(shí)鐘同步方式，建立起一個(gè)網(wǎng)絡(luò)樹狀結(jié)構(gòu)。每個(gè)從時(shí)鐘參考節(jié)點(diǎn)都與上一級(jí)的父時(shí)鐘參考節(jié)點(diǎn)進(jìn)行時(shí)鐘同步，最后所有時(shí)鐘都同步到根時(shí)鐘節(jié)點(diǎn)。與TPSN 不同的是，同步信號(hào)對(duì)時(shí)鐘采用單向通信，每個(gè)單節(jié)點(diǎn)不進(jìn)行時(shí)鐘同步計(jì)算。所有數(shù)據(jù)上傳到定位服務(wù)器，由定位服務(wù)器進(jìn)行時(shí)鐘同步計(jì)算、時(shí)間差及距離差計(jì)算。

如圖1所示，A 為根時(shí)鐘參考節(jié)點(diǎn)，B 為從時(shí)鐘參考節(jié)點(diǎn)，C 為從時(shí)鐘參考節(jié)點(diǎn)，同時(shí)，B 又為C 的父節(jié)點(diǎn)，以此類推下去。根節(jié)點(diǎn)A 連續(xù)發(fā)送對(duì)鐘信號(hào)A1，A2，…，An。τA1，τA2，…，τAn是發(fā)送對(duì)鐘信號(hào)時(shí)的主時(shí)鐘計(jì)數(shù)，從時(shí)鐘節(jié)點(diǎn)B 接收到對(duì)鐘信號(hào)的時(shí)鐘計(jì)數(shù)為τB1，τB2，…，τBn，同時(shí)，節(jié)點(diǎn)B 也發(fā)射同步信號(hào)B1，B2，…，Bn，從時(shí)鐘節(jié)點(diǎn)C接收節(jié)點(diǎn)B 的同步信號(hào)。標(biāo)簽發(fā)射廣播信號(hào)，節(jié)點(diǎn)A、節(jié)點(diǎn)B、節(jié)點(diǎn)C 接收標(biāo)簽發(fā)射信號(hào)的時(shí)鐘計(jì)數(shù)分別為τAT，τBT，τCT。

圖1 時(shí)鐘同步原理圖

時(shí)鐘同步計(jì)算原理采用式（1）校準(zhǔn)時(shí)鐘起始運(yùn)行偏差，式（2）計(jì)算K 校準(zhǔn)時(shí)鐘頻率偏差。由式（3），式（4）得到τBT時(shí)刻根節(jié)點(diǎn)時(shí)鐘計(jì)數(shù)值τBT(A)，所有節(jié)點(diǎn)接收標(biāo)簽時(shí)刻的根節(jié)點(diǎn)時(shí)鐘計(jì)數(shù)值均可計(jì)算。

同理，可得：

于是得到時(shí)間差計(jì)算如下：

于是可得距離差計(jì)算如下：

式中C=299 792 458 m/s。

以上是插值公式實(shí)現(xiàn)時(shí)鐘同步、時(shí)間差及距離差計(jì)算的公式模型。其中，根時(shí)鐘節(jié)點(diǎn)和從時(shí)鐘節(jié)點(diǎn)測量得到的時(shí)鐘計(jì)數(shù)τA1，τA2，…，τAn，τB1，τB2，…，τBn均上傳至定位服務(wù)器，由定位服務(wù)器完成時(shí)鐘同步的計(jì)算，并根據(jù)各節(jié)點(diǎn)接收標(biāo)簽定位請(qǐng)求信號(hào)時(shí)間τAT，τBT，τCT，根據(jù)式（9），式（10）計(jì)算時(shí)間差后，再根據(jù)式（11），式（12）計(jì)算距離差。

其中k 值的計(jì)算非常重要。在對(duì)鐘過程中，計(jì)算k 值需要兩次對(duì)鐘的同步信號(hào)，節(jié)點(diǎn)收到標(biāo)簽信號(hào)后，需要再接收一次同步信號(hào)才能計(jì)算k 值和時(shí)間差，最后得到距離差。這樣在計(jì)算時(shí)間差和距離差時(shí)會(huì)引入一定的時(shí)間延遲，無法應(yīng)用于高實(shí)時(shí)性要求場合。卡爾曼濾波算法作為當(dāng)下非常優(yōu)秀的濾波算法，它還可以利用歷史數(shù)據(jù)對(duì)下一時(shí)刻的值進(jìn)行預(yù)測估算，提出將卡爾曼濾波算法引入到無線時(shí)鐘同步對(duì)鐘過程中，以消除時(shí)間延遲。

2 基于卡爾曼濾波的無線時(shí)鐘同步算法設(shè)計(jì)

卡爾曼在1960 年提出了一種有關(guān)時(shí)域設(shè)計(jì)的線性系統(tǒng)的濾波器，即卡爾曼濾波[13]?？柭鼮V波是一種估計(jì)方法，能夠?qū)崟r(shí)估計(jì)系統(tǒng)中的參數(shù)，如連續(xù)變化的位置、速度等。估計(jì)量通過一系列受噪聲污染的觀測量來更新，而觀測量是待估計(jì)參數(shù)的函數(shù)，其成功運(yùn)用于目標(biāo)跟蹤、組合導(dǎo)航等領(lǐng)域，具有優(yōu)異的性能[14]。

卡爾曼濾波的基本原理是利用過程噪聲和測量噪聲對(duì)下一時(shí)刻的值進(jìn)行預(yù)測估計(jì)，根據(jù)預(yù)測估計(jì)得到的估計(jì)值和當(dāng)前時(shí)刻的測量值建立系統(tǒng)方程，算法根據(jù)建立的系統(tǒng)方程和觀測方程，通過遞推處理，對(duì)信號(hào)的當(dāng)前值在最小均方誤差下做出最優(yōu)估計(jì)[15]，進(jìn)而可以得到更加平穩(wěn)的數(shù)據(jù)。

卡爾曼濾波主要由系統(tǒng)狀態(tài)方程和系統(tǒng)觀測方程實(shí)現(xiàn)，狀態(tài)方程主要是某個(gè)時(shí)刻和下一個(gè)時(shí)刻狀態(tài)向量的聯(lián)系，父時(shí)鐘節(jié)點(diǎn)采用等間隔發(fā)送時(shí)鐘同步信號(hào)，從基站接收同步信號(hào)時(shí)鐘計(jì)數(shù)狀態(tài)方程為：

式中：Xn表示n 時(shí)刻的系統(tǒng)狀態(tài)向量，A 表示狀態(tài)轉(zhuǎn)移矩陣是根節(jié)點(diǎn)對(duì)時(shí)間隔；Un是n 時(shí)刻對(duì)系統(tǒng)的控制量，因?yàn)椴捎玫乳g隔對(duì)鐘，不存在系統(tǒng)輸入控制變量；B 為0；Wn是過程噪聲。

測量方程主要是測量向量和同一時(shí)刻狀態(tài)向量的聯(lián)系[16]。

式中：Yn表示n 時(shí)刻的測量向量；H 表示測量矩陣，測量值只有從節(jié)點(diǎn)時(shí)鐘計(jì)數(shù)，因此H=[1,0]；Vn表示測量噪聲。系統(tǒng)的過程噪聲和測量噪聲均認(rèn)為是高斯白噪聲?？柭鼮V波迭代計(jì)算的過程如下：

式中：Xn,n-1是利用歷史數(shù)據(jù)狀態(tài)預(yù)測的結(jié)果；Xn-1,n-1是上一狀態(tài)的最優(yōu)結(jié)果；Xn,n是卡爾曼濾波輸出的最終結(jié)果；Un是當(dāng)前時(shí)刻狀態(tài)的控制量，如果無控制量它可以為零，本設(shè)計(jì)中的控制量0；Q 是系統(tǒng)過程噪聲Wn的協(xié)方差；R 是測量噪聲Vn的協(xié)方差；Kg為卡爾曼增益。

卡爾曼增益代表輸出結(jié)果中預(yù)測值和測量值的權(quán)重分配，調(diào)整Q 和R 值可以改變卡爾曼增益大小，在實(shí)際應(yīng)用中常通過調(diào)整Q，R 值大小獲得最佳濾波效果。

將卡爾曼濾波應(yīng)用于時(shí)鐘同步計(jì)算過程中，收到標(biāo)簽發(fā)射的信號(hào)后，應(yīng)用Xn,n-1的預(yù)測值Kn進(jìn)行時(shí)鐘同步計(jì)算，不需要等到下一次時(shí)鐘同步信號(hào)即可計(jì)算時(shí)間差，進(jìn)而消除時(shí)間延遲。如果應(yīng)用濾波輸出Xn,n的Kn進(jìn)行時(shí)鐘同步計(jì)算，則可進(jìn)一步提高時(shí)間差計(jì)算的精度。

3 測試結(jié)果

系統(tǒng)通信節(jié)點(diǎn)采用Decawave 公司推出的DW1000射頻芯片。DW1000 采用的是IR?UWB 通信技術(shù)，典型帶寬500 MHz。DW1000 射頻芯片內(nèi)置了63.897 6 GHz的采樣時(shí)鐘，計(jì)時(shí)分辨率為15.65 ps，高精度的時(shí)鐘保證了信號(hào)飛行時(shí)間的測量精度。DW1000 支持6 個(gè)信道，頻率范圍為3.5～6.5 GHz，采用脈沖位置調(diào)制（Pulse Position Modulation，PPM）和二進(jìn)制相移鍵控（Binary Phase Shift Keying，BPSK）調(diào)制方式[17]。

控制器采用意法半導(dǎo)體（ST）公司的STM32F107 32 位ARM 微控制器，其自帶完整的串行外設(shè)接口（Serial Peripheral Interface，SPI）控制器，能夠勝任對(duì)DW1000 射頻芯片的控制及數(shù)據(jù)收發(fā)工作。STM32F107芯片的外圍電路主要包括外部晶振、退耦電容、SPI 接口電路等。

主從基站的時(shí)鐘頻率是不斷變換的，其主要受溫度影響，也會(huì)隨著供電電壓波動(dòng)而變化，為減少線性化處理帶來的誤差，可以采取以下兩個(gè)措施：

1）使用高精度、高穩(wěn)定度的晶振作為DW1000 的頻率源，可采用有源溫補(bǔ)晶振或恒溫晶振。

2）縮短對(duì)鐘的時(shí)間間隔，對(duì)鐘時(shí)間間隔越短，線性化處理帶來的誤差越小。

分別在150 ms，300 ms，500 ms，800 ms 對(duì)鐘時(shí)間間隔下，采用線性插值及引入卡爾曼濾波進(jìn)行距離差測量，實(shí)驗(yàn)測得1 000 組數(shù)據(jù)，得到如下數(shù)據(jù)結(jié)果。

表1 中Max 一行中的數(shù)據(jù)表示從時(shí)鐘的時(shí)鐘計(jì)數(shù)間隔的最大值，Min 一行中的數(shù)據(jù)表示從時(shí)鐘的時(shí)鐘計(jì)數(shù)間隔的最小值，Max-Min 一行中的數(shù)據(jù)表示最大值與最小值之差。表1 反映了從時(shí)鐘計(jì)數(shù)的波動(dòng)情況，150 ms 對(duì)鐘間隔所得數(shù)據(jù)穩(wěn)定性更好。

表1 時(shí)鐘計(jì)數(shù)波動(dòng)表

利用實(shí)驗(yàn)測得的時(shí)鐘計(jì)數(shù)數(shù)據(jù)，通過插值法、卡爾曼濾波預(yù)測k值、卡爾曼濾波法進(jìn)行了距離差的計(jì)算，得到1 000組數(shù)據(jù)，分別對(duì)這3種方法所得的距離差數(shù)據(jù)進(jìn)行方差計(jì)算，所得結(jié)果如表2所示。

表2 距離差測試結(jié)果

根據(jù)表2 中的數(shù)據(jù)可知，本次所設(shè)計(jì)的卡爾曼濾波器在150 ms 對(duì)鐘時(shí)間間隔下效果較好，于是選擇了對(duì)鐘時(shí)間間隔為150 ms。

4 結(jié) 論

經(jīng)過卡爾曼濾波后計(jì)算所得的距離差相比線性插值法的無線時(shí)鐘同步計(jì)算所得的距離差的方差較小，因此說明了經(jīng)過卡爾曼濾波后所得數(shù)據(jù)的穩(wěn)定性較好，也進(jìn)一步說明了本次的卡爾曼濾波器設(shè)計(jì)成功，可以提高TDOA 的定位精度。通過實(shí)驗(yàn)得出，本次所設(shè)計(jì)的卡爾曼濾波器使定位精度提高了1 cm，還有待進(jìn)一步的改進(jìn)使定位精度再次提高。

通過實(shí)驗(yàn)，利用設(shè)計(jì)的卡爾曼濾波器中的先驗(yàn)值進(jìn)行計(jì)算，得到的距離差相比線性插值法的無線時(shí)鐘同步計(jì)算所得的距離差的方差雖有差距，但相差并不是很大，在滿足定位精度的同時(shí)，利用先驗(yàn)值的定位計(jì)算減小甚至消除了時(shí)間延遲，提高了定位速度，可滿足高實(shí)時(shí)性定位要求的場合。

注：本文通訊作者為韋子輝。