激光传感器融合的ekf定位算法研究

激光传感器融合的ekf定位算法研究

0位置传感器的限制移动机器人的定位是机器人学研究的重点和热点。在机器人的即时定位中,单纯依靠里程计的航位推算法显然不合适,由于里程计存在累计误差,随着机器人运动时间的推移,最终将导致定位失败。一般是借助外部传感器,如激光传感器和视觉传感器等,在扩展卡尔曼滤波器(EKF)的框架下对传感器数据进行融合,从而获得机器人的位姿信息。但是EKF也存在由非线性系统线性化带来计算精度的下降,甚至造成滤波器的不稳定。同时推导Jacobian矩阵也是比较困难的,有时难以实现。针对EKF滤波器的不足,文献在UT变换的基础上提出了Unscented卡尔曼滤波器(UKF),UKF直接利用非线性系统模型,无须线性化,从而提高了滤波精度。同时UKF不需要推导复杂的Jacobian矩阵,易于计算机编程。本文针对室内结构化环境下移动机器人的运动,提取室内环境中的线段特征,在UKF的大框架下对里程计数据和激光传感器数据进行融合,从而获得高精度的移动机器人的位姿。1段特征的提取本文针对室内结构化环境,对激光数据采用哈夫变换进行分簇处理,将每个簇得数据用最小二乘法提取环境中的线段特征。1.1局部直线检测如图1所示,变换(HoughTransform)是模式识别领域中对二值图像进行直线检测的有效方法,它检测已知点的共线性,是一种全局性的检测方法。可以很好地抑制干扰和噪声,同时还可以将已知数据点集拟合成多条直线。在标准参数化方式下,平面直角坐标系中的直线l表达为ρ=xcosθ+ysinθ(ρ≥0,0≤θ<2π)(1)其中,ρ和θ含义如图1所示。根据(1)式,直线l上不同的点在参数空间中被变换为一簇相交于点p点的正弦曲线。显然,若能确定参数空间中的p点(局部最大值),就实现了直线检测。首先将ρ和θ离散化,由(1)式计算出每一个数据点相应的(ρ,θ)。根据哈夫变换得投票机制,可获得局部最大值,即数据点所在直线的参数。如图2所示,对激光传感器采集的一帧数据进行哈夫变换处理,可将数据点集分成AB、BC、CD3个簇,每个簇代表一条线段。1.2乘意义下的品变拟合经上述方法处理以后的数据就可以进行线段拟合了。这里将(ri,φi)转化为直角坐标(xi,yi),其中,xi=ricosφi,yi=risinφi,并假设各数据点具有相同的不确定性。文献对此做了详细的研究,给出了最小二乘意义下的线段特征及其协方差矩阵,αl、ρl可以表示为αl=12arctan(ba-c)-π2(2)ρl=ˉxcosαl+ˉysinαl(3)其中,ˉx=1ΝΝ∑i=1xi;ˉy=1ΝΝ∑i=1yi;a=Ν∑i=1(xi-ˉx)2;b=2Ν∑i=1(xi-ˉx)(yi-ˉy);c=Ν∑i=1(yi-ˉy)2。线段参数的协方差矩阵为Ρl=[σ2ρρσ2ρασ2αρσ2αα]=aσ2yy-bσ2xy+cσ2xx(a-c)2+b2[e2-e-e1]+[σ2yycos2ψ+σ2xxsin2ψ-2σ2xysinψcosψΝ000](4)其中,ψ=αl+π/2;e=ˉxcosψ+ˉysinψ;σxy=0;σxx=σyy=σr;σr为激光传感器数据的均方差。2机器人运动系统的建模2.1两组k、k的距离设机器人在k时刻的全局位姿为(xk,yk,θk)T,uk=(δk,Δθk)T为k时刻的输入控制量,其中,δk为机器人在k时刻移动的距离,Δθk为机器人在k时刻旋转的角度。对于两轮差分驱动,δk=(δrk+δlk)/2,Δθk=(δrk-δlk)/d,其中,δrk和δlk分别为右轮和左轮移动的距离;d为两轮之间的距离。则里程计模型可表示为Xk+1=F(Xk,uk)+vk=(xk+δkcos(θk+Δθk)yk+δksin(θk+Δθk)θk+Δθk)+vk(5)很显然,里程计模型是一个非线性系统,vk为高斯白噪声。2.2全局使用拟合设Yk=(ρl,αl)为k时刻直线在机器人坐标系下的特征参数。Gk为k时刻全局环境模型,其中包含c条线段。则由图3的几何关系可得观测方程为Yk,i=Η[Xk,Gk]+nk=(ρi,g-√x2k+y2kcos(βi-αi,g)αi,g-θk)+nk(i=1,⋯,c)(6)其中,(ρg,αg)为直线在全局坐标系下的特征参数;nk是高斯白噪声。3基于ukf的机器人定位3.1求解jacab1针对一个非线性模型,EKF将非线性模型用泰勒展开式展开,因此EKF的估计值实际上是一阶近似的。由于舍去了二阶项和高阶项,一阶线性近似就有可能将较大的误差引入到滤波后随机变量的实际均值和协方差矩阵中去,而且推导Jacobian矩阵也是比较困难的,有时难以实现。针对EKF在非线性系统中存在的不足,文献提出了Unscented卡尔曼滤波器(UKF),并成功地应用于非线性系统。UKF是以UT变换为基础的,UT变换是一种计算随机变量非线性变换的统计量的方法。设x是一个L维随机变量,其均值和协方差分别为ˉx和Pxx,y是另一随机变量,它与x成非线性关系:y=f(x)。UT变换就是选择一些点,使得其样本均值和协方差矩阵为ˉx和Pxx,将这些点带入到非线性函数中得到变换后的点,并用变换后的点的均值和协方差矩阵来估计ˉy和Pyy。3.2机器人位姿预测(1)初始化。计算公式如下:ˆX0|0=E[X0](7)Ρ0=E[(X0-ˆX0|0)(X0-ˆX0|0)Τ](8)(2)对于给定的ˆXk-1|k-1、Pk-1|k-1,用UT法求状态一步预测ˆXk|k-1,以及预测误差的协方差阵Pk|k-1。计算σ点ε(i)k-1|k-1,i=0,…,2L,即ε(0)k-1|k-1=ˆXk-1|k-1(9)ε(i)k-1|k-1=ˆXk-1|k-1+(√(L+λ)Ρk-1|k-1)i(i=1,⋯,L)(10)ε(i)k-1|k-1=ˆXk-1|k-1-(√(L+λ)Ρk-1|k-1)i(i=L+1,⋯,2L)(11)Wm0=λ/(L+λ)(12)W(c)0=λ/(L+λ)+(1-α2+β)(13)Wmi=W(c)i=1/[2(L+λ)](i=1,⋯‚2L)(14)其中,λ=α2(L+κ)-L,α决定ˉx周围采样点分布的远近程度,κ是比例因子,一般为零,β包含x的先验分布信息。通常令α=0.1,κ=0,β=2。对上述σ点,根据(5)式的里程计模型对k时刻系统的状态进一步预测。X(i)k=F(X(i)k-1|k-1)+vk-1(15)ˆXk|k-1=2L∑i=0WmiX(i)k(16)Ρk|k-1=2L∑i=0W(c)i(X(i)k-ˆXk|k-1)×(X(i)k-ˆXk|k-1)Τ+Ρvk(17)(3)用UT求σ点ˆXk|k-1,Ρk|k-1通过量测方程的传播。由(9)～(11)式计算σ点ε(i)k;然后进行观测方程预测:设k时刻全局地图共有c条线段,根据(6)式的观测方程对全局地图中第j条线段,在k时刻机器人位姿下的特征参数进一步预测。Y(i)k=H(ε(i)k)+nk(i=0,…,2L)(18)ˆYk=2L∑i=0WmiY(i)k(i=0,⋯‚2L)(19)ΡjˆYkˆYk=2L∑i=0Wci(Y(i)k,j-ˆYk,j)(Y(i)k,j-ˆYk,j)Τ+Ρn(20)ΡjˆXkˆYk=2L∑i=0Wci(X(i)k,j-ˆXk|k-1)(Y(i)k,j-ˆYk,j)Τ(j=1,⋯,c)(21)(4)特征匹配。特征匹配是将系统观测模型预测的线段特征,与激光传感器得到的局部地图的实际线段特征进行匹配。为了减少假匹配,只对长度大于30cm的线段进行匹配。同时为了获取统计意义上的最佳匹配对,可以计算预测线段特征ˆYk,j与实测线段特征Yik之间的Mahalanobis距离,即d(Yik,ˆYjk,j)=(Yik-ˆYk,j)Τ(Ρil+ΡjˆYkˆYk)-1×(Yik-ˆYk,j)(22)其中,Pil为第i条实测线段特征的协方差矩阵。由于Mahalanobis距离服从χ2α(n)分布,这里n=2,在置信度α为95%的条件下,2线段特征的匹配条件为d(Yik,ˆYjk,j)<5.991。(5)机器人位姿更新。设由第4步特征匹配获得的匹配线段的数目为m条,设ˆX0k=ˆXk|k-1,Ρ0k=Ρk|k-1,对匹配成功的m条线段(j=1,…,m)按照下列各式进行迭代,对机器人的位姿进行修正,即Κjk=ΡjˆXkˆYkΡjˆYkˆYk-1(23)ˆXjk=ˆXj-1k+Κjk(Yjk-ˆYjk)(24)Ρjk=Ρj-1k-ΚjkΡjˆYkˆYk(Κjk)Τ(25)当迭代过程结束时,机器人在k时刻的最终位姿为ˆXk=ˆXmk,Ρk=Ρmk。(6)全局环境模型更新。在机器人位姿更新以后,需要对(6)式中的全局环境模型进行更新。对于步骤(4)中满足匹配条件的2条线段,按照下式进行特征参数的合并,即Y^k=Ρm((Ρlj)-1Ykj+(ΡY^kY^kj)-1Y^kj)(j=1,⋯,m)(26)Ρm=((Ρlj)-1+(ΡY^kY^kj)-1)-1(j=1,⋯,m)(27)然后按照(28)式和(29)式将线段的特征参数转换到全局环境模型中。ρg=ρl+(xk,yk)(cosθk-sinθksinθkcosθk)(cosαlsinαl)(28)αg=αl+θk(29)对于不满足匹配条件的局部特征线段,则直接按照(28)式和(29)式转换到全局地图中,从而完成全局环境模型的更新。4前测值与定姿对比本文的算法在上海广茂达公司的“能力风暴”越野版机器人上进行了实验,该机器人配备了里程计和激光传感器。激光传感器测量数据的距离标准差σr=±0.01m,角度标准差σφ=±4.36×10-4rad。通常,环型的轨迹很难进行准确的位姿估计。所以在本实验中,设定机器人的移动轨迹为一个半径为3m的顺时针圆形,速度v=0.5m/s,为了验证本文算法对机器人方向角估计的准确度,在机器人上安装了电子罗盘,比较算法对方向角的估计值和电子罗盘的读数。图4所示给出了移动机器人在室内环境中,利用UKF估计算法走圆形路径的定位效果图。从图中可以看出,基于UKF的定位算法的定位结果与移动机器人的实际位姿是比较相符的。图5所示为本文算法估计的机器人的位姿与实测机器人位姿的偏差。从图中可以看出,在机器人开始移动的一段时间内,估计值与实测值误差较大,这是因为开始阶段机器人获得的环境信息的线段特征相对较少,同一条线段匹配的次数也较少,不确定性较大。随着时间的推移,环境信息的线段特征越来越多,同一条线段匹配次数的不断增加,其不确定性将越来越小。所以