基于擴(kuò)展卡爾曼濾波的目標(biāo)追蹤算法

馮燕

(湖南機(jī)電職業(yè)技術(shù)學(xué)院信息工程學(xué)院，湖南長(zhǎng)沙 410151)

1 概述

網(wǎng)絡(luò)技術(shù)、通信技術(shù)和微傳感器技術(shù)的快速發(fā)展[1]，推動(dòng)無線傳感網(wǎng)絡(luò)(Wireless Sensor Network,WSN)廣泛地應(yīng)用于生活中的各個(gè)領(lǐng)域[2]。研究機(jī)構(gòu)和研究人員針對(duì)WSN 應(yīng)用開展的大量的研究工作，基于WSN 的目標(biāo)定位與追蹤是最主要的研究熱點(diǎn)之一[3]。WSN由大量散布于監(jiān)測(cè)區(qū)域的傳感節(jié)點(diǎn)組成，可以實(shí)現(xiàn)對(duì)監(jiān)測(cè)區(qū)域中目標(biāo)對(duì)象的快速定位和追蹤，這些傳感節(jié)點(diǎn)能夠估算目標(biāo)之間的距離，例如立體視覺[4]、激光雷達(dá)[5]、超寬帶[6]、Wi-Fi[7]等。利用設(shè)備上已有的硬件可以節(jié)省接收設(shè)備占用控件并降低WSN系統(tǒng)的成本，接收信號(hào)強(qiáng)度(Received Signal Strength Indicator,RSSI)由于實(shí)現(xiàn)方式簡(jiǎn)單，無需在設(shè)備中新增外部設(shè)備，能夠利用無線數(shù)據(jù)傳輸?shù)奈锢硇?yīng)來估算距離[8]，進(jìn)而實(shí)現(xiàn)目標(biāo)的定位和追蹤，得到了研究人員的廣泛關(guān)注。

無線信號(hào)在空氣中傳輸時(shí)，信號(hào)強(qiáng)度會(huì)隨著傳輸距離的增大而逐漸衰減，RSSI 和收發(fā)器之間的距離存在映射關(guān)系，根據(jù)不同的傳輸環(huán)境，提出了一些經(jīng)典的無線信道模型，如自由空間模型[9]、瑞利分布模型和對(duì)數(shù)-正態(tài)模型[10]等。根據(jù)這些信道模型，利用網(wǎng)絡(luò)中傳感節(jié)點(diǎn)接收到的RSSI 值可以估算出監(jiān)測(cè)區(qū)域中目標(biāo)對(duì)象的位置，并對(duì)其進(jìn)行動(dòng)態(tài)跟蹤[11]。采用RSSI指紋識(shí)別是目前采用較多的一種目標(biāo)跟蹤方法，該方法需要提前獲悉監(jiān)測(cè)區(qū)域的環(huán)境部署，預(yù)先創(chuàng)建室內(nèi)環(huán)境的無線信號(hào)輻射圖，隨著目標(biāo)對(duì)象在監(jiān)測(cè)環(huán)境中移動(dòng)，通過匹配RSSI的變化確定目標(biāo)的位置以實(shí)現(xiàn)對(duì)目標(biāo)的跟蹤。但是，這種方法需要在使用前進(jìn)行大量、煩瑣的映射，而且對(duì)檢測(cè)區(qū)域信號(hào)輻射環(huán)境和信號(hào)發(fā)射環(huán)境變化高度敏感，當(dāng)監(jiān)測(cè)環(huán)境中存在比較嚴(yán)重的多徑效應(yīng)時(shí)，會(huì)影響目標(biāo)定位和追蹤的精度。

在室內(nèi)環(huán)境中，存在墻壁、門、窗和辦公家居等多種障礙物，而且還經(jīng)常會(huì)出現(xiàn)移動(dòng)的人，這些障礙物會(huì)導(dǎo)致室內(nèi)環(huán)境中存在嚴(yán)重的多徑效應(yīng)，從而影響RSSI的精度，進(jìn)而降低對(duì)目標(biāo)追蹤的精度，針對(duì)這一問題，提出了一種基于擴(kuò)展卡爾曼濾波的目標(biāo)追蹤算法，該算法根據(jù)機(jī)器人位移估算的RSSI 和接收到的RSSI 重新構(gòu)建RSSI 與距離的映射關(guān)系，考慮室內(nèi)環(huán)境中存在的多徑效應(yīng)問題，利用狀態(tài)轉(zhuǎn)移方程估算下一階段機(jī)器人的移動(dòng)位置，通過預(yù)測(cè)的更新階段，準(zhǔn)確獲取機(jī)器人移動(dòng)的每一步位置?；趯?shí)際的室內(nèi)辦公環(huán)境對(duì)提出算法進(jìn)行驗(yàn)證，實(shí)驗(yàn)結(jié)果表明，采用提出算法可以很好地預(yù)測(cè)目標(biāo)對(duì)象在監(jiān)測(cè)區(qū)域的移動(dòng)軌跡，得到較低的定位誤差。

2 RSSI與距離映射關(guān)系

考慮一個(gè)存在n 個(gè)傳感節(jié)點(diǎn)，m 個(gè)目標(biāo)對(duì)象的WSN 系統(tǒng)，每個(gè)傳感節(jié)點(diǎn)的位置已知，xi=[xi,yi],i=1,2,...,n，目標(biāo)對(duì)象位置未知，隨機(jī)部署在監(jiān)測(cè)區(qū)域中，需要利用網(wǎng)絡(luò)中部署好的傳感節(jié)點(diǎn)追蹤目標(biāo)對(duì)象的位置和運(yùn)行軌跡。對(duì)于室內(nèi)監(jiān)測(cè)區(qū)域，目標(biāo)對(duì)象能夠接收到通信半徑內(nèi)的多個(gè)傳感節(jié)點(diǎn)的信標(biāo)報(bào)文，每個(gè)報(bào)文中都存儲(chǔ)著對(duì)應(yīng)傳感節(jié)點(diǎn)的ID 和位置信息?？梢圆捎脤?duì)數(shù)-正態(tài)模型來描述無線信號(hào)在室內(nèi)環(huán)境的RSSI分布特性，對(duì)應(yīng)的RSSI表示為：

其中，np表示路徑衰減因子，它是一個(gè)動(dòng)態(tài)參數(shù)，會(huì)隨著環(huán)境的不同而變化；d表示無線信號(hào)傳輸距離，單位為米；d0表示參考距離，一般將其設(shè)為1 米；P0表示參考值，通常指距離為1米處的RSSI值。對(duì)數(shù)-正態(tài)模型提供了一個(gè)對(duì)數(shù)衰減過程，可以通過調(diào)整模型中的多個(gè)參數(shù)來適應(yīng)不同的室內(nèi)環(huán)境。通過對(duì)式(1)進(jìn)行轉(zhuǎn)換，對(duì)應(yīng)的距離d表示為：

式(2)無需提前預(yù)知每個(gè)發(fā)射器和接收器的傳輸功率和增益，通過路徑衰減因子和RSSI參考值可以快速估算出室內(nèi)環(huán)境中傳感節(jié)點(diǎn)與目標(biāo)對(duì)象之間的距離，進(jìn)而實(shí)現(xiàn)對(duì)監(jiān)測(cè)區(qū)域中目標(biāo)對(duì)象的定位和追蹤。但是在式(2)中，僅通過路徑衰減因子np來反映室內(nèi)監(jiān)測(cè)區(qū)域中存在的環(huán)境變化，幾種典型室內(nèi)環(huán)境的路徑衰減因子np的取值如表1所示，從表1可以看出，對(duì)于同一類室內(nèi)區(qū)域，通常采取相同的路徑衰減因子值，但是在實(shí)際環(huán)境中，同一類室內(nèi)區(qū)域中的不同空間的環(huán)境可能存在非常大的差異，如，同一樓層辦公區(qū)域中的大廳和辦公室的環(huán)境差異比較大，大廳一般空間較大，障礙物少，而辦公室空間狹窄，障礙物多，存在明顯的多徑效應(yīng)，但是同一樓層辦公區(qū)域通常采用相同的路徑衰減值，因此在進(jìn)行目標(biāo)追蹤時(shí)會(huì)產(chǎn)生較大的誤差。

表1 不同環(huán)境下的路徑衰減因子

3 基于擴(kuò)展卡爾曼濾波的目標(biāo)追蹤算法

考慮在室內(nèi)監(jiān)測(cè)區(qū)域中隨機(jī)部署1個(gè)移動(dòng)機(jī)器人，該機(jī)器人上裝配著無線收發(fā)器可以接收到WSN網(wǎng)絡(luò)中各傳感節(jié)點(diǎn)的RSSI，僅考慮傳感節(jié)點(diǎn)和移動(dòng)機(jī)器人之間的距離關(guān)系，采用擴(kuò)展卡爾曼濾波方法實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的目標(biāo)定位和追蹤。在室內(nèi)環(huán)境中，由于存在家具、墻壁、行走的人等障礙物，存在嚴(yán)重的多徑效應(yīng)。為了減少多徑效應(yīng)對(duì)RSSI的影響，結(jié)合機(jī)器人通過線性位移估算的RSSI和接收到的RSSI值來對(duì)每個(gè)位置的RSSI進(jìn)行預(yù)處理。機(jī)器人在監(jiān)測(cè)區(qū)域移動(dòng)時(shí)，除了能夠接收到通信半徑內(nèi)各個(gè)傳感節(jié)點(diǎn)的RSSI值，也能夠利用自身攜帶的傳感器的位移生成估算的RSSI，當(dāng)機(jī)器人在進(jìn)行RSSI測(cè)量時(shí)，如果已經(jīng)移動(dòng)了一段距離，則將移動(dòng)距離之間的估算的RSSI最大值和距離增加移動(dòng)距離的RSSI 相關(guān)聯(lián)，最小值與距離減少移動(dòng)距離的RSSI相關(guān)聯(lián)。預(yù)處理后的RSSI可以表示為：

對(duì)于給定距離d的RSSI值，如果RSSI的變化為正值，則偏離量為正，否則為負(fù)。采用擴(kuò)展的卡爾曼濾波器來估算目標(biāo)對(duì)象的狀態(tài)，主要包括目標(biāo)對(duì)象的坐標(biāo)、方位等，可用狀態(tài)向量x來表示：x=[x,y,θ,x1,y1,θ1,x2,y2,θ2,…,xn,yn,θn]，其中x,y,θ表示機(jī)器人初始位置的坐標(biāo)和方位，xi,yi,θi表示第i個(gè)傳感節(jié)點(diǎn)的坐標(biāo)和方位。當(dāng)機(jī)器人在監(jiān)測(cè)區(qū)域移動(dòng)時(shí)，可以將從i位置移動(dòng)到j(luò)位置的數(shù)據(jù)用一個(gè)向量組表示為：Δx=[Δd,Δθ]，對(duì)應(yīng)的狀態(tài)轉(zhuǎn)移方程可以表示為：

其中，k+1表示下一階段濾波器迭代的值，k表示當(dāng)前狀態(tài)的預(yù)測(cè)值。式(4)是一個(gè)非線性方程，根據(jù)控制向量組預(yù)測(cè)下一個(gè)狀態(tài)，通過獲取系統(tǒng)的線性化模型，對(duì)應(yīng)的觀測(cè)函數(shù)h 可以表示為：

其中，下標(biāo)i表示網(wǎng)絡(luò)中的第i個(gè)傳感節(jié)點(diǎn)，觀測(cè)函數(shù)h將狀態(tài)變量轉(zhuǎn)換為測(cè)量向量形式，測(cè)量向量中包含與當(dāng)前狀態(tài)中各傳感節(jié)點(diǎn)的估計(jì)距離值，因此，觀測(cè)函數(shù)需要獲取傳感節(jié)點(diǎn)與機(jī)器人當(dāng)前位置之間的距離。式(5)表示一個(gè)非線性函數(shù)，為了讓濾波器能夠更新測(cè)量階段的協(xié)方差，利用雅可比行列式得到其對(duì)應(yīng)的線性化形式，其對(duì)應(yīng)的雅可比行列式G可以表示為：

利用最后一次濾波器迭代的值，使用擴(kuò)展卡爾曼濾波器更新協(xié)方差矩陣，構(gòu)建機(jī)器人相鄰移動(dòng)節(jié)點(diǎn)的距離方程，將狀態(tài)變量轉(zhuǎn)換為測(cè)量向量形式的觀測(cè)函數(shù)，該函數(shù)中包含與當(dāng)前移動(dòng)位置相關(guān)的距離映射關(guān)系。通過預(yù)測(cè)和更新兩個(gè)階段，可以得到機(jī)器人每一步的坐標(biāo)位置和協(xié)方差。

4 性能分析

在一個(gè)面積為100 平方米的辦公區(qū)域隨機(jī)部署能夠覆蓋整個(gè)區(qū)域的傳感節(jié)點(diǎn)，每個(gè)傳感節(jié)點(diǎn)的發(fā)射功率都設(shè)置為0dBm，周期性地廣播信標(biāo)報(bào)文，1 個(gè)配備無線收發(fā)器的移動(dòng)機(jī)器人在監(jiān)測(cè)區(qū)域移動(dòng)，給定機(jī)器人的初始位置，機(jī)器人在監(jiān)測(cè)區(qū)域內(nèi)進(jìn)行線性或旋轉(zhuǎn)運(yùn)動(dòng)，對(duì)于機(jī)器人每一次的移動(dòng)，都定義一個(gè)指定的初始位置和預(yù)設(shè)好的運(yùn)行路徑，每隔0.1 秒從各個(gè)傳感節(jié)點(diǎn)收集RSSI 值。在監(jiān)測(cè)區(qū)域中，給機(jī)器人指定運(yùn)行軌跡進(jìn)行測(cè)量，選擇直線軌跡、圓角矩形軌跡、圓形軌跡和弧形軌跡進(jìn)行目標(biāo)跟蹤，每次運(yùn)動(dòng)之間的差異受到高斯噪聲的影響，為了減少測(cè)量誤差對(duì)目標(biāo)跟蹤結(jié)果的影響，對(duì)每一種運(yùn)行軌跡重新測(cè)量100次。為減少多徑效應(yīng)的干擾，將在每個(gè)位置采集到的RSSI進(jìn)行加權(quán)平均后傳遞到預(yù)過濾階段進(jìn)行處理。

圖1描述了機(jī)器人在辦公區(qū)域的一個(gè)5米×5米的辦公室沿圓角矩形軌跡進(jìn)行移動(dòng)的過程，在測(cè)試過程中，該辦公室部署了8個(gè)傳感節(jié)點(diǎn)，機(jī)器人需要沿著圖中預(yù)設(shè)的圓角矩形進(jìn)行移動(dòng)。從圖中機(jī)器人的實(shí)際運(yùn)行路線可以看出，機(jī)器人能夠沿著預(yù)設(shè)的圓角矩形進(jìn)行移動(dòng)，通過部署在辦公室的8個(gè)傳感節(jié)點(diǎn)可以很好地實(shí)現(xiàn)對(duì)目標(biāo)機(jī)器人的定位和運(yùn)行軌跡追蹤。當(dāng)機(jī)器人沿著矩形軌跡中的直線進(jìn)行移動(dòng)時(shí)，運(yùn)行路線基本能夠和預(yù)定軌跡一致，因?yàn)闄C(jī)器人在直線運(yùn)行時(shí)，只需要考慮其和傳感節(jié)點(diǎn)之間的距離，通過提出算法可以準(zhǔn)確地根據(jù)當(dāng)前狀態(tài)獲得下一次移動(dòng)的位置。當(dāng)機(jī)器人遇到圓角矩形區(qū)域的弧形拐角區(qū)域時(shí)，實(shí)際運(yùn)行路線會(huì)稍微偏移預(yù)定軌跡，但偏移的程度較低，這是因?yàn)樵趫A角矩形的弧形拐角區(qū)域，由于角度偏移量接近90 度，機(jī)器人除了要計(jì)算距離，還需要計(jì)算偏轉(zhuǎn)的方位，偏轉(zhuǎn)角度過大會(huì)導(dǎo)致機(jī)器人在進(jìn)行位置估算時(shí)出現(xiàn)一定的誤差，從而導(dǎo)致出現(xiàn)在弧形拐角區(qū)域出現(xiàn)偏離預(yù)定軌跡的情況。

圖1 目標(biāo)追蹤軌跡

機(jī)器人在辦公區(qū)域沿圓角矩形軌跡進(jìn)行目標(biāo)追蹤的定位誤差情況如圖2 所示，圖中橫坐標(biāo)表示迭代次數(shù)，縱坐標(biāo)表示定位誤差，從圖中可以看出，當(dāng)機(jī)器人沿直線運(yùn)行時(shí)，目標(biāo)追蹤的定位誤差較低；當(dāng)機(jī)器人遇到圓形拐角時(shí)，目標(biāo)追蹤的定位誤差較大，這是由于機(jī)器人遇到圓形拐角時(shí)需要旋轉(zhuǎn)自身方向并計(jì)算對(duì)應(yīng)的角度，由于圓角矩形框的弧形拐角位置的角度偏移接近90 度，機(jī)器人在移動(dòng)過程中進(jìn)行位置估算時(shí)會(huì)出現(xiàn)旋轉(zhuǎn)角度不準(zhǔn)確的情況出現(xiàn)，而且在狹小的辦公室環(huán)境中，由于墻壁和其他障礙物的存在，會(huì)存在多徑效應(yīng)，導(dǎo)致計(jì)算方位時(shí)也會(huì)出現(xiàn)一定的偏差。在目標(biāo)追蹤區(qū)域?yàn)? 米×5 米的辦公室環(huán)境中，能夠?qū)崿F(xiàn)對(duì)機(jī)器人的移動(dòng)軌跡追蹤，其平均定位誤差為0.65米。

圖2 目標(biāo)追蹤誤差

5 結(jié)束語

室內(nèi)目標(biāo)追蹤是WSN 領(lǐng)域的研究熱點(diǎn)，室內(nèi)環(huán)境復(fù)雜多變，墻壁、門窗和家具等障礙物會(huì)導(dǎo)致多徑效應(yīng)問題，嚴(yán)重影響無線信號(hào)的RSSI 值，導(dǎo)致無法實(shí)現(xiàn)對(duì)網(wǎng)絡(luò)中目標(biāo)對(duì)象的準(zhǔn)確追蹤。針對(duì)室內(nèi)復(fù)雜環(huán)境中存在的問題，提出了一種基于擴(kuò)展卡爾曼濾波的目標(biāo)追蹤算法，提出算法融合機(jī)器人移動(dòng)過程中估算的RSSI 和從傳感節(jié)點(diǎn)處接收到的RSSI，重構(gòu)RSSI 和距離之間的映射關(guān)系，采用狀態(tài)轉(zhuǎn)移方程估算當(dāng)前移動(dòng)位置的坐標(biāo)，通過預(yù)測(cè)和更新兩個(gè)階段，實(shí)現(xiàn)對(duì)機(jī)器人運(yùn)動(dòng)軌跡的定位和追蹤。實(shí)驗(yàn)結(jié)果證明了提出算法的有效性，當(dāng)機(jī)器人在室內(nèi)運(yùn)動(dòng)時(shí)，提出算法能夠準(zhǔn)確估算機(jī)器人每一步的移動(dòng)坐標(biāo)，實(shí)現(xiàn)對(duì)其的移動(dòng)軌跡追蹤，尤其當(dāng)機(jī)器人在直線上移動(dòng)時(shí)，由于只需要考慮和傳感節(jié)點(diǎn)之間的距離和自身的移動(dòng)距離，機(jī)器人可以準(zhǔn)確地沿著預(yù)設(shè)的直線軌跡進(jìn)行移動(dòng)。