基于改进SIFT的室内机器人惯性视觉定位系统*

李 港，赖钦伟，蒋 林,1b，雷 斌,1b，梁铧杰

(1.武汉科技大学a.机器人与智能系统研究院；b.冶金装备及其控制教育部重点实验室，武汉 430081；2.珠海市一微半导体有限公司，珠海 519000)

0 引言

室内机器人的定位求解计算与传感器有着重要的关联，本质上是对传感器数据的处理。目前，室内移动机器人常用的定位传感器可以分为激光传感器与视觉传感器。激光传感器是通过测量与周围物体的相对距离实现自身位置的计算[1]。激光传感器定位精度较高，但存在价格昂贵、体积大等问题[2]。

视觉传感器相较于激光传感器数据更加直观丰富且成本较低，因此基于视觉传感器的室内定位算法更容易进行推广应用。TAREEN等[3]在大量数据集的基础上对比传统特征提取匹配算法，分析得出其中实时性最优的算法为ORB[4]算法，因此ORB常用于各种实时场景。各种图像变换下SIFT算法准确度最高，但SIFT算法复杂度过高，实时性较差[3]。BAY等[5]基于SIFT提出SURF，在计算关键点主方向以及关键点周边像素方向的方法上，使用哈尔小波转换。相比于SIFT算法，实时性有所提高，提取匹配效果有所下降。谭光兴等[6]在SIFT的基础上简化算法，通过建立十字形分区的特征描述子，将描述子的维数从128降低至64维，但仍无法到达实时的效果。

随着深度学习技术不断的发展[7]，也出现了一些基于深度学习框架的特征检测与匹配算法，如LOFTR[8]、D2-NET[9]、SUPERPOINT[10]等。其中DANIEL提出的基于自监督框架模型SUPERPOINT是非常经典的端到端学习特征，模型会输出全像素级别的特征点和特征描述符，结合特征匹配SUPERGLUE算法[11]，较其他基于深度学习框架，有着很高的匹配准确率与光照尺度不变性。其算法存在主要的问题就是复杂的深度学习框架与包含大量参数的模型对移动端配置要求过高，且在移动端过于耗时。

结合以上分析，考虑到单目视觉传感器计算量小，成本低且适合在嵌入式系统上实现，因此在机器人实验平台上搭载单目广角视觉传感器，采用顶视室内天花板的单目视觉定位方案。针对天花板特征稳定且不存在大的尺度变换的特点进一步改进SIFT，压缩尺度空间、降低描述子维度。描述子的初匹配采用基于权重的曼哈顿距离结合FLANN算法，误匹配筛除采用基于渐近样本一致性算法加速迭代过程，并利用IMU数据进一步约束匹配。在改进SIFT算法的基础上构建实时的前端惯性视觉里程计，基于权重融合IMU信息，进一步提高移动机器人室内视觉定位精度。回环检测利用惯导位姿为初值局部搜索SIFT特征点实现重定位。

1 天花板视觉

1.1 机器视觉模型

相比于朝前的相机，天花板特征较为稳定，不易受动态环境的干扰。结合机器人的运动特性分析，机器运动主要为沿Y轴的移动与绕Z轴的转动，均为平面运动且与天花板平面始终平行，因此用朝上的相机布局方式能避免大部分的尺度变换[12]。朝上的安装方式使得相机拍摄的图像变换方式主要为仿射变换，提高了定位精度。因此采用机器人视觉传感器顶视室内天花板，可视化模型如图1所示。

(a) 机器人模型 (b) 机器人观测模型图1 机器人模型与机器人观测模型

在单目相机的具体配置上，选用120°广角相机，最大帧率为30 Hz。广角相机通过扩大相机视角的方式使拍摄画面尽可能全的包含天花板信息。相机成像的具体效果如图2所示。由图可以看出，即使机器在房间较边缘区域，拍摄图像也能基本包含整个天花板。在图像用于室内定位时，单个房间仅需数张图像就可以包含该房间全部的天花板特征，这个特点使移动机器人在同一房间的拍摄画面始终具有很高的特征点重复率，简化了视觉定位系统的数据量与复杂度。

(a) 客厅 (b) 走廊图2 天花板视觉示意图

1.2 坐标系转换

视觉定位的本质是求解图像中的事物的位置与实际坐标之间存在着的某种特定的关系，也就是坐标系的转换，具体包括图像坐标系、相机坐标系、机器坐标系与世界坐标系。而首先需要求解的就是相机的内参，内参包含了图像坐标系到相机坐标系的转换关系。每个摄像机的内参都是固定且不同的，需要通过构建摄像机模型然后经过实验算法求解出摄像机的参数，该过程为相机标定[13]。

在实际算法中用第一个时刻的机器坐标系为世界坐标系。空间中一点在世界坐标系下的坐标为(xw,yw,zw)，结合机器位姿转换到此刻的机器坐标系下为(xr,yr,zr)，机器坐标系以机器的几何中心为原点，再利用机器安装数据转换到相机坐标系下为(xc,yc,zc)，经针孔相机模型成像，在图像像素坐标系下为(u,v)。具体转换关系的表达式为：

(1)

式中，K为相机的内参矩阵，由相机标定算出；Rcr为相机坐标系与机器坐标系的旋转矩阵；tcr为平移向量；Tcr为Rcr与tcr组成的变换矩阵；Trw为机器的位姿，会随着机器运动发生改变，即机器人的运动轨迹，也是定位任务中要求解的目标。

针对广角相机存在较大畸变，需要利用相机标定计算得到的畸变参数对像素点做反畸变。为了不丢失天花板信息，采用鱼眼模型做不丢失像素的反畸变。可视化效果如图3所示。

(a) 客厅 (b) 走廊图3 鱼眼反畸变效果

2 改进SIFT算法

2.1 压缩尺度空间

原SIFT算法在特征检测阶段会建立高斯差分金字塔(difference of gaussian，DOG)。首先利用原图生成六层高斯金字塔，再利用中间层下采样得到下一组第一层，如此往复，得到多个尺度的高斯金字塔。利用高斯金字塔同组相邻层做差得到高斯差分金字塔DOG，定义为：

D(x,y,σ)=[G(x,y,kσ)-G(x,y,σ)]·I(x,y)

(2)

在DOG中，纹理丰富与边缘等区域具有比较大的值。SIFT利用DOG多尺度的特性使提取的特征具有尺度不变性，但也因此加大了算法复杂度，增加了提取特征点的用时与后续匹配的错误率。

结合SIFT的多尺度原理分析移动机器人的运动，机器在运动时只有Y轴的移动与绕Z轴的旋转，在运动方式的基础上相机朝上的安装方式使得天花板特征点不存在尺度变换，因此原算法的多尺度差分金字塔信息存在大量冗余信息，通过压缩多尺度差分金字塔层数以降低算法耗时，加速算法，同时也能提高后续匹配准确率。通过对比DOG金字塔配置与提取特征数目、算法耗时、匹配准确率之间的关系得到最合理的DOG层数。经过验证DOG为14层时在天花板数据集具有最优的平均匹配准确率为61%，且算法时间为原算法时间62%。压缩后的可视化DOG金字塔如图4所示。

图4 DOG金字塔

2.2 描述子改进

原SIFT通过计算关键点邻域内像素的梯度幅值和梯度方向，叠加构成128维特征点描述子。

一张图的特征点数往往成百上千，高维度的描述子导致算法花费时间长，计算成本大，匹配计算复杂度高，同时特征描述子包含了冗余数据，因此，对描述子进行降维，可以提高匹配效率。简化后描述子的具体划分如图5所示，采用3个“回”字子区域[14]，3个区域用不同灰度值填充表示，每个区域依旧分为8个梯度方向，叠加幅值得到24维描述子。

图5 描述子邻域

另外，由于距离特征点越近的像素点对特征点的特征描述影响越大这一特点，3个区域分别取不同权重进行匹配。

w=[w1,w2,w3]

(3)

传统匹配方式为欧氏距离，欧氏距离定义为：

(4)

本文采用曼哈顿距离替换欧式距离，结合位置权重的曼哈顿距离为：

(5)

相比于欧氏距离，曼哈顿距离可明显缩短运算时间，提高了算法的匹配效率，结合位置权重也能提高匹配的准确率。

2.3 PROSAC算法删除误匹配

在匹配阶段在利用匹配距离选取最近匹配之后利用FLANN初步筛选匹配，之后常用RANSAC算法进一步删除离群点。RANSAC采取随机采样作为初值，这使得迭代过程具有随机性，且迭代次数不稳定，这种不稳定会随着数据错误率的提高而大幅度提升。本文采用PROSAC算法去除误匹配。相比随机匹配点作为初值，PROSAC先将数据排序，再按照质量和的高低来选取数据[15]，选取质量最高的数据作为迭代初值，因此具有更高的效率和准确性。

利用最近邻与次近邻比值作为质量评价依据，表达式为：

(6)

μ=(β1,β2,…,βn)

(7)

式中，lf为特征点最近曼哈顿距离；ls为次近曼哈顿距离；μ为最近邻与次近邻比值合集；β数值越低匹配质量越高，利用最优部分β数值作为PROSAC初值。

3 天花板视觉定位算法

在改进SIFT算法的基础上构建天花板视觉定位系统。由于单目视觉存在尺度不确定性，因此结合IMU信息获取真实尺度，目前视觉与IMU融合定位方法主要分为松耦合式融合方法和紧耦合式融合方法[16]。松耦合方法将视觉传感器和 IMU分别计算得到的位姿直接进行融合，算法复杂度低，时效性高，并在定位精度上取得了一定的成效。紧耦合方法则是将图像数据和IMU数据联合进行位姿估计，考虑到相机和IMU之间的约束关系，共同构建优化模型，算法精度高，复杂度高。考虑到算法实时性，本文采用基于松耦合数据的融合方案，实现惯性数据和视觉数据的融合并计算移动机器人的轨迹，算法整体框架如图6所示。

图6 SIFT定位框架

首先在相邻帧之间利用改进的SIFT特征匹配，PROSAC筛出错误匹配点。利用IMU短时定位准确的特性约束匹配。在跟踪阶段利用IMU数值为初值，根据重投影特征点位置，设定一定阈值约束匹配点范围去除误匹配。对筛选出的正确匹配特征点做反畸变，在利用反畸变后的点对结合对极几何求解旋转矩阵和平移向量。由于单目相机的尺度不确定性，得到的平移向量与真实场景像素之间存在一个不确定缩放尺度，需要借助IMU数据估计。

IMU数据首先需要进行初始化操作，消除零偏，再利用预积分对齐图像，求得对应时刻的图像位姿。

视觉里程计会得到平移向量tv，存在缩放尺度为s，IMU通过加速度预积分输出位移为ti，通过最小化向量误差估计最优的尺度因子s*：

(8)

在得到视觉尺度后利用视觉估计与惯性估计的加权和[17]作为真实位移的估计量t*：

t*=λs*tv+(1-λ)ti

(9)

再通过每两帧之间的平移向量求和为里程信息Lk：

(10)

在得到位姿之后利用最小化重投影误差进一步优化位姿。最小化重投影误差表达式为：

(11)

利用优化后的位姿为初值给定一定阈值局部搜索SIFT特征点进行回环检测，消除累计误差。

4 算法对比实验

4.1 实验环境

本文使用改进后的SIFT分别在室内天花板数据集上进行了特征提取与匹配实验，验证算法提取匹配效率与精度。考虑实际定位过程，周围环境光线会发生变化，拍摄的图像也可能会产生光照变换、旋转、遮挡视角等现象，故实验数据来自于多个不同场景，且挑选了多种特殊情况的图像对分别验证。在对比提取匹配的基础上与开源的视觉定位方案对比跟踪定位精度。

实验场景为普通家居环境，相机图像的分辨率为640×480。实验设备采用处理器为3.40 GHz的四核、内存为8 GB的个人计算机与移动机器采用ARM-A7嵌入式处理器，机器外观如图7所示。

图7 机器外形

4.2 特征提取与匹配实验

首先在PC端与ARM端分别运行算法，并在程序中记录用时，在大规模运行后记录平均用时。

表1为5种算法在不同平台的平均单帧提取特征点与描述子用时运行时间，可以看出改进后的SIFT耗时较原算法提高了58%，在PC端可以达到30 Hz左右的帧率，在ARM端也可达到8 Hz左右的帧率。

表1 算法提取用时对比 (ms)

在天花板数据集多个场景上对比验证各个算法，选取改进SIFT算法部分匹配效果做可视化如图8所示。

(a) 相邻平移 (b) 相邻旋转

在对比各个算法在不同场景的特征提取的效果之后，分析整理详细提取数据对比表如表2所示。

表2 算法各场景准确率对比

由表2可以对比分析，在天花板数据上改进SIFT较原算法各个场景匹配成功率均有明显提高。在相邻帧与光照变化帧，改进SIFT与SUPERPOINT+SUPERGLUE算法匹配准确率接近；在大角度选择时，基于深度学习方案准确率大幅下降，改进SITF依旧保持高的准确率。

4.3 视觉定位实验

在提取匹配实验的基础上，进一步分析改进SIFT的视觉定位精度效果。天花板数据集作为实验场景，对比SIFT构建的视觉惯性里程计与ORB-SLAM2、PL-SLAM开源算法的定位精度，给其提供IMU真实尺度。3个算法对比两个场景定位效果如图9所示，图9a和图9b为ORB-SLAM2与PL-SLAM的定位轨迹与真实轨迹对比，图9c为本文算法实现定位效果。

(a) ORB-SLAM2跟踪效果

通过绝对轨迹误差的均方根误差对实验结果进行量化评估，结果如表3所示。其中RMSE为轨迹的均方根误差，单位为m。ORB-SLAM2在两个场景平均RMSE为0.137 5 m；PL-SLAM为0.147 5 m；本文算法平均RMSE为0.076 m。

表3 算法RMSE对比

5 结论

本文提出了一种基于SIFT的室内移动机器人视觉定位系统。在PC端与ARM端分别验证本文算法。实验结果表明，改进后的 SIFT算法较其他算法与原算法在时效性与准确性均有明显提升。提出的定位方法平均绝对轨迹误差的均方根误差为0.076 m。分析实验结果得出结论如下：

(1)提出的改进SIFT方法提高了算法实时性，在算力受限设备上达到实时性要求，同时保证了特征点质量。

(2)多组天花板场景下的实验结果表明，改进算法提取的STFT特征点具有较高的唯一性，相较于ORB需要依赖庞大的词袋文件，SIFT特征点可以用于进行回环检测，且具有较高准确率。

(3)提出的惯性视觉定位算法在算力有限的平台可以实时运行跟踪定位与回环检测，且较开源算法具有更高的定位精度。