基于噪声缩放的自适应UKF-SLAM算法

王祖麟,秦 菘,梁毓明

(江西理工大学电气工程与自动化学院,江西赣州341000)

基于噪声缩放的自适应UKF-SLAM算法

王祖麟,秦 菘,梁毓明

(江西理工大学电气工程与自动化学院,江西赣州341000)

针对扩展卡尔曼滤波(EKF)算法在移动机器人同时定位和环境建模(SLAM)中的缺点,即非线性系统简单线性化所导致的系统状态方程的不准确性、雅克比矩阵的计算所导致的计算复杂化以及噪声模型不确定性所导致的滤波稳定性降低等问题,提出一种对噪声自适应的UKF-SLAM算法。该算法通过对噪声缩放进而改变噪声模型,利用观测残差序列准确估计观测噪声模型协方差,运用预测的新息协方差和IAE开窗法求其系统状态噪声缩放因子,从而准确估计系统状态噪声模型协方差,实现对不确定的噪声模型能够自适应UKF-SLAM算法。UKF的Sigma点采样策略是比例对称采样。实验结果证明,该方法相对EKF算法和UKF算法具有较高的定位精度和自适应能力。

同时定位和环境建模;无迹卡尔曼滤波;噪声缩放;在线自适应;比例对称采样;开窗法

1 概述

实现智能移动机器人的关键技术之一就是移动机器人同时定位和环境建模(Simultaneous Location and Mapping,SLAM),即一方面机器人通过自身传感器实现对周围环境模型的建立,另一方面机器人依靠所建立的环境模型估计自身所在环境中的位置[1]。SLAM可以理解为机器人同时估计其位置和环境模型特征的估计问题。

目前最经典的算法就是扩展卡尔曼滤波(Extend Kalman Filtering,EKF)[2],该算法是通过把机器人的系统模型进行泰勒展开,将其高阶忽略,把非线性系统变为线性系统,但是当其高阶项无法忽略时,该系统就会出现较大误差,并且滤波器很难稳定,最终导致滤波器无法收敛,使得所建立的环境模型精度较低[3]。此外,EKF算法必须获取系统噪声和观测噪声的先验估计特性,在实际中是很难实现的,况且如果这些先验数据不准确或者信息量不足,则会造成滤波器估计精度降低。

针对以上EKF问题,人们提出了很多算法解决这个问题。文献[4]提出无迹卡尔曼滤波器(UKF)滤波器,该算法是一种用采样策略将非线性逼近线性的办法。UKF[5]是以UT(Unscented Transformation)变换为基础,采用KF线性滤波框架,其采样形式为确定性采样,不存在PF的粒子退化问题,对非线性分布统计量的计算精度至少为2阶。但是UT变换采样点到中心点的距离会随着系统位数的增加而越来越远,进而出现非局部效应。文献[6]提出了比例对称采样可以解决采样非局部效应。文献[7]提出了基于平方根容积卡尔曼滤波的SLAM算法。文献[8]提出了基于线段特征匹配的EKF-SLAM算法。

在实际过程中,UKF也会存在噪声模型不准确对滤波器精度和稳定性的影响,并且主要是滤波增益和噪声模型不能在线调节,不具备在线自适应能力。考虑了以上UKF算法的缺点,文献[9-10]提出了将UKF和粒子滤波器相结合的算法,文献[11]提出了通过调节UKF参数的改进办法。本文提出基于噪声缩放的UKF自适应SLAM算法,该算法利用机器人观测残差序列的均值和协方差改变观测噪声协方差值;并运用IAE开窗法和新息预测值得到系统状态噪声协方差缩放因子,进而求取系统噪声方差。

2 问题描述

SLAM的基本含义是机器人定位和环境建模同时进行,一方面机器人利用已经创建好的环境模型矫正机器人的位置状态,从而提高机器人的定位精度;另一方面机器人依靠可靠的机器人位置,建立出精度更高的环境模型。其问题可以描述为:当机器人进入一个未知环境中,k时刻的机器人位置状态为s(k),k时刻的地图特征为m(k),故机器人的状态向量表示为X(k)=(s(k),m(k))T,u(k)表示时刻k-1到k时刻的运动控制输入,Z(k)表示为k时刻的系统观测向量。SLAM其本质可以理解为在贝叶斯框架下的机器人定位和环境模型的预估问题, SLAM用概率描述可以表示为:

根据贝叶斯定理可知:

其中,η为归一化常数;p(Z(k)|X(k))为机器人观测模型;p(X(k)|u(k),Z(k))为机器人运动模型,也就是根据观测模型的概率和运动模型的概率分布递归的求其机器人的位置状态和环境地图模型。

针对第1节中所提到的EKF-SALM算法的不足和缺点,文献[12]提出了UKF算法,其算法的核心是采用SUT变换和确定性采样策略逼近非线性系统。基本的原理是在系统状态分布中选取一定量的采样点,使这些采样点的期望值和协方差等于系统状态分布的期望值和协方差;然后将这些点带入到非线性系统函数中,就得到了相应的非线性函数点集,最后通过得到的点集求取变换后的期望值和协方差。

根据以上关于SLAM问题的描述,设机器人的状态方程和观测方程分别为:

其中,系统状态噪声均值为w(k);协方差为Q(k);系统的观测噪声均值为v(k);协方差为R(k)。

在利用UT变换算法时,首要任务是确定Sigma点的采样策略。目前提出的采样策略有单行采样,对称采样等[13]。为了解决非局部效应,本文采用的是比例对称采样[6-7],利用UKF算法求其状态向量的均值和协方差Px,假设X(k)为n维向量,那么选取采样个数为L=2n+1按照比例对称策略进行Sigma点近似,其遵循如下的条件函数来确保能够准确地抓取Xi的必要特征。

遵循上述条件函数,求得比例修正的对称采样Sigma点χi′为:

那么X(k)对应的均值和协方差矩阵可近似的表示为:

以上就是UKF算法对均值和协方差的非线性变换计算,根据算法可以发现相对于EKF算法有至少存在2阶的估计精度,不需要计算雅克比矩阵,算法比EKF更容易实现;而相对于粒子滤波器,采用的粒子点是较少的,计算量更小。

3 UKF-SLAM算法

系统状态噪声和观测噪声的协方差对于UKF算法在SLAM提高滤波精度,定位精度和地图建模精度尤为重要。因为Q(k)和R(k)直接影响着当前时刻状态信息和上一时刻状态信息之间的权值。而确定R(k)和Q(k)的一般是依靠系统状态噪声和观测噪声的先验知识。根据前面介绍的UKF算法,如果均值X(0)和协方差P(0)取值存在误差,那么UKF算法的第2步Sigma点测采用就会受到影响,进而影响算法的预测部分;而且经系统的观测方程,使得观测量Z(k)会存在一定的误差。

经过以上分析可以看出如果R(k)和Q(k)的不确定性将对整个算法的滤波精度产生较大影响,进而影响SLAM的定位精度和地图建模精度。

针对在SLAM中UKF算法出现的系统状态噪声和观测噪声模型不确定性,所导致的UKF算法收敛速度变慢,估计精度降低,甚至出现滤波发散等问题,本文提出了一种在线自适应调整噪声模型并同时控制系统噪声模型异常干扰源对滤波效果产生影响的算法。

3.1 噪声自适应的UKF算法

根据UKF算法和机器人的状态方程和观测方程,首先定义观测新息值矩阵d(k)和观测残差值矩阵ε(k):

其中,d(k)是由k时刻的预测状态值和输入值所推算出来的;而ε(k)是由k时刻的更新状态值和输入值推算出来的。可以看出d(k)仅仅使用了预测值来求,所以带有噪声不确定性带来的误差,可以反映出系统所受到的扰动。

根据观测噪声模型和UKF算法,对ε(k)进行处理,得到其均值和协方差如下所示:

其中,P(k)为系统方程的协方差矩阵的更新值,根据UKF算法可以求出,进而可以估计出观测噪声协方差矩阵R(k)。

基于观测协方差的自适应算法一般采用的是开窗法,有IAE滤波和RAE滤波2种[14],IAE滤波使用m个历史新息值来推算当前新息的协方差矩阵估计值(k):

一个最优的滤波器应该满足,通过上面开窗法IAE预测的k时刻的协方差值(k)和UKF算法所预测的P(k|k-1)基本保持一致。即:

由于观测模型噪声矩阵R(k)不确定性带来的误差,将会导致(k)和预测的系统状态协方差P(k|k-1)并不相等。即:

式(16)和式(17)可以用来判断系统噪声模型是否发生改变,分以下2种情况讨论观测模型是否发生变化。

(1)如果系统的状态噪声模型没有发生改变,式(13)是成立的。

根据以上分析,只要知道UKF算法k时刻预测状态协方差值P(k|k-1)和利用开窗法LAE所预测的k时刻的协方差值(k),那么就可以根据式(13)和式(14)判断系统状态噪声模型有没有发生变化,进而可以知道系统状态噪声模型的具体变化情况。

定义缩放因子β(k)为:

根据式(16),系统状态噪声的协方差Q(k)可以表示为:

缩放因子β(k)取平方根是为了噪声更加平滑,使得整个系统噪声模型更加平滑,其可以取大于1也可以取小于1的数值,使得更加充分的调节噪声模型。

3.2 噪声自适应的UKF-SLAM算法

根据UKF算法原理和噪声自适应的UKF算法原理,从而实现噪声自适应的UKF-SLAM算法,具体步骤如下:

(1)系统初始化,初始化k=0时刻的均值、协方差。

(2)对于i=1,2,…,2n,进行Sigma点采样。采用策略是比例对称采样。可参考式(4)到式(8);

(4)首先按式(11)确定系统观测噪声协方差R(k),然后根据观测模型对第(3)步的预测部分进行更新,设Pzz为观测模型的协方差,Pxz为系统状态模型和观测模型之间的协方差,K(k)为滤波增益。

其更新方程如下:

4 系统建模与实验分析

4.1 系统建模

系统的模型主要包括机器人坐标中的模型、机器人运动模型、机器人控制模型、传感器观测模型、系统噪声模型,本实验中采用的模型来自于悉尼大学的经典数据库[15],其中机器人坐标中的模型如图1所示。

图1 机器人坐标中的模型

机器人运动模型如下:

机器人观测模型如下:

那么k时刻m个特征观测量为机器人的观测向量Z(k),如下所示:

4.2 实验环境

实验是在PC机上用MATLAB编写软件仿真进行的,实验所构造的环境区域为40×40的地图。机器人从起始状态S(0)=(0.005,0.005,0.001)开始步进,其仿真参数如表1所示。

表1 仿真参数

记NAUKF-SLAM为噪声自适应UKF-SLAM算法。对于UKF-SLAM算法和NAUKF-SLAM算法,用到的参数设定为:α取1,β取2,κ取3-n。

4.3 实验结果分析

在以上的实验环境,设定系统状态噪声δd(k)为2 cm,δθ(k)为0.5°,观测噪声δd(k)为0.1 m,δθ(k)为 1°。对 EKF-SLAM、UKF-SLAM 和 NAUKFSLAM算法分别进行仿真实验,并对以上实验结果进行分析和比较。

3种算法分别在系统状态噪声和观测噪声在服从上述噪声分布的情况下,机器人步进2 000次建立的路径估计曲线和地图特征估计点,其仿真效果分别如图2～图4所示。该仿真图是在同一地图中进行的,机器人真实路径和地图路标是相同的,其中实线为机器人实际行走的轨迹;虚线为算法估计的路径;+表示算法估计的路标;◇表示实际路标的位置。

图2 EKF-SLAM算法仿真结果

图3 UKF-SLAM算法仿真结果

图4 NAUKF-SLAM算法仿真结果

从效果图可以看出UKF-SLAM算法对机器人位姿的估计和对地图的建模比EKF更加准确,因为UKF-SLAM算法至少可以达到2阶的定位精度和环境建模精度;而NAUKF-SLAM算法对机器人路径估计基本和实际的运动路线相吻合,并且从地图特征的可信区域可以看出NAUKF-SLAM对地图特征的估计和实际的地图特征基本重合,实现了对机器人位姿的精确估计和环境模型的精确建模,因为在NAUKF-SLAM算法中所建立的噪声自适应模型可以实时的调节其噪声值。此外,由于提供的噪声是服从高斯分布的白噪声,所以3种算法最后都是收敛的,精度都逐渐提高。

为了验证NAUKF-SLAM算法对噪声的自适应性,3种算法在同一个地图中进行实验:地图大小为40 m×40 m,机器人运行时间为500 s,特征点数为40个。为了更好的比较3种算法性能,求取算法的估计值和真实值之间的误差。算法的估计值包括对机器人位置的估计、机器人姿态的估计以及对环境特征点的估计。每种算法都是在同一地图上进行N次实验,采样归一化误差来表示其估计值和真实值之间的关系,如对机器人位置的估计误差关系为:E(S(k),S*(k)),S(k)表示机器人实际的路径,S*(k)表示算法估计的机器人路径,其公式如下:

其中,N=50。

首先在上述的噪声不变的情况下,添加(0,1)正态分布的环境噪声,对3种算法进行实验,其3种误差分别对应图5～图7。虽然添加了高斯噪声,造成了对系统噪声模型和观测模型的影响,但是NAUKF-SLAM的定位精度和环境特征点估计精度都较好,体现出其对噪声的自适应性。

图5 高斯分布噪声下机器人位置平均估计误差

图6 高斯分布噪声下机器人姿态平均估计误差

图7 高斯分布噪声下特征点平均估计误差

为了进一步验证NAUKF-SLAM对噪声的自适应性,取2种不同的噪声对其实验。

添加(0,1)均匀分布的环境噪声,然后对3种算法进行实验,其3种误差如图8～图10所示。从图中可以看出3种算法的误差都有所增加,但是NAUKF-SLAM算法误差曲线基本在其他2种算法曲线下,其定位精度和地图建模精度更高。

图8 均匀分布噪声下机器人位置平均估计误差

图9 均匀分布噪声下机器人姿态平均估计误差

添加参数为0.01的指数分布的环境噪声,其3种误差分别对应图11～图13。虽然加入了指数噪声,3种算法的误差也都有所增加,但是NAUKFSLAM误差曲线基本在其他2种算法误差曲线以下,其定位精度和地图建模精度上更高。

图10 均匀分布噪声下特征点平均估计误差

图11 指数分布噪声下机器人位置平均估计误差

图12 指数分布噪声下机器人位姿平均估计误差

图13 指数分布噪声下特征点平均估计误差

综合以上的实验分析,可以看出随着噪声的增大或者减小,UKF-SLAM算法其误差比EKF-SLAM算法较小,这是因为EKF-SLAM算法仅仅保证了1阶的精度,而UKF-SLAM至少有2阶精度的存在。NAUKF-SLAM算法通过实时计算观测残差值的均值和协方差改变观测噪声协方差R(k),利用开窗法和新息预测值实时计算出状态噪声缩放因子β(k)进而改变系统状态噪声协方差Q(k),以实现更加精确的机器人定位和地图建模。

5 结束语

针对EKF算法在SLAM中由于噪声模型的改变所导致的精度降低甚至发散的问题,本文提出了一种通过改变噪声模型的自适应的UKF-SLAM算法(NAUKF-SLAM),通过实时观测新息的均值和协方差来自适应观测噪声模型R(k),以及通过IAE开窗法和新息协方差预测值计算出缩放因子β(k),进而自适应系统状态噪声模型Q(k),以达到对系统噪声模型的改变,或者有其他环境噪声加入时,能够自适应地调节其噪声模型,进而提高机器人SLAM精度。实验结果表明,本文算法SLAM精度优于EKFSLAM和UKF-SLAM算法,一定程度上满足了SLAM的精度要求。但是该算法对系统噪声Q(k)是全局缩放的,其缩放因子β(k)没有分配到每个单独的状态分量,可能对其精度有一定的影响,因此,下一步将在本文算法基础上,对UKF和粒子滤波器结合同时定位和环境建模进行研究。

[1] Durrant-Whyte H,Bailey T.Simultaneous Localization and Mapping:Part I[J].IEEE Robotics and Automation Magazine,2006,13(2):99-108.

[2] Dissanayake M W M G,Newman P,Clark S,et al.A Solution to the Simultaneous Localization and Mapbuilding(SLAM)Problem[J].Transactions of Robotics and Automation,2001,17(3):229-241.

[3] Singh S.Noise Impact on Time-series Forecasting Using an Intelligent Pattern Matching Technique[J].Pattern Recognition,1999,32(8):1389-1398.

[4] Andrade-Cetto J,Vidal-Calleja T,Sanfeliu A.Unscented Transfor mation ofVehicleStatesin Slam[C]// Proceedings of IEEE International Conference on Robotics and Automation.Barcelona,Spain:[s.n.],2005:324-329.

[5] Juliter S J.The SphericalSimplex Unscented Transformation[C]//Proceedings of American Control Conference.Denver,USA:[s.n.],2003:2430-2434.

[6] Wan E A,vander Merwe R.The Unscented Kalman Filter, in Kalman Filtering and Neural Networks[EB/OL].(2004-03-10).http://www.cse.ogi.edu/PacSoft/projects/sec/ wan01b.ps.

[7] 康轶非.平方根容积卡尔曼滤波在移动机器人SLAM中的应用[J].机器人,2013,35(2):186-193.

[8] 张国良,汤文俊.基于线段特征匹配的EKF-SLAM算法[J].控制工程,2012,10(6):119-124.

[9] Wang X,Zhang H.A UPF-UKF Framework for SLAM [C]//Proceedings of IEEE International Conference on Roboticsand Automation.Piscataway,USA:IEEE, 2007:1664-1669.

[10] Kim C,SakthivelR,ChungW K.Unscented Fast SLAM:A Robust and Efficient Solution to the SLAM Problem[J].IEEE Transactions on Robotics,2008,24 (4):808-820.

[11] Shojaie K,ShahriA M.Iterated Unscented SLAM Algorithm for Nacigation of an Autonomous Mobile Robot[C]//Proceedings of IEEE/RSJ Internation Conference on Intelligent Robots and Systems.Piscataway, USA:IEEE Press,2008:1582-1587.

[12] Julier S J,Uhlmann J K.Unscented Filtering and Nonlinear Estimation[J].Proceedings of the IEEE, 2004,92(3):401-422.

[13] Julier S J,Uhlmann J K.A Consistant,Debiased Method for Converting Between Polar and Cartesian Coordinate Systems[C]//Proceedings of the 11th International Symposium on Aerospace Defense Sensing,Simulation and Controls.Orlando,USA:Press,1997:110-121.

[14] Mohamed A H,Schwarz K P.Adaptive Kalman Filtering for INS/GPS[J].Journal of Geodesy,1999,73(4): 193-203.

[15] Australian Centre for Field Robotics.Source Code [EB/OL].(2008-06-10).http://www-personal.acfr. usyd.edu.au/tbailey/.

编辑 索书志

Adaptive UKF-SLAM Algorithm Based on Noise Scaling

WANG Zu-lin,QIN Song,LIANG Yu-ming
(Faculty of Electronic Engineering and Automation,Jiangxi University of Science and Technology,Ganzhou 341000,China)

For Extend Kalman Filtering(EKF)algorithm disadvantage on the Simultaneous Location and Mapping

(SLAM),that is simple linearization of nonlinear systems resulting from the inaccuracy of the system state equation,Jacobi matrix calculation resulting from computational complexity,and noise uncertainty caused by the filtering reduced stability and other issues,this paper proposes a noise adaptive UKF-SLAM algorithm.In order to achieve adaptive UKF-SLAM algorithm,the paper scales the noise to change the noise model.Using the observed innovation sequence to accurately estimate the covariance of the measurement noise model.And using a new message convariance and IAE fenestration to find the system noise scaling factor,and thus accurately setimates the convariance of the system state noise model,it achieves a adaptive UKF-SLAM algorithm.The sampling strategy of UKF Sigma points is scaling symmetric sampling.Experimental results show that the algorithm has a high accuracy on SLAM compared with the EKF-SLAM and UKF-SLAM.

Simultaneous Location and Mapping(SLAM);Unscented Kalman Filtering(UKF);noise scaling;online adaptive;scale symmetric sampling;windowing method

1000-3428(2014)10-0143-07

A

TP18

10.3969/j.issn.1000-3428.2014.10.028

国家自然科学基金资助项目(61262013);江西省教育厅科技计划基金资助项目(GJ11133)。

王祖麟(1954-),男,教授,主研方向:智能机器人,嵌入式系统;秦 菘(通讯作者),硕士;梁毓明,讲师、博士。

2013-10-16

2013-12-03E-mail:chinaqinsong@sina.cn

中文引用格式:王祖麟,秦 菘,梁毓明.基于噪声缩放的自适应UKF-SLAM算法[J].计算机工程,2014,40(10): 143-149,154.

英文引用格式:Wang Zulin,Qin Song,Liang Yuming.Adaptive UKF-SLAM Algorithm Based on Noise Scaling[J]. Computer Engineering,2014,40(10):143-149,154.