摘 要：针对移动机器人同步定位与地图构建（simultaneous localization and mapping，SLAM）存在的计算量大、数值不稳定等问题，结合容积卡尔曼滤波（cubature Kalman filter，CKF）原理，设计了一种基于平方根CKF（squareroot cubature Kalman filter，SRCKF）的SLAM算法（SRCKF-SLAM）．SRCKF-SLAM算法通过移动机器人运动模型和观测模型进行预测和观测，并以目标状态均值和协方差的平方根进行迭代更新，确保了协方差矩阵的对称性和半正定性，改进了数值精度和稳定性．仿真实验结果表明，相比CKF-SLAM算法，SRCKF-SLAM算法的均方根误差降低了14%，一致性区域内点数的百分比提高了36%，SRCKF-SLAM算法有效地满足了移动机器人SLAM导航需求．
关键词：移动机器人；同步定位与地图构建；平方根容积卡尔曼滤波
SRCKF Based Simultaneous Localization and Mapping of Mobile Robots
Abstract:In order to solve the large computing cost and numerical instabilities of simultaneous localization and map-ping (SLAM), a square root cubature Kalman filter (SRCKF) based SLAM algorithm (SRCKF-SLAM) for mobile robots isdesigned according to cubature Kalman filter (CKF) principle. The SRCKF-SLAM algorithm accomplishes prediction andobservation through motion model and observation model, and it is updated iteratively by propagating square root factors ofthe mean and covariance of the state variable, which guarantees the symmetry and positive semi-definiteness of the covari-ance matrix and therefore improves numerical accuracy and stability. The simulation experiments show that, compared withthe CKF-SLAM algorithm, the root-mean square error of the SRCKF-SLAM algorithm decreases 14%, and the percentageof points in consistency area increases 36%, therefore the SRCKF-SLAM algorithm effectively satisfies the requirement ofSLAM navigation of mobile robots.
Keywords:mobile robot; simultaneous localization and mapping; square root cubature Kalman filter
　　1 引言（Introduction）
　　同步定位与地图构建（SLAM）[1-2]是指移动机器人从一个未知的位置出发，在不断运动的过程中利用自身传感器对环境的动态感知信息递增式构建环境地图，并且利用环境中目标的位置计算自身的位姿．该方法最早由Smith和Cheeseman于1988年提出，用于解决移动机器人的定位与地图构建问题，他们运用扩展卡尔曼滤波（extended Kalmanfilter，EKF）对状态空间中的机器人位姿和地图特征同时进行估计．
　　目前SLAM问题的解决方法以概率估计为主．最早的概率估计方法是基于EKF的SLAM算法[3]，此类算法存在难以解决的数据关联问题，且计算量过大、精度不高．针对存储量与计算量问题，Thrun等提出了基于稀疏扩展信息滤波（sparse extendedinformation filter，SEIF）的SLAM算法[4]，但该方法要求环境中存在易于提取和区分的特征，如点、线和面特征．许多学者也研究了基于无迹卡尔曼滤波（unscented Kalman filter，UKF）的SLAM算法，但利用UKF处理高维数（维数n>4）系统时，为避免非正定协方差矩阵的传播，需要仔细“调整”UKF中的参数[5]．为克服UKF在高维系统中出现的数值不稳定及精度降低问题，英国莱斯特大学的研究人员提出了基于容积卡尔曼滤波（CKF）的SLAM算法[6]，与UKF-SLAM相比，CKF-SLAM对状态的最大估计误差降低了40%．近年来，Doucet等提出利用Rao-Blackwellized粒子滤波器解决SLAM问题[7]，但该方法中每个粒子中存储着各自的地图和机器人定位结果，计算量非常大，此外粒子退化和样本贫化也是其无法避免的缺陷[8]．针对FastSLAM的上述问题，文[9]将CKF的平方根版本应用于FastSLAM中，并采用仿真实验验证了算法的有效性．CKF是一种新型的非线性滤波方法，由加拿大学者Arasaratnam在2009年提出[10]．
　　CKF根据容积准则，通过一组具有相同权重的点经过非线性系统方程转换后产生新的点来给出下一时刻系统状态的预测，避免了对非线性模型的线性化处理，其精度达3阶[11]．该滤波方法一提出就被广大学者用于姿态估计[12-13]、导航[14-15]及SINS（捷联惯导系统）大方位角初始对准[16]．然而标准CKF用于SLAM时，在递推过程中，存在计算量大、数值不稳定等缺点．
　　针对上述问题，本文结合CKF，提出了一种基于平方根CKF（SRCKF）[17]的SLAM辅助导航算法．SRCKF使用基于容积准则的数值积分方法直接计算非线性随机函数的均值和方差，实现时只需计算函数值，避免了求导运算，降低了计算复杂度，且该算法直接对协方差矩阵的平方根进行递推更新，确保了协方差矩阵的对称性和半正定性，改进了数值精度和稳定性．通过实验验证了SRCKF-SLAM算法的导航定位性能优于标准CKF-SLAM和UKF-SLAM算法，证明了该算法的有效性．
　　2 SLAM问题描述（Description of SLAM）
　　SLAM问题的本质是机器人整个路径的概率估计问题，系统的状态向量可以表示为xk= [xTv;k;MT]T(1)其中，xv;k= [xvx;k;xvy;k;xvq;k]T为机器人在k时刻的位姿；M= [m1;m2;¢ ¢ ¢;ml] 表示路标集合矩阵，其中，mi= [xi;yi]T，i =1;2;¢ ¢ ¢;l，l 为路标的索引．解决SLAM问题就是求取状态向量xk 的后验概率密度，即p(xk;Mjuk;zk)，其含义是在已知所有控制输入uk 和观测值zk 的条件下求取机器人位姿xv;k 和地图M的联合条件概率．利用贝叶斯公式，可以得到[18]p(xv;k;Mjuk;zk) =p(zkjxv;k;M)wp(xv;kjxv;k¡1;uk)¢p(xv;k¡1;Mjuk¡1;zk¡1)dxv;k¡1(2)其中，uk 是控制输入，驱动机器人由xv;k¡1 移动到xv;k；zk 是k时刻机器人在位姿xv;k 处对路标的观测量；p(xv;kjxv;k¡1;uk)为运动模型，是已知k时刻机器人控制输入uk 和k¡1时刻位姿xv;k¡1 的条件下求取k时刻位姿xv;k 的条件概率；p(zkjxv;k;M)为观测模型，是在已知k时刻机器人位姿xv;k 和路标集合矩阵M的条件下求取k时刻观测值zk的条件概率．
　　3 SLAM 滤波器设计（Filter design ofSLAM）
　　3.1 CKF原理考虑如下形式的多维权重积分：I(f) =wRnf(x)exp(¡xTx)dx (3)式中：f(¢)为任意函数，Rn为积分区域，x为积分变量．一般情况下，上述积分的解析值无法得到，因此需要用近似的方法获得其解析值的近似解．如何求解上述积分值是滤波器需要解决的问题．Arasaratnam提出的CKF[10]非线性滤波算法，通过容积准则选取2n个具有相同权值的容积点集(wi;xi)来近似积分：I(f) =2nåi=1wif(xi) (4)8><>:xi =r2n2[1]iwi =1=2n; i =1;¢¢ ¢ ;2n(5)其中，n为系统状态向量的维数，容积点总数为2n．基本容积点按下列方式产生，记n维单位列向量e= [1; 0; ¢ ¢ ¢; 0]T，使用符号[1] 表示对e的元素进行全排列和改变元素符号产生的点集，称为完整全对称点集，[1]i表示点集[1] 中的第i 个点．假设离散非线性系统描述为8<:xk=f(xk¡1) +wk¡1zk=h(xk) +vk(6)其中，xk 表示k时刻系统的状态向量，zk 表示k时刻系统状态的量测值，f(xk¡1)和h(xk)为已知任意函数，随机系统噪声wk¡1»N(0;Q)，随机观测噪声vk»N(0;R)．对于系统(6)，CKF选取2n个具有同等权值的容积点计算高斯权重分布，计算出容积点集(wi;xi)后，通过时间更新和量测更新即得到CKF滤波算法．CKF算法在每个滤波周期内的时间更新和量测更新如下．(1)时间更新202 机 器 人 2013年3月设已知k¡1时刻后验概率密度函数p(xk¡1)=N(ˆ xk¡1jk¡1;Pk¡1jk¡1)，通过Cholesky分解误差协方差Pk¡1jk¡1：Pk¡1jk¡1=Sk¡1jk¡1STk¡1jk¡1(7)计算容积点（i =1;2;¢ ¢ ¢;m，m=2n）：Xi;k¡1jk¡1=Sk¡1jk¡1xi +ˆ xk¡1jk¡1(8)通过状态方程传播容积点：X¤i;k¡1jk¡1=f(Xi;k¡1jk¡1) (9)估计k时刻的状态预测值：ˆ xkjk¡1=1mmåi=1X¤i;kjk¡1(10)估计k时刻的状态误差协方差预测值：Pkjk¡1=1mmåi=1X¤i;kjk¡1X¤Ti;kjk¡1¡ˆ xkjk¡1ˆ xTkjk¡1+Qk¡1(11)(2)量测更新通过Cholesky分解Pkjk¡1：Pkjk¡1=Skjk¡1STkjk¡1(12)计算容积点（i =1;2;¢ ¢ ¢;m，m=2n）：Xi;kjk¡1=Skjk¡1xi +ˆ xkjk¡1(13)通过观测方程传播容积点：zi;kjk¡1=h(Xi;kjk¡1) (14)估计k时刻的观测预测值：ˆ zkjk¡1=1mmåi=1zi;kjk¡1(15)估计自相关协方差阵：Pzz;kjk¡1=1mmåi=1zi;kjk¡1zTi;kjk¡1¡ˆ zkjk¡1ˆ zTkjk¡1+Rk(16)估计互相关协方差阵：Pxz;kjk¡1=1mmåi=1Xi;kjk¡1zTi;kjk¡1¡ˆ xkjk¡1ˆ zTkjk¡1(17)估计卡尔曼增益：Wk=Pxz;kjk¡1P¡1zz;kjk¡1(18)k时刻的状态估计值：ˆ xkjk=ˆ xkjk¡1+Wk(zk¡ˆ zkjk¡1) (19)k时刻的状态误差协方差估计值：Pkjk=Pkjk¡1¡WkPzz;kjk¡1WTk(20)可见CKF根据容积准则，计算出偶数个具有相同权值的点，这些点能完全捕获高斯分布变量的均值和方差，并且经过非线性系统方程转换后，其后验均值和方差能够精确到非线性系统泰勒级数展开的3阶项或更高阶项，因此无需对非线性模型进行线性化，不依赖于具体系统模型的非线性方程，算法相对独立，适用于任何形式的非线性模型[19]．
　　3.2 SRCKF滤波器设计考虑到实际工程应用中，需要克服舍入误差引起的滤波发散问题，借鉴卡尔曼滤波中平方根滤波的思想，提出将SRCKF应用于SLAM算法中．与CKF相比，SRCKF具有以下优点：(1)在滤波过程中直接以协方差矩阵的平方根形式进行递推更新，可以降低计算复杂度，获得更高的效率；(2)能保证协方差矩阵的半正定性，有效地避免了滤波器的发散，提高了滤波的收敛速度和数值稳定性．SRCKF算法步骤[10]如下：(1)时间更新对于系统(6)，根据式(7)～(10)，分解误差协方差，计算并传播容积点，计算k时刻的状态预测值；计算预测误差协方差的平方根因子：Skjk¡1=T³hx¤kjk¡1SQ;k¡1i´(21)其中，SQ;k¡1为Qk¡1的平方根：Qk¡1=SQ;k¡1STQ;k¡1，x¤kjk¡1=1pm[X¤1;kjk¡1¡ˆ xkjk¡1 X¤2;kjk¡1¡ˆ xkjk¡1¢¢ ¢ X¤m;kjk¡1¡ˆ xkjk¡1] (22)T()函数表示求取矩阵的平方根因子．(2)量测更新根据式(12)～(15)，分解k时刻的状态误差协方差预测值，计算并传播容积点，估计k时刻的观测预测值．计算预测误差协方差的平方根因子：Szz;kjk¡1=T³hzkjk¡1SR;ki´(23)其中，SR;k 为Rk 的平方根：Rk=SR;kSTR;k，zkjk¡1=1pm[z1;kjk¡1¡ˆ zkjk¡1z2;kjk¡1¡ˆ zkjk¡1¢¢ ¢ zm;kjk¡1¡ˆ zkjk¡1] (24)估计互相关协方差的平方根因子：Pxz;kjk¡1=hkjk¡1zTkjk¡1(25)第35卷第2期 王宏健，等：基于SRCKF的移动机器人同步定位与地图构建 203其中，hkjk¡1=1pm[X1;kjk¡1¡ˆ xkjk¡1 X2;kjk¡1¡ˆ xkjk¡1¢¢ ¢ Xm;kjk¡1¡ˆ xkjk¡1] (26)估计卡尔曼增益：Wk= (Pxz;kjk¡1=STzz;kjk¡1)=Szz;kjk¡1(27)根据式(19)更新k时刻状态估计值，并估计相关误差协方差的平方根因子：Skjk=T³hhkjk¡1¡Wkzkjk¡1WkSR;ki´(28)
　　4 基于SRCKF的移动机器人SLAM算法设计与实现（Design and realization of SR-CKF based SLAM algorithm for mobilerobots）
　　首先建立机器人系统模型，由于机械制造工艺的原因，机器人内部传感器获得的控制量实际上为有色噪声，因此要将有色过程噪声模型转化为高斯白噪声模型[20]，再利用SRCKF进行递推迭代计算．在递推的每一步都进行系统模型噪声的白化处理．利用数据关联完成路标特征探测和地图匹配．通过状态增广不断扩大地图，递推地进行同步定位与地图构建．
　　4.1 系统模型机器人的运动模型、观测模型以及环境特征模型[21]如下：(1)运动模型xv;k+1=264xvx;k+1xvy;k+1xvq;k+1375=264xvx;k+DTVk+1cos(xvq;k+fk+1)xvy;k+DTVk+1cos(xvq;k+fk+1)xvq;k+DTVk+1sinfk+1B375(29)输入：xv;k 为机器人在k时刻的位姿，DT为航位推算传感器的采样时间，Vk 为机器人速度，fk 为舵角，B为两轴间的轴距．输出：xv;k+1 为机器人在k+1时刻的位姿．(2)观测模型zk=24riqi35=24p(xi ¡xvx;k)2+(yi ¡xvy;k)2arctanyi ¡xvy;kxi ¡xvx;k¡xvq;k35(30)输入：(xi;yi)为探测到的第i 个路标特征的位置坐标．输出：传感器观测到的第i 个路标特征到机器人的距离ri 及与机器人前进方向的夹角qi．(3)环境特征模型本文中使用点特征表示环境中的路标，环境中第i 个点特征表示为Pi =[xi;yi]T，由于环境路标是静止的，因此，环境特征模型表示为Pi;k+1=Pi;k; i =1;2;¢ ¢ ¢;n (31)
　　4.2 系统模型噪声的白化处理系统模型的白化处理用增广状态变量维数的方法来解决，将过程噪声的均值部分作为状态向量空间的一部分，系统的状态向量增广为x0= [xv Vn Gn]T(32)其中，Vn 为控制量速度噪声，Gn 为控制量舵角噪声，Vn 与Gn 均为实数，xv 的维数是3，x0的维数是5．
　　4.3 基于SRCKF的SLAM算法实现基于前述系统模型，基于SRCKF的SLAM算法主要步骤为：(1)状态预测基于式(7)～(10)及(21)、(22)估计机器人k时刻的状态预测值及预测误差协方差的平方根因子．(2)观测通过式(30)给出的机器人观测模型获取观测数据，即获取传感器探测范围内机器人探测到的路标特征，并计算出探测到的每个路标特征到机器人的距离ri 及其与机器人前进方向的夹角qi．(3)数据关联根据机器人传感器的观测数据，对新的特征测量和地图中已经存在的特征进行数据关联，正确的数据关联是得到一致地图的充分条件．本文采用独立兼容最近邻算法（individual compatibility nearestneighbor algorithm，ICNN）[22]完成数据关联．测量的真实值和预测值之间的差值用vi j 表示，其协方差用Si j 表示：vi j;k=zi;k¡hj(ˆ xkjk¡1;si) (33)Si j;k=HkPkjk¡1HTk+Ri(34)其中，H表示非线性方程hj 对状态向量的雅可比矩阵在当前最好状态估计值ˆ xkjk¡1 处的值．为判断数据关联的正确性，使用马哈拉诺比斯204 机 器 人 2013年3月距离（Mahalanobisdistance）实现兼容性测试：D2i j=vTi j;kS¡1i j;kvi j;k<c2d;a(35)其中，d=dim(hj)，dim(¢)为求矩阵的维数．a是期望的置信度水平．(4)更新数据关联之后，如果新的测量对应着一个已经存在于地图中的特征，则需要利用观测信息，基于SRCKF对已有特征的状态进行更新，即根据式(12)～(15) 及(23) ～(28) 更新状态向量和预测误差协方差平方根因子．如果前述数据关联计算结果表明新的测量未对应地图中已存在的特征，即观测到一个新特征，则需要对此特征在地图中进行初始化，进而进行状态增广．(5)地图构建前述数据关联将观测z分解为关联观测zk 和新特征点的观测znk．z= [zkznk]T(36)通过状态增广完成地图构建：xnewk=h¡1(znk;xk) (37)Xk= [Xkxnewk]T(38)其中，znk 为新特征点的观测，xnewk为新观测到的特征点．
　　5 算法实验验证与分析（Experimental vali-dation and analysis of the algorithm）
　　5.1 实验条件实验环境区域为200 m£240 m室外环境（假设障碍物静止）．机器人运动的路径点（waypoint）和路标特征（landmark）均是人为指定．本实验设定19个路径点和30个路标特征．机器人从第一个路径点(0;0)开始顺时针沿路径点确定的轨迹运行．实验参数设置如下：机器人：初始状态值xv;0= [0 0 p=4]T，速度V=3 m/s，最大角速度g=20±/s．航位推算系统：采样时间为0.025 s，速度误差sv=0:3 m/s，舵角误差sa=2±．系统噪声Q和观测噪声R分别为Q=264(0:3)200µ3:0p180¶2375; R=264(0:1)200µ1:0p180¶2375激光雷达：采样时间0.2s，最大距离30 m，距离误差sr =0:1 m，角度误差sq =1:0±．数据关联：最大关联距离为4m，认定为新特征点的最小距离是25 m．
　　5.2 实验结果与分析
　　本文在上述相同实验环境下，分别对SRCKF-SLAM、CKF-SLAM、UKF-SLAM和EKF-SLAM算法进行了仿真实验验证，并取50次重复实验结果的均值为最终结果进行对比分析．图1、2分别对应SRCKF-SLAM、CKF-SLAM算法的实验仿真结果．其中图1(b)和图2(b)分别对应图1(a)和图2(a)中矩形包围区域的局部放大图．由图1、图2可见，在机器人导航定位估计方面，SRCKF-SLAM算法与标准CKF-SLAM和UKF-SLAM相比，估计路径与机器人实际走过的路径契合度高，意味着估计精度较高；在地图准确度方面，SRCKF-SLAM创建地图精度更高．−100 −50 0 50 100 150−100−50050100X   50 100 150 200−60−40−2002040 UKF-SLAMEKF-SLAM(a) SRCKF-SLAM算法仿真实验结果 (b)局部放大图图1 SRCKF-SLAM算法仿真实验结果Fig.1 Simulation results of SRCKF-SLAM第35卷第2期 王宏健，等：基于SRCKF的移动机器人同步定位与地图构建 205−100 −50 0 50 100 150−100−50050100X   60 80 100 120 140 160 180−60−40−2002040 UKF-SLAMEKF-SLAM(a) CKF-SLAM仿真实验结果 (b)局部放大图图2 CKF-SLAM仿真实验结果Fig.2 Simulation results of CKF-SLAM(1)估计误差实验分析图3、图4是EKF-SLAM、UKF-SLAM、CKF-SLAM和SRCKF-SLAM算法在进行路径估计时X轴、Y轴估计误差的对比曲线．t 代表实验迭代过程（共7175步）．1000 2000 3000 4000 5000 6000 7000 8000 012345678910t /ℹX   EKF-SLAMUKF-SLAMCKF-SLAMSRCKF-SLAM图3 4种SLAM算法X轴误差比较Fig.3 X-axis errors of four SLAM algorithms1000 2000 3000 4000 5000 6000 7000 8000 01234567t /ℹ  EKF-SLAMUKF-SLAMCKF-SLAMSRCKF-SLAM图4 4种SLAM算法Y轴误差比较Fig.4 Y-axis errors of four SLAM algorithms对比图3和图4实验结果，有以下分析结果：从估计算法稳定性角度分析，在X轴上，4种SLAM算法中SRCKF算法稳定性最好，误差值变化量在3 m以内；CKF算法的稳定性仅次于SRCKF算法，误差值变化量在4 m以内；UKF算法的稳定性较差，仅优于EKF算法；而EKF算法的稳定性最差，其误差值变化量高达9 m．在Y轴上，SRCKF算法稳定性最好，误差值变化量在4 m以内；UKF算法的稳定性仅次于SRCKF算法，误差值变化量在5 m以内；CKF算法的稳定性较差，仅优于EKF算法；而EKF算法的稳定性最差，其误差值变化量高达6 m．从估计精度角度分析，4种SLAM算法在Y轴的估计精度在后半段都有明显提高，且SRCKF与CKF算法的估计精度在X轴后半段也有明显提高．EKF算法的精度最低；UKF算法在X轴的估计精度后半段不断降低，略优于EKF算法，且其Y轴估计精度介于SRCKF和CKF算法之间；CKF算法虽然在X轴的估计精度有优势，但是其Y轴的估计精度较差，仅优于EKF算法；SRCKF算法在X轴的估计精度与CKF算法相当，该方法在Y轴的估计精度优势非常明显，相同时刻下比CKF算法的估计精度最大时提高了2 m．综上所述，EKF-SLAM的误差最大，UKF-SLAM与CKF-SLAM的估计精度整体相差不大，而在机器人探索全过程中，SRCKF-SLAM的定位估计精度较高、误差较小，且数值稳定性好，验证了SRCKF-SLAM算法有效且优越．(2)均方根误差和平均运行时间实验分析定义SLAM算法估计的均方根误差[10]为206 机 器 人 2013年3月Ek=s1tsteptstepåk=1³(xvx;k¡ˆ xvx;k)2+(xvy;k¡ˆ xvy;k)2´(39)式中，k是任意时刻，tstep 是总的运行步数，(xvx;k;xvy;k)与(ˆ xvx;k; ˆ xvy;k)分别是移动机器人在k时刻的真实位置和估计位置．表1列出了4种SLAM算法的均方根误差和平均运行时间．各算法的均方根误差对比结果表明，SRCKF算法估计的均方根误差最小．理论上，SRCKF和CKF算法只采用了2n个同等权重的容积点，而UKF算法采用了2n+1个sigma点，因此UKF算法的运行时间应该较长．从实验运行时间上，亦证明了UKF算法耗时最长，EKF算法耗时最短，SRCKF算法与CKF算法耗时相当．表1 4种SLAM算法实验数据统计Tab.1 Experimental statistics of four SLAM algorithmsSLAM算法 均方根误差/m 平均运行时间/sSRCKF-SLAM 2.9005 28.6070CKF-SLAM 3.3650 28.7872UKF-SLAM 3.9326 30.6437EKF-SLAM 5.4640 21.2982(3)一致性分析采用归一化估计方差NEES（normalizedestima-tion error squared）作为对移动机器人SLAM过程中定位结果的一致性分析依据，NEES定义为ek=xv;k¡ˆ xTv;kP¡1kjkxv;k¡ˆ xv;k(40)式中，xv;k 为机器人k时刻实际位置，ˆ xv;k 为机器人k时刻的估计位置，Pkjk 为机器人在k时刻的位置协方差．由于机器人位姿向量的维数为3，ek 符合自由度为3的卡方分布．对SRCKF-SLAM、CKF-SLAM、UKF-SLAM和EKF-SLAM分别进行n（n=50）次实验，使用n次的平均NEES（meanNEES，MNEES）作为一致性分析依据：¯ ek=1nnåi=1ei;k(41)由卡方分布属性可知，¯ ek 符合自由度为50£3的卡方分布，考虑双边概率为95%的区域作为一致性区域时，可得¯ ek 的双边区域为[2:36;3:72]．当¯ ek位于此区域内时，当前机器人位置估计满足一致性要求；否则，机器人位置估计不满足一致性要求．图5为4种SLAM算法MNEES随时间变化曲线．表2为4种SLAM算法一致性区域内点数分布统计．由图5可知，CKF-SLAM、UKF-SLAM和EKF-SLAM只在最初短时间内满足一致性要求，而SRCKF-SLAM的MNEES曲线基本在[2:36;3:72] 之间，可以认为该算法是一致性估计．由表2可知，SRCKF-SLAM一致性区域内点数分布的百分比最大，其所得机器人位姿估计的一致性明显优于其它算法．1000 2000 3000 4000 5000 6000 7000 8000 024681012141618t /ℹMNEES EKF-SLAMUKF-SLAMCKF-SLAMSRCKF-SLAM图5 4种SLAM算法MNEES随时间的变化Fig.5 MNEES of four SLAM algorithms表2 一致性区域内点数的统计Tab.2 Statistics of points in consistency areaSLAM算法 一致性区域内点数 总点数 百分比/%SRCKF-SLAM 4164 7175 58.03CKF-SLAM 1595 7175 22.23UKF-SLAM 1521 7175 21.20EKF-SLAM 1129 7175 15.74
　　6 结论（Conclusion）
　　本文提出一种基于SRCKF的SLAM算法，该算法利用平方根滤波，采用协方差平方根代替协方差矩阵参加递推更新，确保了协方差矩阵的对称性和半正定性，从而保证了滤波的数值稳定性，有效地提高了系统状态估计精度，通过一致性分析亦验证了该算法满足一致性要求．为面向工程实际应用需要，下一阶段计划采用悉尼大学野外机器人研究中心（ACFR）所提供的停车场数据集完成SRCKF-SLAM算法的实际应用与验证，提高算法的实时性和克服传感器测量误差对SLAM算法的影响等将是未来研究工作的重点．致谢：感谢ACFR的研究者提供的EKF-SLAM与UKF-SLAM仿真程序包．
　　参考文献（References）[1] Durrant-Whyte H F, Bailey T. Simultaneous localization and第35卷第2期 王宏健，等：基于SRCKF的移动机器人同步定位与地图构建 207mapping(SLAM): Part I, The essential algorithms[J]. IEEERobotics and Automation Magazine, 2006, 13(2): 99-108.[2] Bailey T, Durrant-Whyte H F. Simultaneous localization andmapping (SLAM): Part II, State of the art[J]. IEEE Roboticsand Automation Magazine, 2006, 13(3): 108-117.[3] Smith R C, Cheeseman P. On the representation and estimationof spatial uncertainty[J]. International Journal of Robotics Re-search, 1986, 5(4): 56-68.[4] Thrun S, Liu Y F, Koller D, et al. Simultaneous localizationand mapping with sparse extended information filters[J]. Inter-national Journal of Robotics Research, 2004, 23(7/8): 693-716.[5] Julier S J, Uhlmann J K. Unscented filtering and nonlinear esti-mation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422.[6] Bharani Chandra K P, Gu D W, Postlethwaite I. CubatureKalman filter based localization and mapping[C]//18th IFACWorld Congress. Laxenburg, Australia: IFAC, 2011: 2121-2125.[7] Doucet A, de Freitas N, Murphy K, et al. Rao-Blackwellisedparticle filtering for dynamic Bayesian networks[C]//16th Con-ference on Uncertainty in Artificial Intelligence. San Francisco,CA, USA: Morgan Kaufmann Pubishers, 2000: 176-183.[8] Doucet A, Godsill S, Andrieu C. On sequential Monte Carlosampling methods for Bayesian filtering[J]. Statistics and Com-puting, 2000, 10(3): 197-208.[9] Song Y, Li Q L, Kang Y F, et al. Square-root cubature Fast-SLAM algorithm for mobile robot simultaneous localizationand mapping[C]//IEEE International Conference on Mechatron-ics and Automation. Piscataway, NJ, USA: IEEE, 2012: 1162-1167.[10] Arasaratnam I, Haykin S. Cubature Kalman filters[J]. IEEETransactions on Automatic Control, 2009, 54(6): 1254-1269.[11] Dhital A. Bayesian filtering for dynamic systems with applica-tions to tracking[D]. Barcelona, Spain: Universitat Politecnicade Catalunya, 2010.[12] Bae J, Kim Y. Nonlinear estimation for spacecraft attitude us-ing decentralized unscented information filter[C]//InternationalConference on Control, Automation and Systems. Piscataway,NJ, USA: IEEE, 2010: 1562-1566.[13] Jia B, Xin M, Cheng Y. Sparse Gauss-Hermite quadrature filterwith application to spacecraft attitude estimation[J]. Journal ofGuidance, Control, and Dynamics, 2011, 34(2): 367-379.[14] Liu J, Cai B G, Tang T, et al. A CKF based GNSS/INS train in-tegrated positioning method[C]//IEEE International Conferenceon Mechatronics and Automation. Piscataway, NJ, USA: IEEE,2010: 1686-1689.[15] Pesonen H, Piche R. Cubature-based Kalman filters for posi-tioning[C]//7th Workshop on Positioning, Navigation and Com-munication. Piscataway, NJ, USA: IEEE, 2010: 45-49.[16] 孙枫，唐李军．基于CKF的SINS大方位失准角初始对准[J]．仪器仪表学报，2012，33(2)：327-333.Sun F, Tang L J. Initial alignment of large azimuth misalignmentangle in SINS based on CKF[J]. Chinese Journal of ScientificInstrument, 2012, 33(2): 327-333.[17] 郝燕玲，杨峻巍，陈亮，等．平方根容积卡尔曼滤波器[J]．弹箭与制导学报，2012，32(2)：169-172.Hao Y L, Yang J W, Chen L, et al. Square root cubature Kalmanfilter[J]. Journal of Projectiles, Rockets, Missiles and Guidance,2012, 32(2): 169-172.[18] Thrun S, Fox D, Burgard W. Monte Carlo localization with mix-ture proposal distribution[C]//17th National Conference on Ar-tificial Intelligence. Cambridge, MA, USA: MIT Press, 2000:859-865.[19] Havlicek M, Jan J, Brazdil M, et al. Nonlinear estimation ofBOLD signal based on cubature particle filter[C]//20th Inter-national EURASIP Conference. Brno, Czech: Brno UniversityTechnology Vut Press, 2010: 328-332.[20] 弋英民，刘丁．有色过程噪声下的轮式机器人同步定位与地图构建[J]．电子学报，2010，38(6)：1339-1343.Yi Y M, Liu D. Colored-state-noise simultaneous localizationand map building for wheel robots[J]. Acta Electronica Sinica,2010, 38(6): 1339-1343.[21] Australian Centre for Field Robotics. Source code[DB/OL].(2008-06-10) [2012-05-21]. http://www-personal.acfr.usyd.edu.au/tbailey/.[22] Ribas D, Ridao P, Tardos J D, et al. Underwater SLAM in man-made structured environments[J]. Journal of Field Robotics,2008, 25(11/12): 898-921.