融合IMU的视觉SLAM系统研究

赵永达

摘要    基于特征点法的视觉SLAM系统在运行的过程中，运动过大，环境特征过少都会影响SLAM系统的运行效果。针对这个问题，提出了一种融合IMU的视觉SLAM系统。首先，提取关键点，并结合PNP与G2O方法估计相机的最优位姿，并构建地图。然后，统计当前帧关键点数量，若特征点足够少，特征点法无法估计相机的位姿变化，则结合陀螺仪与轮式里程计估计位姿;最后，如果特征点数量增加，提取关键特征，结合PNP与G2O的方法估计相机位姿的变化。通过实验可以看出融合IMU的视觉SLAM系统，可以准确的估计相机相机的位姿变化。

【关键词】特征点法 IMU 陀螺仪 轮式里程计

近年来，同时定位与地图构建（Simultaneous Localization And Mapping，SLAM）发展迅速涌现了大量优秀成果，如基于直接法SVO，以及基于特征点法的ORB-SLAM.CNN也被引入到了SLAM系统中，但以上的方法过于依赖图像数据，如果获取的图像中关键点不容易提取，将严重影响算法的准确性。

因此，本文提出一种融合IMU的SLAM算法。本方法首先按照特征点法估计位姿，当特征点变少时，结合轮式里程计以及陀螺仪估计位姿的变化，并构建地图。

1 算法简介

融合IMU的视觉SLAM算法首先获得一帧帧描述环境信息的图像，包括彩色图像和深度图像，并对彩色图像提取关键特征，通过PNP与G2O结合的方法估计相机的位姿变化。如果提取关键特点数量少时，采用IMU方法计算位姿。通过点云拼接构建地图。

2 关键特征法估计位姿

使用ORB算法提取图像特征，提取关键特征的同时，统计关键点的个数，如果某一帧图像提取关键点的个数小于50，则使用IMU计算位姿。在提取关键特征的同时，对关键特征进行简单的优化，ORB特征的提取分为提取Fast关键点和计算Brief描述子两个部分，在获取相邻两帧图像的匹配点的同时，统计所有匹配点中最小的海明距离d。對匹配点进行简单的优化，如果一对匹配点的海明距离大max（3d，40），认为该匹配点对不合格，从匹配结果中删除该匹配点对。按照该方法验证所有的匹配点对。

对于空间中的任意一个点P，用其次坐标表示为（x，y，z，1），P点投影到不同的图像中表示为

公式

，相机的位置和姿态表示为R、t，K为相机内参，得到关系：，可以看出[R|t]矩阵有12个未知量，因此取至少六个点，通过SVD分解可求得[R|t]。由于存在观测噪声，计算相机位姿会产生重投影误差。通过将误差求和构建最小二乘问题，误差项表示如（1）所示，通过G2O方法，估计最优位姿ξ。

公式

3 IMU方法估计位姿

采用IMU方法可以将位姿表示为特殊欧式群：

其中T是线性坐标，T的左上角为旋转矩

阵R，右侧为平移向量t，SO（3）是特殊正交群，

可以表示为

公式

表示空间中的坐标点。

4 实验结果分析

运行融合IMU的视觉slam系统，估计相机的位姿，并对相机的真实位置进行记录。实验运行在系ubuntu14.04系统上，4核的cpu型号为Intel（R）Core（TM）i5-5200U，内存大小为8GB，显卡的型号是Intel（R） HD Graphics 5500。运行结果如图1所示。其中蓝色曲线为相机的真是运动轨迹，红色曲线为

融合IMU的视觉SLAM系统的估计结果。绿色曲线为ORB特征点法估计的轨迹。

5 结束语

通过实验可以看出由于相机运动到特征点较少的地方，特征点法的位姿估计，产生了较大的误差。本文提出的融合IMU的视觉SLAM系统，可以较准确的估计位姿，具有更好的稳定性。

参考文献

[1]Mur-Artal R， Montiel J M M， Tardos J D. ORB-SLAM： a Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics， 2015， 31（5）：1147-1163.

[2]Forster C， Pizzoli M， Davide Scaramuzza*. SVO： Fast Semi-Direct Monocular Visual Odometry[C]// IEEE International Conference on Robotics and Automation （ICRA）， Hong Kong， 2014. IEEE， 2014.

[3]张攀.惯性传感器辅助的立体视觉SLAM实现[D].厦门大学，2013.

[4]王泽华，梁冬泰，梁丹等.基于惯性/磁力传感器与单目视觉融合的SLAM方法[J].机器人，2018，40（06）：933-941.

[5]顾照鹏，董秋雷.基于部分惯性传感器信息的单目视觉同步定位与地图创建方法[J].计算机辅助设计与图形学学报，2012，24（02）：155-160.

[6]周绍磊，吴修振，刘刚等.一种单目视觉ORB-SLAM/INS组合导航方法[J].中国惯性技术学报，2016，24（05）：633-637.