基于集员估计的室内移动机器人多传感器融合定位-周波.pdf

《基于集员估计的室内移动机器人多传感器融合定位-周波.pdf》由会员分享，可在线阅读，更多相关《基于集员估计的室内移动机器人多传感器融合定位-周波.pdf（10页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、第34卷第4期2017年4月控制理论与应用Control Theory & ApplicationsVol. 34 No. 4Apr. 2017基基基于于于集集集员员员估估估计计计的的的室室室内内内移移移动动动机机机器器器人人人多多多传传传感感感器器器融融融合合合定定定位位位DOI: 10.7641/CTA.2017.60209周波,钱堃,马旭东,戴先中(东南大学自动化学院复杂工程系统测量与控制教育部重点实验室,江苏南京210096)摘要:本文针对室内移动机器人的长距离实时鲁棒定位问题进行了研究,考虑到单一定位手段存在的不足,以二维扫描激光和里程计作为主要的定位设备,采用多传感器数据融合技术实

2、现了移动机器人的精确定位.论文首先通过引入基于点-直线特征匹配的改进迭代最近邻(iterative closest point, ICP)扫描匹配方法对激光采集的环境点云信息进行迭代匹配以得到相对位姿变换估计,并推导了其估计不确定性的保守包络矩阵形式,然后通过建立定位过程和观测模型,引入扩展非线性集员滤波器作为多传感器融合方法,利用扫描匹配结果校正由里程计滑移带来的定位误差,并获取定位自身的不确定性边界估计.实验结果表明了本文所提出的室内定位方法的精度、实时性和鲁棒性.关键词:移动机器人;定位;扫描匹配;集员滤波;数据融合中图分类号: TP24文献标识码: AMulti-sensor fusi

3、on for mobile robot indoor localizationbased on a set-membership estimatorZHOU Bo, QIAN Kun, MA Xu-dong, DAI Xian-zhong(Key Laboratory of Measurement and Control of Complex Systems of Engineering of Ministry of Education,School of Automation, Southeast University, Nanjing Jiangsu 210096, China)Abstr

4、act: The robust long-distance localization problem of indoor mobile robots is studied in this paper. Taking intoaccount the defects produced by using only a single localization means, a 2D laser scanner and an odometer are adoptedas the main localization devices, with their data fused to achieve pre

5、cise localization of the mobile robot. An improvediterative closest point (ICP) algorithm based on the point-line matching approach is proposed to estimate the relative posetransformation of the robot from point clouds collected by the laser scanner, and the underlying uncertainties for the poseesti

6、mation are also derived as a conservative envelope matrix. Through establishing the localization process and measure-ment models, the extended nonlinear set membership filtering (ESMF) is introduced as a multi-sensor fusion method tocorrect the cumulative errors of the odometer with the scan matchin

7、g data, and the boundary estimations of pose uncer-tainties are also obtained. Experimental results show that the accuracy, the real-time property and the robustness of theproposed indoor localization system can be guaranteed.Key words: mobile robot; localization; scan matching; set member filter; d

8、ata fusion1引引引言言言(Introduction)移动机器人的自定位问题即根据机器人自身携带的定位传感器的测量信息,确定移动机器人在运动环境中相对于世界坐标系的位置及其本身的姿态.用于室内移动机器人定位的传感器种类较多,其中里程计由于简单易用且价格便宜被广泛使用,但其自身存在着累积误差随时间无边界增长的缺陷,且运动学建模不确定性、轮地滑动等外界干扰也会影响其定位的精度、从而导致单一的里程计定位方法无法用于长距离定位问题1.其他常用的室内定位方法还包括声纳2、激光3、视觉4和无线传感器网络(包括RFID、蓝牙、ZigBee、WiFi、超宽带等不同应用形式)5定位方法等等.其中,基于激

9、光数据扫描匹配(scan match-ing)的自定位方法较之其他定位方法具有高精度、高更新频率、大范围测量能力、不受光照干扰、无需室内布网、成本适中、可同时进行环境感知等特点,正逐渐被广泛应用到室内移动机器人应用领域,成为当前室内机器人定位研究的一大热点,此外鉴于单一收稿日期: 20160408;录用日期: 20170112.y通信作者. E-mail: ; Tel.: +86 25-83794165.本文责任编委:侯增广.国家自然科学基金项目(61673254, 61573100, 61573101),复杂工程系统测量与控制教育部重点实验室开放课题(MCCSE2012B04)资助.Supp

10、orted by National Natural Science Foundation of China (61673254, 61573100, 61573101) and the Open Project Program of Key Laboratory ofMeasurement and Control of Complex Systems of Engineering, Ministry of Education (MCCSE2015A05).万方数据542控制理论与应用第34卷定位手段存在的不足,一种自然的研究趋势是将多种定位手段结合起来进行考虑,由此兴起了采用多传感器融合技术的

11、组合定位研究.多传感器信息融合技术的主要优点在于观测信息的重复性和互补性可提供更好的性能.目前,信息融合通常采用贝叶斯估计方法来解决,如针对线性系统的Kalman滤波(Kalman filter, KF)6、针对非线性系统的扩展Kalman滤波(extended Kalman filter, EKF)7和无迹Kalman滤波(unscented Kalman filter, UKF)8,以及基于Monte-Carlo采样的粒子滤波(particle filter,PF)910等.这些概率化方法存在的缺陷一是需要假设噪声特性已知或满足一定的分布条件(例如Kalman滤波需要满足高斯白噪声假设),

12、然而噪声的统计特性往往非常复杂,导致这种概率化假设在实际中很难得到满足和验证;二是对于非线性系统来说概率化方法很难获取状态的无偏估计和对应的严格估计边界,因此其鲁棒性难于得到保证.有鉴于此,本文引入基于扩展集员滤波(extendedset-membership filter, ESMF)的非线性系统状态估计方法1112,该方法只要求噪声“未知但有界”(unknown-but-bounded, UBB),由于在实际系统中噪声的边界信息较之概率化假设来说更容易获取(例如当先验知识不够时可选取较为保守的噪声边界),因此可克服上述概率化估计方法在应用中存在的局限性.同时集员估计方法的另一优点是其保证边

13、界估计的性质,即可以获得所有估计量可能状态取值的集合(可行集),从而获得了状态的“硬边界”信息,使得这种鲁棒滤波方法可以非常方便地与需要状态边界信息的控制方法(如鲁棒控制和最优控制等)进行结合,从而可用于后继的机器人轨迹优化生成和控制任务之中,以保证机器人具有更好的安全性、鲁棒性和稳定性1113.本文采用基于集员估计理论的多传感器融合自定位方法,通过激光扫描匹配的方法对里程计累积误差进行周期性校正,来提高定位的精度,从而解决室内长距离实时鲁棒定位的问题.从方法而言,本文提出了一种改进的点直线匹配(point-to-line iterativeclosest point, PLICP)方法,在原

14、始PLICP算法的基础上,结合trimmed滤波和cached kd-tree数据结构进行存储和搜索,以改进整体算法的实时性.此外,本文提出了基于Hessian矩阵的位姿协方差估计方法,从而可获取对扫描匹配精确性的定量分析,解决了对扫描匹配结果的不确定性进行在线估计的问题,并有利于和其他传感器数据的信息融合,以提高定位结果的精度.最后,针对协方差估计方法所估计得到的不确定性边界趋于保守的特点,本文采用集员估计方法进行融合处理,克服了传统概率化方法的局限性,从而可获取位姿估计的严格保证边界,以提高长距离定位的鲁棒性.2基基基于于于扫扫扫描描描匹匹匹配配配的的的位位位姿姿姿估估估计计计(Scan-

15、matchingbased pose estimation)针对基于推算导航原理的里程计定位误差随时间增长的问题,本文采用基于二维激光扫描数据的扫描匹配方法进行辅助定位,以对里程计的累计误差进行周期性校正.所谓扫描匹配是指通过定义当前扫描数据帧与某个参考扫描集(如前一个扫描匹配帧或是某个全局参考模型)之间的匹配度量函数,以此作为优化目标,采用最小二乘的方式迭代求解两者之间的最优位姿变换关系以完成定位.扫描匹配最常用的方法为迭代最近邻(iterative closest point, ICP)算法14,然而该原始算法存在着对迭代初值较为敏感、易受孤立值干扰及收敛速度慢等缺陷,导致其很难用于实时定

16、位过程15.针对这些缺陷,大量学者进行了相关的改进研究,提出了各种变体算法16,21,以改进ICP算法的精度、收敛性和实时性能等,促进了该算法在机器人领域的应用.2.1 PLICP算算算法法法(PLICP algorithm)标准ICP算法是一种基于点-点对应准则的匹配方法,根据最近点规则(即对应点间的欧式距离最小)确定当前扫描的每一点在参考扫描中所对应的关联点,其前提是假设能够匹配的两个点测量来自同一环境特征.但由于激光测量的离散性,实际上并不存在能真正匹配的两个测量点,即关联都是近似的.针对这一缺陷,本文引入基于点-直线匹配的PLICP算法1920,不再将当前扫描帧中的点di与单个的最近邻

17、参考点m1j进行匹配,而是和最近邻点m1j、次近邻点m2j所构成的直线特征m1j m2j进行匹配,由此将度量函数定义为点di与m1j m2j欧氏距离沿直线法向量方向的投影.除此之外,本文在PLICP算法的基础上进行如下改进,包括采用里程计测量结果作为迭代初值,采用cached kd-tree存储多个最近邻点以提高算法的搜索效率,以及采用Trimmed措施对关联点进行过滤等,以进一步提高ICP算法的精度和收敛速度20.具体算法如下:在直角坐标系下,将当前扫描和参考扫描分别表示为D = d1;d2; ;dn;M = m1;m2; ;mn;其中: di和mi(i = 1;2; ;n)分别为扫描得到的

18、点云数据, (R;T)表示当前扫描D和参考扫描M之间的位姿变换关系,而k表示算法迭代的次数.1使用里程计读数初始化初始变换(R0;T0);2设定迭代初值如下:D0 = d0i|d0i = R0di + T0; di D;3对于第k步迭代,根据最近点规则CP(closest万方数据第4期周波等:基于集员估计的室内移动机器人多传感器融合定位543point)计算当前扫描中点dki在参考扫描集M中的最近的两个点mk1j和mk2j ,并采用cached kd-tree存储;4计算线段mk1j mk2j的法向量nkj;5 Trimmed滤波：对D中所有的点,重复上述过程,并根据nkj(mk1j di)排

19、序,将值最大的 %点的权值!i取为0,其余权值为1;6计算变换(Rk;Tk),使ni=1!i(nkj)T(mk1j (Rdki + T)2 min;7令Dk+1 =dk+1i |dk+1i =Rkdki + Tk; dki Dk;8 dk+1 =ni=1dk+1i mk1j 2/n;9返回到3,直到d = |dk+1 dk| iter;其中: 为指定的距离门限, iter为迭代次数.其中第6步中误差度量函数的求解可通过对原始ICP算法的奇异值分解过程略作改进获得1718,具体推导细节如下.2.2度度度量量量函函函数数数(Metric function)基于点-直线的对应规则早已经被提出,但在此

20、之前,对上述第6步中非线性方程的求解都是线性化后通过线性最小二乘的方法求解,得到非解析解.而应用下文介绍的方法,可以求解在平面情况下步骤6中方程的解析解.优化目标为mint; i(R( )pi +t) i2Ci; (1)其中: pi; i;t R2为点矢量; R( )是22的旋转矩阵; 2C = TC 定义为一个包含了点点关联规则(Ci = !iI22, !i表示权值)和点-直线关联规则(Ci=!ininTi ,其中ni R2表示线段的法向量)的一般表达式.1)转化为二次型求解.将三维向量(tx;ty; )表示为如下的四元数:x = x1;x2;x3;x4 = tx;ty;cos ;sin ;

21、其中x23 + x24 = 1.对向量pi = (pi0;pi1)T,定义如下矩阵Mi:Mi =(1 0 pi0 pi10 1 pi1 pi0); (2)则式(1)可重写为minxi(Mix i)TCi(Mix i): (3)为了得到简单的二次型,将式(3)展开并简化i(xTMTi CiMix+ Ti Ci i 2 Ti CiMix):(4)忽略常数项进一步得到xT (iMTi CiMi)| z Mx+ (i2 Ti CiMi)| z gx: (5)定义矩阵W =(022 022022 I22;),则对方程(1)的求解转化为minxxTMx+gx; (6)s.t. xTWx = 1: (7)2

22、)根据拉格朗日乘子法求解.定义函数 (x):(x) = xTMx+gx (xTWx1); (8)得到最优解的必要条件为 x = 0,即有2xTM +gT + 2 xTW = 0: (9)求解得到x = (2M + 2 W)Tg = FTg; (10)其中F = 2M + 2 W.将式(10)代入式(7)中,得到只有未知数 的方程gTF1WFTg = 1: (11)将矩阵F分解为4个子矩阵F =(A BBT D + 2 I): (12)在二次型方程(9)中含有稀疏矩阵W,因此只需计算F1.应用矩阵求逆引理有F1 =( A1BQ1 Q1); (13)其中Q = (DBTA1B + 2 I) = (

23、S + 2 I):令G = A1BQ1,则式(11)可以改写为gT(GGT GQTGQT Q1QT)g = 1: (14)将Q1用以下形式描述:Q1 = (S + 2 I)1 = SA + 2 Ip( ) ; (15)其中:SA = det(S)S1;p( ) = det(S + 2 I):从而有万方数据544控制理论与应用第34卷Q1QT = (SA + 2 I)(SA + 2 I)Tp( )2 : (16)最终式(11)可转化成以下关于 的四次多项式:2 4gT(A1BBTAT A1B(symm) I)g +4gT(A1BSABTAT A1BSA(symm) SA)g +gT(A1B(SA

24、)TSABTAT A1B(SA)TSA(symm) (SA)TSA)g = p( )2: (17)由于p( )是二阶多项式,可见上式是一个四阶多项式.从而,可以得到式(11)中 的解析解,再将其代入式(10)就能得到x的解析解.2.3不不不确确确定定定性性性计计计算算算(Uncertainty computation)对于里程计,其协方差矩阵可以根据其设备误差而确定,且固定不变.与里程计不同, ICP算法的协方差矩阵需要根据每次匹配的结果来计算得到.如何估计ICP算法的协方差矩阵,使得ICP算法能够和其他传感器数据进行融合,是需要解决的难点问题.本文将采用Hessian矩阵方法来计算位姿估计的

25、协方差,该方法的主要思想是:对整个问题进行线性化,通过对误差函数的Hessian矩阵求解获得协方差估计,可用于在线计算.具体实现可通过如下的线性回归模型完成.对于线性回归模型Z = MX +v;Z = M X; (18)其中: X为状态,对于扫描匹配来说就是位姿(x;y;)T, Z为观测值, M为观测矩阵, v表示零均值的高斯白噪声,协方差为 2;第2个式子为估计方程.则需要最小化的误差函数为E( X) = (Z M X)T(Z M X); (19)则关于状态X的最优估计和相应协方差计算为X = (MTM)1MTZ;C( X) = (MTM)1 2; (20)其中 2的一个无偏估计为s2 =

26、Emin( X)/(nk); (21)其中: n为观测个数; k为估计状态的参数个数,本文应选取为3.对式(20)二次求导得到其Hessian矩阵为H = dE2( X)d X2 = 2MTM MTM = 12H:(22)综合式(20)和式(22),从而得到协方差估计为C( X) = (12H)1 2: (23)以下将针对扫描匹配进行计算.通过平移t = (x;y)T和旋转R( ),扫描匹配将非线性误差度量函数Emin(R( );t)最小化,即有Emin(R( );t) =Ni=1R( )di +tmi2: (24)上式中, di= (xi;yi)T和mi= (xi;yi)T为搜索到的第i组对

27、应点.其中: di为当前扫描中的点, mi为参考扫描中的点.在转角 较小的情况下,式(24)可进行如下的线性化:(cos sin sin cos )(1 1); (25)从而得到(xi xiyi yi)=(1 0 yi0 1 xi)xy: (26)上式为线性方程且与式(18)有相同的形式,因此可以根据式(23)计算协方差矩阵的最优估计,其优点在于实时性好,并且能够捕获实际不确定性误差分布的几何形状.但需要注意的是,该方法只能在点正确关联的情况下(这一点在实际应用中很难保证)才能够得到正确的协方差估计,否则给出的将是保守的不确定性估计,即其协方差估计给出的边界范围较之真实的不确定性分布范围(可采

28、用Monte-Carlo采样方法逼近)来说要大得多,从而违背了诸如KF, PF等常规概率化滤波方法所需的噪声假设条件,从而限制了概率化融合定位方法的应用性能.为了解决这个问题,本文提出了采用基于有界噪声假设的集员滤波方法进行后继融合定位,针对上述协方差估计趋于保守的本质,较之概率估计方法而言更适合于与集员估计方法相结合,可为其提供不确定性边界对应的包络矩阵形式,从而可获取位姿估计严格的保证不确定性边界,从而提高长距离室内定位的鲁棒性.3集集集员员员估估估计计计融融融合合合定定定位位位(Fusion localizationbased on set-membership estimator)如图

29、1所示,整个定位流程可分两步来进行:i)将上一刻采集的激光扫描记为参考扫描,本时刻的扫描记为当前扫描,采用PLICP算法进行匹配以得到机器人的位姿变化信息;ii)借助扩展集员滤波(EMSF)将里程计测得的机器人位置信息与i)中得到的更新位置信息进行数据融合,从而实现较高精度的鲁棒定位.万方数据第4期周波等:基于集员估计的室内移动机器人多传感器融合定位545图1定位流程Fig. 1 Localization flowchart3.1运运运动动动学学学建建建模模模(Kinematic modeling)移动机器人多传感器融合定位需要考虑以下两个问题: 1)不同传感器的采样频率不同.里程计的采样频率

30、远高于激光的采样频率,如何对不同采样频率的定位信息进行有效融合是一个问题; 2)不同传感器的测量模型不同.针对上述两个问题本文采用以下方法解决:将里程计的测量作为推算状态方程的控制量输入,当里程计传感器数据到来时刻对位姿进行预测;扫描匹配作为更新模型,当采集到激光数据时即通过扫描匹配对里程计预测状态进行更新校正.为此根据推算导航的原理可建立如下的定位状态模型:pk+1 =xk+1yk+1k+1=xk + sk cos kyk + sk sin kk + k: (27)上式中: pk = (xk;yk; k)T表示k时刻机器人的位姿;输入量(sk; k)T为k时刻里程计的测量值.根据上一节可知,

31、采用PLICP算法匹配得到的是在参考扫描坐标系表示下机器人的旋转和平移值pk=(xk;yk; k)T,则全局坐标系下机器人的旋转和平移值(xk;yk; k)T为xkyk k= cos k sin k 0sin k cos k 00 0 1xkyk k: (28)由此可由扫描匹配推得k时刻机器人的位姿:xk+1yk+1k+1=xkykk+xkyk k: (29)将上式推得的k时刻机器人的位姿作为扫描匹配的测量信息,则可得到如下扫描匹配观测模型:zICP = (xk+1 yk+1 k+1)T +vICP;k; (30)式中vICP;k为测量噪声.3.2扩扩扩展展展集集集员员员滤滤滤波波波(Exte

32、nded set-membership filter(ESMF)考虑一般的离散非线性系统:xk+1 = f (xk) +wk;zk+1 = h(xk+1) +vk+1; (31)其中: xk Rn和zk Rm分别为系统的状态矢量和观测矢量; f()和h()为非线性方程,且假设其二阶可导; wk Rn和vk+1 Rm分别为过程噪声和测量噪声,且满足wk E(0;Qk);vk+1 E(0;Rk+1):记号E(a;P)表示椭球集x Rn|(xa)TP1(xa) 1; (32)其中: a为椭球的中心; P为椭球的包络矩阵,且满足对称正定性.设系统的初始状态椭球集为x0 E(x0;P0),且k时刻估计得

33、到的状态椭球集为xk E(xk;Pk),则k + 1(k = 0;1;)时刻迭代算法为1)计算k时刻状态变量的不确定性区间Xk = xk Pk; xk +Pk; (33)2)用区间分析技术计算线性化误差的最大区间XnRk =diagXTk ; ;XTk Hk (Xk)Xk; (34)其中Hk为f在xk处的Hessian矩阵;万方数据546控制理论与应用第34卷3)计算线性化误差的外包椭球集,即 Qki;iRk = n(XnRk)2; Qki;jRk = 0; i = j; (35)式中: XiRk表示XRk的第i个元素; Qki;jRk表示Qk的(i;j)元素;4)采用椭球直和技术计算最终的等

34、价过程噪声集wk E(0; Qk).这里的虚拟误差wk为线性化误差和过程噪声的叠加,且有Qk = Qk1 Qk +QkQk ; Qk (0;1); (36)其中 Qk用于调整得到最优椭球.对测量方程进行类似的处理,即可得到等价的测量噪声椭球.5)使用SMF方法预测和更新k时刻状态:预测:xk+1;k = f (xk); (37)Pk+1;k = Jk Pk1 kJTk +Qkk ; k (0;1);(38)其中Jk为f在xk处的Jacobean矩阵;更新:Wk = HkPk+1;k1 kHTk +Rk+1k ; k (0;1);(39)ek = zk+1 h(xk+1;k); (40)xk+1

35、 = xk+1;k + Pk+1;k1 kHTk W1k ek; (41)Pk+1 = Pk+1;k1 k (I HTk W1k HkPk+1;k1 k );(42)k = 1eTkW1k ek; (43)Pk+1 = k Pk+1; (44)则最终k + 1时刻xk+1的估计集为xk+1 E(xk+1;Pk+1): (45)当 k 1时, Pk+1为非正定阵,则其表示的椭球边界无意义,这说明对初始状态和噪声的边界估计不准确,因此该参数值可用来指示算法的健壮性.此外,值得注意是,由于两个椭球集的直和或交集的外包椭球不是唯一的,因此可通过选择适当的滤波器参数来获得某指标下最优的椭球集12.4实实

36、实验验验结结结果果果(Experiment results)本文采用的实验平台为如图2所示的Pioneer3-DX型轮式地面移动机器人,其配备有SICK公司的LMS200二维激光测距仪.实验环境为如图2右所示具有丰富的结构化和非结构化特征的12 m7 m大小的室内办公室环境,图3为其对应的平面地形图.实验设定条件如表1所示.所有实验数据的算法处理均在Intel Core2 2.8 GHz/2G RAM PC机上的MATLAB2011a环境下运行.图2移动机器人平台和实验场所Fig. 2 Mobile robot platform and experiment environments图3环境平

37、面地形图Fig. 3 Planar map of environments表1实验设定Table 1 Experiment parameters参数名称设定值机器人运行速度0. 05 0.1 m/s机器人转弯角速度0.1 0.5 rad/s里程计采样频率10 Hz激光采样频率1 Hz激光扫描范围180激光扫描分辨率0:5激光最大量程8.191 m实验中机器人沿如图3中红色曲线逆时针环绕2圈(图中1, 2, 3处的圆圈是由于实验用的机器人逆时针原地转弯导致的),共记录4639组里程计数据以及463组激光扫描.图4为上述记录的463组扫描中,机器人直线前行,转弯以及一般曲线前行过程记录的两帧激光扫

38、描匹配采样结果,其中:黑色圆点表示上一时刻的参考扫描数据,红色*表示本时刻的扫描数据,而蓝色则代表匹配转换后的当前扫描数据,坐标系取为上一时刻以机器人为中心的参考坐标系.从图中(特别是局部放大图中),容易看出蓝色匹配数据中部分数据可和上一时刻的黑色扫描数据相重合,通过门限过滤和Trimmed操作后,即可使得部分点相关联,万方数据第4期周波等:基于集员估计的室内移动机器人多传感器融合定位547从而达到精确匹配的目的.此前本课题组的相关研究20证明了本文所采用的PLICP算法匹配结果的有效性,以及相对于常规ICP算法在匹配精度上的改进效果.(a)对直线前行情况匹配结果(b)对转弯情况匹配结果(c)

39、对一般情况匹配结果图4 PLICP单帧匹配结果Fig. 4 Matching results of PLICP图5为数据融合定位的结果.从图中可以清楚地看到里程计因累积误差的存在,在室内长距离定位过程中对定位结果产生很大影响(本实验中机器人的真实轨迹为一个闭合的环形,而里程计的起点和终点之间位置上存在很大差别).而数据融合估计的机器人轨迹是基本闭合的,且该路径与真实轨迹基本吻合,可见PLICP扫描匹配的校正作用.图6(a)和(b)分别展示了第10001500次ESMF融合估计及第14001500次ESMF融合估计的状态边界图;图(c)则为第10001019次ESMF融合估计状态更新的边界椭圆(

40、其中:红色椭圆为前10次的,黑色的为后10次的).从图中能够看到,更新椭球(边界)经历了从小变大,又急剧变小的周期过程,与扫描匹配的周期性校正结果相对应.由于里程计与激光扫描更新周期不一致(本实验中里程计周期为10 Hz,激光扫描的周期为1 Hz,相当于里程计更新10次观测才更新一次扫描匹配),如此当只有里程计更新的时候,估计误差的椭球会逐渐增大,而一旦有激光扫描更新时,扫描匹配得到相对位移信息会对里程计的累积误差进行校正,将估计的位姿拉回到接近真实值,且更新椭球变小,使得图中边界呈现周期性的变化.图(d)为第1001次(该帧正好同时存在里程计和扫描激光观测值)ESMF融合的状态预测椭圆和更新

41、后状态的边界椭圆,图中观测更新后的误差椭圆要远远小于预测椭圆,显示了扫描匹配对里程计的累积误差的修正效果.图5 ESMF融合定位结果Fig. 5 Fusion localization results of ESMF(a)第10001500帧ESMF估计边界万方数据548控制理论与应用第34卷(b)第14001500帧ESMF估计边界(c)第10011020帧ESMF误差椭球(d)第1001帧ESMF误差椭球图6 ESMF估计边界及误差椭球Fig. 6 Bounds and ellipsoid of ESMF为了进一步验证ESMF融合方法的有效性,将其与EKF融合方法进行比较.图7中的子图(a

42、)和(b)为分别采用EKF和ESMF的融合定位结果(两者的过程噪声等参数取相同的值).根据实验中轨迹可以明显看到ESMF融合结果相比EKF更接近扫描匹配的值,这是与协方差矩阵(对应集员方法为包络矩阵)的过估计结果向对应的,证明ESMF的椭球更新机制可获取更为准确的定位结果.(a) EKF定位(b) ESMF定位图7 EKF和ESMF定位比较Fig. 7 Localization comparison of EKF and ESMF图8(a)为14001500帧的ESMF融合估计值和EKF估计值的比较.从图中可以见到, ESMF估计值比EKF更接近于扫描匹配的校正值,更符合真实值预期.这是由于E

43、KF与ESMF不同的测量更新原理导致的.当预测椭球和观测椭球大小相差无几且相交时,状态估计结果除了ESMF估计边界比EKF的3 边界稍大以外,没有很大的差别;当预测椭球和观测椭球不存在交集时,这个时候状态估计是无解的, ESMF估计求得的交集为空;而由于EKF是概率估计仍然可求得状态估计,从而得到了错误的估计.当预测椭球和观测椭球是包含关系时, ESMF则直接用求交集的方法取被包含的那个椭球,然EKF则仍然使用概率方法求解,其值结果显然较之ESMF方法准确性较差.图8(b)为14001500帧对应的ESMF融合估计保证边界和EKF估计的3 边界.ESMF估计保证边界要比EKF估计的3 边界大一

44、些,但由于ESMF是一种保证边界估计,其状态值一定在估计的边界内.为此,相比EKF, ESMF有更好的鲁棒性、安全性等,更有利于后续的规划或避碰等研究.万方数据第4期周波等:基于集员估计的室内移动机器人多传感器融合定位549(a)第14001500帧估计值(b)第14001500帧估计边界图8第14001500帧EKF和ESMF估计比较Fig. 8 Comparison of EKF and ESMF for Frame 14001500此外,本实验中激光扫描的周期为1 s,而PLICP算法执行的平均匹配时间约为0.3152 s, ESMF数据融合的时间约为2:95105 s,为此可保证整体定

45、位算法的实时性,满足实时定位的需求.5结结结论论论(Conclusions)里程计自身所存在的累积误差导致其不能用于长距离高精度鲁棒定位,本文提出了一种基于2D激光和里程计的融合定位方法,利用扫描匹配的结果来周期性地校正里程计数据,以提高定位精度和位姿估计的鲁棒性.对于扫描匹配引入基于点直线的PLICP算法并加以改进,且推导了其求解和不确定性计算过程.对于数据融合部分,引入了基于不确定集表示的集员滤波方法.该方法有效地利用了不确定性协方差矩阵估计趋于保守的特点,从而可获取较之常规概率化估计方法更为鲁棒的位姿估计结果,且可给出位姿估计的严格不确定性边界信息.进一步研究包括对本文提出的室内定位方法

46、在移动机器人平台上的实时定位实验验证和动态环境定位,以及考虑未知环境下的同时定位和建图问题,从而将其纳入SLAM框架下以提高机器人的定位精度.参参参考考考文文文献献献(References):1 BORENSTEIN J, FENG L Q. Measurement and correction of sys-tematic odometry errors in mobile robots J. IEEE Transactions onRobotics and Automation, 1996, 12(6): 869 880.2 STECKEL J, BOEN A, PEREMANS H. B

47、roadband 3-D sonar sys-tem using a sparse array for indoor navigation J. IEEE Transactionson Robotics, 2013, 29(1): 161 171.3 ALMEIDAJ,SANTOSVM.Realtimeegomotionofanonholonom-ic vehicle using LIDAR measurements J. Journal of Field Robotics,2013, 30(1): 129 141.4 NOURANI-VATANI N, BOGRES P V K. Corre

48、lation-based visualodometry for ground vehicles J. Journal of Field Robotics, 2011,28(5): 742 768.5 MARTINELLI F. A robot localization system combining RSSI andphase shift in UHF-RFID signals J. IEEE Transactions on ControlSystems Technology, 2015, 23(5): 1782 1796.6 ANDERSON B D, MOORE J B. Optimal Filtering M. New Jersey:Prentice-Hall, 1979.7 BARCZYK M, B