扩展卡尔曼体系下的SLAM算法计算框架

摘 要：扩展卡尔曼体系下的SLAM算法是指SLAM算法中含有状态预测过程和状态更新过程。为了便于理解扩展卡尔曼体系下的SLAM算法计算过程，本文从滤波算法计算框架角度对它们分别进行了描述，对基于EKF的SLAM算法进行了说明和总结。

关键词：扩展卡尔曼体系；SLAM算法；计算框架

1 前言

SLAM问题的本质是移动机器人整个路径的概率估计问题。作为解决SLAM问题的主要方法之一，扩展卡尔曼滤波算法（EKF）在模型基本匹配时具有较高的估计精度，且具有模型简单、易于实现等特点[1]。

2 EKF-SLAM算法

在应用EKF处理SLAM问题时，其变量被假设服从高斯分布，可用均值及方差表示， 分别为系统在k时刻的状态估计值及状态估计协方差[2]：

其中，fx，fu分别为f（・）相对于X和u的Jacobian矩阵，nv，nM，nu分别为机器人位姿 、地图 、及控制命令uk的维数。fvx、fvu分别为fv（・）相对于Xv和u的Jacobian矩阵。f（・）是系统的状态转移函数，通常状况下是非线性的[3]。在EKF-SLAM中，系统噪声υk及观测噪声ωk认为是均值为零，方差分别为Qk及Rk的高斯白噪声，且系统噪声与观测噪声相互独立[4]。

EKF-SLAM算法步骤：

⑴预测：利用系统的运动模型，结合k时刻系统状态估计值 和控制命令uk，预测系统在k+1时刻的状态（ ）、状态估计的协方差矩阵（ ）和系统的观测值（ ）：

⑵观测：利用传感器观测模型得到机器人对于第i个路标的实际观测 ，结合预测步骤得到的 ，此时可以得到新息（Innovation），以及该新息的协方差矩阵：

⑶更新：EKF-SLAM算法利用下式更新k+1时刻的系统状态估计及其相应的协方差矩阵：

⑷向量增广：传感器在任意时刻观测到的环境特征，可能即包括地图中已有的特征也有新特征。其中已有特征可以用来更新状态预测值，而新特征则要进行初始化并加入到状态向量中[5]

其中，gx，gz分别为g（・）相对于Xv和Z的Jacobian矩阵。

3 小结

扩展卡尔曼滤波算法（EKF）的运用使得非线性问题线性化，这样线性化后的预测值的方差依然可以按原来的方法进行线性计算。其原理是在上一时刻的预测点将系统状态方程和观测方程进行Taylor展开，并忽略所有非线性展开高阶项，来实现对非线性方程的近似线性化。目前EKF已成为了最基本的SLAM问题解决方案。

[参考文献]

[1]周武，赵春霞，沈亚强，等.基于全局观测地图模型的SLAM研究[J].机器人，2010，32（5）：647-654.

[2]张文玲，朱明清，陈宗海.基于强跟踪UKF的自适应SLAM算法[J].机器人，2010，32（2）：190-195.

[3]郭剑辉.移动机器人同时定位与地图构建方法研究[D].[博士学位论文].南京：南京理工大学计算机科学与技术学院，2008.

[4]段战胜，韩崇昭，党宏社.一种带未知时变系统噪声水平的目标跟踪滤波器[J].系统仿真学报，2004，16（11）：2591-2593.

[5]范文兵，刘春风，张素贞.一种强跟踪扩展卡尔曼滤波器的改进算法[J].控制与决策，2006，21（1）：73-76.