基于強(qiáng)跟蹤UKF的室內(nèi)目標(biāo)跟蹤

張英坤

（1.河北省科學(xué)院應(yīng)用數(shù)學(xué)研究所，石家莊 050081；2.石家莊開發(fā)區(qū)冀科雙實(shí)科技有限公司，石家莊 050081；3.河北省信息安全認(rèn)證工程技術(shù)研究中心，石家莊 050081）

隨著無線通信技術(shù)、微型計(jì)算機(jī)技術(shù)和傳感器技術(shù)的快速發(fā)展，無線傳感器網(wǎng)絡(luò)[1-3]得到了廣泛應(yīng)用，目標(biāo)跟蹤[4-6]是其中一個(gè)重要的研究領(lǐng)域。在目標(biāo)跟蹤領(lǐng)域，基于卡爾曼濾波理論[7-10]的目標(biāo)跟蹤算法被應(yīng)用于目標(biāo)的狀態(tài)估計(jì)。UKF濾波算法[11-13]是非線性系統(tǒng)中常用的濾波算法，其狀態(tài)估計(jì)依賴于歷史數(shù)據(jù)，當(dāng)測(cè)量數(shù)據(jù)存在偏差時(shí)，容易導(dǎo)致濾波效果變差甚至發(fā)散。本文在分析標(biāo)準(zhǔn)UKF算法的基礎(chǔ)上，利用強(qiáng)跟蹤思想[14]對(duì)其進(jìn)行改進(jìn)，并搭建了基于無線傳感器網(wǎng)絡(luò)的測(cè)試平臺(tái)，對(duì)強(qiáng)跟蹤UKF算法的性能進(jìn)行了驗(yàn)證。

1 實(shí)驗(yàn)環(huán)境的搭建

利用麻省理工大學(xué)的Cricket系統(tǒng)搭建一個(gè)基于無線傳感器網(wǎng)絡(luò)的可用于室內(nèi)目標(biāo)跟蹤的實(shí)驗(yàn)環(huán)境，如圖1所示。該網(wǎng)絡(luò)由4個(gè)Cricket節(jié)點(diǎn)構(gòu)成，被跟蹤目標(biāo)由機(jī)器人攜帶一個(gè)Cricket節(jié)點(diǎn)組成。Crieket節(jié)點(diǎn)利用射頻信號(hào)和超聲波脈沖之間的到達(dá)時(shí)間差來計(jì)算節(jié)點(diǎn)之間的距離。

圖1 無線傳感器網(wǎng)絡(luò)Fig.1 Wireless sensor network

4個(gè)Cricket節(jié)點(diǎn)分布在已知的固定位置，同時(shí)發(fā)送射頻信號(hào)和超聲波脈沖，攜帶Cricket節(jié)點(diǎn)的機(jī)器人接收射頻信號(hào)，當(dāng)接收到射頻信號(hào)后，啟動(dòng)超聲波檢測(cè)器等待接收相應(yīng)的超聲信號(hào)，檢測(cè)到超聲波脈沖時(shí)，利用射頻信號(hào)到達(dá)時(shí)間和超聲波脈沖到達(dá)時(shí)間之差來計(jì)算機(jī)器人與Cricket節(jié)點(diǎn)之間的距離。

2 系統(tǒng)模型的建立

本文研究的室內(nèi)目標(biāo)跟蹤屬于二維平面上的單目標(biāo)跟蹤問題，其跟蹤過程可以描述如下：攜帶一個(gè)Cricket節(jié)點(diǎn)的機(jī)器人在4個(gè)Cricket節(jié)點(diǎn)構(gòu)成的無線傳感器網(wǎng)絡(luò)中運(yùn)動(dòng)，根據(jù)機(jī)器人的運(yùn)動(dòng)方式建立機(jī)器人的系統(tǒng)模型，即狀態(tài)方程和觀測(cè)方程，再利用UKF算法迭代估計(jì)機(jī)器人的運(yùn)動(dòng)狀態(tài)，從而實(shí)現(xiàn)對(duì)機(jī)器人的跟蹤。下面給出機(jī)器人運(yùn)動(dòng)的狀態(tài)方程和觀測(cè)方程。

2.1 機(jī)器人運(yùn)動(dòng)的狀態(tài)方程

采用如下狀態(tài)方程來描述機(jī)器人的運(yùn)動(dòng)：

式中：Xk為機(jī)器人運(yùn)動(dòng)的狀態(tài)向量，（xk，yk）為 k 時(shí)刻機(jī)器人的位置坐標(biāo)；為 k 時(shí)刻機(jī)器人在X、Y方向的速度；Φk-1為狀態(tài)轉(zhuǎn)移矩陣；Γk-1為過程噪聲輸入矩陣；wk-1為過程噪聲。

2.2 機(jī)器人運(yùn)動(dòng)的觀測(cè)方程

假設(shè)無線傳感器網(wǎng)絡(luò)中的節(jié)點(diǎn)具有相同的噪聲統(tǒng)計(jì)特性。當(dāng)機(jī)器人在無線傳感器網(wǎng)絡(luò)中運(yùn)動(dòng)時(shí)，每個(gè)時(shí)刻均有4個(gè)節(jié)點(diǎn)檢測(cè)到信號(hào)。令表示 k時(shí)刻第i個(gè)節(jié)點(diǎn)測(cè)量到的自身與機(jī)器人之間的距離，將其作為觀測(cè)量，則有：

式中：（xi，yi）為第 i個(gè)節(jié)點(diǎn)的位置坐標(biāo)。

3 基于強(qiáng)跟蹤的UKF算法

3.1 UKF濾波理論

3.1.1 無跡變換

UKF算法的關(guān)鍵步驟是無跡變換UT（unscented transform），UT變換是一種計(jì)算非線性變換中隨機(jī)變量數(shù)字特征的方法，其實(shí)現(xiàn)原理為在原先狀態(tài)分布中按某一規(guī)則取一些點(diǎn)，使這些點(diǎn)的均值和協(xié)方差等于原狀態(tài)分布的均值和協(xié)方差；將這些點(diǎn)帶入函數(shù)中，相應(yīng)得到非線性函數(shù)值點(diǎn)集，通過這些點(diǎn)集求取變換后的均值和協(xié)方差，其Sigma點(diǎn)的選擇是基于先驗(yàn)均值和先驗(yàn)協(xié)方差矩陣的平方根的相關(guān)列實(shí)現(xiàn)的。具體計(jì)算方法如下：

（1）計(jì)算 2n+1個(gè) Sigma點(diǎn)

式中：n表示第n次采樣；α常取為一個(gè)小的正值，如0.001，它決定了采樣點(diǎn)相對(duì)于均值的分布情況；β為關(guān)于狀態(tài)變量分布的先驗(yàn)知識(shí)在高斯分布時(shí)最佳取值為 2；γ為尺度調(diào)節(jié)因子；k通常取 0為均值的權(quán)值；為協(xié)方差的權(quán)值。

3.1.2 UKF濾波原理

考慮如下非線性模型：

其中：Xk為狀態(tài)向量； f（·）為 n 維向量函數(shù)；hi（g）為m維向量函數(shù)；wk為過程噪聲項(xiàng)，為高斯白噪聲，服從 N（0，Qk）分布；Zk為觀測(cè)向量；vk為量測(cè)噪聲項(xiàng)，亦為高斯白噪聲，服從 N（0，Rk）分布。

則UKF濾波計(jì)算過程如下：

步驟1初始化k=1，計(jì)算狀態(tài)及協(xié)方差矩陣：

步驟2確定采樣點(diǎn)：

步驟3時(shí)間更新：

步驟4觀測(cè)更新：

步驟5濾波更新：

步驟6k=k+1，轉(zhuǎn)至步驟2重復(fù)計(jì)算。

3.2 強(qiáng)跟蹤濾波器對(duì)UKF的改進(jìn)

強(qiáng)跟蹤的充分條件是選擇一個(gè)適合的時(shí)變?cè)鲆婢仃嘖k使得：

由于模型不確定性的影響，當(dāng)濾波器的狀態(tài)估計(jì)值偏離系統(tǒng)狀態(tài)時(shí)，必然會(huì)在輸出新息序列的均值和幅值上表現(xiàn)出來。若在線調(diào)整增益陣Kk，強(qiáng)迫式（21）仍然成立，使得新息序列仍然保持正交，則可以強(qiáng)迫強(qiáng)跟蹤濾波器保持對(duì)實(shí)際系統(tǒng)狀態(tài)的跟蹤。強(qiáng)跟蹤濾波器對(duì)UKF的改進(jìn)主要表現(xiàn)在通過實(shí)時(shí)調(diào)整狀態(tài)預(yù)報(bào)誤差的協(xié)方差矩陣和相應(yīng)的增益矩陣來達(dá)到。因此修改式為

式中：0≤ρ＜1為遺忘因子，一般取 ρ=0.95；β≥1為一個(gè)選定的弱化因子，引入弱化因子的目的是為了使?fàn)顟B(tài)估計(jì)值更加平滑，此數(shù)值可以根據(jù)經(jīng)驗(yàn)來設(shè)定，也可以通過計(jì)算機(jī)仿真來確定。

4 實(shí)驗(yàn)及結(jié)果分析

為了驗(yàn)證強(qiáng)跟蹤UKF算法的性能，將其應(yīng)用于本文搭建的無線傳感器網(wǎng)絡(luò)中。機(jī)器人攜帶的節(jié)點(diǎn)每隔0.5 s發(fā)射一次射頻信號(hào)和超聲波脈沖，其他4個(gè)節(jié)點(diǎn)則檢測(cè)相應(yīng)的射頻信號(hào)和超聲波脈沖，因此，采樣周期T=1 s。機(jī)器人沿著中心位于（100 cm，100 cm）、半徑為 50 cm的圓做勻速圓周運(yùn)動(dòng)，起始位置位于（150 cm，100 cm），角速度為則初始線速度近似為 （0 cm/s，8.73 cm/s）， 機(jī)器人的初始狀態(tài)為 X0=（150 cm，0 cm/s，100 cm，8.73 cm/s）。

在上述條件下，采用強(qiáng)跟蹤UKF算法與標(biāo)準(zhǔn)UKF算法的跟蹤結(jié)果如圖2、圖3和圖4所示。從圖2可以看出，強(qiáng)跟蹤UKF的跟蹤曲線相較于標(biāo)準(zhǔn)UKF更加接近于預(yù)定軌跡；圖3和圖4分別給出了X方向和Y方向的跟蹤誤差絕對(duì)值曲線。從中可以看出強(qiáng)跟蹤UKF的跟蹤精度要明顯優(yōu)于標(biāo)準(zhǔn)UKF，且具有較好的魯棒性。

圖2 跟蹤軌跡曲線Fig.2 Tracking trajectory curve

圖3 X方向誤差絕對(duì)值曲線Fig.3 Error absolute value curve of X direction

圖4 Y方向誤差絕對(duì)值曲線Fig.4 Error absolute value curve of Y direction

5 結(jié)語

為減少陳舊數(shù)據(jù)對(duì)目標(biāo)跟蹤過程產(chǎn)生的不利影響，本文對(duì)標(biāo)準(zhǔn)UKF算法進(jìn)行改進(jìn)，利用強(qiáng)跟蹤思想修正協(xié)方差矩陣和相應(yīng)的增益矩陣，搭建基于無線傳感器網(wǎng)絡(luò)的測(cè)試平臺(tái)對(duì)強(qiáng)跟蹤UKF算法的性能進(jìn)行驗(yàn)證，實(shí)驗(yàn)結(jié)果表明，強(qiáng)跟蹤UKF算法具有更高的跟蹤精度，且魯棒性較好。

[1]張健，李鷗.改進(jìn)的無線傳感器網(wǎng)絡(luò)TDOA定位算法[J].計(jì)算機(jī)工程與應(yīng)用，2010，46（16）：117-120.

[2]馬靈芝.基于TDOA無線傳感定位算法的研究及應(yīng)用[D].山東：山東大學(xué)，2012.

[3]孫大洋，錢志鴻，韓夢(mèng)飛，等.無線傳感網(wǎng)絡(luò)中多邊定位的聚類分析改進(jìn)算法[J].電子學(xué)報(bào)，2014，42（8）：1601-1607.

[4]權(quán)太范.目標(biāo)跟蹤新理論與技術(shù)[M].北京：國(guó)防工業(yè)出版社，2009.

[5]薛皓.時(shí)間異步條件下的無線傳感器網(wǎng)絡(luò)目標(biāo)跟蹤關(guān)鍵技術(shù)研究[D].北京：北京郵電大學(xué)，2009.

[6]侯杰虎.基于Kalman濾波器的視頻運(yùn)動(dòng)目標(biāo)跟蹤算法研究[D].成都：成都理工大學(xué)，2012.

[7]付夢(mèng)印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京：科學(xué)出版社，2003.

[8]秦勤.雷達(dá)目標(biāo)跟蹤的卡爾曼濾波方法的研究[D].大連：大連海事大學(xué)，2006.

[9]申逸.Kalman濾波技術(shù)在目標(biāo)跟蹤中的應(yīng)用研究[D].湖南：國(guó)防科學(xué)技術(shù)大學(xué)，2006.

[10]王思思，齊國(guó)清.自適應(yīng)SCKF在機(jī)動(dòng)目標(biāo)跟蹤中的應(yīng)用[J].電光與控制，2013，20（8）：6-10

[11]S Sarkka.On unscented Kalman filtering for state estimation of continuous-time nonlinear systems[J].IEEE Trans on Automation Control，2007，52（9）：1631-1641.

[12]劉慧霞，楊峰，梁彥.基于UKF的超視距雷達(dá)跟蹤算法研究[J].計(jì)算機(jī)測(cè)量與控制，2005，13（10）：1094-1096.

[13]任福君，張秀華，姜永成，等.改進(jìn)UKF算法在移動(dòng)機(jī)器人定位系統(tǒng)中的應(yīng)用[J].哈爾濱工程大學(xué)學(xué)報(bào)，2012，33（10）：1289-1294.

[14]陳巴特爾，周東華.強(qiáng)跟蹤濾波器在倒立擺中的應(yīng)用[J].控制工程，2012，19（1）：141-144.