多采摘機(jī)器人分布作業(yè)目標(biāo)跟蹤算法研究

王　娜，謝軍艷

(1.河南工業(yè)職業(yè)技術(shù)學(xué)院 機(jī)電自動(dòng)化學(xué)院，河南 南陽　473000；2.濟(jì)源職業(yè)技術(shù)學(xué)院 機(jī)電工程系，河南 濟(jì)源　459000)

0　引言

隨著互聯(lián)網(wǎng)和物聯(lián)網(wǎng)的相互結(jié)合，無線網(wǎng)絡(luò)的應(yīng)用場合越來越多，其已經(jīng)滲透并慢慢應(yīng)用到工業(yè)生產(chǎn)和日常生活的各方面。近年來，無線通訊技術(shù)和機(jī)器人技術(shù)發(fā)展迅速，工業(yè)機(jī)器人逐漸開始采用無線通信方式，而本身帶有通訊功能的無線傳感網(wǎng)絡(luò)也逐漸受到人們的關(guān)注。本文引入無線傳感技術(shù)，提出了一種基于網(wǎng)絡(luò)節(jié)點(diǎn)的分布式目標(biāo)跟蹤算法。該算法將無線傳感網(wǎng)絡(luò)與數(shù)據(jù)融合技術(shù)相結(jié)合，應(yīng)用于采摘機(jī)器人編隊(duì)的目標(biāo)跟蹤。

1　無線傳感網(wǎng)絡(luò)的體系結(jié)構(gòu)及其定位技術(shù)

1.1　無線傳感網(wǎng)絡(luò)的體系結(jié)構(gòu)

無線傳感網(wǎng)絡(luò)的體系結(jié)構(gòu)如圖1所示。一個(gè)典型的無線傳感器網(wǎng)絡(luò)結(jié)構(gòu)主要由傳感器、匯聚節(jié)點(diǎn)或通信衛(wèi)星及任務(wù)管理節(jié)點(diǎn)等部分構(gòu)成。從內(nèi)部結(jié)構(gòu)看，傳感器節(jié)點(diǎn)內(nèi)部一般集成一個(gè)比較小型的嵌入式控制系統(tǒng)，該載體結(jié)構(gòu)單一，處理和存儲(chǔ)信息能力有限，常常需要紐扣電池給其供電；但從網(wǎng)絡(luò)功能而言，單個(gè)節(jié)點(diǎn)都具有終端和路由器兩種用途，不但可以搜集本身監(jiān)測到的信息，還能接收并存儲(chǔ)其他節(jié)點(diǎn)信息，也可以同其他節(jié)點(diǎn)協(xié)作實(shí)現(xiàn)指定功能。匯聚節(jié)點(diǎn)作為傳感器網(wǎng)絡(luò)與等外部網(wǎng)絡(luò)的小型信號(hào)中轉(zhuǎn)站，其處理、存儲(chǔ)信息及通訊能力都較強(qiáng)，具備發(fā)布節(jié)點(diǎn)任務(wù)的功能。

圖1　無線傳感網(wǎng)絡(luò)的體系結(jié)構(gòu)

大量線傳感器節(jié)點(diǎn)分布在監(jiān)測區(qū)域，它們以無線網(wǎng)絡(luò)為平臺(tái)，通過自組織形式，順著其它節(jié)點(diǎn)逐級(jí)跳動(dòng)傳遞和交換信息；傳感節(jié)點(diǎn)采集到的數(shù)據(jù)通過傳感器網(wǎng)絡(luò)傳遞到匯聚節(jié)點(diǎn)，通過網(wǎng)絡(luò)、衛(wèi)星通訊傳遞到管理節(jié)點(diǎn)，用戶中心通過管理節(jié)點(diǎn)的數(shù)據(jù)信息發(fā)布監(jiān)測任務(wù)。

1.2　無線傳感網(wǎng)絡(luò)的定位技術(shù)

在無線傳感器網(wǎng)絡(luò)中，定位采用依靠已有節(jié)點(diǎn)信息，判斷未知節(jié)點(diǎn)信息，并有序建立傳感網(wǎng)絡(luò)的空間關(guān)系，從而達(dá)到定位的目的。一般將己知和未知位置節(jié)點(diǎn)分別稱為參考和未知節(jié)點(diǎn)。比較常見的定位方式是測量節(jié)點(diǎn)信號(hào)強(qiáng)度，并根據(jù)電波傳播模型對(duì)目標(biāo)節(jié)點(diǎn)位置進(jìn)行計(jì)算，這一技術(shù)重點(diǎn)是以信號(hào)衰減程度為參考值確定未知節(jié)點(diǎn)位置信息。

1.2.1RSSI測距法

RSSI測距法是一種根據(jù)無線信號(hào)強(qiáng)度變化判斷已知節(jié)點(diǎn)和未知節(jié)點(diǎn)距離遠(yuǎn)近的方法。在預(yù)先知道發(fā)射信號(hào)功率大小的情況下，通過傳感節(jié)點(diǎn)測量感知到的功率，即可根據(jù)信號(hào)傳播模型判斷距離。信號(hào)衰減模型表達(dá)方程為

(1)

其中，p(d)表示與基站之間相距d時(shí)傳感節(jié)點(diǎn)實(shí)際測得的信號(hào)強(qiáng)度值；p(d0) 表示與基站之間相距d0的無線信號(hào)強(qiáng)度；n為比例因子，依賴于障礙物結(jié)構(gòu)與材料；nW為無線傳感節(jié)點(diǎn)與基站間障礙物的數(shù)量；WAF為信號(hào)傳播路徑損耗附加值。一般無線傳感節(jié)點(diǎn)與相鄰節(jié)點(diǎn)之間進(jìn)行通訊時(shí)，射頻信號(hào)強(qiáng)度能夠被信息獲得者直接獲取，且該步驟無需加強(qiáng)現(xiàn)有無線信號(hào)帶寬和能量消耗。

1.2.2節(jié)點(diǎn)位置計(jì)算

在無線網(wǎng)絡(luò)定位中，未知傳感節(jié)點(diǎn)可以根據(jù)與相鄰已知節(jié)點(diǎn)的距離遠(yuǎn)近或相對(duì)角度，采用三邊、多邊或極大似然法計(jì)算其位坐標(biāo)。因三邊測量法具有操作簡便、速度快等優(yōu)點(diǎn)，本文采用該算法進(jìn)行節(jié)點(diǎn)位置的計(jì)算。

在立體空間中，知道某未知節(jié)點(diǎn)與4個(gè)以上已知節(jié)點(diǎn)的距離信息，就能計(jì)算該點(diǎn)準(zhǔn)確坐標(biāo)；而在無線傳感網(wǎng)絡(luò)中，由于是二維空間，因此只需要未知節(jié)點(diǎn)與c的距離信息，即可計(jì)算該點(diǎn)坐標(biāo)。三邊測量示意如圖2所示。

圖2　節(jié)點(diǎn)位置三邊測量示意圖

圖2中，假設(shè)p(x,y)為未知節(jié)點(diǎn)坐標(biāo)，N1(x1,y1)、N2(x2,y2)和N3(x3,y3)為3個(gè)已知節(jié)點(diǎn)坐標(biāo)，p(x,y)到3點(diǎn)的距離分別為d1、d2和d3。根據(jù)二維坐標(biāo)計(jì)算方法，可以得到

(2)

式(2)化簡后得到未知位置節(jié)點(diǎn)p(x,y) 的坐標(biāo)為

(3)

2　構(gòu)建多采摘機(jī)器人協(xié)作作業(yè)地圖

1)環(huán)境特征值的集合表達(dá)式為

(4)

2)目標(biāo)觀測值的集合表達(dá)式為

(5)

其中，d和γ分別為已知信標(biāo)特征點(diǎn)與采摘機(jī)器人的相對(duì)距離和相對(duì)角度。

3)編隊(duì)相鄰采摘機(jī)器人Rj觀測值集合可表示為

(6)

4)偽觀測值集合表達(dá)式為

其中，lRi為采摘機(jī)器人Ri獲得的偽觀測值，該值大于等于0。

圖3　環(huán)境特征觀測示意圖

圖3中，Rj為編隊(duì)相鄰的采摘機(jī)器人；T為追蹤目標(biāo)；lm為環(huán)境特征；箭頭虛線為Ri的另外3個(gè)探測目標(biāo)。

假設(shè)在時(shí)間節(jié)點(diǎn)k的Ri系統(tǒng)狀態(tài)變量為

(8)

其中，L為探測到的環(huán)境狀態(tài)分量。另外，Ri對(duì)Ri的狀態(tài)估計(jì)為

(9)

其中，(X(Ri,Rjxy)T為R和Ri兩者的相對(duì)位置狀態(tài)；(X(Ri,Rjθ)T為R和Ri兩者的相對(duì)位置狀態(tài)。

在復(fù)雜的作業(yè)環(huán)境中，采摘機(jī)器人編隊(duì)分布式作業(yè)目標(biāo)跟蹤過程可以描述為：①采摘機(jī)器人Ri在作業(yè)中進(jìn)行定位、構(gòu)建作業(yè)地圖及目標(biāo)跟蹤等一系列活動(dòng)；②Ri與編隊(duì)中另一采摘機(jī)器人Rj建立通訊并得到對(duì)Rj的觀測值，然后計(jì)算出對(duì)Rj姿態(tài)信息估計(jì)值，并傳輸給Rj；③Rj接收姿態(tài)信息估計(jì)值后，采用數(shù)據(jù)融合技術(shù)更新其信息。

3　目標(biāo)跟蹤算法設(shè)計(jì)

3.1　目標(biāo)跟蹤算法節(jié)點(diǎn)通信開銷

高斯混合模型表明：每一個(gè)概率分布都能通過有限個(gè)高斯函數(shù)組合實(shí)現(xiàn)。由n個(gè)傳感網(wǎng)絡(luò)節(jié)點(diǎn)組建的WMSN對(duì)目標(biāo)實(shí)行跟蹤，需要通過具體的監(jiān)測值得到目標(biāo)狀態(tài)信息，然后根據(jù)后驗(yàn)概率分布組成高斯混合模型，即

(10)

其中，θ={αj,uj,∑j}；j=1,…,q；i=1,…,n；αj為混合比例系數(shù)；uj為均值；Σj為高斯混合項(xiàng)的協(xié)方差；zi的似然函數(shù)為αjujΣj，則

(11)

最大化算法(EM)是專門用于概率參數(shù)模型的迭代算法，是一種通過擬合觀測信息對(duì)模型計(jì)算的方法，主要包含期望和最大值化兩步驟。式(10)中高斯混合模型采用該算法可看作為

1)計(jì)算期望。

(12)

2)計(jì)算最大值化。高斯混合模型αj、uj和Σj的迭代算法為

(13)

(14)

(15)

給式(13)、式(14)和式(15)定義參數(shù)，則

(16)

(17)

(18)

則高斯混合模型可用本地參數(shù)表示為

(19)

(20)

(21)

從式(19)、式(20)和式(21)3個(gè)式子可以看出：采用本地參數(shù)置換后，高斯混合模型進(jìn)行無線傳感節(jié)點(diǎn)間的數(shù)據(jù)交換的通信開銷大大降低。

3.2　分布式數(shù)據(jù)融合技術(shù)

分布式數(shù)據(jù)融合技術(shù)一般采用平均一致性濾波器將單個(gè)節(jié)點(diǎn)得到的環(huán)境特征進(jìn)行計(jì)算，然后將其送融合中心用權(quán)值協(xié)方差，最后計(jì)算其全局狀態(tài)評(píng)估。分布式數(shù)據(jù)融合結(jié)構(gòu)如圖4所示。

圖4　分布式數(shù)據(jù)融合結(jié)構(gòu)

圖4中，分布式數(shù)據(jù)融合體系中傳感節(jié)點(diǎn)均具有相同等級(jí)的地位，位于待追蹤目標(biāo)周邊節(jié)點(diǎn)均能對(duì)跟蹤目標(biāo)采樣并采用平均一致性濾波器計(jì)算其姿勢(shì)狀態(tài)信息，然后通過附近傳感節(jié)點(diǎn)間進(jìn)行通信及開展數(shù)據(jù)融合，求出跟蹤目標(biāo)的全局狀態(tài)評(píng)估。

根據(jù)分布式數(shù)據(jù)融合原理可以求出本地輸入ui的平均值，即

(22)

因此，式(19)～式(21)的全局表達(dá)式為

(23)

(24)

(25)

目標(biāo)跟蹤算法模型中全局參數(shù)θ表達(dá)式可改寫為

(26)

(27)

(28)

其中，式(27)和(28)分別為高斯混合模型對(duì)跟蹤目標(biāo)姿態(tài)信息求解的全局評(píng)估均值和協(xié)方差。

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

為了驗(yàn)證基于無線傳感網(wǎng)絡(luò)的目標(biāo)跟蹤算法在采摘機(jī)器人分布式作業(yè)的可行性和有效性，本文通過仿真實(shí)驗(yàn)對(duì)其進(jìn)行了驗(yàn)證。實(shí)驗(yàn)在MatLab7.0平臺(tái)下進(jìn)行。機(jī)器人編隊(duì)包括兩臺(tái)蘋果采摘機(jī)器人，仿真區(qū)域?yàn)? 000m×5 000m，環(huán)境中隨機(jī)分布著500個(gè)節(jié)點(diǎn)。仿真參數(shù)設(shè)置如表1所示，結(jié)果如圖5所示。

由圖5可以看出：實(shí)線為待跟蹤目標(biāo)機(jī)器人，虛線為跟蹤作業(yè)機(jī)器人。該算法在初始位置估計(jì)與跟蹤目標(biāo)相差600m時(shí)，沒有出現(xiàn)發(fā)散現(xiàn)象，而是根據(jù)節(jié)點(diǎn)探測目標(biāo)距離和角度直接跟蹤目標(biāo)，經(jīng)過一段時(shí)間后跟蹤效果明顯提升。試驗(yàn)結(jié)果表明：該無線傳感網(wǎng)絡(luò)目標(biāo)跟蹤算法在采摘機(jī)器人分布式作業(yè)中，在滿足目標(biāo)跟蹤精度狀態(tài)下，大幅度減少了數(shù)據(jù)融合的通信開銷，能夠準(zhǔn)確完成對(duì)目標(biāo)跟蹤的要求，應(yīng)用前景廣闊。

表1　仿真參數(shù)設(shè)置

圖5　仿真結(jié)果圖

5　結(jié)論

本文首先分析了無線傳感網(wǎng)絡(luò)的結(jié)構(gòu)及其定位技術(shù)特點(diǎn)；然后構(gòu)建了多采摘機(jī)器人協(xié)作作業(yè)地圖，根據(jù)實(shí)際需求設(shè)計(jì)研究了一種與數(shù)據(jù)融合技術(shù)相結(jié)合的分布式目標(biāo)跟蹤算法，應(yīng)用于采摘機(jī)器人編隊(duì)的目標(biāo)跟蹤；最后，利用MatLab7.0平臺(tái)對(duì)目標(biāo)跟蹤算法進(jìn)行了仿真。試驗(yàn)結(jié)果表明：該算法在采摘機(jī)器人分布式作業(yè)中，在滿足目標(biāo)跟蹤精度狀態(tài)下，大幅度降低了數(shù)據(jù)融合的通信開銷，能夠準(zhǔn)確完成對(duì)目標(biāo)跟蹤的要求。