WSNs環(huán)境下基于高斯混合容積卡爾曼濾波的移動機器人定位算法*

陳曉飛,凌有鑄,陳孟元

(安徽工程大學(xué),安徽省電氣傳動與控制重點實驗室,安徽 蕪湖 241000)

WSNs環(huán)境下基于高斯混合容積卡爾曼濾波的移動機器人定位算法*

陳曉飛,凌有鑄*,陳孟元

(安徽工程大學(xué),安徽省電氣傳動與控制重點實驗室,安徽 蕪湖 241000)

針對移動機器人的定位問題,提出一種面向無線傳感器網(wǎng)絡(luò)WSNs(Wireless Sensor Networks)環(huán)境下,結(jié)合高斯混合容積卡爾曼濾波(GM-CKF)優(yōu)化的定位算法。將WSNs對移動機器人的觀測、機器人自身對環(huán)境特征的觀測以及機器人自身運動控制量進行數(shù)據(jù)融合,并利用帶有門限判別和選擇性高斯分割的GM-CKF算法,對機器人的預(yù)估位置實施預(yù)測修正,降低計算求解的空間維數(shù),提高定位精度。仿真實驗結(jié)果表明,所提出的方法比傳統(tǒng)機器人自定位法定位精度有所提高,算法精度較標準的CKF算法提高了39.11%,比EKF算法提高了65.81%。

無線傳感器網(wǎng)絡(luò);高斯混合容積卡爾曼濾波;移動機器人;定位

定位問題一直是移動機器人MR(Mobile Robot)研究領(lǐng)域的基礎(chǔ)和關(guān)鍵技術(shù)之一[1-2]。在許多運用中,只有機器人位置狀態(tài)已知,才能更有效的發(fā)揮監(jiān)測功能。移動機器人的機動性能突出,但感知能力在某些環(huán)境下還存在一定的局限性。而WSNs環(huán)境下,傳感器節(jié)點可以根據(jù)目標傳感信息,自動感知目標實時位置,從而實現(xiàn)定位跟蹤[3]。

目前移動機器人主要采用的定位方法有基于自身攜帶加速度計、陀螺儀等傳感器的自定位法,通過激光測距、超聲測距、圖像匹配的地圖定位法,基于視覺與聽覺的定位方法等[4-8]。密歇根大學(xué)的Rex Wong[9]等人將WSNs節(jié)點作為動態(tài)路標,組成局部定位系統(tǒng),輔助機器人定位,與自定位等傳統(tǒng)方法相比具有較好的定位精度。文獻[10]為了提高機器人的定位精度和穩(wěn)定性,設(shè)計仿真了一種基于EKF濾波算法的定位方法,仿真結(jié)果表明,該算法使得定位精度得到了大幅提高。文獻[11]使用了一種異質(zhì)傳感器信息融合的粒子群定位算法,不僅定位精度得到了有效提高,還改善了定位的收斂速度。文獻[12]針對觀測中多傳感器的信息融合會產(chǎn)生噪聲誤差影響的問題,提出了一種CKF定位導(dǎo)航算法,結(jié)果顯示,該算法的有效性和可行性均高于傳統(tǒng)的EKF和UKF算法。文獻[13]提出了一種WSNs環(huán)境下,利用極大似然估計函數(shù)求解CKF觀測矩陣的方法,取得了較好的定位效果。

本文將CKF算法進行改進,增加了門限判別和適度分割策略,提出了一種改進型的GM-CKF濾波算法,并將其應(yīng)用于面向WSNs環(huán)境下的機器人定位問題。通過仿真分析可以看出,與傳統(tǒng)的自定位方式及僅有WSNs節(jié)點的協(xié)作定位方式相比,本文所提出的方法在精度和穩(wěn)定性方面都有所提高。

1 問題描述與模型建立

1.1 問題描述

WSNs環(huán)境下基于GM-CKF濾波算法的移動機器人定位問題實質(zhì)是:機器人利用WSNs節(jié)點的觀測以及自身傳感器對環(huán)境特征的觀測,將測量數(shù)據(jù)進行融合,獲得局部位置信息,并利用帶有門限判別和選擇性高斯分割的GM-CKF算法,對機器人的預(yù)估位置,實施預(yù)測修正進而計算出自身精確定位、建立環(huán)境模型的過程,定位系統(tǒng)模型如圖1所示。

圖1 定位系統(tǒng)模型

1.2 系統(tǒng)建模

(1)

式中:g(Mi,Xk)為節(jié)點對機器人的測量方程,δk表示無線通信帶來的誤差,為節(jié)點與機器人間的觀測高斯噪聲。WSNs環(huán)境為靜態(tài)網(wǎng)絡(luò),對應(yīng)的狀態(tài)轉(zhuǎn)移方程為單位矩陣,因此系統(tǒng)的狀態(tài)方程[14]可簡化為移動機器人的運動模型,即機器人的自身運動控制量,可表示為:

(2)

式中:xk,yk,θk分別表示機器人在到達每個狀態(tài)Xk處對應(yīng)的坐標與姿態(tài)角,k時刻的運動輸出uk=[Δd,Δθk]中Δd表示機器人在[k-1,k]時刻的運動距離,Δθk為偏移的角度。

與一般機器人定位問題不同的是:WSNs環(huán)境下,定位觀測融合了WSNs的觀測信息與移動機器人自身的運動控制量。因而WSNs環(huán)境下移動機器人定位問題的觀測模型可表示為:

(3)

式中:Xk表示k時刻系統(tǒng)的狀態(tài),Zk為k時刻節(jié)點對機器人的觀測值,fk(Xk-1,uk)為機器人的運動函數(shù)方程,εk為傳感區(qū)域內(nèi)因環(huán)境導(dǎo)致的位置觀測噪聲,gk(Xk,Mk)為節(jié)點對機器人的測量方程,δk表示無線射頻觀測產(chǎn)生的高斯噪聲,εk、δk分別服從關(guān)于N(0,Qk)和N(0,Rk)的高斯分布。

2 基于GM-CKF的定位跟蹤算法

2.1 容積卡爾曼濾波算法

容積卡爾曼濾波算法[15](CKF)的實質(zhì)是利用測量值重新構(gòu)建系統(tǒng)的狀態(tài)向量,即用前一時刻的估計值和現(xiàn)時刻的觀測值求出現(xiàn)時刻的估計,以“預(yù)測—實測—修正”的思想來消除隨機干擾。

標準的CKF的基本步驟如下:

(1)時間更新

①對k-1時刻估計方差進行分解

(4)

②Cubature點計算

(5)

③Cubature點傳播

(6)

④求得預(yù)測狀態(tài)和預(yù)測協(xié)方差

(7)

(2)量測更新

⑤對預(yù)測協(xié)方差矩陣進行分解得到

(8)

計算Cubature點

(9)

Cubature點傳播

Zi,k|k-1=g(Xi,k|k-1)

(10)

⑥量測估計值計算

(11)

⑦計算更新后的量測誤差方差

(12)

⑧計算協(xié)方差

(13)

⑨計算卡爾曼濾波增益

(14)

因此,狀態(tài)向量和相應(yīng)的估計協(xié)方差為:

(15)

2.2 高斯混合容積卡爾曼濾波算法

(16)

式中:p(zk|xk,i)為第n個分量對應(yīng)的似然函數(shù),可表示為:

(17)

(18)

于是就利用了含高斯分量的似然函數(shù),對子濾波器的權(quán)重進行求精。通過設(shè)置權(quán)限γw可以將權(quán)限為0或接近0的子濾波器進行移除。又由于機器人動態(tài)性較強,在每一時刻線性度有所不同,于是引入了全局非線性程度判別量:

(19)

(20)

圖2 基于GM-CKF定位算法框圖

基于GM-CKF定位算法框圖如圖2所示,經(jīng)過上述步驟的分割與判別,將當前時刻的高斯分量再進行數(shù)據(jù)融合作為下一個時刻的計算輸入量。這樣使得該算法的運算量更少,有效性和可靠性也得到提高。

3 仿真結(jié)果及分析

3.1 仿真環(huán)境及參數(shù)

仿真環(huán)境為一個100 m×100 m的平面區(qū)域,包含36個隨機分布的WSNs節(jié)點。機器人以0.4 m/s的恒定速度按指定位置運動,期間與節(jié)點每隔0.6 s進行一次相互通信和測量。假設(shè)進行迭代估計的初始位置為(0,0),部分可視化數(shù)據(jù)使用MATLAB進行仿真分析,仿真參數(shù)如表1所示。

表1 仿真參數(shù)

3.2 仿真結(jié)果及性能分析

仿真實驗中,將機器人自定位法、WSNs輔助下的WSNs-MR協(xié)作定位方法、同WSNs環(huán)境下基于高斯混合容積卡爾曼濾波的WSNs-GMCKF-MR定位方法進行對比,并分析了實驗數(shù)據(jù)。仿真定位結(jié)果如圖3～圖5所示,“黑色實線”為機器人行進的實際軌跡,“黑色虛線”為基于自定位法的仿真定位結(jié)果,“△”為WSNs節(jié)點位置,機器人自定位法仿真結(jié)果如圖3所示,圖3中節(jié)點處于關(guān)閉狀態(tài)。

圖3 移動機器人自定位結(jié)果

圖4 WSNs-MR定位結(jié)果

圖5 WSNs-GMCKF-MR定位結(jié)果

圖6 3種方法誤差對比

3.2.1 誤差分析

圖6是MR、WSNs-MR和WSNs-GMCKF-MR 3種法在相同環(huán)境下估計誤差的對比曲線。其中X軸代表時間,Y軸代表定位估計的誤差。

從仿真結(jié)果圖3可以看出機器人自定位法的誤差最大,從圖5和圖6可以看出,WSN-GMCKF-MR的誤差則低于其他兩種,原因是它采用了高斯分割與權(quán)值門限的策略,減小了非線性帶來的影響,且不易產(chǎn)生數(shù)值濾波發(fā)散的問題,驗證了WSNs-GMCKF-MR算法的有效性和精確性。

3.2.2 算法驗證

由于觀測過程中會產(chǎn)生觀測噪聲,由圖6可以看出WSNs-GMCKF-GM方法的波動范圍和波動次數(shù)少于MR和WSNs-MR的定位方法。圖7為3種不同算法應(yīng)用于WSNs-GM中的仿真誤差對比圖,可以看出GM-CKF算法能正常收斂,除了起始時刻,其他各個歷元間的位置誤差較小,且沒有明顯的野值,與EKF和傳統(tǒng)CKF相比,GM-CKF算法的收斂速度更快,定位精度也明顯提高,說明GM-CKF這種非線性濾波能夠用于處理含高斯噪聲的定位模型。

圖7 3種算法誤差對比

表2還給出了EKF、CKF、GM-CKF算法在相同實驗條件下的均方根誤差和運行時間。用位置估計時產(chǎn)生的均方根誤差對定位的精度進行誤差分析:

(21)

從表2可以看出,EKF算法在3種算法中精度最低,運行時間也最短,CKF算法時間上較EKF有所增加,而定位精度也有了大幅提高。GM-CKF算法雖然在運算時間上最長,但精度比標準的CKF算法提高了39.11%,比EKF算法提高了65.81%,但ms級的運算時間依然可以滿足實時定位的要求。

表2 3種算法RMSE和運行時間

4 結(jié)論

本文針對移動機器人的定位問題,提出一種面向WSNs環(huán)境下,結(jié)合高斯混合容積卡爾曼濾波優(yōu)化的機器人定位算法。融合了WSNs的通信交互能力,為惡劣環(huán)境下實現(xiàn)動態(tài)高效的定位提供了可能,也增加了定位的精確性。仿真實驗表明,所采用的帶有門限判別和高斯分割的混合容積卡爾曼濾波(GM-CKF)算法能夠有效克服高非線性和異常誤差導(dǎo)致的不利影響,減小了由于濾波發(fā)散帶來的誤差,收斂速度更快,定位精度也得到提高。

[1] 朱磊,樊繼壯,趙杰. 改進粒子濾波器的移動機器人同步定位與地圖構(gòu)建方法[J]. 重慶大學(xué)學(xué)報,2014,37(4):39-45.

[2] 譚民,王碩. 機器人技術(shù)研究進展[J]. 自動化學(xué)報,2013,39(7):963-972.

[3] 程磊,周明達,吳懷宇,等. 無線傳感器環(huán)境下粒子群優(yōu)化的多機器人協(xié)同定位研究[J]. 智能系統(tǒng)學(xué)報,2015,10(1):138-142.

[4] Wang Ke,Wang Wei,Zhuang Yan. A MAP Approach for Vision-Based Self-Localization of Mobile Robot[J]. Acta Automatica Sinica,2008,34(2):159-166.

[5] 夏凌楠,張波,王營冠,等. 基于慣性傳感器和視覺里程計的機器人定位[J]. 儀器儀表學(xué)報,2013,34(1):166-172.

[6] 董永峰,王安娜,周艷聰,等. 一種新的基于RFID的室內(nèi)移動機器人自定位方法研究[J]. 計算機應(yīng)用研究,2016,33(3):749-753,758.

[7] He Feng. A Practical Self-Localization Scheme for Mobile Robots Using Sonar Sensors[J]. High Technology Letters,2009,15(1):13-19.

[8] Zhou Fengyu,Yuan Xianfeng,Yang Yang. A High Precision Visual Localization Sensor and Its Working Methodology for an Indoor Mobile Robot[J]. Frontiers of Information Technology and Electronic Engineering,2016,17(4):365-374.

[9] Wong R,Hu S,Cabrera-Mora F,et al. A Distributed Algorithm for Mobile Robot Localization and Mapping in Wireless Sensor Networks[C]//Zhangjiajie,China,2008:560-566.

[10] 張鳳,黃陸君,袁帥,等. NLOS環(huán)境下基于EKF的移動機器人定位研究[J]. 控制工程,2015,22(1):14-19.

[11] 劉洞波,劉國榮,喻妙華. 融合異質(zhì)傳感信息的機器人粒子濾波定位方法[J]. 電子測量與儀器學(xué)報,2011,25(1):38-43.

[12] 王宏健,李村,么洪飛,等. 基于高斯混合容積卡爾曼濾波的UUV自主導(dǎo)航定位算法[J]. 儀器儀表學(xué)報,2015,36(2):254-261.

[13] 鄧先瑞,聶雪媛,劉國平. WSNs下移動機器人HuberM-CKF離散濾波定位[J]. 計算機應(yīng)用研究,2016,33(6):1839-1842.

[14] 李陽銘,孟慶虎,梁華為,等. 基于粒子濾波的無線傳感器網(wǎng)絡(luò)輔助同步定位與地圖創(chuàng)建方法研究[J]. 機器人,2008,30(5):421-427,434.

[15] 金長江,張兵,曹祥杰. 基于容積卡爾曼濾波的異類多傳感器一致性融合算法[J]. 傳感技術(shù)學(xué)報,2016,29(7):1006-1010.

[16] 陳曉飛,凌有鑄,陳孟元. 移動機器人輔助下基于GM-的無線傳感器網(wǎng)絡(luò)節(jié)點定位研究[J]. 電子測量與儀器學(xué)報,2016,30(9):1298-1305.

Mobile Robot Localization Algorithm Based on Gaussian Mixture Consider Kalman Filter in WSNs Environment*

CHENXiaofei,LINGYouzhu*,CHENMengyuan

(Anhui Polytechnic University,Anhui Key Laboratory of Electric Drive and Control,Wuhu Anhui,241000,China)

Targeting localization of mobile robots,an optimized localization algorithm interweaved with Gaussian Mixture Consider Kalman Filter(GM-CKF)under Wireless Sensor Network(WSNs)is proposed in this article. The process is as follows. Combining the observation of mobile robots by WSNs,the observation of environmental characteristics by mobile robots themselves together with the amount of self control,a date fusion is conducted. With the aid of GM-CKF algorithm featuring threshold discriminant analysis and selective Gaussian Segmentation,the robots’estimated positions are predictively rectified so that calculated space dimension could be decreased and locational preciseness could be increased. The simulations are carried out to demonstrate that the method promoted in this article produces better preciseness which is 39.11% higher than that of the well-performed CKF and 65.81% higher than that of EKF,compared with the old self-localization one.

WSNs;GM-CKF;mobile robot;localisation

陳曉飛(1991-),男,安徽工程大學(xué)碩士研究生,目前主要研究方向為機器人、無線傳感器網(wǎng)絡(luò)、智能信息處理等,492486492@qq.com;凌有鑄(1962-),男,畢業(yè)于哈爾濱工業(yè)大學(xué),現(xiàn)任安徽工程大學(xué)教授,碩士生導(dǎo)師。目前主要研究方向為自動控制原理、移動機器人等; 陳孟元(1984-),男,畢業(yè)于安徽工程大學(xué),現(xiàn)任安徽工程大學(xué)講師。目前主要研究方向為嵌入式系統(tǒng)開發(fā)、圖像處理、傳感器信息融合及優(yōu)化等。

項目來源:2016年安徽高校自然科學(xué)研究項目(KJ2016A794);安徽工程大學(xué)研究生實踐與創(chuàng)新項目(Y040116006)

2016-08-21 修改日期:2016-11-10

TP393;TN92

A

1004-1699(2017)01-0133-06

C:7230

10.3969/j.issn.1004-1699.2017.01.024