基于扩展卡尔曼滤波的同步定位与地图构建（SL A M）算法研究进展

刘 畅

（天津职业技术师范大学机械工程学院，天津300222）

同步定位与地图构建（Simultaneous location and mapping，SLAM），指身处未知环境的机器人通过自身传感器信息实现定位并生成环境增量式地图的过程[1]。该问题是概率机器人学领域的重要问题，是利用基于模型的方法与基于行为的方法相结合的形式来处理机器人在未知环境自主移动问题的一门学科，是移动机器人实现智能的根本[2]。大疆科技工程师表示：“关于无人机的所有美好愿景都基于SLAM技术，SLAM技术是区分无人机与玩具的标准，是无人机进入能飞时代后最核心的技术”[3]。SLAM问题的难点在于其系统误差的高度耦合性，定位需要精准的地图，同时构建精准的地图需要精确的机器人位姿。因此，早期对于SLAM问题的研究的主要内容在于累积噪声的消除。

1 SL A M问题的数学模型

同步定位与建图是可以归结为状态估计问题，其核心在于已知含有噪声的机器人控制量、环境观测信息来估算环境特征和机器人路径并使其误差达到最小，其数学描述如图1所示[4]。

其中，xk代表k时刻表示机器人位姿的状态向量；mi表示机器人观测到的路标点，即环境特征；uk为机器人的控制向量；zk，*为机器人在k时刻观测向量的集合，其中*指代路标点的序号，如在k-1时刻观测向量为：zk-1，i，zk-1，i.采用集合的方式表述上述变量：

所有环境特征的集合为 m1∶l＝ {m1，……，ml}；k 时刻之前所有观测向量的集合为 z1∶k＝ {z1，……，zk}；k时刻之前所有控制向量的集合为u1∶k＝{u1，……，uk}.

假设SLAM问题中的运动与观测模型遵循马尔可夫假设，即未来与过去数据相互独立，所以可将其描述为关于路径与环境地图的估计，其概率模型为：P（xk，m1∶l｜z1∶k，u1∶k）.

2 SL A M问题的研究方法

早期的SLAM算法为基于扩展卡尔曼滤波的EKFSLAM算法。卡尔曼滤波器是递推算法的一种，通过实时获得的受噪声污染的离散观测数据，对系统状态进行线性、无偏及最小误差方差的最优估计[5]。

首先，通过运动方程及观测方程对SLAM过程进行描述，有

其中，wk、vk，j为噪声项，yj为路标点坐标值。假设整个SLAM系统为线性高斯系统，有：

其中x为包含机器人位姿信息及地图信息的联合状态矢量，Ak为状态转移矩阵，Ck为系统观测矩阵。根据高斯分布性质有：

此外，根据观测方程有;

根据SLAM系统概率模型得出，k时刻状态量的估计满足高斯分布，设其后验概率分布的均值与协方差分别为 ，则有：

由此可得：

设

则有：

同样可得：

由此完成了k时刻状态量的后验概率分布估计，该过程被称为“更新”。卡尔曼滤波器通过控制量“预测”再利用观测值“更新”，其结构图如图2所示。

图2 卡尔曼滤波算法

然而在实际应用中所面临的大部分观测方程与运动方程为非线性函数。为此，研究人员将卡尔曼滤波器进行推广，得到适用于非线性系统的扩展卡尔曼滤波器，其基本思想在于将观测方程运动方程在特定点附近进行泰勒展开，并只保留一阶项，从而近似为线性系统，即

则有预测过程：

计算卡尔曼增益：

更新过程为：

在非线性优化方法中，模型将轨迹中离散的所有位姿设为变量，将运动方程与观测方程作为变量间约束，构造代价函数：

其中，z指代观测数据，在以视觉传感器为数据来源的SLAM中指代像素坐标（us，vs），ξ即相机外参R、t对应的李代数形式，也即前面数学模型中的x，p为三维空间点坐标，即数学模型中的的路标m.

3 与机器学习相结合的SL A M研究

现有的SLAM经典算法仍存在局限，基于EKF的SLAM算法存在引入线性化误差、难以表达回环、对数据关联失误的鲁棒性差，而基于非线性优化方法的SLAM算法仍存在容易陷入局部最小，在巨大场景下计算效率低导致无法实时化。随着人工神经网络学科的发展，研究人员开始寻找新的SLAM问题解决途径：针对观测运动方程运动方程高度非线性、实际物理模型不能完全获取或系统参数变化等情况，文献[6]中论述了一种利用神经网络构建逼近函数e（X（k），u（k），β（k））的方法拟合实际模型与理论模型之间的误差，并将该误差加入到时间更新模型中，以提高系统状态估计的精确度，其结构如图3所示。

图3 神经网络复合卡尔曼滤波算法

在经典的基于扩展卡尔曼滤波的SLAM系统中，系统状态状态向量由机器人位姿向量Xv=[xv，yv，φv]T和环境特征位置向量 Xm=[xT1，xT2……xTm]组成：X=[xTv，xTm]T，系统协方差矩阵由机器人位姿协方差矩阵Pvv、环境特征协方差矩阵Pmm和机器人位姿与环境特征协方差矩阵Pvm组成神经网络复合卡尔曼滤波算法将前向神经网络的权值Xβ融入到SLAM模型中，将SLAM问题的维数扩展，则系统状态向量为：X=[Xv，Xβ，Xm]T，系统协方差矩阵中增加了机器人位姿、环境特征与神经网络权值的协方差 Pvβ及 Pβm，为

早期应用的神经网络模型隐含层层数普遍较少，这限制了模型对系统分类、拟合的表现。而近年来，人工神经网络的规模大约每2.4年翻一番[7]，其归根结底来源于计算机硬件更新的推动，GPU（Graphics Processing Unit，图形处理器）以其出色的浮点计算性能、高效的并行结构为多层神经网络模型开发部署提供了理想平台，推动了深度学习的发展。与传统人工神经网络相比，深度学习模型非线性操作层级数更多[8]，能够构建更为复杂的模型处理更高数量级的数据，从应用上讲，深度学习能够处理更高层次、更抽象的特征，也因此部分学者致力于采用卷积神经网络等深度学习模型来代替传统的SLAM框架实现对于SLAM领域的颠覆，而其表现仍需等待时间来考证[9-12]。

[1]高 翔，张 涛，刘 毅，等.视觉SLAM十四讲从理论到实践[M].北京：电子工业出版社，2017.

[2]Sebastian Trun，Wolfram Burgard，Dieter Fox.概率机器人[M].北京：机械工业出版社，2017.

[3]常 建.无所不能的SLAM无人车、无人机、AR都离不开它[J].计算机与网络，2017，43（5）：18-19.

[4]赵一路.移动机器人SLAM问题研究[D].上海：复旦大学，2010.

[5]Charles K.Chui，Guanrong Chen.卡尔曼滤波及其实时应用[M].北京：电子工业出版社清华大学出版社，2013.

[6]王忠善.移动机器人同时定位与地图创建的研究[D].哈尔滨：哈尔滨工业大学，2008.

[7]伊恩·古德费洛，约书亚·本吉奥，亚伦·库维尔.深度学习[M].北京：人民邮电出版社，2017.8.

[8]尹宝才，王文通，王立春.深度学习研究综述[J].北京工业大学学报，2015，41（1）：48—59.

[9]Wang Sen，Ronald Clark ，Wen Hongkai et al.DeepVO：To wards End-to-End Visual Odometry with Deep Recurrent Convolutional Neural Networks[C]//2017 IEEE International Conference on Robotics and Automation（ICRA）.Singapore，2017：2043－2050.

[10]Antonio Loquercio，Marcin Dymczyk，Bernhard Zeisl et al.Efficient Descriptor Learning for Large Scale Localization[C]//2017 IEEE International Conference on Robotics and Au tomation（ICRA）.Singapore，2017：3170-3177.

[11]Gao Xiang，Zhang Tao.Loop Closure Detection for Visual SLAM Systems Using Deep Neural Networks[C]//Processing of the 34th Chinese Control Conference.Hangzhou，China，2017：5851－5856.

[12]John Mccormac，Ankur Handa，Andrew Davison et al.Se manticFusion：Dense 3D Semantic Mapping with Convolu tional Neural Networks[C]//2017 IEEE International Confer ence on Robotics and Automation（ICRA）.Singapore，2017：4628-4635.