摘 要:定位是移动机器人最基本的问题之一。应用了迭代卡尔曼滤波(iekf)集成航位推算和全局观测信息,解决机器人的定位问题。该方法在卡尔曼滤波测量更新阶段,多次迭代计算估计状态,直到误差小于一定的阈值。减少了由于泰勒展开的截断带来的定位误差,使得算法的收敛稳定性增强。最后通过仿真实验与ekf方法比较。结果表明,iekf在移动机器人定位中是一种有效的方法。 关键词:迭代卡尔曼滤波; 误差分析; 移动机器人; 机器人定位
application of iterated kalman filtering in robot localization
long hui1, hu li2, zhou yan-yu3
(1.hunan biological and electromechanical polytechnic, changsha 410126, china;2.hunan intelligent control system company, changsha 410001, china;
3.college of information engineering, central south university, changsha 410075, china)
abstract: localization is one of the most fundamental problem in mobile robots. the localization problem was solved by iterated extended kalman filtering (iekf) by combining the reckon reference and global observation information. in the measurement update phase of kalman filtering, the state estimation is iterated many times until estimation error is lower than the threshold and localization error aroused from taylor expand is reduced effectively. the convergent stability was improved by the method. finally, iekf and ekf methods are compared. the simulation result shows that iekf is an effective method in localization of mobile robots.keywords: iterated kalman filtering; error analysis; mobile robots; robot localization
0 引 言
定位是自主机器人导航当中最基本的问题之一[1],对于一个自主机器人系统,精确的空间定位是其实现自主导航的前提。Www.11665.CoMleonard和 durrant-whyte 把导航问题归结为3个子问题[2],包括回答“我在哪里?”,“我要去哪里?” 和“我怎样到达那里?”。其中第一个问题就是定位问题。如果机器人没有获取自身的位置,就无法进行后续的目标识别和路径规划。现有大多数定位方法的关键是通过融合内部传感器的测量信息与外部传感器的测量信息,以减少过程噪声和传感器的测量误差,得到更精确的机器人位置信息。
kalman滤波[3]是一种线性无偏最小方差估计,采用时间递推得到的,模型简单,数据存储量小,特别适用于计算机应用,已被广泛应用于航天、航空、航海、系统工程、通信等多种领域状态的估计和控制中。
1 问题描述
在二维空间,移动机器人的位置可以用3个变量表示(x,y,θ)[4],其中x,y和θ分别表示机器人的位置坐标和方位角。机器人具有的运动模型如下:
x(k+1)=f[x(k),u(k),w(k)]
=x(k)y(k)θ(k)+δd(k)cos[θ(k)+δθ(k)/2]
δd(k)sin[θ(k)+δθ(k)/2]
δθ(k)(1)
z(k)=h[x(k),v(k)](2)
式中:x(k)为系统在k 时刻状态; z(k)为k 时刻状态变量的观测量; u(k)=δd(k)
δθ(k)为系统的控制输入;δd和δθ为k到k+1时刻机器人的相对位移和运动方向;w(k)和v(k)分别为高斯加性过程噪声和测量噪声,且具有如下的统计特性:
e[v(k)]=e[w(k)]=0;
e[v(k)vt(j)]=q(k)δ(k); j=k
e[w(k)wt(j)]=r(k)δ(k); j=k
e[v(k)wt(j)]=0(3)
机器人运动过程和观测过程均受到随机高斯白噪声干扰,在此的目的是根据系统方程和观测方程,并结合噪声统计特性及给定的初始位置估计机器人的位置,机器人位置如图1所示。
图1 机器人位置示意图
2 迭代扩展卡尔曼滤波定位
2.1 扩展卡尔曼滤波
卡尔曼滤波是由r.e. kalman于1960 年提出的,它是一种简单有效的线性最优递推估计算法,可估计带噪声的线型动态系统状态。然而,假设卡尔曼滤波器是线性系统,但实际上机器人的运动模型与观测模型是非线性的。因此通常采用扩展卡尔曼滤波器(extended kalman filter,ekf),并通过一阶泰勒展开来近似非线性模型。依据curran [5]综合航位推算、超声和红外传感器数据,提出了基于ekf的定位和运动控制算法。为了简化定位算法,jensfelt和christensen[6]应用加权霍夫变换算法提取了环境特征,采用最小化环境特征模型和激光雷达数据,建立了基于ekf的位姿跟踪算法。基于ekf的滤波算法分为时间更新和测量更新2个阶段,具体步骤如下:
(1) 初始化。给定机器人初始位置和协方差矩阵:
+0=[x(0),y(0),θ(0)]t;p(0)+
(2) 时间更新。
当k≥1,则:
(k)-= f[(k-1)+,u(k)]
p(k)- =axp(k-1)atx+bup(k-1)btu + q(k)
ax=fxx=x(k)+,bx=fuu=u(k)
(3) 测量更新:
(k)+=(k)-+k(k)[z(k)-(k)]
=h[x(k)]
p(k)+=[i-k(k)h(k)]p(k)-
其中:卡尔曼增益:
k(k)=p(k)-h(k)t[h(k)p(k)-h(k)t+
r(k))-1h(k)=h(x)xx=x(k)+
(4) 重复第(2)和第(3)步。
算法中采用矩阵a,b,h系统运动方程和测量方程对状态和输入求偏导数的雅克比矩阵。
2.2 迭代扩展卡尔曼滤波定位
由于ekf舍去了高阶无穷小,在非线性误差比较严重时,会带来估计的不准确。针对ekf线性化存在的误差,很多学者做了大量研究,如迭代ekf算法[7]、基于uscent变换的uscent kalman filter(ukf)算法、中心差分卡尔曼滤波算法、粒子滤波和upf滤波以及相应的改进算法[8]等。
其中,迭代ekf[9]算法在原有ekf基础上,对量测更新的状态估计值采用多步迭代,从而减少由于泰勒展开带来的截断误差。在此应用iekf方法实现移动机器人运行状态的位置。基于上述思想,在任意时刻计算k迭代估计值的步骤如下[10]:
(1) 将x(k)的一步预测状态向量(k)-作为迭代的初始值x(0)(k),依据观测方程线性化,计算滤波值与扩展卡尔曼滤波相同,并把结果(k)作为x(1)(k)。
(2) 在x(1)(k)处重新将观测方程线性化,整个滤波过程重复一遍,新估计值(k)+作为x(2)(k)。
(3) 迭代过程中,当两步状态差值满足式(4):
|i+1j(k)-ij(k)|≤ε, j = 1,2,…,n(4)
迭代终止,否则返回步骤(2)。其中ε是预先给定的迭代阈值。
3 仿真分析
假设机器人按正方形轨迹运动,起点位置(100,100),则机器人首先沿着x轴正方向运动,每步10 cm,逆时针方向作圆周运动。为了真实地反映出kalman滤波的效果,采用monte-carlo方法,以及多次实验取均值的方法进行研究,可以计算出估计的误差均值和方差,其表达式为:
x(k)=1m∑mi=1[xi(k)-i(k|k)](5)
式中:m为monte-carlo仿真的次数;k为取样点数。仿真的次数越多,实验的效果越接近于实际,但计算的速度明显变慢,因此在仿真时,需要根据实际适当选取。在本程序中,取m=50。
测量噪声v(k)为高斯白噪声(记为wgn),v(k)～n(0,10-5 )。 图2为机器人原始运行轨迹和ekf方法、iekf估计值轨迹。从图2中可以看出,iekf运行轨迹更接近真实轨迹。图3是采用ekf和iekf方法y沿轴方向估计误差值的均值随着采样点数的增多而发生变化的情况。
由仿真结果可看出,执行iekf算法相对于ekf算法效果更好。 这主要归功于状态估计的多步迭代,在一定程度上修正了由于泰勒展开引起的截断误差,使得算法收敛的稳定性得到增强。
图2 iekf机器人的原始轨迹、观测数据和估计值轨迹
图3 目标跟踪误差曲线
4 结 语
在此,针对扩展卡尔曼滤波算法精度较低的缺点,提出了对算法测量更新阶段的状态估计值,并采用多步迭代更新,使其在满足一定阈值时,迭代结束。该算法一定程度上减小了局部线性化带来的截断误差。仿真结果与ekf算法进行了对比,结果表明该算法的滤波精度有明显提高。
参考文献
[1]王志文,郭戈.移动机器人导航技术现状与展望[j].机器人,2003,25(5):470-474.
[2]leonard j, durrant-whyte h.mobile robot localizaiton by tracking deometric beacons[j]. ieee transactions on robotics and automation, 1991,7(3): 376-382.
[3]negenborn rudy.robot localization and kalman filters on finding your position in a noisy world[d]. holland: utrecht university, 2003.
[4]seyr martin, jakubed stefan.mobile robot predictive trajectory tracking[c]. icinco. [s.l.]: icinco, 2005: 112-119.
[5]currant a, kyriakopoulos k j.sensor-based self localization forwheeled mobile robots[j].journal of robotic systems,1995, 12 (3): 163-176.
[6]jensfelt p, christensen h i.pose tracking using laser scanning and minimalistic environmental models [j].ieee transaction on robotics and automation,2001, 17(2): 138-147.
[7]sibley g, sukhatme g, matthies l.the iterated sigma point kalman filter with applications to long range stereo[c]// proceedings of robotics science and systems.philadelphia, usa: mit press, 2006: 263-270.
[8]wang x, zhang h.a upf-ukf framework for slam[c]// proceedings of ieee international conference on robotics and automation.roma, italy: ieee, 2007: 1664-1669.
[9]郭杭.迭代扩展卡尔曼滤波用于实时gps数据处理[j].武汉测绘科技大学学报,1999,24(2):112-114,123.
[10]高为广,潘娜娜,张晓东.改进的迭代ekf算法在伪卫星定位中的应用[j].测绘科学,2008,33(4):73-75.
