WSNs的改進(jìn)PF算法對(duì)移動(dòng)機(jī)器人的定位跟蹤*

徐 馳， 付麗霞， 毛劍琳， 張 勇， 楊曉東

(1.昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院,云南 昆明 650500;2.昆明理工大學(xué) 津橋?qū)W院,云南 昆明 650106)

0 引 言

無(wú)線傳感器網(wǎng)絡(luò)(wireless sensor networks,WSNs)作為新興的研究領(lǐng)域，由于其具有可快速部署、結(jié)構(gòu)簡(jiǎn)單、能耗較低等一般無(wú)線網(wǎng)絡(luò)所不具備的優(yōu)點(diǎn)和特點(diǎn)，已廣泛應(yīng)用于國(guó)防軍事，搶險(xiǎn)救災(zāi)、環(huán)境監(jiān)測(cè)、交通管理等領(lǐng)域。

自主移動(dòng)機(jī)器人采用非固定路徑移動(dòng)的智能小車，由于具有更大的使用靈活性，目前已成為機(jī)器人技術(shù)研究的一個(gè)熱點(diǎn)。定位技術(shù)是自主導(dǎo)航移動(dòng)機(jī)器人必須具備的一個(gè)基本功能，是移動(dòng)機(jī)器人能否實(shí)現(xiàn)自由導(dǎo)航的基礎(chǔ)[1]。

1 SPF算法

PF算法也稱為序貫MC(sequential MC,SMC)方法，是一種基于遞推貝葉斯估計(jì)和MC方法的統(tǒng)計(jì)濾波方法，它根據(jù)大數(shù)定理采用MC方法來(lái)求解遞推貝葉斯估計(jì)中的積分問題[3]。

設(shè)動(dòng)態(tài)空間模型如下

(1)

若已知狀態(tài)的初始概率密度函數(shù)為p(x0|z0)=p(x0)，則狀態(tài)預(yù)測(cè)方程為

(2)

狀態(tài)更新方程為

(3)

SPF算法可總結(jié)如下[4](Ns為粒子個(gè)數(shù))：

2)更新:在k時(shí)刻，更新粒子權(quán)值

(4)

歸一化

(5)

則可得k時(shí)刻未知參數(shù)x的最小均方估計(jì)為

(6)

5)時(shí)刻k=k+1，轉(zhuǎn)到第2步。

2 機(jī)器人的運(yùn)動(dòng)模型

本文考慮兩輪驅(qū)動(dòng)移動(dòng)機(jī)器人的狀態(tài)模型和觀測(cè)模型。

移動(dòng)機(jī)器人通過控制左右2輪正向和反向旋轉(zhuǎn)能在原地做任意角度轉(zhuǎn)動(dòng)。當(dāng)一個(gè)包含前向速度和角速度的控制量uk(v,γ(k))施加于機(jī)器人時(shí)，預(yù)測(cè)機(jī)器人運(yùn)動(dòng)的狀態(tài)模型為[5～7]

(7)

式中 (x(k),y(k))和φ(k)分別為k時(shí)刻機(jī)器人的位置與方位角，v為機(jī)器人的運(yùn)動(dòng)速度，γ為角速度，wxyφ∈N(0,σxyφ)為噪聲項(xiàng)，用于描述輪子打滑等位置特征的影響。

機(jī)器人利用自身配備的距離/方向傳感器探測(cè)陸標(biāo)(本文中為傳感器網(wǎng)絡(luò)單個(gè)節(jié)點(diǎn))，得到陸標(biāo)的距離和方位角。根據(jù)圖1可以得到陸標(biāo)的距離和方向角的近似值，其觀測(cè)模型可以表示為

(8)

其中,(xr,yr)為機(jī)器人的坐標(biāo)，(xi,yi)為第i個(gè)陸標(biāo)的坐標(biāo)，φ為機(jī)器人的朝向，wr和wθ分別為量測(cè)距離和角度的噪聲序列。

圖1 機(jī)器人量測(cè)模型

3 改進(jìn)的PF算法

PF算法可以作為解決非線性、非高斯問題的有效手段，但是該算法仍然存在著一些問題。其中最主要的問題是需要用大量的樣本數(shù)量才能很好地近似系統(tǒng)的后驗(yàn)概率密度。機(jī)器人面臨的環(huán)境越復(fù)雜，描述后驗(yàn)概率分布所需要的樣本數(shù)量就越多，算法的復(fù)雜度就越高。因此，有效地減少樣本數(shù)量是必要的。

隨機(jī)生成的粒子會(huì)導(dǎo)致誤差較大，而且不斷地采樣導(dǎo)致算法復(fù)雜度的增加，為此，先采用遺傳算法對(duì)初始粒子進(jìn)行挑選，使適應(yīng)度較高的粒子集作為PF算法的初始粒子集，并保證其與初始粒子具有相同的統(tǒng)計(jì)特性[8]，這樣就能夠使得粒子更加逼近狀態(tài)的真實(shí)情況，從而減少跟蹤誤差，提高精度。

(9)

(10)

(11)

(12)

改進(jìn)的跟蹤算法流程如圖2所示。

圖2 跟蹤過程流程圖

4 仿真結(jié)果

采用MC方法來(lái)對(duì)改進(jìn)的效果進(jìn)行仿真，機(jī)器人采用兩輪驅(qū)動(dòng)，主要參數(shù)為：機(jī)器人初始位置為(1 000,1 000)cm，x和y方向的速度分別為0.2,0.1 m/s，角速度為0.1 rad/s，采樣時(shí)間為50 s，采樣時(shí)間間隔為1 s，粒子數(shù)為100。圖3給出了機(jī)器人移動(dòng)過程的仿真結(jié)果。

圖3 機(jī)器人運(yùn)動(dòng)的實(shí)際軌跡與跟蹤軌跡

圖3顯示出了機(jī)器人運(yùn)動(dòng)的實(shí)際軌跡和跟蹤軌跡，從圖中可以看出:經(jīng)過遺傳優(yōu)化過的2種方法GIPF1和GIPF2不僅要比SPF算法跟蹤效果要好，而且也比單純采用方差自適應(yīng)進(jìn)行改進(jìn)的算法要好，盡管有些位置的效果不理想，但和SPF相比，效果要明顯優(yōu)于SPF算法。圖4給出了5種算法下跟蹤的均方根誤差，從圖中可以看出:隨著采樣時(shí)間的增加，SPF算法理想的誤差會(huì)逐漸增大，而改進(jìn)的2種算法都在一定的范圍內(nèi)波動(dòng)，但是和SPF算法相比，誤差明顯減少，而經(jīng)過遺傳優(yōu)化的算法誤差會(huì)進(jìn)一步減小，由此可知，對(duì)初始粒子進(jìn)行遺傳優(yōu)化，會(huì)更進(jìn)一步提高跟蹤精度。

表1給出了5種算法的運(yùn)行時(shí)間，誤差最值與均值對(duì)比，從表中可以看出:在時(shí)間上，IPF1算法和標(biāo)準(zhǔn)算法時(shí)間上相差無(wú)幾，但是誤差卻大大減小，IPF2算法誤差和IPF1比起來(lái)較稍大，但是運(yùn)行時(shí)間卻比IPF1略小，優(yōu)于SPF算法，而采用了遺傳算法優(yōu)化過的算法無(wú)論是在時(shí)間還是在誤差均值和最值方面，都要好于SPF算法和單純采用的算法。

表1 5種跟蹤算法下的各項(xiàng)性能

圖4 跟蹤過程的誤差

5 結(jié) 論

本文提出的GVAPF算法，對(duì)SPF算法，先用遺傳算法獲得初始粒子，然后再用技術(shù)，通過實(shí)時(shí)更新系統(tǒng)噪聲的方差，減少系統(tǒng)重采樣過程中噪聲的誤差，從而消除SPF中粒子誤差的累積效應(yīng)。仿真表明:經(jīng)過遺傳算法優(yōu)化的2種方法都比SPF算法要優(yōu)越，在運(yùn)行時(shí)間可忽略的情況下，大大提高了跟蹤的精度，結(jié)果表明這種方法在機(jī)器人跟蹤過程中的有效性。

參考文獻(xiàn):

[1] 李群明,熊 蓉,褚 健.室內(nèi)自主移動(dòng)機(jī)器人定位方法研究綜述[J].機(jī)器人,2003,25(6)：560-567.

[2] Dellaert F,Burgard,Fox D,et al.Monte-Carlo localization for mobile robots：Artificial intelligence[C]∥Proceedings of the IEEE International Conference on Robotics and Automation，ICRA’98,Detroit,1999：1322-1328.

[3] Ababneh J I,Batained M H.Linear phase FIR filter design using particle swarm optimization and genetic algorithms[J].Digital Signal Processing,2008,18(4)：657-668.

[4] 朱志宇.粒子濾波算法極其應(yīng)用[M].北京：科學(xué)出版社,2010：29.

[5] 夏益民,楊益民.基于UPF的移動(dòng)機(jī)器人定位新方法[J]. 微電子學(xué)與計(jì)算機(jī),2009,26(2)：159-162.

[6] Dissanavalce G,Newman P M,Clark S,et al.A solution to simultaneous localization and map (SLAM)building problem[J].IEEE Transactions on Robotics and Automation,2001,17(3)：229-241.

[7] 田力偉,黃建國(guó).粒子濾波在機(jī)動(dòng)目標(biāo)純方位跟蹤中的應(yīng)用[J].微電子學(xué)與計(jì)算機(jī),2005,22(10)：81-84.

[8] 楊 寧,錢 峰,朱 瑞.基于遺傳算法的改進(jìn)粒子濾波算法[J].上海交通大學(xué)學(xué)報(bào),2011,45(10)：1526-1530.

[9] Doucet A,Godsill S，Andrieu C.On sequential Monte Carlo sampling methods for Bayesian filtering[J].Statistics and Computing,2000,10(3)：197-208.

[10] Yang Xiaodong,Xiang Fenghong,Mao Jianlin,et al.Improved particle filter algorithms for target tracking in binary wireless sensor networks[C]∥Proceedings of the 30th Chinese Control Confe-rence,Yantai,2011.

[11] Miguez J,Bugallo M F,Djuric P M.A new class of particle filters for random dynamical systems with unknown statistics[J].EURASIP Journal on Applied Signal Processing,2004(15)：2278-2294.

[12] 楊曉東.面向二元無(wú)線傳感器網(wǎng)絡(luò)的目標(biāo)跟蹤算法研究[D].昆明：昆明理工大學(xué)，2012：33-35.

[13] 葉 龍,王京玲,張 勤.遺傳重采樣粒子濾波器[J].自動(dòng)化學(xué)報(bào)，2007，33(8)：885-887.