一种实时的单目视觉SLAM改进算法
该算法采用一个摄像头作为外部传感器来提取机器人行进过程中周围环境的特征信息，用实时性良好的FAST提取环境特征点，结合逆深度参数化进行特征点非延时初始化，用压缩扩展卡尔曼滤波更新地图。实验研究表明，该方法提高了算法的鲁棒性和实时性。
0 引言
移动机器人自主导航是自治机器人领域研究的热点。而同时定位与地图构建（Simultaneous Localization And Mapping，SLAM）[1]被认为是自主导航的关键，因此研究SLAM算法有着深远意义。一个视觉传感器可看作一种被动传感器，与看作主动传感器的激光、红外和声呐等传感器相比，价格低廉，抗干扰性强，获取信息丰富。因为视觉传感器通过发射光或波来获取数据而不会改变环境，并且所获得的图像包含更多的信息，所以，研究基于机器视觉感知机制的SLAM更具有价值和应用前景。
世界范围内开展了大量该领域的广泛研究，并且已得出一些非常有价值和应用前景的研究成果。20世纪90年代，Horswill研制了POLLY机器人，该机器人采用单目视觉进行导航，但是只能工作在一个颜色固定的场景中。在以后的几年中，研究人员还提出采用占据栅格的地图构建框架和特征位置检测算法，即通过单个摄像头在线处理RGB图像序列，该系统并不采用传统的匹配方法，而是计算每个位置处找到物体的概率。在立体视觉中，机器人可通过两个或多个摄像头来测量距离信息，然而，使用多个摄像头会增加处理成本，实时性会变得非常差。DAVISON A J等人在2007年实现了仅用一个火线接口摄像头作为传感器的monoSLAM[2]算法。但是该单目视觉SLAM算法利用扩展卡尔曼滤波（Compressed Extend Kalman Filtering，CEKF），对于运行时间较长的任务，路标数会不断增大，最终导致计算资源不足以实时更新地图，该问题的产生是由于每个路标都与其他路标相关联。而CEKF算法可在不影响结果准确度的条件下显著减少计算需求，正好能够改进monoSLAM算法存在的问题，所以本文利用实时性良好的FAST角点检测和CEKF对目前的monoSLAM算法进行改进，并运用特征点的逆深度非延时初始化技术，加入了摄像机的经典二参数径向畸变模型，在降低计算复杂度的同时提出一种鲁棒性更高的实时单目视觉SLAM算法。
1 图像特征的检测与提取
视觉SLAM算法是基于环境的特征和纹理的，所以特征点检测和提取作为计算机视觉中一个很重要的技术应用到解决视觉SLAM中。加速分割测试特征（Features from Accelerated Segment Test，FAST）算法是由 ROSTEN E和DRUMMOND T[3]在2006年提出来的。它仅仅利用中心点周围像素同其比较的信息就能够提取到特征点。相对于许多其他的特征点检测算法，FAST角点检测计算速度非常快，可以应用到实时场景中。FAST角点检测算法提出后，机器视觉领域中特征提取实时性能才有了显著的提升，目前它以其高计算效率、高重复性成为机器视觉领域最流行的角点检测算法。
FAST特征点提取算法来自角点的定义，角点就是极值点，可以设定特定的临界值进行角点检测。特征点检测就是基于候选角点周围的一个像素块，利用像素块上面各个像素点与候选角点灰度值的差值点的个数来判断是否是一个角点，一般如果这些点的个数大于总数的3/4，就判别该点为一个环境特征点。
其中，I（x）代表了像素块上任意一点的灰度值，I（p）为候选中心角点的灰度值，灰度差临界值设为t，N为达到灰度差临界值的像素点的个数。如果N像素点的个数大于总数的3/4，则p是一个特征点。
FAST角点检测的步骤如下：
（1）从图片中选取一个候选点p，其中，把它的像素值设为I（p）；
（2）设定一个合适的灰度差临界值t；
（3）检测候选点周围像素块的16个像素点。如果在这16个像素点中有12个像素点的灰度值与候选点差值的绝对值大于t，那么它就是一个环境特征点；
（4）利用机器学习算法快速排除那些伪角点。该机器学习算法只检查圆上具有代表性的4个点像素，首先检查上下两点，看它们是否高于或低于阈值，如果是，再检查左右两点。如果p是一个特征点，则上述4个点中至少有3个灰度差的绝对值要大于t，如果都不满足上述条件，那么p不是一个特征点。
在实验室内用FAST角点检测算法提取的特征点，如图1所示。
2 单目视觉同时定位与构图
SLAM的实质是移动机器人能够建立环境地图并同时根据该地图来推导自身位置，初始时地图和移动机器人位置均未知，已知机器人的运动学模型，并在一个存在大量人工路标或自然路标的位置环境中运动，根据对环境特征的观测，同时估计自身和环境特征的位置。
单目视觉SLAM中根据携带的内部传感器对当前所在位置进行预估计，也就是SLAM算法中的预测过程，然后通过观测已经创建好的地图特征点逐步来更新自己的位置和地图特征点信息，最后通过单目视觉传感器观测提取环境特征点，创建并更新环境地图。单目视觉SLAM算法框图如图2所示。
2.1 单目视觉SLAM全状态模型
单目视觉SLAM的全状态模型由目前已经创建的地图中所有路标的状态和相机运动状态组成，可表示为：
其中，xc是相机的状态，yi是当前地图中所有环境特征点的状态。
协方差矩阵表示为：
2.2 单目视觉SLAM运动模型
在实验中通过实验人员手持一个摄像头移动来模拟真实的移动机器人探索未知环境，这样相机的运动模型就是机器人的运动模型。在这里使用了CIVERA J等人提出来的恒定速度模型[4]，相机的运动模型使用13个变量进行建模。用相机的光心运动状态等效成移动机器人的运动状态，相机的光心用rWC来表示，相机坐标系到世界坐标系之间的转换关系用四元数qWC表示，用vW表示相机线速度，相机角速度用C表示。因此，得到相机的运动状态表示为：
其中，下标C表示camera，上标WC指定由相机坐标系到世界坐标系，上标W指定以笛卡尔坐标系作为参考系，上标C指定以相机坐标系作为参考系，因此，可得到单目视觉SLAM的运动模型：
此外相机运动过程中会受到很多噪声干扰，为了降低干扰，对相机的动力学模型假设成速度不变高斯模型，即任何速度噪声满足零均值高斯分布。
2.3 单目视觉SLAM观测模型
由于采用单个摄像头只能获取前向信息，而不能获取图像中特征的深度也就是距离信息，进而不能获得地图中特征点的三维信息，因此必须对提取特征点作一定的处理，这就需要特征点逆深度参数化。环境特征点参数化表示方法有欧氏参数化和反深度参数化[5]。
（1）欧氏参数化及其观测模型
（2）反深度参数化及其观测模型
其中，yi的前3个元素表示相机首次观测到该路标点那一时刻光心的三维世界坐标，第4个元素是指在笛卡尔坐标系下从相机的光心观测路标点的极角，同样第5个元素为仰角，第6个元素表示深度的倒数，h是观测方程，向量m为该特征点坐标在笛卡尔坐标系下的投影。
2.4 单目视觉SLAM预测更新模型
在这个过程中就需要利用CEKF对整个系统进行预测和更新，CEKF算法存储和保持一个局部区域所采集的所有信息，以及与该区域中路标数二次方成正比的成本，然后以类似于完全SLAM的成本将这些信息转移到全局地图中的其余部分，但是只需一次迭代运行。
2.4.1 预测过程
预测状态变量：
预测误差协方差：
其中，F是fv（x）的雅可比矩阵，Q是fv（x）关于噪声n的偏导数，Pk（1：13，1：13）表示Pk里面包含元素的第1到第13行或者第1到第13列，end指的是矩阵的最后一行或一列。
2.4.2 更新过程
计算卡尔曼增益：
3 实验结果
本实验图像采集场景为实验室环境，用一个分辨率为640×480普通手机摄像头采集图像。在MATLAB 2013a下对该单目视觉SLAM算法进行仿真实验，程序运行的系统为Win8.1 64位环境，计算机的CPU为Intel酷睿i5-3230M，内存为4 GB，相机速度为25 f/s的速度下可以满足实时性要求，得到图3和图4所示的结果。
图3是初始图像帧，灰色点是提取的特征点，由于只有一个摄像头，在第一幅图像中无法对特征点进行定位，可以看到图3中右半部分环境特征点是发散的，而随着图像序列的推移，特征点匹配的成功率提升，到最后可以看到图4的实验结果，可以看到图4右半部分的特征点已经收敛了，也就是已经定位，其灰色收敛椭圆代表特征点定位情况，曲线代表摄像机的运动轨迹，至此实现了该算法。