靳果袁鑄

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

靳果袁鑄

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

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

移動機器人；擴展卡爾曼濾波；無跡卡爾曼濾波；定位算法改進；位置預測仿真

Class NumberTN609

1引言

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

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

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

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

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

2.1擴展卡爾曼濾波算法

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

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

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

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

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

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

2.2無跡卡爾曼濾波算法

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

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

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

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

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

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

2.3卡爾曼濾波算法的改進

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

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

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

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

3仿真結(jié)果

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

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

圖1 EKF算法位置預測仿真

圖2 UKF算法位置預測仿真

圖3 EKF算法定位誤差

圖4 UKF算法定位誤差

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

4結(jié)語

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

［1］陶敏，陳新，孫振平.移動機器人定位技術(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］郝晨，李航.擴展卡爾曼和無跡卡爾曼濾波應用對比研究［J］.沈陽師范大學學報：自然科學版，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）的應用比較分析［J］.西安文理學院學報：自然科學版，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］.計算機仿真，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］安雷，張國良，張維平，等.移動機器人擴展卡爾曼濾波定位與傳感器誤差建模［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)移動機器人定位技術(shù)研究［J］.中國測試，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］.農(nóng)業(yè)機械學報，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仿真組中世界模型的維護［C］.中國機器人大賽論文集，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］.計算機仿真，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］張凱淵，劉佩林，錢久超，等.多傳感器融合機器人室內(nèi)定位系統(tǒng)設(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］孟祥萍，張本法，苑全德.自適應擴展卡爾曼濾波器在移動機器人定位中的應用［J］.計算機系統(tǒ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).基于無跡卡爾曼濾波的機器人無線室內(nèi)定位估計算法［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)自主移動機器人定位方法［J］.華中科技大學學報（自然科學版），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］.太原科技大學學報，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日

國家自然科學基金項目（編號：71501153）；河南省基礎(chǔ)與前沿技術(shù)研究計劃（自然科學基金）（編號：122300410416）資助。

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