一种基于IEKF的轮式移动机器人自定位算法

许 垚,孙勇智,马连伟

(1.浙江科技学院 自动化与电气工程学院,杭州 310023;2.浙大宁波理工学院 信息科学与工程学院,浙江 宁波 315100)

随着人们对未知领域探索的不断深入,移动机器人扮演着越来越重要的角色,而移动机器人完成各项任务的前提是要能够在其运动过程中实时地进行自主定位与导航。一般而言,机器人的准确定位主要依靠各种传感器来提取并分析自身和周围环境的信息以得到某一时刻它在环境中的位置,比较常用的传感器有里程计、激光雷达、视觉和超声传感器等,然而单个传感器通常只测量一至两个参数且精度有限。为了实现移动机器人的精确定位并尽可能减小误差,在现代机器人中常采用多个传感器融合[1]的方法来提高测量精度。目前,将多个传感器信息进行融合的方法主要有利用卡尔曼滤波[2]和人工神经网络[3]两种。

常用传感器模型大多具有非线性的特点,在使用时会产生一定的非线性误差。对此,扩展卡尔曼滤波(extended Kalman filter,EKF)算法得益于其非线性滤波的方式被广泛应用到移动机器人的定位中。然而受到泰勒级数展开项数的限制及各种环境噪声的影响,传统的EKF算法会产生一定的线性化误差从而导致滤波精度的下降。对此,研究人员进行了许多改进算法的研究。强敏利等[4]利用EKF算法在测量更新阶段对所需信息进行多次迭代运算,在一定程度上减小了非线性误差,提高了定位精度。曹军等[5]通过比较EKF算法预测和观测过程中的数据来判断环境干扰程度的大小,从而实时调整先验误差协方差矩阵,提高了算法的实时性和抗干扰性能。程璐等[6]将Cholesky分解和Sage-Husa滤波与EKF算法结合起来,提出一种自适应EKF算法,在提高了算法精确性和稳定性的同时,使算法具有更好的鲁棒性。王亚娜等[7]和LIU等[8]分别采用粒子滤波和容积卡尔曼滤波来进行室内多传感器的融合定位研究,仿真结果表明算法精度均有了较大的提高。但粒子滤波和容积卡尔曼滤波算法相对而言计算量较大,较难满足一些低成本移动机器人实时性定位的要求。

基于上述研究,本研究利用迭代扩展卡尔曼滤波(iterated extended Kalman filter,IEKF)算法融合里程计、相机传感器和激光雷达数据并结合路标特征信息,在状态更新阶段多次迭代计算机器人的滤波估计位置及相应的协方差矩阵,以获得更加准确可靠的位置信息。该算法模型简单,且计算量与粒子滤波和容积卡尔曼滤波比较相对较小,与EKF算法接近,在减少误差的同时进一步提高了定位精度。

1 定位算法描述

1.1 坐标系转换

某一时刻机器人在全局坐标系中的位置如图1所示,(XW,YW)为平面全局坐标系,(XR,YR)为机器人局部坐标系,机器人坐标系以两驱动轮连线的中点P为坐标原点。由于本研究选用的移动机器人运动模型的前两轮为驱动轮,后两轮为从动轮,因此,在建立其运动模型时,只需考虑两驱动轮对其施加的作用即可。

图1 机器人在全局坐标系中的位置示意图 Fig.1 Position diagram of robot in global coordinate system

在全局坐标系下,机器人的位置可用参考点P替代,以坐标轴XR的正方向为机器人的运动方向。参考点在坐标系中的位置定义为(x,y),方位角定义为θ。由此,机器人在坐标系中的位置可表示为(x,y,θ)。对于机器人在全局坐标系下的位置(xW,yW,θW)T和在局部坐标系下的位置(xR,yR,θR)T,结合两坐标系之间的映射矩阵R(θ),则存在如下转换关系:

1.2 里程计航迹推算

里程计可以用来测量机器人走过的路程及方位信息,可以提供机器人相对初始位置的坐标及方位角[9]。本研究采用航迹推算原理,按照位移矢量相加的方式对机器人的位置进行推算。在全局坐标系中,以参考点P的坐标(x,y)作为机器人的位置,定义初始时刻t0的初始位置坐标为(x0,y0),tn(n>0)时刻机器人的位置坐标为(xn,yn),θ为方位角,里程计航迹推算模型如图2所示。

图2 里程计航迹推算模型Fig.2 Odometer dead reckoning model

图2中,di为从ti到ti+1时刻机器人移动的路程,θi为ti时刻机器人的偏航角,Δθi为从ti到ti+1时刻机器人偏航角的变化量。对于tn(n>0)时刻,机器人的位置坐标(xn,yn)可以按如下公式进行推算:

1.3 相机成像模型

在机器人的运动过程中,利用相机传感器对环境路标特征进行识别并计算与路标之间的距离和夹角,相机成像模型如图3所示。

图3 相机成像模型Fig.3 Camera imaging model

图3中,OXYZ为相机坐标系,oxy为图像坐标系,f为焦距,空间中某一点Q在相机坐标系下的坐标为(X,Y,Z),投影至成像平面上的点q,通过坐标变换并结合相机参数可换算得到投影中心O到空间中点Q之间的距离及夹角。基于此,在机器人运动过程中,相机传感器便可得到其与路标之间的距离和方向信息。

1.4 激光测距模型

激光雷达在机器人运动的过程中对周围环境进行实时扫描,通过激光光束发射和返回的时间差可计算得到激光雷达到第i个路标的距离ρi(k)和方向φi(k)[10],则激光测量信息Zi(k)可表示为

Zi(k)=(ρi(k),φi(k))T。

1.5 匹配融合

一般情况下,激光雷达获得的测量信息比通过坐标变换计算得到的结果具有更小的误差,故本研究在相机识别到路标特征以后,将此时激光雷达的测量信息与相机传感器的测量值进行匹配和比较,若两者的差值小于设定的阈值范围,则将激光雷达的测量信息作为实际观测值代入算法中进行计算;否则,将相机传感器的测量值代入计算。以此来对两个传感器的测量信息进行融合互补,确保获得的观测值误差较小并最终得到较为准确的机器人位置估计。

1.6 位置修正

从初始时刻开始,机器人首先通过里程计进行航迹推算来估计自身的位置,然后在下一时刻到达估计的位置后利用传感器获得的实际观测值对算法的估计观测量进行修正并通过作差得到一个新息量,最后将该新息量代入算法的迭代更新过程以得到最终的估计结果并借此对当前时刻的机器人位置进行修正。

2 IEKF算法分析

IEKF算法是基于传统EKF算法提出的一种改进算法。基于EKF滤波在对非线性函数进行一阶泰勒级数展开后得到一个近似的线性化模型,在这一过程中易产生非线性误差的问题,IEKF在测量更新阶段将上一步修正后的输出作为下一次迭代修正误差的输入值,通过多次迭代来修正和减小误差。

定义一个移动机器人系统模型:

X(k+1)=f(X(k))+v(k)。

(1)

Z(k)=h(X(k))+w(k)。

(2)

式(1)和式(2)中:X(k)和Z(k)分别为k时刻系统的状态向量和量测向量;f(X(k))和h(X(k))分别为系统的状态转移矩阵和观测矩阵;v(k)和w(k)分别为系统过程噪声向量和观测噪声向量且满足v(k)～N(0,Q(k))和w(k)～N(0,R(k)),两者均为零均值高斯白噪声,Q(k)和R(k)为它们的非负定协方差矩阵。

2.1 运动学模型

根据图1,为求得机器人完整的运动学模型,将di投影映射到两个坐标轴上并用机器人的移动速度V和时间的乘积来表示,则有如下运动学模型

(3)

式(3)中:ωl和ωr分别为左右两驱动轮的旋转角速度;r和h分别为两驱动轮的轮半径和轮间距。在采样周期ΔT很小的情况下,可认为ωl和ωr在每个采样周期内保持不变。

由X=(x,y,θ)T便可确定机器人在全局坐标系中的具体位置,将式(3)改写为离散形式

结合式(1),整理可得机器人系统模型的状态方程:

(4)

2.2 观测模型

激光雷达和相机传感器在识别并捕捉到环境中的路标特征信息后,它们可以计算得到自身与特征点之间的距离和方位。由激光雷达的测距模型可得系统的观测矩阵

(5)

式(5)中:(xi,yi)为在全局坐标系中第i个路标的位置坐标;X(k)=[x(k),y(k),θ(k)]T为机器人的主体位置。不难发现,该观测矩阵具备非线性特征,需要进行线性化处理即经一阶泰勒级数展开后才能按照IEKF算法做相应的运算。结合式(2),可得系统的观测方程

(6)

2.3 算法流程

IEKF融合里程计、相机传感器和激光雷达定位的具体步骤为:

2)对机器人进行状态估测。根据机器人系统模型的状态转移矩阵,利用k时刻的状态量推测出k+1时刻的状态。

状态预测协方差方程为

P(k+1)=F(k)P(k)F(k)T+Q(k)。

(7)

式(7)中:F为f的雅可比[11]矩阵,

当传感器获得新的量测向量Z(k+1)之后,计算新息和滤波器增益,并用它们对状态进行修正。新息方程为

对应的新息协方差方程为

S(k+1)=H(k+1)P(k+1)H(k+1)T+R(k)。

(8)

式(8)中:H为h的雅可比矩阵,

4)状态修正。此时滤波器的增益矩阵方程为

W(k+1)=P(k+1)H(k+1)TS(k+1)-1。

对应的机器人状态更新为

状态协方差更新方程为

P+(k+1)=P(k+1)-W(k+1)S(k+1)W(k+1)T。

5)迭代更新。将步骤4)中更新的状态量和协方差矩阵重新赋值,

(9)

将式(9)的赋值结果分别代入步骤4)中状态和协方差更新方程进行计算,所得结果按式(9)重新进行赋值。如此迭代循环,最终得到较为精确的位置估计。

3 试验结果与分析

根据以上所给出的移动机器人运动模型和观测模型,以及IEKF滤波算法,在MATLAB中对机器人在空旷的室内环境中利用IEKF进行多传感器融合定位问题进行仿真实验,并进行对比分析。仅使用里程计定位的结果如图4所示,图4(a)中黑色的小圆点表示环境中的路标特征点。从图4(b)中可以看出,在机器人移动过程中,里程计所推算得到的估计位置与机器人的真实位置存在较大的误差,定位精度不高,难以满足精度要求。利用EKF滤波和IEKF滤波融合定位的结果分别如图5和图6所示。

图4 仅使用里程计定位的结果Fig.4 Results of using only odometer for localization

图5 EKF滤波融合定位的结果Fig.5 Results of using EKF for localization

图6 IEKF滤波融合定位的结果Fig.6 Results of using IEKF for localization

对比图4和图6可以看出,利用IEKF算法进行多传感器融合滤波定位所得到的机器人位置估计比仅使用里程计单独进行定位得到的位置要精确得多。可见,将里程计、相机传感器和激光雷达融合定位的方法有效地降低了里程计单独使用时产生的误差,能够得到较精确的机器人位置估计。

结合图5和图6可以看出,利用EKF滤波算法融合多传感器信息进行定位所得到的机器人位置估计与真实值基本上相符,但在长距离和转角处的位置估计略有偏差,总体精度不及使用IEKF滤波算法,IEKF在转角处及整条路径中的机器人位置估计都具有较高的精度。

4 结 语

本研究利用多传感器信息融合技术,为进一步提高移动机器人的定位精度,将里程计、相机传感器和激光雷达的信息结合起来,基于IEKF滤波算法实现了机器人的自定位,且通过仿真试验表明,在路径相对距离较短、轨迹相对简单的情况下,定位效果相比较于传统的定位方法有了较大程度的提高,可以得到较满意的定位精度。但本试验是在路标特征点提前提取并得到具体位置坐标的基础上进行传感器观测的,而且假设了环境噪声是已知的高斯白噪声,因此,在环境特征和噪声未知的实际情况下如何得到与试验结果同样精确的机器人位置尚有待进一步研究。