余輝榮 ，夏候凱順 ，葉景志 ，鄔依林 ，2

（1.華南理工大學(xué) 自動(dòng)化科學(xué)與工程學(xué)院，廣州 510640；2.廣東第二師范學(xué)院 計(jì)算機(jī)科學(xué)系，廣州 510310）

多個(gè)目標(biāo)的實(shí)時(shí)、精確跟蹤是無(wú)線傳感器網(wǎng)絡(luò)的重要應(yīng)用和研究難點(diǎn)。隨著被跟蹤目標(biāo)數(shù)量及其運(yùn)動(dòng)特征多樣性的增加，若要繼續(xù)保持有效跟蹤，則對(duì)于分布式傳感器節(jié)點(diǎn)的協(xié)同調(diào)度效率和定位算法的要求也同步上升。一個(gè)優(yōu)良的定位算法必須具備自組織性、低能耗、高魯棒性和分布式計(jì)算等特點(diǎn)[1]。無(wú)線傳感器網(wǎng)絡(luò)中的目標(biāo)跟蹤算法基本上分為直接通信法[2]、基于二進(jìn)制探測(cè)的方法[3]、基于樹(shù)狀結(jié)構(gòu)的方法[4]、基于簇狀結(jié)構(gòu)的方法[5]、基于預(yù)測(cè)機(jī)制的方法[6]、基于粒子濾波的方法[7]、基于自適應(yīng)機(jī)制的方法[8]和對(duì)偶空間轉(zhuǎn)換跟蹤方法[9]等。

傳統(tǒng)的最小二乘定位算法及常規(guī)的卡爾曼濾波定位算法是最常用的方法。前者具有收斂速度快、跟蹤及時(shí)等優(yōu)點(diǎn)，但測(cè)量信號(hào)中的噪聲在融合后也被凸顯，導(dǎo)致定位軌跡波動(dòng)較大；后者是一種高效率的自回歸濾波器，能夠從一系列不完全及包含噪聲的測(cè)量中，估計(jì)出動(dòng)態(tài)系統(tǒng)的狀態(tài)[10]，抗觀測(cè)噪聲干擾能力強(qiáng)，其缺點(diǎn)是信號(hào)收斂速度較慢。因此，對(duì)于目標(biāo)運(yùn)動(dòng)特性較復(fù)雜的情況 （如圖1所示），當(dāng)運(yùn)動(dòng)速度發(fā)生突變時(shí)，運(yùn)用單一的任一定位算法都無(wú)法達(dá)到快速且準(zhǔn)確的追蹤。

圖1 目標(biāo)運(yùn)動(dòng)特性示意圖Fig.1 Schema of target motion characteristics

本文針對(duì)上述兩種算法的固有缺陷，提出了一種自適應(yīng)的混合估計(jì)算法。該算法以基于單個(gè)觀測(cè)值的擴(kuò)展卡爾曼濾波模型為基礎(chǔ)，結(jié)合收斂速度更快的最小二乘法對(duì)卡爾曼濾波估計(jì)進(jìn)行修正，有效結(jié)合兩者的優(yōu)點(diǎn)，以達(dá)到更優(yōu)的跟蹤效果。

1 傳統(tǒng)定位算法分析

常規(guī)的TOA定位算法包括最小二乘法、基于泰勒展開(kāi)的增量式算法、卡爾曼濾波算法等，其中以最小二乘法和卡爾曼濾波算法為主。

圖2 最小二乘原理圖Fig.2 Schema of least squares

1.1 最小二乘定位算法

在二維平面系統(tǒng)中，通過(guò)測(cè)得已知的n個(gè)傳感器節(jié)點(diǎn)（x1，y1），（x2，y2），（x3，y3），（xn，yn）…與目標(biāo)的距離 d1，d2，…，dn，求得目標(biāo)坐標(biāo)（x0，y0）。根據(jù)圖 2 列出以下方程組：

整理得：AX=B

其中：

采用最小方差的估計(jì)方法可得到坐標(biāo)：

1.2 擴(kuò)展卡爾曼濾波定位算法

對(duì)于多個(gè)跟蹤對(duì)象，假設(shè)各對(duì)象的線性離散系統(tǒng)模型如下：

式中：N為網(wǎng)絡(luò)中目標(biāo)個(gè)數(shù)；k為采樣時(shí)刻；Xi，k為k時(shí)刻被估計(jì)的n維系統(tǒng)狀態(tài)變量；Φi，k/k-1為k-1時(shí)刻到k時(shí)刻系統(tǒng)n維的狀態(tài)轉(zhuǎn)移矩陣；Zi，k為k時(shí)刻的m維觀測(cè)量；Hi，k為k時(shí)刻的m×n維量測(cè)矩陣，過(guò)程噪聲 Wi，k和觀測(cè)噪聲 Vi，k是均值為 0，符合高斯正態(tài)分布的白噪聲序列，其協(xié)方差矩陣分別為Qi，k-1和 Ri，k。

常規(guī)卡爾曼濾波遞推方法如下[11]：

在非線性的目標(biāo)跟蹤系統(tǒng)中，要利用卡爾曼算法，將非線性過(guò)程圍繞當(dāng)前的狀態(tài)估計(jì)利用泰勒級(jí)數(shù)展開(kāi)方法進(jìn)行近似線性化，再用卡爾曼濾波算法解決非線性系統(tǒng)的濾波問(wèn)題，即擴(kuò)展卡爾曼濾波算法（EKF）[12]。

1.3 單個(gè)觀測(cè)值的擴(kuò)展卡爾曼濾波模型

將輪式移動(dòng)機(jī)器人作為多目標(biāo)跟蹤系統(tǒng)的移動(dòng)目標(biāo)，根據(jù)機(jī)器人的運(yùn)動(dòng)學(xué)方程建立濾波器模型的狀態(tài)方程：

其中：第 i個(gè)移動(dòng)機(jī)器人的狀態(tài)為 Xi=［xiνi，xyiνi，y］T，分別代表X軸、Y軸的坐標(biāo)和速度；mi代表機(jī)器人的質(zhì)量；T為采樣周期，此時(shí)狀態(tài)轉(zhuǎn)移矩陣是常數(shù)矩陣。

觀測(cè)方程 Zi，k=Hi，kXi，k的建立如下：

假設(shè)簇節(jié)點(diǎn)（xi，a，yi，a）在某一時(shí)刻 k 與目標(biāo) i之間的理論距離值為 di，a：

線性化可得：

其中：Zi，k=di，a，Xi，k=[xi，k，yi，k] 為 k 時(shí)刻第 i個(gè)機(jī)器人的坐標(biāo)，觀測(cè)矩陣為

2 基于擴(kuò)展卡爾曼濾波的混合估計(jì)算法

在多目標(biāo)跟蹤應(yīng)用系統(tǒng)中，由于目標(biāo)運(yùn)動(dòng)特性及其跨越網(wǎng)格區(qū)域的多樣性，同時(shí)存在無(wú)線傳感器網(wǎng)絡(luò)因數(shù)據(jù)掉包而引起的觀測(cè)值丟失問(wèn)題，觀測(cè)環(huán)境變得異常復(fù)雜。最小二乘定位算法受測(cè)量值噪聲影響大，而傳統(tǒng)的擴(kuò)展卡爾曼濾波定位算法也存在一定程度的延時(shí)問(wèn)題，使得網(wǎng)絡(luò)對(duì)于速度發(fā)生突變的目標(biāo)的精確跟蹤變得困難，甚至出現(xiàn)丟失目標(biāo)的嚴(yán)重后果。兩者的性能對(duì)比如表1所示。

表1 兩種傳統(tǒng)定位算法的性能比較Tab.1 Performance comparison of two traditional location algorithms

采用卡爾曼濾波定位算法時(shí)，當(dāng)目標(biāo)發(fā)生突變運(yùn)動(dòng)時(shí)，目標(biāo)狀態(tài)一般也發(fā)生較大的變化。此時(shí)卡爾曼的單步預(yù)測(cè)狀態(tài)估計(jì)與真實(shí)狀態(tài)誤差很大，但是由觀測(cè)值體現(xiàn)出來(lái)的系統(tǒng)狀態(tài)誤差卻較小。因此，以觀測(cè)值為基準(zhǔn)，通過(guò)增強(qiáng)新加入信息的修正作用，以此加大濾波增益K，達(dá)到加快濾波跟蹤速度、減小估計(jì)誤差的目的。

以單個(gè)目標(biāo)為例：卡爾曼增益Kk與狀態(tài)估計(jì)誤差協(xié)方差矩陣Pk+1/k和觀測(cè)噪聲協(xié)方差矩陣Rk有關(guān)。若系統(tǒng)采用相同結(jié)構(gòu)和參數(shù)的傳感器節(jié)點(diǎn)，則Rk可視為常數(shù)。Kk隨Pk+1/k的增大而增大，且Pk+1/k體現(xiàn)了濾波的精度，Pk+1/k越小說(shuō)明濾波越準(zhǔn)確[13]。因此，Kk的增大是由濾波不準(zhǔn)確，協(xié)方差矩陣較大所要求的。

其中：Kk為常規(guī)卡爾曼濾波在k時(shí)刻的增益；為附加增益。系統(tǒng)狀態(tài)估計(jì)方程重寫為

其余遞推公式保持不變。

式中：k0為系統(tǒng)狀態(tài)發(fā)生突變的時(shí)刻；Kz，k0為 k0時(shí)刻足以實(shí)現(xiàn)目標(biāo)快速跟蹤的增益矩陣；λ為常數(shù)，一般取0.8～0.9?？赏ㄟ^(guò)設(shè)定一個(gè)非常大的協(xié)方差陣Pz，對(duì)式（8）、式（9）進(jìn)行修正以確定 Kz，k0[13]。 協(xié)方差陣Pz代表濾波估值與狀態(tài)真值之間的誤差，但其并沒(méi)有一種標(biāo)準(zhǔn)的理論取法。

本文結(jié)合收斂速度快的最小二乘法來(lái)確定Kz，k0。在目標(biāo)發(fā)生運(yùn)動(dòng)突變時(shí)，最小二乘法以其快速收斂的特性保證獲取較為準(zhǔn)確的狀態(tài)估計(jì)，但其仍然存在較大的噪聲，因此先采用一階低通濾波器消除高頻噪聲：

假設(shè)利用最小二乘法經(jīng)低通濾波器得到k0時(shí)刻的系統(tǒng)狀態(tài)估計(jì)為X?z，k0，則通過(guò)設(shè)計(jì)一個(gè) Pz，k0：

可得：

最終卡爾曼增益設(shè)計(jì)為

模型參數(shù)及初值的確定：

上述混合估計(jì)算法的模型參數(shù)及初值可按常規(guī)的卡爾曼濾波器來(lái)確定。若多目標(biāo)跟蹤系統(tǒng)采用周期驅(qū)動(dòng)異步測(cè)量策略且采樣周期為T，則過(guò)程噪聲協(xié)方差：

其中，λ為過(guò)程噪聲因子。

過(guò)程噪聲和量測(cè)噪聲必須滿足一定的數(shù)量級(jí)關(guān)系才能得到好的跟蹤效果。量測(cè)噪聲協(xié)方差的辨識(shí)原則為

P0可選擇范圍比較大，可設(shè)為單位陣I。系統(tǒng)初值X0則可通過(guò)最小二乘定位算法確定。

3 算法效果驗(yàn)證

3.1 實(shí)驗(yàn)平臺(tái)簡(jiǎn)介

系統(tǒng)在邏輯結(jié)構(gòu)上分為感知層、網(wǎng)絡(luò)通信層、應(yīng)用層，實(shí)體垂直架構(gòu)依次為分布式測(cè)量節(jié)點(diǎn)、基站、USB-ZigBee通信網(wǎng)關(guān)、數(shù)據(jù)服務(wù)中心。系統(tǒng)架構(gòu)如圖3所示。

圖3 實(shí)驗(yàn)平臺(tái)架構(gòu)圖Fig.3 Configuration of experimental platform

12個(gè)分布式網(wǎng)絡(luò)超聲波測(cè)距節(jié)點(diǎn)均勻分布，在平面上組成一個(gè)大小為4 m×6 m，坐標(biāo)從 （0，0）到（400，600）的有效監(jiān)視區(qū)域。將監(jiān)視區(qū)域劃分為6個(gè)網(wǎng)格，每個(gè)網(wǎng)格大小為2 m×2 m，包含4個(gè)節(jié)點(diǎn)；基站是在無(wú)線網(wǎng)絡(luò)覆蓋區(qū)中，負(fù)責(zé)整個(gè)網(wǎng)絡(luò)的資源分配和信息解析的單元；USB-ZigBee通信網(wǎng)關(guān)是負(fù)責(zé)基站和數(shù)據(jù)服務(wù)中心之間的信息傳遞，實(shí)現(xiàn)上下行雙向通信的單元；數(shù)據(jù)服務(wù)中心是采集無(wú)線傳感器網(wǎng)絡(luò)原始測(cè)量數(shù)據(jù)和利用數(shù)據(jù)融合定位算法計(jì)算目標(biāo)位置信息的單元，并負(fù)責(zé)繪制目標(biāo)軌跡曲線和存儲(chǔ)歷史數(shù)據(jù)；輪式移動(dòng)機(jī)器人可在監(jiān)測(cè)網(wǎng)絡(luò)內(nèi)任意移動(dòng)，作為系統(tǒng)的跟蹤目標(biāo)。

3.2 速度突變情況的效果

針對(duì)移動(dòng)目標(biāo)速度突變運(yùn)動(dòng)的情況，將本文的混合估計(jì)算法嵌入到多目標(biāo)跟蹤實(shí)驗(yàn)平臺(tái)中，可得到如圖4的實(shí)驗(yàn)結(jié)果。圖4（a）是目標(biāo)以0.5 rad/s的速度運(yùn)動(dòng)到兩網(wǎng)格交界處并突然停止（速度變?yōu)?）的理想軌跡；圖 4（b）、（c）、（d）分別為最小二乘算法、常規(guī)卡爾曼濾波定位算法、混合估計(jì)算法的實(shí)際跟蹤軌跡圖。與收斂快速的最小二乘法相比，混合估計(jì)算法雖然收斂速度稍慢，但其對(duì)目標(biāo)速度不發(fā)生大突變時(shí)的跟蹤更趨平滑準(zhǔn)確；而與收斂速度慢的標(biāo)準(zhǔn)卡爾曼濾波相比，其對(duì)于目標(biāo)速度不會(huì)突變時(shí)的跟蹤效果相當(dāng)，但是對(duì)于移動(dòng)目標(biāo)速度突變時(shí)的收斂速度則快得多，跟蹤效果更佳。

圖4 速度突變情況下性能對(duì)比Fig.4 Performance comparison of mutation rate case

3.3 實(shí)際跟蹤結(jié)果分析

圖5是兩臺(tái)輪式移動(dòng)機(jī)器人同相位協(xié)同控制的實(shí)際效果圖。其中一臺(tái)機(jī)器人在左上方的網(wǎng)格內(nèi)圍繞網(wǎng)格中心做半徑為0.5 m的圓周運(yùn)動(dòng)，線速度為0.3 m/s，角速度為0.6 rad/s，角速度變化在10%以內(nèi)；另一臺(tái)機(jī)器人則在最下面四個(gè)網(wǎng)格圍繞中心節(jié)點(diǎn)做半徑為1 m，通過(guò)無(wú)線網(wǎng)絡(luò)反饋位置信息給機(jī)器人調(diào)節(jié)控制器輸出，使得其角速度和軸向角跟前一臺(tái)機(jī)器人一致的圓周運(yùn)動(dòng)，實(shí)現(xiàn)雙機(jī)器人的同相位協(xié)同控制。

圖5 雙機(jī)器人協(xié)同控制效果圖Fig.5 Diagram of dual-robot cooperative control

實(shí)驗(yàn)結(jié)果表明，混合估計(jì)算法合理地融合了兩種定位算法的優(yōu)點(diǎn)，不僅能準(zhǔn)確跟蹤正常運(yùn)動(dòng)的目標(biāo)而不丟失，且對(duì)于移動(dòng)機(jī)器人在網(wǎng)格切換時(shí)速度和觀測(cè)模型發(fā)生突變時(shí)的運(yùn)動(dòng)也能達(dá)到平滑、快速跟蹤的效果。

4 結(jié)語(yǔ)

本文討論了無(wú)線傳感器網(wǎng)絡(luò)多目標(biāo)跟蹤應(yīng)用中目標(biāo)運(yùn)動(dòng)特性多樣化的復(fù)雜情況及由其導(dǎo)致的跟蹤困難問(wèn)題，并針對(duì)該問(wèn)題提出了混合估計(jì)算法。該算法是一個(gè)自適應(yīng)的校正過(guò)程，當(dāng)目標(biāo)移動(dòng)速度不突變時(shí)，其能夠以常規(guī)的卡爾曼濾波定位算法出現(xiàn)，達(dá)到平滑且準(zhǔn)確的目標(biāo)跟蹤性能；而當(dāng)目標(biāo)運(yùn)動(dòng)速度發(fā)生突變時(shí)，其又能夠結(jié)合收斂速度快的最小二乘法來(lái)校正卡爾曼濾波增益，達(dá)到快速跟蹤的目的。通過(guò)搭建一個(gè)基于卡爾曼濾波的無(wú)線傳感器網(wǎng)絡(luò)多目標(biāo)實(shí)時(shí)跟蹤實(shí)驗(yàn)平臺(tái)，進(jìn)行相關(guān)算法的效能驗(yàn)證。實(shí)驗(yàn)結(jié)果表明，本文的自適應(yīng)混合估計(jì)算法具有良好的跟蹤性能。

[1] 杜曉通.無(wú)線傳感器網(wǎng)絡(luò)技術(shù)與工程應(yīng)用[M].北京：機(jī)械出版社，2010.

[2]Yong D，Chen W，Li X.Using mobile beacons to locate sensors in obstructed environments[J].Journal of Parallel and Distributed Computing，2010，70（6）：644-656.

[3]Zhang R B，Zhang L L，F(xiàn)eng Y B.Very low energy consumption wireless sensor localization for danger environments with single mobile anchor node[J].Wireless Personal Communications，2008，47（4）：497-521.

[4] Kuang XH，Shao HH，F(xiàn)eng R.A new distributed localization scheme for wireless sensor networks[J].Acta Automatica Sinica，2008，34（3）：344-348.

[5]Mirela M，Mihaela C.Improved sensor network lifetime with multiple mobile sinks[J].Pervasive and Mobile Computing，2009，5（5）：542-555.

[6] 鄭巍，劉三陽(yáng)，寇曉麗.動(dòng)態(tài)傳感器網(wǎng)絡(luò)移動(dòng)代理路由算法[J].控制與決策，2010，25（7）：1035-1039.

[7] Wang G J，Wang T，Jia W J.Adaptive location updates for mobile sinks in wireless sensor networks[J].Journal of Supercomputing，2009，47（2）：127-145.

[8]Camp T，Bolenq J，Davies V.A survey of mobility models for ad hoc network research[J].Wireless Communications and Mobile Computing，2002，2（5）：483-502.

[9] Kuo-Feng Ssu，Chia-Ho Ou，Jiau HC.Localization with mobile anchor points in wireless sensor networks[J].IEEE Transactions on Vehicular Technology，2005，54（3）：1187-1197.

[10]Yang Liu.Sun Zhen-dong.EKF-based adaptive sensor scheduling for target tracking[C]//IEEE International Symposium on Information Science and Engieering，2008：171-174.

[11]Rajendran V，Obraczka K，Garcia-Luna-Aceves JJ.Energy-efficient，collision-free medium access control for wireless sensor networks[C]//Proceeding of SenSys03，2003：181-192.

[12]Ye Jingzhi，Zhao Ling，Luo Wenfeng.Performances of localization algorithms in a prototype WSN system[J].Advanced Materials Research，2012（457-458）：723-727.

[13]孫楓，劉希斌.一個(gè)快速跟蹤的卡爾曼濾波算法及其在艦船組合導(dǎo)航中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2000，8（4）：19-23.