靳果袁鑄

基于EKF和UKF的移動(dòng)機(jī)器人定位算法優(yōu)化與仿真?

靳果袁鑄

（河南工業(yè)職業(yè)技術(shù)學(xué)院南陽473000）

為了研究卡爾曼濾波算法在非線性系統(tǒng)中的定位預(yù)測(cè)效果，對(duì)擴(kuò)展卡爾曼濾波算法和無跡卡爾曼濾波算法的應(yīng)用結(jié)果做了分析對(duì)比，并且根據(jù)機(jī)器人的受力情況，在濾波算法中引入修正因子，對(duì)狀態(tài)估計(jì)方程進(jìn)行改進(jìn)。仿真實(shí)驗(yàn)表明：無跡卡爾曼濾波算法在非線性系統(tǒng)中的定位效果優(yōu)于擴(kuò)展卡爾曼濾波算法；修正因子對(duì)兩種算法都具有改進(jìn)效果，提高了定位精度。

移動(dòng)機(jī)器人；擴(kuò)展卡爾曼濾波；無跡卡爾曼濾波；定位算法改進(jìn)；位置預(yù)測(cè)仿真

Class NumberTN609

1引言

移動(dòng)機(jī)器人定位技術(shù)是是機(jī)器人學(xué)的重要內(nèi)容，是提高機(jī)器人自動(dòng)化水平的關(guān)鍵環(huán)節(jié)［1］。移動(dòng)機(jī)器人定位技術(shù)主要根據(jù)機(jī)器人當(dāng)前位置情況和傳感器觀測(cè)值等信息，產(chǎn)生對(duì)機(jī)器人位姿的狀態(tài)估計(jì)和誤差計(jì)算。

移動(dòng)機(jī)器人運(yùn)動(dòng)系統(tǒng)是非線性系統(tǒng)，主要采用擴(kuò)展卡爾曼濾波（EKF）算法和無跡卡爾曼濾波（UKF）算法得到位置信息預(yù)測(cè)和濾波結(jié)果［2］。EKF通過泰勒展開將非線性濾波問題轉(zhuǎn)換成線性濾波問題，從而將標(biāo)準(zhǔn)卡爾曼濾波算法應(yīng)用到非線性系統(tǒng)中，EKF體現(xiàn)靈活性的同時(shí)也表現(xiàn)出了不足，比如，非線性系統(tǒng)函數(shù)的雅克比矩陣難以得到，強(qiáng)非線性系統(tǒng)應(yīng)用EKF算法易產(chǎn)生發(fā)散問題等［3］；UKF通過無損變換（Unscented transform，UT）使非線性系統(tǒng)方程適用于線性系統(tǒng)下的標(biāo)準(zhǔn)卡爾曼濾波算法，UKF沒有忽略泰勒展開的高階項(xiàng)，有效克服了EKF精度低、穩(wěn)定性差的缺陷，但是當(dāng)系統(tǒng)噪聲較大時(shí)UKF的濾波結(jié)果精確度較低［4］。

本文對(duì)EKF算法和UKF算法在移動(dòng)機(jī)器人定位技術(shù)中的預(yù)測(cè)結(jié)果進(jìn)行了比對(duì)研究，在此基礎(chǔ)上，分析了機(jī)器人運(yùn)動(dòng)過程中的受力情況，引入修正因子對(duì)兩種算法進(jìn)行了優(yōu)化，提高了定位精度，并且對(duì)三種算法——EKF算法、UKF算法和改進(jìn)算法完成了仿真研究和對(duì)比。

2非線性系統(tǒng)濾波算法

普通卡爾曼濾波利用最小均方誤差獲得目標(biāo)的狀態(tài)估計(jì)，適用于線性系統(tǒng)，移動(dòng)機(jī)器人運(yùn)動(dòng)過程屬于非線性系統(tǒng)，其分析方法是將非線性關(guān)系進(jìn)行線性近似，轉(zhuǎn)化為線性系統(tǒng)處理，主要利用EKF——將非線性環(huán)節(jié)線性化處理，忽略高階項(xiàng)和UKF——用采樣方法近似非線性分布。

2.1擴(kuò)展卡爾曼濾波算法

EKF算法以普通卡爾曼濾波算法為基礎(chǔ)，將非線性的系統(tǒng)函數(shù)做泰勒展開，忽略高階項(xiàng)，使原系統(tǒng)近似為線性系統(tǒng)，再利用經(jīng)典卡爾曼濾波算法對(duì)線性化系統(tǒng)模型進(jìn)行濾波［5～8］。

移動(dòng)機(jī)器人的系統(tǒng)方程在EKF算法下可描述為

其中，X(k)為系統(tǒng)的狀態(tài)向量，Z(k)為系統(tǒng)的觀測(cè)向量，f、h為非線性系統(tǒng)系統(tǒng)方程，W(k)為狀態(tài)噪聲，V(k)為觀測(cè)噪聲，G為噪聲輸入矩陣。

狀態(tài)估計(jì)和協(xié)方差的一步預(yù)測(cè)結(jié)果為

EKF增益以及最終的擴(kuò)展EKF狀態(tài)估計(jì)和協(xié)方差為

其中，A、H為非線性系統(tǒng)的雅可比行列式，由f、h求偏導(dǎo)得到。

2.2無跡卡爾曼濾波算法

UKF以UT變換為基礎(chǔ)，結(jié)合卡爾曼線性濾波，利用無跡變換來處理均值和協(xié)方差的非線性傳遞，用確定的采樣點(diǎn)來近似目標(biāo)狀態(tài)的后驗(yàn)概率密度，使非線性系統(tǒng)方程適用于線性假設(shè)下的普通卡爾曼濾波系統(tǒng)［9～12］。

移動(dòng)機(jī)器人的系統(tǒng)方程在UKF算法下的描述形式同EKF算法，可用式（1）表示。因此，k時(shí)刻狀態(tài)估計(jì)的Sigma點(diǎn)集為{Xi(k|k)}，則Sigma點(diǎn)的狀態(tài)估計(jì)和觀測(cè)預(yù)測(cè)結(jié)果為

系統(tǒng)的狀態(tài)估計(jì)和狀態(tài)估計(jì)協(xié)方差的一步預(yù)測(cè)結(jié)果為

系統(tǒng)的觀測(cè)預(yù)測(cè)和觀測(cè)預(yù)測(cè)協(xié)方差的一步預(yù)測(cè)結(jié)果為

由此，UKF增益以及最終的狀態(tài)估計(jì)和狀態(tài)估計(jì)協(xié)方差為

如果初速度為v0，積分可得t時(shí)刻的速度v(t)，對(duì)v(t)積分可得位移s(t)，即：

2.3卡爾曼濾波算法的改進(jìn)

研究過程中發(fā)現(xiàn)，將移動(dòng)機(jī)器人運(yùn)動(dòng)過程中的受力情況與濾波算法相結(jié)合，可以提高算法的精確度，降低累積誤差［13～14］。因此，引入修正因子——機(jī)器人運(yùn)動(dòng)過程中所受外力，對(duì)EKF算法和UKF算法做出改進(jìn)［15］。

根據(jù)力學(xué)和運(yùn)動(dòng)學(xué)觀點(diǎn)，移動(dòng)機(jī)器人受到的外力取決于自身產(chǎn)生的驅(qū)動(dòng)力和地面施加的摩擦力，研究表明：1）摩擦力與速度為正比關(guān)系，二者方向相反，即：f=μv；2）驅(qū)動(dòng)力與最大速度為正比關(guān)系，即F=μvmax；3）摩擦力等于驅(qū)動(dòng)力時(shí)，速度穩(wěn)定且達(dá)到最大值。

由此，移動(dòng)機(jī)器人受到的外力N以及N所產(chǎn)生的加速度可表示為

將式（19）所得結(jié)果與濾波算法所得結(jié)果求均值，即得到改進(jìn)后的機(jī)器人位置預(yù)測(cè)值。

3仿真結(jié)果

為了驗(yàn)證移動(dòng)機(jī)器人運(yùn)動(dòng)過程中EKF算法和UKF算法的定位預(yù)測(cè)結(jié)果，以及修正因子對(duì)濾波算法的優(yōu)化效果，分別對(duì)兩種濾波算法和引入修正因子后的定位結(jié)果進(jìn)行仿真研究。

假設(shè)移動(dòng)機(jī)器人運(yùn)動(dòng)的二維平面為160m× 160m，初始位置為（0m，150m）附近，仿真結(jié)果如圖1～圖4所示：圖1和圖2所示為EKF算法和UKF算法的定位結(jié)果，圖3和圖4所示為EKF算法和UKF算法的誤差對(duì)比。

圖1 EKF算法位置預(yù)測(cè)仿真

圖2 UKF算法位置預(yù)測(cè)仿真

圖3 EKF算法定位誤差

圖4 UKF算法定位誤差

可以看出，UKF算法比EKF算法效果更好，UKF結(jié)果精度較高，誤差較小，而且修正因子的引入提高了定位精度，誤差值大幅度降低，對(duì)UKF和EKF都有較好的優(yōu)化效果。

4結(jié)語

本文對(duì)比研究了EKF算法和UKF算法在移動(dòng)機(jī)器人運(yùn)動(dòng)系統(tǒng)中的應(yīng)用效果，結(jié)果表明，EKF算法對(duì)機(jī)器人的定位預(yù)測(cè)具有良好的效果，誤差值在可接受范圍內(nèi)，相比之下，UKF算法誤差更小，精度更高，在一定程度上彌補(bǔ)了EKF算法忽略高階項(xiàng)的缺點(diǎn)。研究發(fā)現(xiàn)，將卡爾曼濾波與機(jī)器人受力情況結(jié)合起來，在EKF算法和UKF算法中引入修正因子，可以進(jìn)一步提高移動(dòng)機(jī)器人的定位精度，仿真結(jié)果驗(yàn)證了改進(jìn)方法的有效性。

［1］陶敏，陳新，孫振平.移動(dòng)機(jī)器人定位技術(shù)［J］.火力與控制，2010，35（7）：169-172.

TAO Min，CHEN Xin，SUN Zhenping.Positioning Tech?niques of Mobile Robot［J］.Fire Control＆Command Con?trol，2010，35（7）：169-172.

［2］郝晨，李航.擴(kuò)展卡爾曼和無跡卡爾曼濾波應(yīng)用對(duì)比研究［J］.沈陽師范大學(xué)學(xué)報(bào)：自然科學(xué)版，2015，33（2）：279-283.

HAO Chen，LIHang.Comparation ofextended Kalman fil?ter and unscented Kalman filter［J］.Journal of Shenyang Normal University：NatSci Ed，2015，33（2）：279-283.

［3］許迅雷.無跡卡爾曼濾波（UKF）的應(yīng)用比較分析［J］.西安文理學(xué)院學(xué)報(bào)：自然科學(xué)版，2011，14（2）：57-60.

XU Xun-lei.A Comparative Analysis of Unscented Kal?man Filter（UKF）［J］.Journal of Xi'an University of Arts &Science：Natural Science Edition，2011，14（2）：57-60.

［4］黃銚，張?zhí)祢U，高清山，等.一種提高無跡卡爾曼濾波精確度的方法［J］.計(jì)算機(jī)仿真，2010，27（3）：348-352.

HUANG Yao，ZHANG Tianqi，GAO Qingsha，et al.A Method Improving the Accuracy of UKF［J］.Computer Simulation，2010，27（3）：348-352.

［5］安雷，張國(guó)良，張維平，等.移動(dòng)機(jī)器人擴(kuò)展卡爾曼濾波定位與傳感器誤差建模［J］.信息與控制，2012，41（4）：406-412.

AN Lei，ZHANG Guoliang，ZHANG Weiping，et al.EKF Localization and Sensor Error Modeling for Mobile Robots［J］.Information and Control，2012，41（4）：406-412.

［6］Z.X.Ma，Z.Y.Liu，etc.Application ofadaptive Kalman Fil?tering based on Fuzzy Logic to the Integrated GPS/INS Navigation［J］.Information and Control，2006，25（4）：457-461.

［7］陳朋，陳偉海，于守謙.基于UKF的室內(nèi)移動(dòng)機(jī)器人定位技術(shù)研究［J］.中國(guó)測(cè)試，2011，37（5）：1-5.

CHEN Peng，CHEN Weihai，YU Shouqian.Research oflo?calization for indoor mobile robot based on unscented Kal?man filter［J］.China Measurement&Testing，2011，37（5）：1-5.

［8］解少博，林程.基于無跡卡爾曼濾波的車輛狀態(tài)與參數(shù)估計(jì)［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2011，42（12）：6-12.

XIE Shaobo，LIN Cheng.State and Parameters Estimation of Vehicle Based on UKF［J］.Transactions of the Chinese Society of Agricultural Machinery，2011，42（12）：6-12.

［9］劉汝佳，孫增圻.RoboCup3D仿真組中世界模型的維護(hù)［C］.中國(guó)機(jī)器人大賽論文集，2005.

LIU Rujia，SUN Zengqi.Maintaining World Model in Ro?boCup 3D Simulation League［J］.Proceedings of the Chi?nese robotics contest，2005.

［10］張青，李龍澍，劉躍.基于卡爾曼濾波的仿真機(jī)器人定位方法［J］.計(jì)算機(jī)仿真，2013，30（12）：317-320.

LI Longshu，ZHANG Qing，LIU Yue.Simulation Robot Location Method Based On Kalman Filter［J］.Computer Simulation，2013，30（12）：317-320.

［11］張凱淵，劉佩林，錢久超，等.多傳感器融合機(jī)器人室內(nèi)定位系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)［J］.信息技術(shù)，2014，38（11）：83-87.

ZHANG Kaiyuan，LIU Peilin，QIAN Jiuchao，et al.De?sign and implementation of multi-sensor fused robot in?door localization system［J］.Information Technology，2014，38（11）：83-87.

［12］孟祥萍，張本法，苑全德.自適應(yīng)擴(kuò)展卡爾曼濾波器在移動(dòng)機(jī)器人定位中的應(yīng)用［J］.計(jì)算機(jī)系統(tǒng)應(yīng)用，2015，24（12）：176-181.

MENG Xiangping，ZHANG Benfa，YUAN Quande.Adap?tive Extended Kalman Filter in the Application ofthe Mo?bile Robot Localization［J］.Computer Systems&Appli?cations，2015，24（12）：176-181.

［13］趙中偉，戴文戰(zhàn).基于無跡卡爾曼濾波的機(jī)器人無線室內(nèi)定位估計(jì)算法［J］.機(jī)電技術(shù)，2015（5）：2-5.

ZHAO Zhongwei，DAI Wenzhan.Wireless indoor loca?tion estimation algorithm for robot based on Unscented Calman filter［J］.Mechanical and Electrical Technology，2015（5）：2-5.

［14］高云峰，呂明睿，周倫，等.一種室內(nèi)自主移動(dòng)機(jī)器人定位方法［J］.華中科技大學(xué)學(xué)報(bào)（自然科學(xué)版），2013（1）：245-248.

GAO Yunfeng，LüMingrui，ZHOU Lun，et al.Localiza?tion method in indoor environment for autonomous mo?bile robot［J］.Journal of Huazhong University of Science and Technology.Nature Science，2013（1）：245-248.

［15］謝朦，陳志梅，邵雪卷.基于RSSI的改進(jìn)四邊測(cè)距機(jī)器人定位算法［J］.太原科技大學(xué)學(xué)報(bào)，2015，36（1）：18-23.

XIE Meng，CHEN Zhimei，SHAO Xuejuan.An Improved Quadrilateral Localization Algorithm of Robot Based on RSSI.Journalof Taiyuan University of Science and Tech?nology，2015，36（1）：18-23.

Optimization and Simulation of Mobile Robot Localization Algorithm Based on EKF and UKF

JIN Guo YUAN Zhu
（Henan Polytechnic Institute，Nanyang 473000）

In order to study the location prediction effect of Kalman filter algorithm in nonlinear system，the application re?sults ofthe extended Kalman filter algorithm and the unscented Kalman filter algorithm are analyzed and compared and according to the force condition ofthe mobile robot，the modificatory factor is introduced into the localization algorithm to improve the state esti?mation equation.The simulation results show that the location prediction effect of the unscented Kalman filter algorithm is better than thatofextended Calman filter algorithm in nonlinear system and the modificatory factor produces an improvementeffecton both algorithms.

mobile robot，extended Kalman filtering，unscented Kalman filter，localization algorithm improvement，loca?tion prediction simulation

TN609

10.3969/j.issn.1672-9722.2017.08.005

2017年3月11日，

2017年4月21日

國(guó)家自然科學(xué)基金項(xiàng)目（編號(hào)：71501153）；河南省基礎(chǔ)與前沿技術(shù)研究計(jì)劃（自然科學(xué)基金）（編號(hào)：122300410416）資助。

靳果，男，碩士，助教，研究方向：電子信息。袁鑄，男，碩士，講師，研究方向：電子信息。