一種防碰撞卡爾曼濾波同步定位算法

雷智杰，沈 重

（海南大學(xué)信息與通信工程學(xué)院，海南 ?？?570228）

高精度的室內(nèi)信號跟蹤和移動物體定位扮演著越來越重要的角色，并且從曾經(jīng)只用在軍事用途到現(xiàn)在已經(jīng)逐步走向了民用領(lǐng)域，諸多科研單位都在從事著高精度室內(nèi)定位技術(shù)的相關(guān)研究.由于室內(nèi)外環(huán)境有著巨大的差異，室內(nèi)空間往往是一個封閉多障礙物，對信號傳輸有著許多干擾的復(fù)雜環(huán)境［1］.因此，對室內(nèi)移動節(jié)點位置的精準預(yù)測是困難并且具有挑戰(zhàn)性的工作.

脈沖超寬帶（Impulse Radio Ultra-Wide Band，IR-UWB）是一類不使用載波來進行信息傳輸和交互的通信手段，在其交互信息的過程中依靠使用微秒級的脈沖信號來完成信息交互.這些脈沖信號通常都是非正弦窄波，所以信號更容易生成，所占用的頻譜范圍也更寬［2］.IR-UWB技術(shù)被應(yīng)用到室內(nèi)定位中，在確定位置的精確程度，傳送信息所使用的帶寬寬度，包括整個系統(tǒng)的穩(wěn)定性上與其他技術(shù)相比都有著較大的領(lǐng)先.由于IR-UWB技術(shù)有較好的抗多徑時延效果，穿透障礙物的能力優(yōu)秀，布局和操作性都較為簡單，定位精確度可以達到厘米級別，這一系列優(yōu)勢使其快速地脫穎而出成為室內(nèi)定位最重要的研究領(lǐng)域.

IR-UWB室內(nèi)定位技術(shù)涵蓋許多種測距方法，在討論時鐘同步問題中所使用的是到達時間差值（Time Difference of Arrival，TDOA）的方法［3］.在此系統(tǒng)中想要精準地得到移動節(jié)點的位置信息必須嚴格執(zhí)行主基站和從基站之間的時鐘同步，因此解決主從基站間時鐘同步信號所產(chǎn)生的偏移和漂移是提升該系統(tǒng)定位精確度和系統(tǒng)穩(wěn)定性的重中之重.文獻［4］在一個同步周期內(nèi)，主基站只需要播放一次時鐘同步消息，系統(tǒng)便可以自建一個有著全網(wǎng)絡(luò)節(jié)點的樹，根據(jù)貝葉斯估計算法便可以準確地估計出母節(jié)點與子節(jié)點相對漂移.文獻［5］將接收方對接收方同步（Receiver-to-Receiver Synchronization，RRS）和發(fā)射方對接收方同步（Sender-to-Receiver Synchronization，SRS）2種同步網(wǎng)絡(luò)協(xié)議相聯(lián)合，使用卡爾曼濾波和遞歸算法不斷迭代來預(yù)測和校正相對偏移，來提高時鐘同步的性能.文獻［6］使用分量解耦融合原理修正主基站和從基站的時鐘偏差，考慮了精度與能耗的關(guān)系，可以實現(xiàn)在不增高能耗的同時提升精度.文獻［7］考慮到雙向測距定位在可伸縮方面的局限性，提出了一種基于多用戶時差的無線時鐘同步定位方法，使得時鐘同步率在1Hz左右就足以達到分米波范圍內(nèi)的精度.文獻［8］總結(jié)了DW1000 ScenSor（一種使用IEEE 802.15.4a的收發(fā)器）用作測試平臺的情況，比較了幾種定位系統(tǒng)中所有傳感器的無線同步方法.為了使主從基站時鐘同步的性能和系統(tǒng)的穩(wěn)定性提高，筆者在基于卡爾曼濾波追蹤的前提下對脈沖超寬帶室內(nèi)定位系統(tǒng)的時鐘同步追蹤性能開展討論.

1 TDOA定位時鐘同步方案

本文進行仿真討論的是TDOA時鐘同步系統(tǒng)，共有4個基站，其中一個為主基站，其余3個全部為從基站，構(gòu)成1主3從的時鐘同步系統(tǒng)，具體如圖1所示.主基站根據(jù)一定時間周期不間斷地發(fā)送時鐘同步信號，3個從基站依據(jù)從主基站收到的時鐘同步信號和定位標簽的有效信息分別針對每一組的不同情況進行相對的時鐘補償，推算出TAG較為精準的時間到達數(shù)值，然后將TAG的時間到達數(shù)值，和TAG相應(yīng)的ID數(shù)值返回發(fā)送給中央服務(wù)器CLE，中央服務(wù)器通過上位機軟件里的TDOA定位算法，相關(guān)函數(shù)等一些其他算法推算出TAG相對于各個基站的位置［9］，最終給出一個較為精準的標簽位置信息.

圖1 IR-UWB主從基站時鐘同步示意模型

假定全部基站都可以接發(fā)攜有時間標記的時鐘同步數(shù)據(jù)檢驗包（Clock Check Packet，CCP），并且起始主基站發(fā)射CCP信號的時間周期為T，則第j輪發(fā)送時鐘校正消息時，主基站在T1，j時間點發(fā)射CCP信號，則3個從基站收到該數(shù)據(jù)包同時依據(jù)當?shù)貢r間記錄數(shù)據(jù)到達時刻為T2，j，Δj代表收到時鐘信號時刻和下一個相鄰時鐘信號的時刻間隔，因為有著系統(tǒng)時鐘相位偏移所以產(chǎn)生了信號時刻間隔，如圖2所示.因此依據(jù)文獻［10］，發(fā)射第j個時鐘信號信息的時刻模型為

圖2 主從基站時空關(guān)系模型

其中，ψj代表時鐘信號在分組傳達的過程中隨機所產(chǎn)出的時延.

主從基站間的時鐘同步一般情況下是追蹤2個接收時間來完成，首先是主基站廣播CCP的時刻（依據(jù)主基站當?shù)貢r鐘）和CCP在傳播所需要的時間，其次是從基站收到CCP的時刻（依據(jù)從基站當?shù)貢r鐘）.

從圖3可以看出，主從基站時鐘同步步驟是使用主基站的CCP發(fā)射時刻，3個從基站的CCP收到時刻，還有主從基站的最優(yōu)預(yù)測時間.假定主從基站的位置信息是確定的，則CCP在信道中傳播的時間可以依據(jù)傳播距離與傳播時間的比值來取得，最后通過時鐘追蹤便可以推導(dǎo)出主基站和從基站的真實時鐘誤差和最優(yōu)預(yù)測時鐘誤差.卡爾曼濾波（Kalman filter）和數(shù)字鎖相環(huán)（Digital Phase-Locked-Loop，DPLL）其原理性能還有結(jié)構(gòu)都較為相似，因此2種技術(shù)在時鐘校正和時鐘同步追蹤中都有著廣泛的應(yīng)用.與傳統(tǒng)卡爾曼濾波相比，DPLL要進行周期更短的時鐘同步信號發(fā)射，也就意味著要更多的發(fā)射次數(shù).另外，卡爾曼濾波還降低了時鐘同步信息分組的占用空間，可以將更多的時間用來接收定位信息的數(shù)據(jù)包.因此，在IR-UWB定位中采用卡爾曼濾波進行時鐘同步追蹤是更好的選擇.

圖3 相對時間偏移追蹤

2 基于卡爾曼濾波的時鐘追蹤方法

卡爾曼濾波進行時鐘追蹤的原理是使用眼前時間的觀察數(shù)值，前一時間節(jié)點的估計數(shù)值和估計偏差一起推算出一個最佳數(shù)值來估計下一個時間節(jié)點的數(shù)值，相當于是使用最小均方誤差來完成最優(yōu)估計［11］.在濾波器的真實操作中，第k+1個同步信號主基站和從基站所產(chǎn)生的信號漂移是依據(jù)第k個周期的數(shù)值進行估計.假定發(fā)射時鐘同步信號的發(fā)射周期為T，Uθ，k是第k個周期信號所產(chǎn)生的校對更正偏移量，Uγ，k是第k個周期信號所產(chǎn)生校對更正漂移量，則θk是第kT周期信號的偏移量，γk是第kT周期信號的漂移量.因此在(k+1)T這一時間點，時鐘信號的迭代關(guān)系式為

其中，ωθ，k是時鐘信號偏移方差值，ωγ，k為時鐘信號漂移方差值.給定ωk=[ωθ，kωγ，k]T，其加性協(xié)方差矩陣為Q.界定輸入向量和矩陣如下

卡爾曼濾波的迭代過程為：

估計

最小均方誤差矩陣估計

其中，P k是X k的最小誤差估計矩陣.

卡爾曼增益矩陣

其中，R k+1是觀測噪聲的協(xié)方差矩陣，測量矩陣H k+1為單位矩陣.

校正

最小均方誤差矩陣

3 防碰撞卡爾曼濾波算法

綜上可知，卡爾曼追蹤器比較適合用作IR-UWB室內(nèi)定位主從基站的時鐘同步追蹤.但是使用卡爾曼濾波有一個前提：全部的計算數(shù)值必須是實際信號信息和高斯噪聲所構(gòu)成.基于此原則，卡爾曼濾波器才能夠在有噪聲的信息中通過計算獲取準確的預(yù)測.當TAG的位置信息數(shù)據(jù)和主基站發(fā)射的CCP信號碰撞在一起便容易產(chǎn)生錯誤數(shù)據(jù)，這些錯誤的數(shù)據(jù)會干擾濾波器做出正確的判斷，常常將那些錯誤的數(shù)據(jù)認定成可以信賴的偏移和漂移量，并且依據(jù)這些錯誤的量值進行計算和預(yù)測最終產(chǎn)生較大誤差.因為卡爾曼濾波器是一個低通濾波器，糾正錯誤的能力和速度都比較差.所以一旦有碰撞或錯誤引起誤差，此誤差會持續(xù)影響主從基站時鐘同步算法的魯棒性.

因此，筆者提出一種監(jiān)控錯誤和避免錯誤發(fā)生的算法.算法如下：

卡爾曼濾波增益矩陣

界定一個信息矩陣X，

其中，X代表實際偏移與估計偏移的誤差值，該信息可以反饋出卡爾曼濾波在這一時刻預(yù)測結(jié)果與真實結(jié)果的關(guān)聯(lián)程度.

假定一個偏離指標（Outlier Metric，OM）認定為此刻讀取數(shù)值不被信賴的門限指標，依據(jù)仿真結(jié)果選定為上一時刻預(yù)測結(jié)果的4～7倍.一旦數(shù)據(jù)超過偏離指標時，改進的可糾錯卡爾曼濾波會將此數(shù)據(jù)認定為一個錯誤數(shù)據(jù)并且停止對當前狀態(tài)的更替，繼續(xù)采用上一個正確的預(yù)測值來對該時刻預(yù)測結(jié)果進行平滑處理，防止較大的數(shù)據(jù)錯誤對卡爾曼濾波狀態(tài)更新的準確性產(chǎn)生強烈的干擾.

4 實驗結(jié)果與分析

采用Matlab進行仿真，對真實偏移量和卡爾曼濾波器預(yù)測的偏移量進行記錄和追蹤.給定主基站和從基站的位置信息坐標分別是（1.2，1.21，1.88）和（12.1，1.22，1.31）.主基站發(fā)射CCP信號到從基站需要的時間為T OF主-從=0.000 000 035 625 26 s，發(fā)射CCP的時間間隔為200 ms，仿真過程給定噪聲為4e-12，仿真結(jié)果如圖4所示.

圖4 時鐘同步消息在有較大錯誤信息時引起的時鐘偏差

圖4a和b中藍色虛線為真實偏移數(shù)值，紅色實線為卡爾曼預(yù)測的偏移值.在仿真過程中，在100 s和400 s時人為引入大數(shù)據(jù)誤差，可以從圖4a中看出使用普通的卡爾曼濾波進行時鐘偏移預(yù)測，當遇到較大的錯誤時，預(yù)測結(jié)果會誤以為錯誤數(shù)據(jù)是可信值，從而使用錯誤值來更新當前狀態(tài)，從而使系統(tǒng)產(chǎn)生一個巨大的波動，并且修正也需要一定時間.在使用改進的一種防碰撞的爾曼濾波時，由于引入了偏離指標，當數(shù)據(jù)的數(shù)值超過偏離指標時則默認此數(shù)據(jù)為不信任的數(shù)據(jù)，對輸出的預(yù)測時鐘偏移量進行平滑處理.從圖4b中可以看出，改進的防碰撞可糾錯卡爾曼濾波抗突發(fā)錯誤的能力大大提升，能夠識別錯誤數(shù)據(jù)，并迅速對錯誤數(shù)據(jù)做出拋棄處理，整個濾波系統(tǒng)的準確性和穩(wěn)定性都有較好的提升.

為了更好地驗證改進的防碰撞卡爾曼濾波時鐘同步性能，以實驗室團隊開發(fā)的HN EVK RTLS 4.0系統(tǒng)為硬件測試平臺，搭建測試環(huán)境.選取實驗室辦公室場景為本次的測試環(huán)境，區(qū)域大小為6*10的矩形區(qū)域，由一個主基站和3個從基站所構(gòu)成，基站坐標分別為（0，0）（10，0）（10，6）（0，6）.測試環(huán)境如圖5所示.

圖5 測試環(huán)境照片與平面示意圖

設(shè)定基站M B1為時鐘同步基準基站，基準基站MB1周期性地向區(qū)域內(nèi)3個從基站發(fā)射時鐘同步數(shù)據(jù)檢驗包（CCP），使該區(qū)域內(nèi)基站間時鐘同步保持一致.移動節(jié)點發(fā)射定位數(shù)據(jù)，所在接受范圍的基站都會接收定位數(shù)據(jù)包.依據(jù)衡量出的到達時間差來預(yù)測移動節(jié)點的坐標.基站M B1和基站SB2的到達時間差為

定義距離誤差e則

在基站間保持絕對時鐘同步的前提下，移動節(jié)點發(fā)射時鐘檢驗包到基站間的傳送時間差值即為TDOA數(shù)值，假設(shè)沒有其他噪聲干擾，則距離誤差e等于0.在實際環(huán)境測試中，在基站間沒有保持絕對時鐘同步的情況下，TDOA衡量值誤差除了噪聲影響，剩下的主要影響便是基站間時鐘差所造成的，距離誤差e會隨著時鐘差增大而增大.

使用相同的方法獲取距離誤差e2和e3.將每一個距離誤差進行30次的測量，獲取30個數(shù)據(jù).測試傳統(tǒng)協(xié)議的時鐘同步方案和改進的防碰撞卡爾曼濾波同步算法，2個方案的距離誤差計算出的累計分布函數(shù)（CDF）如圖6所示.

圖6 2種同步算法C D F誤差

從圖6中可以看出，e1在最左邊表明測距誤差是最低，e3在最右邊表明測距誤差是最大.對比圖6（a）和6（b）可以看出，在使用改進卡爾曼濾波同步算法后，可以將距離誤差縮小3 cm左右.由于改進的防碰撞可糾錯卡爾曼濾波，在碰撞錯誤發(fā)生時能快速識別出錯誤數(shù)據(jù)，對預(yù)測結(jié)果進行快速有效的平滑處理，較好地提升了系統(tǒng)的時鐘同步性能.

5 小結(jié)

對于IR-UWB定位系統(tǒng)，主從基站間的時鐘同步對系統(tǒng)的穩(wěn)定性和定位的精準性有著重要的影響.采用卡爾曼濾波進行時鐘偏移預(yù)測是主從基站時鐘同步的一個常用方案，但卡爾曼濾波糾正錯誤的能力和速度都比較差，這會使得當錯誤發(fā)生時系統(tǒng)的魯棒性較低.筆者提出一種防碰撞可糾錯的卡爾曼濾波，在碰撞錯誤發(fā)生時能快速識別出錯誤數(shù)據(jù)，對預(yù)測結(jié)果進行快速有效的平滑處理，較好地提升了系統(tǒng)準確性和魯棒性.