本發(fā)明涉及的是移動機器人定位領域，尤其是一種UHF-RFID環(huán)境下移動機器人定位的方法。

背景技術：

無線定位是軍事活動和保障人類正常的交通安全的必要手段。對于經(jīng)濟發(fā)展迅速的中國，無線定位對社會發(fā)揮著越來越重要的作用，如機場大廳、展廳、倉庫、超市、圖書館、地下停車場、礦井、航空的安全航行和交通管制、車輛的運輸調(diào)度、空間飛行器的定位和測控、搜索救援、移動通信等領域都要用到無線定位。定位是指確定物體在某一參考坐標系中的位置。因此存在重要的應用價值。常用的無線定位技術有紅外線，超聲波，WIFI，藍牙，UWB，ZIGBEE，RFID等，其中，RFID因其抗干擾能力強，可在惡劣環(huán)境下進行工作，鑒于其非視距和非接觸的優(yōu)點，將其運用于定位能實現(xiàn)高精度的定位需求。

基于場景識別的LANDMARC經(jīng)典定位系統(tǒng)，通過對定位場景的訓練和現(xiàn)場的信息采集，并將所得的信息進行分析以后存入數(shù)據(jù)庫，利用相近性原理來以實現(xiàn)移動目標的跟蹤。然而，在該情景下要提高定位精度，則需要增加參考標簽的數(shù)量。VIRE方法就是在不增加額外的參考標簽情況下，有效地提高了系統(tǒng)的定位精度。如果VIRE方法閾值的選取不準確，將造成某區(qū)域中沒有產(chǎn)生共同的選擇標簽集合，從而造成方法的發(fā)散。特別地，在模糊地圖的契合度不滿足要求的情況下，將導致整個定位系統(tǒng)的崩潰。目前在UHF-RFID定位系統(tǒng)中，還沒有技術能夠通過融合虛擬參考標簽和內(nèi)部傳感器(如里程計，陀螺儀，加速度計等)實現(xiàn)移動機器人位姿估計，從而提高整個定位系統(tǒng)的定位精度。

技術實現(xiàn)要素：

為了克服現(xiàn)有方法誤差大且易發(fā)散、以及對插值所產(chǎn)生的虛擬信號強度值依賴性過強導致定位發(fā)散的問題的不足，本發(fā)明提供了一種在保證對環(huán)境的適應性以及定位的實時性前提下，能有效提高定位精度和魯棒性的基于多假設UKF的UHF-RFID全局定位方法，更適合室內(nèi)復雜環(huán)境下對移動機器人的定位。

本發(fā)明解決技術問題所采用的技術方案是：

一種針對UHF-RFID系統(tǒng)的多假設UKF目標跟蹤方法，所述方法包括以下步驟：

步驟1)利用VIRE方法估計得到移動機器人的初始位置S(x₀,y₀)，確定機器人初始位置所在的標簽區(qū)域，在區(qū)域內(nèi)均勻布置粒子，并通過這些粒子來描述移動機器人的初始狀態(tài)；

步驟2)收集各超高頻天線的量測信息，通過VIRE方法生成移動機器人的位置估計作為觀測輸入，然后，應用無跡卡爾曼濾波方法UKF分別對各粒子狀態(tài)進行預測和更新，并計算各粒子的權值；

步驟3)根據(jù)粒子的權值，對其進行篩選，再對其進行重采樣；

步驟4)融合篩選之后的粒子得到移動機器人的當前狀態(tài)估計，重復執(zhí)行步驟1)—4)，以實現(xiàn)UHF-RFID環(huán)境下的移動機器人跟蹤。

進一步，所述步驟1)中，初始位置所在的區(qū)域表示成Ω，然后在Ω內(nèi)均勻選取M個粒子，給定這些粒子初始狀態(tài)為其中為隨機產(chǎn)生的，把這些粒子狀態(tài)作為移動機器人的初始狀態(tài)。

再進一步，所述步驟2)中，所述的量測信息為應用VIRE算法對移動目標位置的估計結(jié)果，初始粒子均勻采樣，粒子i初始狀態(tài)分量x,y由VIRE方法估計出，給定任意航向角初始協(xié)方差矩陣為權值利用獲得2n+1個sigma點。

所述步驟2)中，系統(tǒng)的觀測為z_k＝[x_p,k y_p,k]^T，則得由卡方分布概率密度函數(shù)，得每個粒子在各個時刻的權值其中，χ²(2)表示自由度為2的卡方分布，x_p,k+1，y_p,k+1分別為k+1時刻系統(tǒng)觀測量z_k+1在x方向和y方向上的坐標分量，和分別為粒子i在k時刻預測觀測量在x方向和y方向上的分量，分別為k+1時刻粒子在x方向和y方向上觀測協(xié)方差矩陣。

所述步驟3)中，當粒子權值舍去該粒子；反之，則稱該粒子有效并保留，有效粒子數(shù)記為n_c；同時將對粒子進行重新采樣，當有效粒子數(shù)n_c＜η，保留的粒子，并在方差為σ²范圍內(nèi)進行重新采樣，使有效粒子數(shù)始終保持在η；反之，則跳過重采樣階段。

所述步驟4)中，對篩選、重采樣后的粒子進行歸一化處理，通過采用加權平均的方法融合篩選后的粒子，從而得到移動機器人狀態(tài)估計。

本發(fā)明的有益效果主要表現(xiàn)在：在初始位姿未知情況下，實現(xiàn)了UHF-RFID環(huán)境下的移動機器人定位。傳統(tǒng)采用VIRE方法對移動機器人進行定位時，并未有效地移動機器人內(nèi)部傳感器信息(如里程計，陀螺儀等)。定位精度很大程度上依賴于UHF-RFID所處的定位環(huán)境，且該算法無法對移動機器人姿態(tài)角進行估計。針對這個問題，本發(fā)明提供了一種基于多假設UKF的移動機器人定位方法，充分利用了UHF-RFID與機器人內(nèi)部傳感器的信息，有效地克服了單一UKF濾波器不穩(wěn)定和易發(fā)散的缺點。該方法首先通過VIRE算法將非線性觀測轉(zhuǎn)化為線性觀測，然后采用多假設UKF方法實現(xiàn)移動機器人的定位。該方法相比于現(xiàn)有的VIRE目標定位方法，該方法可以有效提高系統(tǒng)的定位精度，并對移動機器人姿態(tài)角進行跟蹤，同時，相對于傳統(tǒng)UKF方法，具有更好的定位精度和收斂速度。

附圖說明

圖1超高頻定位系統(tǒng)的場景布置示意圖。

圖2采用帶里程計移動小車移動狀態(tài)分析圖。

圖3無線傳感器網(wǎng)絡的目標定位系統(tǒng)示意圖。

圖4單假設狀態(tài)估計流程圖。

圖5針對UHF-RFID系統(tǒng)的多假設UKF目標跟蹤系統(tǒng)流程圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明作進一步描述。

參照圖1～圖5，一種針對UHF-RFID系統(tǒng)的多假設UKF目標跟蹤方法，所述方法包含以下步驟：

圖1中描述了UHF-RFID下移動機器人的定位環(huán)境。圖中1代表超高頻天線，2代表參考標簽，3代表移動機器人。該定位區(qū)域為8*8，區(qū)域內(nèi)含有四個超高頻天線分布于待定位區(qū)域邊界，16個參考標簽，參考標簽如圖1中均勻布置在監(jiān)視區(qū)域內(nèi)。其中，四個天線通過輪詢方式采集各參考標簽的RSSI數(shù)據(jù)。移動機器人攜帶里程計、移動便簽。

結(jié)合圖2所示，移動機器人運動學模型可描述為如下的狀態(tài)空間模型：

x_k+1＝f(x_k,u_k)+w_k+1 (10)

z_k＝Hx_k+v_k (11)

其中，k為離散時間，系統(tǒng)狀態(tài)x_k＝[x_p,k,y_p,k,θ_k]^T。x_p,_k、y_p,_k分別為k時刻機器人在x軸和y軸上的坐標，θ_k為姿態(tài)角，為狀態(tài)轉(zhuǎn)移矩陣。u_k系統(tǒng)輸入矩陣，狀態(tài)觀測z_k＝[x_p,_k y_p,_k]^T,觀測矩陣w_k為過程噪聲，服從零均值、方差陣為Q的高斯分布，v_k為系統(tǒng)觀測噪聲，服從零均值方差為R的高斯分布。n_X為系統(tǒng)狀態(tài)的維數(shù),n_Z為觀測向量的維數(shù).假設移動機器人初始位姿和初始方差分別為x_0|0和P_0|0,且x_0|0與w_k，v_k統(tǒng)計意義上不相關。

如圖4所示，計算粒子一步預測狀態(tài)和方差，其中粒子數(shù)i＝1,2,...M,。

更新

結(jié)合圖4，式(13)中，為粒子i在k時刻取的Sigma點集，是粒子i取的Sigma點在k+1的狀態(tài)預測值。縮放比例參數(shù)λ＝a²(n+κ)-n，用來降低總得預測誤差，a的選取控制了采樣點的分布狀態(tài)，κ為待選參數(shù)，其具體取值沒有界限，通常要確保矩陣半正定。通過式(14)-(16)加權求均值，得到系統(tǒng)狀態(tài)預測值和狀態(tài)預測協(xié)方差矩陣。式(17)獲得粒子i的觀測預測值。是粒子在k+1時刻的狀態(tài)預測值，分別為Sigma點計算均值和方差的權值。為第i個粒子在k時刻狀態(tài)預測方差矩陣，Q為系統(tǒng)的過程噪聲方差矩陣。結(jié)合式(18)-(20)可求得粒子i的估計值。

結(jié)合圖5，在更新過程中，z_k+1為k+1時刻系統(tǒng)觀測值，為粒子在k+1時刻預測觀測值，H為觀測矩陣，且為粒子在k+1時刻新息矩陣。R分別表示第i個粒子在k+1時刻新息方差矩陣和觀測噪聲方差矩陣。表示第i個粒子在k+1時刻的卡爾曼增益矩陣，表示第i個粒子在k+1時刻的狀態(tài)估計誤差方差矩陣。當粒子權值舍去該粒子。反之，則稱該粒子有效并保留，有效粒子數(shù)記為n_c，同時，為了避免粒子退化，將對粒子進行重新采樣。當有效粒子數(shù)n_c＜η，保留的粒子，并在(權值最大粒子)方差為σ²范圍內(nèi)進行重新采樣，使有效粒子數(shù)始終保持在η。反之，則跳過重采樣階段。

通過采用加權平均的方法融合篩選、重采樣后的粒子。

其中n為篩選、重采樣后的粒子數(shù),為移動機器人位姿估計，為粒子歸一化權值。