基于EKF和UKF的移动机器人定位算法优化与仿真∗

靳果袁铸

基于EKF和UKF的移动机器人定位算法优化与仿真∗

靳果袁铸

（河南工业职业技术学院南阳473000）

为了研究卡尔曼滤波算法在非线性系统中的定位预测效果，对扩展卡尔曼滤波算法和无迹卡尔曼滤波算法的应用结果做了分析对比，并且根据机器人的受力情况，在滤波算法中引入修正因子，对状态估计方程进行改进。仿真实验表明：无迹卡尔曼滤波算法在非线性系统中的定位效果优于扩展卡尔曼滤波算法；修正因子对两种算法都具有改进效果，提高了定位精度。

移动机器人；扩展卡尔曼滤波；无迹卡尔曼滤波；定位算法改进；位置预测仿真

Class NumberTN609

1引言

移动机器人定位技术是是机器人学的重要内容，是提高机器人自动化水平的关键环节［1］。移动机器人定位技术主要根据机器人当前位置情况和传感器观测值等信息，产生对机器人位姿的状态估计和误差计算。

移动机器人运动系统是非线性系统，主要采用扩展卡尔曼滤波（EKF）算法和无迹卡尔曼滤波（UKF）算法得到位置信息预测和滤波结果［2］。EKF通过泰勒展开将非线性滤波问题转换成线性滤波问题，从而将标准卡尔曼滤波算法应用到非线性系统中，EKF体现灵活性的同时也表现出了不足，比如，非线性系统函数的雅克比矩阵难以得到，强非线性系统应用EKF算法易产生发散问题等［3］；UKF通过无损变换（Unscented transform，UT）使非线性系统方程适用于线性系统下的标准卡尔曼滤波算法，UKF没有忽略泰勒展开的高阶项，有效克服了EKF精度低、稳定性差的缺陷，但是当系统噪声较大时UKF的滤波结果精确度较低［4］。

本文对EKF算法和UKF算法在移动机器人定位技术中的预测结果进行了比对研究，在此基础上，分析了机器人运动过程中的受力情况，引入修正因子对两种算法进行了优化，提高了定位精度，并且对三种算法——EKF算法、UKF算法和改进算法完成了仿真研究和对比。

2非线性系统滤波算法

普通卡尔曼滤波利用最小均方误差获得目标的状态估计，适用于线性系统，移动机器人运动过程属于非线性系统，其分析方法是将非线性关系进行线性近似，转化为线性系统处理，主要利用EKF——将非线性环节线性化处理，忽略高阶项和UKF——用采样方法近似非线性分布。

2.1扩展卡尔曼滤波算法

EKF算法以普通卡尔曼滤波算法为基础，将非线性的系统函数做泰勒展开，忽略高阶项，使原系统近似为线性系统，再利用经典卡尔曼滤波算法对线性化系统模型进行滤波［5～8］。

移动机器人的系统方程在EKF算法下可描述为

其中，X(k)为系统的状态向量，Z(k)为系统的观测向量，f、h为非线性系统系统方程，W(k)为状态噪声，V(k)为观测噪声，G为噪声输入矩阵。

状态估计和协方差的一步预测结果为

EKF增益以及最终的扩展EKF状态估计和协方差为

其中，A、H为非线性系统的雅可比行列式，由f、h求偏导得到。

2.2无迹卡尔曼滤波算法

UKF以UT变换为基础，结合卡尔曼线性滤波，利用无迹变换来处理均值和协方差的非线性传递，用确定的采样点来近似目标状态的后验概率密度，使非线性系统方程适用于线性假设下的普通卡尔曼滤波系统［9～12］。

移动机器人的系统方程在UKF算法下的描述形式同EKF算法，可用式（1）表示。因此，k时刻状态估计的Sigma点集为{Xi(k|k)}，则Sigma点的状态估计和观测预测结果为

系统的状态估计和状态估计协方差的一步预测结果为

系统的观测预测和观测预测协方差的一步预测结果为

由此，UKF增益以及最终的状态估计和状态估计协方差为

如果初速度为v0，积分可得t时刻的速度v(t)，对v(t)积分可得位移s(t)，即：

2.3卡尔曼滤波算法的改进

研究过程中发现，将移动机器人运动过程中的受力情况与滤波算法相结合，可以提高算法的精确度，降低累积误差［13～14］。因此，引入修正因子——机器人运动过程中所受外力，对EKF算法和UKF算法做出改进［15］。

根据力学和运动学观点，移动机器人受到的外力取决于自身产生的驱动力和地面施加的摩擦力，研究表明：1）摩擦力与速度为正比关系，二者方向相反，即：f=μv；2）驱动力与最大速度为正比关系，即F=μvmax；3）摩擦力等于驱动力时，速度稳定且达到最大值。

由此，移动机器人受到的外力N以及N所产生的加速度可表示为

将式（19）所得结果与滤波算法所得结果求均值，即得到改进后的机器人位置预测值。

3仿真结果

为了验证移动机器人运动过程中EKF算法和UKF算法的定位预测结果，以及修正因子对滤波算法的优化效果，分别对两种滤波算法和引入修正因子后的定位结果进行仿真研究。

假设移动机器人运动的二维平面为160m× 160m，初始位置为（0m，150m）附近，仿真结果如图1～图4所示：图1和图2所示为EKF算法和UKF算法的定位结果，图3和图4所示为EKF算法和UKF算法的误差对比。

图1 EKF算法位置预测仿真

图2 UKF算法位置预测仿真

图3 EKF算法定位误差

图4 UKF算法定位误差

可以看出，UKF算法比EKF算法效果更好，UKF结果精度较高，误差较小，而且修正因子的引入提高了定位精度，误差值大幅度降低，对UKF和EKF都有较好的优化效果。

4结语

本文对比研究了EKF算法和UKF算法在移动机器人运动系统中的应用效果，结果表明，EKF算法对机器人的定位预测具有良好的效果，误差值在可接受范围内，相比之下，UKF算法误差更小，精度更高，在一定程度上弥补了EKF算法忽略高阶项的缺点。研究发现，将卡尔曼滤波与机器人受力情况结合起来，在EKF算法和UKF算法中引入修正因子，可以进一步提高移动机器人的定位精度，仿真结果验证了改进方法的有效性。

［1］陶敏，陈新，孙振平.移动机器人定位技术［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］黄铫，张天骐，高清山，等.一种提高无迹卡尔曼滤波精确度的方法［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的室内移动机器人定位技术研究［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］解少博，林程.基于无迹卡尔曼滤波的车辆状态与参数估计［J］.农业机械学报，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］张凯渊，刘佩林，钱久超，等.多传感器融合机器人室内定位系统设计与实现［J］.信息技术，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］.计算机系统应用，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］赵中伟，戴文战.基于无迹卡尔曼滤波的机器人无线室内定位估计算法［J］.机电技术，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］高云峰，吕明睿，周伦，等.一种室内自主移动机器人定位方法［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）；河南省基础与前沿技术研究计划（自然科学基金）（编号：122300410416）资助。

靳果，男，硕士，助教，研究方向：电子信息。袁铸，男，硕士，讲师，研究方向：电子信息。