基于SURF特征的单目视觉SLAM方法研究∗

胡 衡梁岚珍

(1.新疆大学电气工程学院，新疆乌鲁木齐830046；2.北京联合大学自动化学院，北京100101)

0 引言

移动机器人在未知场景中进行同步定位与地图创建(Simultaneous Localization and Mapping，SLAM)一直都是移动机器人研究领域的热点[1].视觉SLAM中一般采用SIFT（Scale Invariant Feature Transform）特征点[2]作为场景地图中的路标，因为它对图像缩放、旋转、仿射变换、光照等具有不变性且对噪声不太敏感.但SIFT算法的计算量大、时间复杂度高等缺点，导致图像匹配的速度较慢，因而难以满足实时性高的场合[3].Bay等人[4]在SIFT检测算子的基础上提出了基于积分图像和方形滤波器的SURF(Speeded Up Robust Features)算法，其检测速度快，鲁棒性也较好.针对移动机器人单目SLAM问题，文章采用SURF特征点，利用反向深度法进行特征初始化，提出一种基于扩展卡尔曼滤波器(Extended Kalman Filter，EKF)的单目SLAM方法.

1 基于单目视觉的EKF-SLAM框架

文章使用内参数已知的单目相机完成室内结构化场景的视觉SLAM任务，应用扩展卡尔曼滤波器融合SURF特征信息与机器人位姿信息并实现地图的更新.系统框架如图1所示.

图1 系统框架

首先，使用单目视觉传感器对未知环境进行观测，采集的图像经过畸变校正后进行SURF特征提取，将当前帧与前一帧所提取出的特征点进行匹配，把匹配成功的特征点确定为环境路标并加入到特征地图中.在首次检测出图像SURF特征之后，采用逆深度参数化方法对它们进行非延时初始化，创建出初始地图.之后机器人在行进过程中通过观测和识别场景视觉特征不断创建环境地图.当观测结果与特征地图库中的某个特征路标点相匹配时，用该特征点来更新地图，并依据EKF算法更新机器人的位姿信息，否则将该特征点作为新的环境路标添加到所创建的特征地图库中.

2 基于EKF的单目SLAM算法

2.1 相机的运动模型

一般情况下，相机的状态向量xV由它在世界坐标系中的3维坐标、方位角、线速度、角速度等矢量组成，共13个元素[5]，可表示如下

其中，rw是相机光心坐标、qW C是方位角的四元素表示，vW、ωW分别表示相机运动的线速度和角速度.由此可得出单目SLAM的相机状态模型

2.2 相机的观测模型

相机提供对环境特征的观测信息，测量过程处于特征点的提取阶段.根据Montiel所提出的反向深度法[6]，可将场景中路标点表示成如下6维状态向量

其中(xi,yi,zi)T是首次观测到该特征点时相机光心的3维坐标，θi、ϕi分别表示该特征点沿光轴方向在世界坐标系中的极角与仰角，ρi用来表示反向深度大小.假设所观测特征点的状态为yi，m(θi,ϕi)为该特征点光线投影向量在世界坐标系中的表示，则基于逆深度参数化方法的观测模型[7]如下图2所示.

根据三角关系可知，此时相机到该特征点之间距离为

根据各坐标系间的旋转变换关系，将该特征点由世界坐标系转化到相机坐标系中，得其对应的投影坐标为

图2 相机的观测模型

因此，在没有畸变的情况下特征点在图像上的投影像素坐标可以通过相机的针孔模型得出

实际上，相机在运动过程中一般都会受到多种畸变影响，本文仅考虑相机镜头的径向畸变，对公式(6)的结果采用二阶径向畸变校正，得到经过补偿后的特征点位置如下

2.3 单目SLAM系统的扩展卡尔曼滤波

在SLAM过程中，相机状态xV和环境地图中所有路标点信息yi需要同时不断进行估计和更新，故系统的全状态向量如下

EKF过程一般分为两步，首先根据k时刻的状态对k+1时刻的状态进行预测，得到k+1|k；然后再利用所得到的观测值对状态进行不断更新，得到接近真实值的估计k+1|k+1.

SLAM系统的全状态EKF预测阶段可描述如下

因为已假设所处场景是静止的，所以在预测时各路标点的方差与均值都将保持不变.

在更新阶段中，首先求出卡尔曼增益

然后根据测量值进行更新

其中，zk+1是图像特征点实际观测坐标，I是单位阵.

如果有新特征点添加，则根据目前相机状态和图像观测坐标按图2所示的方法获取新特征点的状态和方差，然后将其增广到系统的全状态估计向量k+1|k+1和协方差矩阵Pk+1|k+1中.

3 仿真结果与分析

选取实验室某处环境为实验场景，根据SURF特征点提取与匹配方法进行单目EKF-SLAM仿真实验.如图3(c)所示，经过8帧的EKF滤波，路标特征点的位置可收敛到一定范围内.随着EKF迭代次数的不断增加，地图特征点信息的不确定性将逐渐减小而最终能够收敛在估计值附近，如图3(d)第60帧的SLAM结果所示.同时，随着地图特征点的位置越来越精确，相机的定位准确性也随之不断提高，相机的三维轨迹如图4所示.

图3 不同时刻路标特征点的分布及SLAM结果

图4 相机三维轨迹图

路标点和相机的距离估计误差分别如图5所示.显然，误差值随着扩展卡尔曼滤波迭代次数的增加而减小，最终逐步趋于稳定范围内.结果显示，路标点在x、z方向上的最大误差不超过0.04 m；相机在x、z方向上的最大误差不超过0.01 m.产生误差的原因主要是因为复杂的实验场景和实验器材测量精度有限.需要说明的是，为了显示直观，除图4外都仅显示了x、z方向上2维结果.

图5 路标点及相机的x、z方向上距离误差

4 结论

文章提出一种基于SURF特征的扩展卡尔曼滤波单目视觉SLAM方法并通过了仿真实现.在传统EKFSLAM基础上，采用SURF算子提取场景图像中的路标点，结合反向深度估计法初始化后，将包含机器人位姿和场景路标点信息的全状态向量及其协方差矩阵应用扩展卡尔曼滤波器进行迭代递推，从而完成系统状态的估计更新和场景地图的创建.实验仿真效果比较理想，表明了该方法能够在仅有单个摄像头的情况下很好地完成室内结构化场景的地图创建任务.