WSNs的改進PF算法對移動機器人的定位跟蹤*

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

(1.昆明理工大學 信息工程與自動化學院,云南 昆明 650500;2.昆明理工大學 津橋學院,云南 昆明 650106)

0 引 言

無線傳感器網絡(wireless sensor networks,WSNs)作為新興的研究領域，由于其具有可快速部署、結構簡單、能耗較低等一般無線網絡所不具備的優點和特點，已廣泛應用于國防軍事，搶險救災、環境監測、交通管理等領域。

自主移動機器人采用非固定路徑移動的智能小車，由于具有更大的使用靈活性，目前已成為機器人技術研究的一個熱點。定位技術是自主導航移動機器人必須具備的一個基本功能，是移動機器人能否實現自由導航的基礎[1]。

1 SPF算法

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

設動態空間模型如下

(1)

若已知狀態的初始概率密度函數為p(x0|z0)=p(x0)，則狀態預測方程為

(2)

狀態更新方程為

(3)

SPF算法可總結如下[4](Ns為粒子個數)：

2)更新:在k時刻，更新粒子權值

(4)

歸一化

(5)

則可得k時刻未知參數x的最小均方估計為

(6)

5)時刻k=k+1，轉到第2步。

2 機器人的運動模型

本文考慮兩輪驅動移動機器人的狀態模型和觀測模型。

移動機器人通過控制左右2輪正向和反向旋轉能在原地做任意角度轉動。當一個包含前向速度和角速度的控制量uk(v,γ(k))施加于機器人時，預測機器人運動的狀態模型為[5～7]

(7)

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

機器人利用自身配備的距離/方向傳感器探測陸標(本文中為傳感器網絡單個節點)，得到陸標的距離和方位角。根據圖1可以得到陸標的距離和方向角的近似值，其觀測模型可以表示為

(8)

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

圖1 機器人量測模型

3 改進的PF算法

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

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

(9)

(10)

(11)

(12)

改進的跟蹤算法流程如圖2所示。

圖2 跟蹤過程流程圖

4 仿真結果

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

圖3 機器人運動的實際軌跡與跟蹤軌跡

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

表1給出了5種算法的運行時間，誤差最值與均值對比，從表中可以看出:在時間上，IPF1算法和標準算法時間上相差無幾，但是誤差卻大大減小，IPF2算法誤差和IPF1比起來較稍大，但是運行時間卻比IPF1略小，優于SPF算法，而采用了遺傳算法優化過的算法無論是在時間還是在誤差均值和最值方面，都要好于SPF算法和單純采用的算法。

表1 5種跟蹤算法下的各項性能

圖4 跟蹤過程的誤差

5 結 論

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

參考文獻:

[1] 李群明,熊 蓉,褚 健.室內自主移動機器人定位方法研究綜述[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] 朱志宇.粒子濾波算法極其應用[M].北京：科學出版社,2010：29.

[5] 夏益民,楊益民.基于UPF的移動機器人定位新方法[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] 田力偉,黃建國.粒子濾波在機動目標純方位跟蹤中的應用[J].微電子學與計算機,2005,22(10)：81-84.

[8] 楊 寧,錢 峰,朱 瑞.基于遺傳算法的改進粒子濾波算法[J].上海交通大學學報,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] 楊曉東.面向二元無線傳感器網絡的目標跟蹤算法研究[D].昆明：昆明理工大學，2012：33-35.

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