logo资料库

论文研究-基于集员估计的移动机器人多传感器组合定位 .pdf

第1页 / 共8页
第2页 / 共8页
第3页 / 共8页
第4页 / 共8页
第5页 / 共8页
第6页 / 共8页
第7页 / 共8页
第8页 / 共8页
资料共8页,全文预览结束
中国科技论文在线 http://www.paper.edu.cn 基于集员估计的移动机器人多传感器组合 定位# 周波,顾文华* 5 10 (复杂工程系统测量与控制教育部重点实验室(东南大学自动化学院),南京 210096) 摘要:自主定位是移动机器人导航算法中需要解决的首要问题。本文将针对室内移动机器人 的长距离定位问题进行研究,考虑到单一定位手段存在的不足,以激光和里程计作为主要的 定位设备,采用多传感器数据融合技术实现了移动机器人的精确定位。论文首先对激光采集 的环境信息进行扫描匹配得到相对位姿变换估计,然后通过建立系统定位模型,引入集员滤 波方法作为多传感器融合技术,利用扫描匹配结果校正由里程计滑移带来的定位误差。实验 结果表明了本定位系统的精度相对于单一的里程计定位有较大的改善。 关键词:定位;扫描匹配;集员滤波;数据融合 中图分类号:TP242.6 15 Multi-sensor fusion localization of mobile robots based on set-membership filter Zhou Bo, Gu Wenhua 20 25 30 (Key Laboratory of Measurement and Control of CSE (School of Automation, Southeast University), Nanjing 210096, China) Abstract: Autonomous localization is the first issue to be solved in mobile robot navigation.In this article, long-distance localization of indoor mobile robot is studied.Considering the lackage of localization technology only in one way, a laser and an odometer are used in our research as the main device. The data fusion technology is used to achieve precise localization of the mobile robot. Firstly, the environmental information collected by laser is matched to estimate the relative position position and orientation transformation of the robot. And then after the establishment of the system model, the set membership filtering method is introduced as a multi-sensor fusion technology to use scan matching results correct the cumulative error of odometer. Experimental results show that the accuracy of the localization system has been improved greatly with respect to single odometer localization approach. Key words: localization; scan matching; set member filter; data fusion 0 引言 移动机器人自身的定位问题就是根据相关定位传感器的测量信息,确定移动机器人在运 35 动环境中相对于世界坐标系的位置及其本身的姿态。里程计由于简单易用且价格便宜被广泛 使用,但由于其自身所存在的累积误差随时间无边界增长,从而无法单独用于长距离定位。 随着激光的普及,扫描匹配正逐渐被应用到移动机器人领域,并成为研究热点。其它的定位 传感设备包括声纳、立体视觉等等,由此兴起了采用多传感器融合技术的组合定位研究。 多传感器信息融合技术首先是从军事领域发展起来的技术,主要优点是在于观测信息的 40 重复性和互补性上可提供更好的性能。目前,信息融合问题通常采用贝叶斯估计方法来解决, 如针对线性系统的 Kalman 滤波(KF)[1]、针对非线性系统的扩展 Kalman 滤波(EKF)[1] 和无迹 Kalman 滤波(UKF)[2,3],以及基于 Monte-Carlo 采样的粒子滤波(PF)[4]等。这些 基金项目:高等学校博士学科点专项科研基金(新教师基金)(20100092120026) 作者简介:周波(1981-),男,东南大学自动化学院副教授。主要研究方向为机器人控制,自主规划和非 线性辨识. E-mail: zhoubo@seu.edu.cn - 1 -
中国科技论文在线 http://www.paper.edu.cn 方法的实质均是在对过程误差和观测误差作概率化假设的基础上,利用所有可获取的测量来 构造待估计状态的后验密度分布。然而,在很多实际应用中,过程噪声和传感器噪声的先验 45 知识往往是不充分且不能精确确定的,从而导致其概率分布假设很难得到满足,尤其是噪声 本身为非高斯、非白噪声或有偏噪声的情形。 有鉴于此,本文引入扩展集员滤波,该方法只要求噪声“未知但有界(UBB)”,这在实 际系统中很容易满足[5-7],克服了卡尔曼滤波的高斯噪声假设的同时可以估计出状态变量的 保证不确定边界,可用于后继的机器人轨迹优化生成和控制任务之中,以保证机器人具有更 50 好的安全性、效率性和稳定性。本文将采用基于集员估计理论的多传感器融合定位方法,通 过激光扫描匹配的方法对里程计累积误差进行周期性校正,来提高定位的精度,从而解决室 内长距离定位的问题。 1 基于扫描匹配的机器人位姿估计 1.1 扫描匹配思想 55 扫描匹配(Scan Matching)的思想就是通过定义当前扫描数据(current scan)和参考模 型(reference model)两个模型之间的匹配度函数,采用迭代的方式求取两者之间的最佳位 姿变换关系。其中,迭代最近邻(Iterative closest point,ICP)方法是实现扫描匹配中的一 种最常用的方法,由 Besl 和 McKay[8] 于 1992 年提出。该算法在应用中存在着收敛速度慢、 对先验位姿信息敏感、易受部分匹配造成的孤立值干扰等缺陷。为了克服这些问题,许多学 60 者对其从初始匹配点集选择、孤立值过滤、关联度函数建立及求解等多个方面进行改进,形 成各种 ICP 算法的变体[9-11],从而改进了 ICP 扫描匹配算法的性能。 1.2 基于点-直线匹配的 PLICP 算法 目前已有的数据对应方法大致可以分为 3 类:点-点(PTP)、点-特征(PTF)、特征- 特征(FTF)。标准 ICP 算法是一种基于点-点对应准则的典型方法,其根据最近点规则(即 65 对应点之间的欧式距离最小)确定当前扫描的每一点在参考扫描中所对应的关联点。基于点 -直线匹配的 PLICP 算法是一种 ICP 算法的变体,它从匹配规则的选取上对 ICP 算法进行了 改进。考虑 ICP 算法总是假设能够匹配的两个点测量是来自同一特征环境的,但由于激光传 感器自身的离散测量特性,实际上并不存在能够真正匹配的两个对应点,即所有对应点之间 的关联都是近似的。针对这一固有缺陷,本文引入的基于点-直线匹配的 PLICP 算法使用点- 70 直线的对应规则替代 ICP 中的点-点对应规则,其基本思想是将当前扫描中的数据点和其对 应的参考模型中的直线特征进行匹配,从而解决匹配误差问题。 在 PLICP 算法中,对当前扫描中 di 需要在参考扫描查找最近的两个点 , 从而将 di 匹配到线段 ,因此该算法需要对原始 ICP 算法中的误差度量函数进行改进,不再 以关联点之间的欧式距离为误差度量,而是该欧式距离沿线段 法向量上的误差为新 75 的点-直线匹配度量。具体的算法如下: 假设在直角坐标系下,当前扫描和参考扫描分别可表示为 D = {d1, d2, …, dn}, D = {d1, d2, …, dn},其中 di 和 mi (i = 1, 2, …, n)分别为扫描得到的点云数据,(R,T)表示当前扫描 D 和 参考扫描 M 之间的位姿变换关系,而 k 表示算法迭代的次数。 ①使用里程计读数初始化初始变换 ; - 2 - 1im2im12iimm12iimm00(,)RT
中国科技论文在线 http://www.paper.edu.cn 80 ②迭代初始化: ,k = 0; ③根据最近点规则CP (Closest Point)计算当前扫描中点 在参考扫描集M中的最近的 两个点 , ; ④计算线段 的法向量 ; ⑤对 排序,将值最大的η%点的对应权值ωi取为0,其余权值为1; 85 ⑥计算变换 ,使 ; ⑦令 ⑧ ; ; ⑨返回到③,直到 或 k < iter 定的距离门限,iter 迭代次数。 其中,第⑥步中误差度量函数的求解可通过对原始 ICP 算法的奇异值分解过程略作修改 90 获得,具体细节可参考文献[12]和[13]. 2 机器人融合定位 多传感器的融合定位是目前移动机器人领域最常用的定位方法。本文通过对连续激光点 云图像进行匹配来获得对应机器人的相对位移信息,并将其用于周期性校正里程计的累积误 差,从而提高机器人室内长距离运行的定位精度。整个定位工作流程可分两步来进行: 95 (1)将上一刻采集的激光扫描记为参考扫描,本时刻的记为当前扫描。采用 PLICP 算法 将当前扫描匹配的参考扫描,得到机器人的位姿变化信息。 (2)借助扩展集员滤波(EMSF)将里程计测得的机器人位置信息与(1)中得到的更新位置 信息进行数据融合,从而实现较高精度的定位。 2.1 运动学建模 100 2.1.1 状态模型 在建立机器人的状态模型时,要求所建立的模型既要符合实际,又要便于数学处理。 为此,本文以匀速模型(Constant Velocity, CV)为基础,建立机器人的状态模型。假设k时 刻机器人的位姿及位姿变化分别为pk = (xk, yk, θk)T和Δpk = (Δxk, Δyk, Δθk)T,将状态变量取为Xk = (xk,yk,θk,Δxk,Δyk,Δθk)T,根据匀速模型(CV),可得机器人状态模型为 105 (1) - 3 - 00000{|,}iiiiDddRdTdDkid1kjm2kjm12kkjjmmkjn1()Tkkkjjinmd(,)kkRT211(())minTnkkkijjiinmRdT111{|=,}kkkkkkkkiiiiDddRdTdD211111nkkkijiddmn1||kkddd1111111100100010010001001000100000010000001kkkkkkkkkkkkkxxyywxxyy
中国科技论文在线 http://www.paper.edu.cn 2.1.2 测量模型  里程计观测模型: 在移动机器人中,里程计是通过安装车轮上的光电编码器通过旋转计数,并结合车轮半 径检测机器人平台前进的位移,其测量值为位移信息(x, y, θ),因此其更新模型为 110 (2)  PLICP算法的观测模型: 采用 PLICP 算法局部匹配方法得到的是在参考扫描坐标系表示下当前机器人位姿变化 ,全局坐标系下机器人位姿变化计算如下: 115 其中,(Δxk,Δyk,Δθk)为全局坐标系表示下,移动机器人的位姿变换。再将上式写成如下形式: (3) 为此,得到PLICP算法对应的非线性更新模型为 (4) (5) 2.2 扩展集员滤波(ESMF)方法 120 考虑一般的离散的、非线性系统: (6) 其中, 和 分别是在采样时刻 k 的状态变量和观测变量;状态方程 和测 量方程 是已知的非线性二阶可导函数; 和 表示随时间变化的过程噪声 和测量噪声,并且 , 。另外,初始状态 。ε(a,P) 125 表示一个 n 维椭球,其定义为 (7) - 4 - ,100000010000001000kkkodoodokkkkxyzvxy(,,)Tkkkkpxy111cossin0sincos0001kkkkkkkkkkkkkkkkxxxxyyyy1cossin0sincos0001cossin0sincos0001kkkkkkkkkkkkkkkkkxxyyxy,cossin0sincos0001kkkICPkkkICPkkxzyv11()()kkkkkkxfxwzhxvnkxmkz()f()hnkwmkv(0,)kkwQ(0,)kkvR00,00,0ˆ(,)xxP1(,){|()()1}nTaPxRxaPxa
中国科技论文在线 http://www.paper.edu.cn 其中, 为椭球集的中心; 为椭球集的包络矩阵,满足对称正定性。 定义 k-1 时刻估计得到的系统状态椭球集记为 ,则 k 时刻 EMSF 算法的迭代过程如下: 130 (1) 计算 k-1 时刻 Xk-1 的每个状态分量的不确定性区间 i = 1,2,…,n (8) 式中, 表示 的(i, j)元素。 (2) 用区间分析技术计算线性化误差的最大区间 ,即 (9) 135 式中,Hi (i=1,2,…,n)为非线性方程的 Hessian 矩阵。 (3) 计算线性化误差的外包椭球集,即 , (10) 式中, 表示 的第 i 个元素; 表示 的(i, j)元素。 (4) 计算最终的等价过程噪声集 140 (11) 用于调整得到最优椭球。对测量方程进行类似的处理,即可得到等价的测量噪声椭球。 (5) 使用 SMF 方法预测和更新 k 时刻状态 (12) 预测: 145 更新: 150 其中, (13) (14) (15) (16) , (17) (18) - 5 - nannP11,11,1ˆ(,)kkkkkxxP,,11,11,11,11,1ˆˆ[,]iiiiiiikkkkkkkkkXxPxP,1,1ijkkP1,1kkP1kRX1121111(,,,)kTTTRkkkknHHXdiagXXXXH,2[]()kkiiikRRQnX,[]0kiikRQijkiRXkRX,[]kiikRQkQ11ˆˆ(0,)kkwQ11ˆ(0,1)1kkkkkkQQQQQQkQ,11,1ˆˆ()kkkkxfx1,11,11111ˆ1kkTkkkkkkkPQPFF1(0,1)k,,1,1ˆˆˆ[()]kkkkkkkkxxKzhx,1,1,1,(1)111kkkkkkTkkkkkkkkkPPPPHWH(0,1)k11111,1ˆ()kkkkkkxxfxFx,1ˆ()kkkkkkxxhxHx1,1ˆ1kkTkkkkkkPRWHH
中国科技论文在线 155 则最终,k 时刻 xk 的估计集为 http://www.paper.edu.cn (19) (20) (21) 显然,当 δk ≥ 1 时,Pk,k 为非正定阵,则其表示的椭球边界无意义,这说明对初始状态 和噪声的边界估计不准确,因此该参数值可用来指示算法的健壮性。此外,值得注意是,由 于两个椭球集的直和或交集的外包椭球不是唯一的,因此可通过选择适当的滤波器参数来 160 ( , , )获得某指标下最优的椭球集。 3 实验结果 本节将针对室内环境下移动机器人长距离定位问题进行实验。环境数据采集所采用的实 验平台为 Pioneer 3-DX 智能移动机器人,其配备的激光传感器为 SICK 公司的 LMS200 二维 激光测距仪。实验环境为如图 1(a)所示具有丰富的结构化和非结构化特征的长约为 12m、宽 165 约为 7m 的室内办公室环境,图 1(b)为其对应的平面地形图。 (a)实验环境 (b)实验地图 (a)Experiment environment (b)Experiment map 图 1 实验环境及其平面地形图 170 Fig.1 Experimental environment and its plane topographic map 在图 2 所示的办公室内,机器人在室内逆时针环绕 2 圈,共记录 4639 组里程计数据以 及 463 组激光扫描。图 2 为 PLICP 算法对其中连续两帧匹配的结果,从中可以看到该匹配 算法对不同地点采集的这两组数据都能实现较好的匹配。 175 (a)帧 121 和 122 匹配结果 (b)帧 421 和 422 匹配结果 (a) Matching result for frame 121 and 122 (b) Matching result for frame 421 and 422 - 6 - ,1TkkkkkKPHW1,1,1ˆˆ[()][()]TkkkkkkkkzhxWzhx,,ˆ(,)kkkkkxxPkQ1kk90空调机器人PeopleBot拖把扫帚打印机1150cm720cm示波器柜子
中国科技论文在线 http://www.paper.edu.cn 图 2 PLICP 算法匹配效果图 Fig.2 PLICP algorithm results 180 图 3 数据融合 Fig.3 Data fusion 185 (a) 第 1000 帧 (b) 第 1000-1019 帧 (a) Frame 1000 (b) Frame 1000 to 1019 图 4 ESMF 边界椭球 Fig.4 Boundary ellipsoid of ESMF 从图 3 中可以清楚地看到里程计因累积误差的存在,在室内长距离定位过程中对定位结 果产生很大影响(本实验中机器人的真实轨迹为一个闭合的环形,而里程计的起点和终点之 间位置上存在很大差别)。扫描匹配定位中虽然可以构成闭环,但是其匹配需要的时间较长, 190 为保证实时性,只能作为一种校正手段。本实验中里程计采集的频率为 10Hz,激光采集频 率为 1Hz。为此,针对两种传感器的采样频率不同的问题,需要在不同的采样时刻,即是不 同的传感器数据到来时刻,采用不同的模型进行更新,实现里程计和扫描匹配信息的有效融 合。最终,图 3 中融合的轨迹与真实轨迹基本相吻合。 图 4(a)为第 1000 次 ESMF 融合的(同时存在里程计和扫描激光观测值)状态预测椭圆、 195 扫描匹配观测噪声椭圆、更新后状态的边界椭圆,注意到观测更新后的误差椭圆要小于原来 预测和扫描匹配观测值分别对应的椭圆,即两两融合后的误差协方差小于任意原始值,说明 更新的效果是存在的。 图 4(b)为第 1000-1019 次 ESMF 融合的状态更新后的边界椭圆,黑色的椭圆为前 10 次 的,红色的为后 10 次的。能够看到当只有里程计更新的时候,估计误差的椭圆逐渐增大, 200 且同时里程计测量值逐渐偏离真实值。说明当没有扫描匹配信息校正时,能够看到里程计的 累积误差变大的现象。而当一旦有扫描激光更新时,扫描匹配得到的精确的相对位移信息会 - 7 -
中国科技论文在线 http://www.paper.edu.cn 对里程计的累积误差进行校正,将估计的位姿拉回到接近真实值,这样一个周期性的校正也 就解释了图 3 中滤波后的轨迹会出现锯齿状波动的原因。 4 结论 205 鉴于里程计自身所存在的累积误差不能用于长距离高精度定位,本文提出了一种基于扫 描匹配和里程计的融合定位方法,利用扫描匹配的结果来周期性地校正里程计数据,以提高 定位精度。对于扫描匹配引入基于点-直线的 PLICP 算法,是一种在 ICP 算法基础上改进的 算法。对于数据融合部分,引入了基于集合表示的集员滤波方法。该方法对状态的估计不再 是单独的一个值,而是一个所有可能状态的集合,从而描述了估计值可能出现的所有取值范 210 围,因此便于与最优控制、鲁棒控制等先进控制方法相结合进行后续的规划或避碰等研究。 最后对室内办公环境进行实验,验证了 ESMF 融合定位的方法可有效实现将扫描匹配得到 的相对位移信息对里程计周期性的校正,从而解决室内移动机器人的长距离精确定位问 [参考文献] (References) 215 220 225 230 235 240 [1] ANDERSON B D, MOORE J B. Optimal Filtering[M]. New Jersey: Prentice-Hall, 1979. [2] JULIER S, UHLMANN J. Unscented filtering and nonlinear estimation[J]. IEEE Transactions on Signal Processing, 2004, 93(2): 401-422. [3] MERWE R, WAN E. The square-root unscented Kalman filter for state and parameter estimation[C]. Proceedings of the International Conference on Acoustics, Speech, and Signal Process, Salt Lake City , Utah, USA, New York: IEEE Press, 2001: 3461-3466. [4] ARULAMPALAM S, MASKELL S, GORDAN N, CLAPP T. A tutorial on particle filters for online non-linear/non-Gaussian Bayesian tracking[J]. IEEE Transaction on Signal Processing, 2002, 50(2): 174-188. [5] SHAMMA J S, TU K Y. Approximate set-valued observers for nonlinear systems[J]. IEEE Trans. on Automatic Control, 1997, 42(5): 648-658. [6] GARULLI A, VICINO A. Set-Membership localization of mobile robots via angle measurements[J]. IEEE Trans. on Robotic and Automation, 2001, 17(4): 450-463. [7] SCHOLTE E, CAMPELL M E. A nonlinear Set-Membership filter for on-line applications[J]. Int. Journal of Robust and Nonlinear Control, 2003, 13(15): 1337-1358. [8] BESL P J, MCKEY N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14 (2): 239-256. [9] LU F, MILIOS E. Robot pose estimation in unknown environments by matching 2D range scans[J]. Journal of Intelligent and Robotic Systems, 1997, 18(3): 249-275. [10] MINGUEZ J, MONTESANO L, LAMIRAUX F. Metric-based iterative closest point scan matching for sensor displacement estimation [J]. IEEE Transactions on Robotics, 2006, 22(5): 1048-1054. [11] CHETVERIKOV D. The Trimmed Iterative closest point algorithm[C]. Proceedings of International Conference on Pattern Recognition, Quebec City, QC, Canada, New York: IEEE Press, 2002: 1021-1025. [12] CENSI A. An ICP variant using a point-to-line metric[C]. Proceedings of IEEE International Conference on Robotics and Automation, Pasadena, California, USA, New York: IEEE Press, 2008: 19-25. [13] CENSI A. Scan matching in a probabilistic framework[C]. Proceedings of the IEEE International Conference on Robotics and Automation, Orlando, Florida, USA, New York: IEEE Press, 2006: 2291–2296. - 8 -
分享到:
收藏