迭代的平方根容积卡尔曼滤波SLAM算法

陶 明，凌有铸，陈孟元，戴雪梅

（安徽工程大学安徽省电气传动与控制重点实验室，安徽芜湖241000）

迭代的平方根容积卡尔曼滤波SLAM算法

陶 明，凌有铸，陈孟元，戴雪梅

（安徽工程大学安徽省电气传动与控制重点实验室，安徽芜湖241000）

平方根容积卡尔曼滤波算法在移动机器人同步定位与地图创建问题中，存在随着地图特征点增多、容积点偏离理想轨迹、状态估计产生较大误差的缺陷。为此，提出一种改进的平方根容积卡尔曼滤波算法。该算法引入迭代测量更新的方法，在更新阶段利用估计值和平方根因子重新确定采样的容积点，使得采样点在高度非线性环境下保持较小失真，进一步提高精度。仿真结果表明，与平方根容积卡尔曼滤波算法相比，该算法能提高机器人位姿精度。

迭代；同步定位与地图创建；采样；扩展卡尔曼滤波；加权处理

1 概述

同步定位与地图创建（Simultaneous Localization and Mapping，SLAM）可以表述为：在未知环境下，移动机器人利用传感器所收集的信息创建特征地图，移动同时不断对自身的位姿进行修正的过程［1］。SLAM关乎移动机器人能否自主运动，是该领域的研究热点。

经典的扩展卡尔曼滤波（Extended Kalman Filtering，EKF）［2-3］是应用于SLAM领域的主流算法，其实质是对非线性的系统模型和观测模型一阶泰勒级数展开，将模型线性化后通过标准的卡尔曼滤波处理。在EKF-SLAM的基础之上，许多学者都提出了改进的新型算法。文献［4］提出迭代的扩展卡尔曼滤波（Iterated Extended Kalman Filtering，IEKF）算法，该算法依托环境特征的迭代与EKF算法的融合，修正了机器人的非线性误差，提高了定位的精度。文献［5］提出了一种基于外部干扰检测对比，膨胀定位误差的抗干扰EKF-SLAM算法，提高了算法的鲁棒性。但是EKF在线性化的过程中仅把展开的二阶以上的高次项做简单的舍弃处理，在定位要求非常精确的场合下，不容易达到理想的效果。且线性化过程中需要计算雅可比矩阵，会造成大幅增加计算量，不适用于大地图环境。

近年来比较流行的无迹卡尔曼滤波（Unscented Kalman Filtering，UKF）［6-7］通过选取不同权值的采样点（Sigma点）随非线性函数传递，经过无迹变换（Unscented Transformation，UT）得到随机变量的统计特征。UKF在卡尔曼滤波的框架下，减少了非线性方程线性化产生的误差，能够使定位精度在同等情况下达到二阶。文献［8］在数学上推导出用协方差矩阵的平方根代替协方差矩阵传递，理论上能有效解决了这一问题，并能很好地应用于SLAM问题中。文献［9］在平方根UKF的基础之上，改变了Sigma点采样的策略，提出一种比例最小偏度单行采样的算法，减少了算法的计算量，提高了实时性。然而UKF算法在滤波过程可能会导致协方差矩阵非正定，影响滤波器的性能。

2009年，A rasaratnam和Haykin提出了容积卡尔曼滤波（Cubature Kalman Filtering，CKF）［10］，提供了一种新的非线性系统状态估计方法。CKF在容积规则下选取一组相应权值的容积点，带入非线性函数传递后经过加权处理得到随机变量的统计特性。CKF与UKF相比，同样减少了非线性方程线性化产生的误差，但CKF以其计算量小、数值精确度高、滤波稳定性强的特点被越来越多的学者肯定，成为一种解决各种估值问题的有效手段。文献［11］提出了一种基于容积平方根卡尔曼滤波（Square Root Cubature Kalman Filtering，SR-CKF）的SLAM算法，提升了整个系统稳定性。

以上述研究内容为基础，本文提出一种迭代平方根容积卡尔曼滤波（Iterated Square Root Cubature Kalm an Filtering，ISR-CKF）算法。该算法在SRCKF算法基础上引入了高斯牛顿迭代方法，汲取了SR-CKF算法的优点：采用协方差的平方根来传递矩阵信息削弱了截断误差对SLAM的误差精度影响。同时通过增加迭代过程以利于量测信息的充分利用。

2 地图创建与同步定位问题描述

SLAM问题的实质是：通过移动机器人内部的运动信息和传感器外部采集的观测信息对机器人的位姿和特征地图进行估计。

机器人运动模型为：

机器人观测模型为：

其中，WK服从N（0，QK）的高斯分布；VK服从N（0，RK）的高斯分布［12］。

因此，通过数学语言SLAM问题可以表述为：

即已知控制量向量U1：K和观测向量Z1：K，估计当前时刻特征地图向量和状态向量SrK的联合概率分布。由式（1）可得求解出每个时刻满足正态分布向量SK的最优解及其协方差PK成为了解决SLAM问题的核心问题。

本文所提出的ISR-CKF算法通过采集到的信息，预估计机器人的位姿和特征地图信息。

其核心是通过容积变换公式式（2）计算条件转移概率密度，进而在高斯域下实现贝叶斯滤波：

其中，f（y）为非线性函数；N（；）为高斯分布；向量y为ny维，y服从均值为μ；方差为ξi表示互相正交的完全对称容积点集。

在式（3）中，ξi有2（ns+nu）列：

ISR-CKF算法根据贝叶斯滤波的基本原理，其具体的求解过程分为预测、更新2个步骤，分别到达求出K时刻先验概率分布和后验概率分布的目的：

（1）利用K-1时刻后验概率分布及K时刻运动控制量预测求出先验概率分布：

（2）利用K时刻观测量与先验概率分布更新后验概率分布：

其中，η为常量。

3 迭代平方根容积卡尔曼滤波

SR-CKF算法是在卡尔曼滤波框架下，利用三阶球面的相径容积规则，通过确定的容积点集传递后加权得到非线性函数的均值和协方差平方根因子。在传递过程中采用协方差矩阵平方根（基于QR分解）代替协方差矩阵信息更新来解决由于计算机舍入误差可能导致的滤波器发散问题。具体的预测阶段算法描述如下：

（1）预测阶段

其中，j为容积点序号，取值为1，2，…，2（nu+ns）；矩阵包含了K-1时刻的位姿信息，特征点信息和运动控制信息。由机器人状态信息SK-1和运动信息uK增广为高斯噪声变量，经式（7）处理可得矩阵和矩阵

2）计算各容积点的先验估计，以容积点j为例：

每个容积点经非线性运动方程传播后可以得到机器人K-1时刻位姿信息，和机器人K时刻运动信息，预测K时刻机器人位姿信息。

3）机器人位姿状态预估计和平方根因子预测

根据容积变换公式式（2）可得：

经过QR分解地图特征误差向量AK|K-1可以得到平方根因子矩阵CK|K-1用于下一步更新阶段：

虽然SR-CKF-SLAM算法能够很好地提高移动机器人位姿定位精度和系统控制的稳定性，但是随着地图特征点的增多，系统观测的维度会逐渐增加，导致容积点偏离理想轨迹，从而在状态估计中产生很大的误差。因此，本文采用迭代方法，在更新阶段利用估计值和平方根因子重新确定采样的容积点，通过容积变换得到系统的统计特性，再结合预测阶段估计的新观测值改善系统状态估计，从而提高算法精度。

（2）更新阶段

以第i个特征点为例，假设迭代初始值分别为SK|K-1和CK|K-1，第l次迭代机器人位姿信息和平方根因子分别为其K时刻的观测向量其观测模型为：

1）计算迭代容积点

2）计算第l次迭代卡尔曼增益

4）设置迭代终止条件

其中，Lmax为迭代最大次数，是预先设置的固定常量。

5）迭代终止，各数据更新：

位姿更新：

当观测到多个特征点时，需要重复式（12）～式（25）。

4 仿真实验与分析

SLAM算法精度研究在Matlab7.0软件环境下，使用主频3.4 GHz，Core-i3双核处理器，4 GB内存的计算机进行仿真实验。在澳大利亚学者Tim Bailey提供的开源仿真实验平台［13］上，分别对UKFSLAM，SR-CKF-SLAM，ISR-CKF-SLAM 3种算法进行仿真实验，分析各个算法之间的精度差异。

仿真实验中的机器人运动方程为：

其中，矩阵mK表示机器人位姿；Ts表示系统内部采样间隔；νK表示机器人行进速度；αK表示转角；L表示轮距。

仿真实验中的机器人观测矩阵方程为：

其中，lK表示机器人与观测到地图特征点的距离；βK表示机器人与观测到地图特征点的夹角；（mχ，i，my，i）为机器人观测到的地图特征点i的位置。

实验假设的地图环境为：250 m×200 m的室外环境，17个确定的路径点，35个地图特征点。实验参数设置如下：机器人行进速度为3 m/s，轮距为4 m，探测传感器探测范围为30 m，最大探测视角为前向180°，系统内部采样间隔为25 ms，速度误差为0.4 m/s，转角误差为2°，距离观测误差为0.15 m，角度观测误差为1°，ISR-CKF-SLAM算法设定迭代次数为5次。

机器人的行进轨迹仿真结果如图1～图3所示。

图1 UKF-SLAM算法机器人的行进轨迹

图2 SR-CKF-SLAM算法机器人的行进轨迹

图3 ISR-CKF-SLAM算法机器人的行进轨迹

机器人行进完全部轨迹大概需要240 s，经过多次实验选取平均值。本文取仿真结果的前180 s进行分析。

在行进的初始阶段3种算法的运行轨迹与理想轨迹偏差基本不大，随着地图特征点的不断增多，3种算法的运行轨迹相较于理想轨迹都有不同程度的偏移，但是ISR-CKF-SLAM算法与UKF-SLAM算法、SR-CKF-SLAM算法相比更契合理想轨迹的运行路线，即意味着位姿估计更加精确。由图4可以得出，ISR-CKF-SLAM无论是在X方向、Y方向、还是位姿角上的估计误差一直保持在一个稳定的范围之内，且与UKF-SLAM、SR-CKF-SLAM相比都保持着较高的精度。这是因为通过迭代方法的测量更新，重新采样的容积点会减小对系统模型的线性化误差，提高了地图特征点位置估计的精度，进而提高移动机器人的定位精度。

图4 3种算法估计误差对比

为了更加直观地表达3种算法性能差异，采用统计数据列表的方式分析。由表1可知，相比SRCKF-SLAM，ISR-CKF-SLAM在位姿X方向误差降低了44%，在位姿Y方向误差降低了10.8%，位姿角误差降低了37.8%。

表1 3种算法的误差比较

5 结束语

本文提出一种迭代的平方根容积卡尔曼滤波SLAM算法（ISR-CKF-SLAM）。通过高斯牛顿迭代方法对SR-CKF算法进行改进，在使用最新更新信息的基础上，优化了容积点采样规则，降低了初始误差和线性化误差对状态估计的影响，使得以此产生的后验概率分布更加接近真实值。仿真结果表明，该算法的机器人位姿精度估计更加准确，并且更加稳定。今后将研究如何优化迭代方法，进一步提高该算法的实时性。

［1］ Durrant-Whyte H，Bailey T.Simultaneous Localization and Mapping：Part I［J］.IEEE Robotics and Automation Magazine，2006，13（2）：99-108.

［2］ Smith R C，Cheeseman P.On the Representation and Estimation of Spatital Uncertainty［J］.Robotics Research，1986，5（4）：231-238.

［3］ Smith R，Self M，Cheeseman P.Estimating Uncertain Spatial Relationships in Robotics［C］//Proceedings of Conference on Uncertainty in Artificial Intelligence. Am sterdam，Holland：［s.n.］，1988：435-461.

［4］ 强敏利，张万绪.IEKF滤波在移动机器人定位中的应用［J］.电子技术与应用，2013，39（2）：74-77.

［5］ 吕太之.一种新的抗外部干扰EKF-SLAM算法［J］.计算机工程，2012，38（21）：1-4.

［6］ Julier S J，Uhlman JK.Unscented Filtering and Nonliner Estimation［J］.Proceedings of the IEEE，2006，13（2）：99-108.

［7］ Julier S J，Uhlm an J K，Durrant-W hyte H F.A New Method for the Nonlinear Transform ation of M eans and Covariances in Filters and Estimators［J］.IEEE Transactions on Automatic Control，2000，45（3）：477-482.

［8］ Merwe R，Wan E A.The Square-root Unscented Kalman Filter for State and Parameter Estimation［C］//Proceedings of IEEE International Conference on Acoustics，Speech，and Signal Processing.Washington D.C.，USA：IEEE Press，2001：3461-3464.

［9］ 汪贵冬，陈跃东，陈孟元.比例最小偏度单行采样的平方根UKF-SLAM算法［J］.计算机工程与应用，2014，50（17）：63-67.

［10］ Arasaratnam I，Haykin S.Cubature Kalman Filters［J］. IEEE Transactions on Automatic Control，2009，54（6）：1254-1269.

［11］ 康轶非，宋永端，宋 宁，等.平方根容积卡尔曼滤波在移动机器人SLAM中的应用［J］.机器人，2013，35（2）：186-193.

［12］ 石杏喜，赵春霞.基于概率的移动机器人SLAM算法框架［J］.计算机工程，2010，36（2）：31-32，41.

［13］ Tim B.SLAM Simulations［EB/OL］.［2010-10-08］. http：//www-personal.acfr.usyd.edu.au/tbailey/.

编辑 刘 冰

SLAM Algorithm of Iterated Square Root Cubature Kalm an Filtering

TAO M ing，LING Youzhu，CHEN Mengyuan，DAIXuemei
（Anhui Key Laboratory of Electric Drive and Control，Anhui Polytechnic University，Wuhu 241000，China）

The disadvantage of Square Root Cubature Kalman Filtering（SR-CKF）algorithm on the Simultaneous Location and Mapping（SLAM）is that with map feature points increasing，the volume points deviate from the ideal trajectory to cause great defects in state estimation.In order to solve that problem，this paper provides an improved square root cubature Kalman filtering algorithm.The algorithm takes advantage of iterative method of the measurement update，which makes the sampling points less distortion and further im proves the accuracy in the highly nonlinear environment. Simulation results show that，compared with SR-CKF algorithm，this algorithm can effectively improve the accuracy of position and attitude.

iteration；Simultaneous Localization and Mapping（SLAM）；sampling；Extended Kalman Filtering（EKF）；weighted processing

陶 明，凌有铸，陈孟元，等.迭代的平方根容积卡尔曼滤波SLAM算法［J］.计算机工程，2015，41（9）：317-321.

英文引用格式：Tao Ming，Ling Youzhu，Chen Mengyuan，et al.SLAM Algorithm of Iterated Square Root Cubature Kalman Filtering［J］.Computer Engineering，2015，41（9）：317-321.

1000-3428（2015）09-0317-05

A

TP24

10.3969/j.issn.1000-3428.2015.09.058

陶 明（1989-），男，硕士研究生，主研方向：智能计算，智能控制；凌有铸（通讯作者），教授；陈孟元，讲师、硕士；戴雪梅，硕士研究生。

2014-12-05

2015-02-27 E-m ail：peerless1207@163.com