一种视觉与IMU融合的室内巡检机器人定位与地图构建方法与流程

本发明涉及室内巡检机器人技术领域，特别是一种视觉与imu(inertiameasurementunit，惯性测量装置)融合的室内巡检机器人定位与地图构建方法。

背景技术：

伴随着人工智能的发展，智能化设备不断增加，对设备的维护工作也迫在眉睫，一旦设备发生故障将会对人们的生命和财产造成巨大危害。因此定期对设备进行巡检、保护设备安全有效的运行具有重大的意义。在变电站、核电站、化工厂等环境存在许多设备摆放在室内进行工作，工作人员需要定时查看设备是否正常工作来进行室内设备的巡检，根据巡检范围和巡检工作量的情况，需要配置人数不等的巡检工作人员24小时进行倒班巡检。对于一些高压、高温环境情况下的巡检，还会对工作人员的身体健康造成巨大伤害。综上所述，传统的巡检方式不仅花费了大量的人力而且效率低、安全性低，因此迫切的需要能够自动巡检的机器人代替传统的人工巡检方式，使其能够自主的完成相应的巡检任务。

采用巡检机器人自动巡检面对的首要问题是导航问题，机器人自身完成自主导航首先需要面对两个问题，即“我在哪儿”、“我周围的环境是什么样的”，这两个问题就是人工智能领域研究的关键技术同时定位与地图构建(simultaneouslocalizationandmapping)，即slam技术。slam是一种在未知环境下实时定位与地图构建的技术，精确的定位和地图构建是机器人完成导航的重要前提。近年来，slam技术在工程上的应用取得了长足的进步，研究人员不断研究slam算法使其定位和建图的精度提升并且不断采用更微型的传感器，使巡检机器人朝着更加微型化、小型化的方向不断发展，如何将slam技术运用到巡检机器人上使其完成导航工作的研究正如火如荼的进行着。视觉slam是当前slam领域的研究热点之一，相机价格便宜、体积小便于安装，并且能够获得丰富的环境信息，能够较为真实的反映当时的环境状况。目前视觉slam最佳方案为orb-slam2(orientedbrief-simultaneouslocalizationandmapping，定向简洁型同时定位和地图构建)，但由于orb-slam2算法定位时容易跟踪相机失败、相机运动存在尺度不确定问题并且构建的稀疏地图无法用于导航，很难在工程上实现。

技术实现要素：

为了克服当前orb-slam2存在的缺点，本发明提出了一种视觉与imu融合的室内巡检机器人定位与地图构建方法，该方法通过视觉信息融合imu信息解决了纯视觉传感器稳定性较差的问题，并且通过深度相机构建了可用于导航的稠密地图。

实现本发明目的的技术方案如下：

一种视觉与imu融合的室内巡检机器人定位与地图构建方法，所述巡检机器人使用orb-slam2算法得到定位和用于导航的地图；所述orb-slam2算法的输入信息为图像数据和imu数据的融合数据；其中，图像数据和imu数据得到融合数据的方法，包括：

步骤1：imu数据预处理，即通过帧间累积法进行积分将imu测量值和图像测量值的数据对齐；

步骤2：图像数据和imu数据融合，包括相机初始化阶段融合和相机跟踪阶段融合；所述相机初始化阶段融合包括imu的误差估计即陀螺仪偏置估计和加速度计偏置估计，之后通过计算加速度计偏置估计恢复图像尺度；所述相机跟踪阶段融合包括imu数据和图像数据通过最小化误差方程进行状态估计得到相机定位。

进一步的技术方案为：所述巡检机器人使用orb-slam2算法得到用于导航的地图为稠密的点云地图，所述稠密的点云地图的建图方法包括：

步骤1：图像数据的坐标系的转换，即将相机坐标系下的像素点转换为世界坐标系；

步骤2：使用最近点迭代法将图像数据的点云进行拼接，拼接后得到全局的稠密地图。

更进一步的技术方案为：所述巡检机器人使用orb-slam2算法得到定位，采用改进的orb特征点匹配和改进的关键帧ncc匹配；

所述改进的orb特征点匹配，包括：

步骤1：使用暴力匹配对相邻图像数据的特征点进行匹配；

步骤2：使用随机采样一致性算法对暴力匹配中匹配错误的特征点进行剔除；

所述改进的关键帧ncc匹配，包括：

步骤1：双线系内插，即通过双线系内插计算关键帧上的像素真值；

步骤2：ncc匹配，即通过ncc匹配算法对两个关键帧进行匹配。

本发明采用目前最佳视觉slam方案orb-slam2作为基本框架，融合imu传感器信息，以解决现有orb-slam2存在的问题。本发明结合室内环境的特点和orb-slam2的缺点对其进行改进，提出了室内巡检机器人viorb-slam2算法。为了提高算法的鲁棒性，使其不易在特征点稀少的地方跟踪失败，在orb-slam2系统中融合了imu惯性传感器。进一步地，改进了orb-slam2的建图方法，使改进后的算法能够建立可用于导航的稠密地图。由于imu传感器本身存在误差，添加之后可能会对系统的精度造成影响，所以更进一步地对原始orb特征点匹配方法进行了改进，从而提高了算法的精度；并且提出了一种关键帧匹配方式，约束了关键帧的匹配条件，提高了关键帧匹配的效果从而提高整个系统的精度。

附图说明

图1为本发明算法的整体框图。

图2为imu和图像采集数据关联图。

图3为空间投影示意图。

图4为双线系内插法示意图。

具体实施方式

本发明的算法框架如图1所示。本算法前端由视觉和imu两个部分组成，融合imu的方法为：首先通过将imu预处理，之后对imu进行误差估计和相机尺度恢复，然后联合相机进行优化。具体如下：

步骤1：imu数据预处理

imu的测量频率一般在100hz以上，而相机的频率只有几十赫兹，在imu采集到大量数据时可能相机只采集到的了几帧图像数据，如图2所示。

如果直接用imu测量值加入到图像中将会造成数据不对齐，所以采用帧间累积法对两个图像中的imu测量值进行积分，积分公式如下所示：

其中，表示第i时刻的位置、速度和旋转，表示第j时刻的位置、速度和旋转，g为重力，w表示世界坐标系，bt表示imu坐标系，wb表示由imu坐标系转换到世界坐标系，ω表示旋转角度，a为加速度。

此时通过帧间累积法进行积分就可以是imu和图像测量值的数据进行对齐，但是slam过程中是一个不断进行优化更新的量，然而在帧间积分时计算位置、速度和旋转都对其进行积分，每次计算都将更新后的的初始值一步步积分，计算量大并且每次积分都会造成误差，长时间累积将会对整个系统造成巨大误差。所以通过对坐标系的转换，将该值转换成如下所示：

其中表示第i时刻和j时刻在imu坐标系下的旋转，表示i时刻imu坐标系到世界坐标系下的旋转。将上式代入积分公式后得到：

此时每次进行帧间累积时不需要对一直变化的进行积分只需要对进行积分即可，而是i时刻到j时刻的旋转，只与i和j时刻有关。所以通过以上公式提前对数据进行积分称为预积分，预积分能够有效的对imu数据进行处理，从而进行接下来的数据融合。

步骤2：视觉imu数据融合

图像数据融合imu数据过程包括相机初始化阶段和相机跟踪阶段的融合。由于imu本身的陀螺仪和加速度计会有偏差，通过相机和imu的联合初始化能够计算出陀螺仪的的偏置和加速度的误差，之后通过计算能够恢复图像的尺度信息。

(1)陀螺仪偏置估计

陀螺仪的偏置估计仅从两个连续的关键帧方向中估计陀螺仪的偏差，对于所有的连续关键帧对，通过最小化陀螺仪积分与orb-slam2计算的相对方向的差异对陀螺仪的偏置进行估计，公式如下所示：

其中n表示关键帧的数量，rwb表示imu坐标系到世界坐标系的旋转矩阵，rbw表示世界坐标系到imu坐标系的旋转矩阵，δri，i+1表示两个关键帧之间陀螺仪的积分，bg表示陀螺仪的误差。

(2)尺度恢复和加速度计偏置估计

通过上式可以估计陀螺仪的偏置，之后可以通过正确的旋转加速度测量值来补偿陀螺仪的误差。由于orb-slam2在跟踪相机时进行了坐标归一化，所以其计算轨迹是的比例是任意的，即其具有尺度不确定性，所以为了恢复尺度在进行相机坐标系和imu坐标系转换时包含了尺度因子，如下所示：

pwb＝spwc+rwcpcb

将上式代入两个连续关键帧的变换方程并忽略此时加速度计得偏差可以得到：

建立预估状态向量x＝[s，gw]^t，其中s为尺度，gw为世界坐标系下的重力加速度，使用三个关键帧联合视觉与imu的预积分数据构建一个ax＝b的最小二乘超定方程，采用奇异值分解法对其进行求解，可以得到：

其中：

式中使用了三个关键帧来建立方程，参数的右上标表示关键帧的序号，例如p²表示第2个关键帧的位置。通过以上公式能够计算出尺度和加速度的初始值，从而能够恢复尺度并且得到imu的初始状态量。

之后通过imu和图像数据融合进行跟踪阶段的联合优化，首先构建整体的优化状态向量，包括当前时刻的旋转、速度、位置、加速度计偏置估计、陀螺仪偏置。构建误差方程，包括视觉误差和imu误差，通过最小化误差方程来对其进行状态估计，公式如下所示，其中i表示上一时刻的状态量，j表示当前时刻的状态量：

其中相机重投影误差表示如下：

式中x表示图像中关键帧的特征点，x表示世界坐标系下的地图点，∑k表示与x相关的信息矩阵。imu的误差表示如下所示：

eb＝b^j-bⁱ

其中ρ表示鲁棒核心代价函数，∑r表示偏差信息矩阵，∑i表示预积分的信息矩阵。

本发明还通过改进orb特征点匹配和改进的关键帧ncc匹配可以提高算法的精度，具体方法如下：

改进的orb特征点匹配：

由于imu本身带有误差，如果加入了imu数据可能会加大整个系统的定位误差。所以以下两节通过改进算法来降低系统的误差。orb-slam2采用提取obr特征点进行匹配，匹配的效率和精度直接影响了系统的效率。

orb-slam2中采用bf(暴力匹配)进行特征匹配。暴力匹配是通过比较待匹配特征点的brief不同二进制位数的个数，即异或值，也称作汉明距离，其定义如下所示：

其中，和表示第i个和第i+1个图像的第i和第j个特征点。orb-slam2的特征匹配中直接比较两个点的汉明距离，如果汉明距离小于一定阈值，则说明这两个点为同一个点。使用暴力匹配的方法虽然匹配速度快，但是可能会产生一些误匹配。

通过使用随机采样一致性算法在orb-slam2中进行特征匹配能够减少误匹配的情况，提高匹配的精度，从而提高算法的定位精度。具体步骤如下：

1.首先计算一个3*3的单应矩阵h，该矩阵需满足数据点个数最多，公式如下所示：

式中，常令h33＝1，所以除此之外还有8个位置参数，没对匹配的点可以求解出一组未知参数，则至少需要4组匹配点来对其进行求解。

2.使用模型h来测试所有的图像特征点数据，并且计算满足该模型的特征点个数与误差函数，若误差小于阈值则把该点对加入到内点集q中，大于阈值则去除。误差函数如下所示：

3.如果内点集的数量过少，则重新选择匹配点计算模型h，直到内点集q的数量大于设定的阈值。

4.当内点集q数量大于阈值时，记录内点的数量，重复以上步骤。

5.当迭代次数到达预设值时终止迭代。迭代次数k公式如下所示：

式中，p为常设为0.0995，w为内点相对于所有特征点的比例，m为计算模型需要的特征点对。通过随机采样一致性算法可以提出暴力匹配中误匹配的点，从而提高图像匹配的精度。改进的关键帧ncc匹配：

orb-slam2中关键帧进行匹配是通过orb特征点进行匹配，关键帧匹配的精度能够直接影响到相机跟踪时的定位精度。该步骤通过在关键帧匹配中提出去均值ncc匹配来约束关键帧的匹配，去均值ncc匹配应用在viorb-slam2中思想是利用不同关键帧中的图像块进行匹配，从而得到更加精确的匹配结果，提高算法的定位精度。

关键帧进行特征点匹配时，不同深度空间的点会投影到同一个二维位置处，将特征点反投影到不同的关键帧中会产生一条极线，该极限上的点对应不同深度空间的点，如图3所示，其中ki和kj是第i个和第j个关键帧，o为相机的光心，x是三维空间投影到相机的点，li为极线。

由于投影和反投影中会有相机参数的影响，所以我们极线上的点并不是与不同深度的空间点真实对应的，把极线上的点称作亚像素，去均值ncc匹配前首先计算极线上的真实像素值，采用双线系内插的方法进行计算，双线性内插是两个变量插值函数的线性插值扩展。其核心思想是在x轴和y轴方向进行插值，假设是关键帧上的坐标，进行插值时首先将该坐标进行向下取整：

x＝∪(x0)

y＝∪(y0)

之后在该坐标点周围选择最近的四个像素点如图4所示，记为和

通过以下公式计算出准确的像素值：

式中f表示该点的像素值，g表示的真实像素值，通过以上公式能够准确计算出极线上的真实像素值。

之后通ncc匹配算法对关键帧进行匹配，具体计算方法为：如图4所示，首先以像素xp为中心，计算半径为n个像素单位为像素平均值，记为然后计算另一个关键帧极线上同样的点记为通过以下公式对两个像素块进行计算：

其中表示xp处的像素值，表示xj处通过双线系内插得到的像素值，表示两个图像的相关性，该结果越接近于1表示相关性越大，即接近于1可以将两个点认为是匹配点，本发明算法将阈值设定为0.8，若计算结果大于0.8则证明两个点为同一个待匹配的点。

室内巡检机器人viorb-slam2算法在orb-slam2关键帧匹配中加入ncc匹配改进了关键帧的匹配方式，增加了匹配的约束条件，从而能够使关键帧的匹配结果更加精确，从而能够得到更加精确的相机定位效果。

本发明还改进了orb-slam2的建图方法，使改进后的算法能够建立可用于导航的稠密地图。

稠密地图构建的方法如下：

orb-slam2建图是通过跟踪阶段提取关键帧图像的特征点，通过将特征点来绘制地图，由于特征点是图像中的一部分点，所有我们只能得到由特征点组成的稀疏地图，而稀疏地图无法达到室内巡检机器人后续的导航、路径规划等需求，所以如果要应用与室内巡检机器人需要构建稠密的点云地图。

rgbd相机可以通过lightcoding结构光方案和时间飞行法进行深度信息的测量，通过测量可以得到每张图片的深度图，之后通过pcl开源库能够将深度图转换为点云图，pcl开源库是一个模块化的c++模块库，其能够实现点云的获取、配准、可视化、识别等等。之后利用相机跟踪时得到图像的旋转平移矩阵信息，根据该信息结合深度数据能够进行稠密地图的构建，具体步骤如下：

1.首先根据orb-slam2算法相机跟踪计算出每帧图像的旋转平移矩阵；

2.以第一帧图像的位置为初始位置建立全局坐标系；

3.计算每个像素点的坐标(u，v)，深度为d在摄像机坐标系下的位置，公式为：

4.根据上式计算出pc＝[x，y，z]，公式为：

5.之后根据旋转矩阵r和平移矩阵t计算每个点的世界坐标，最后通过最近点迭代法进行点云拼接：

由于最后地图坐标是通过世界坐标系进行表达，上述步骤将相机坐标系下的点转化到了世界坐标系，接下来需要将每帧点云进行拼接，构成一个完整的全局地图，点云拼接通过迭代最近法完成。迭代最近法是通过构建一个误差函数，在两片点云中按照一定约束条件，计算出一个旋转平移矩阵使得误差函数最小，通过不断对两片点云中的点进行距离计算，不断迭代使其距离最小，来对两片点云进行拼接。迭代最近点法的具体过程如下所示：

1.首先构建一个误差函数，如下所示：

2.通过rgbd相机获取连续两帧图像的点云；

3.选取两帧点云中距离最近的两个点pi和qi；

4.计算出两个点的旋转矩阵r和平移矩阵t使得误差函数最小；

5计算两个点的距离

6.判断d是否达到给的阈值，如果未达到则返回到第3步，不断进行迭代，如果达到了阈值则通过该旋转平移矩阵对两帧点云进行旋转平移。

通过以上步骤就完成了两帧点云的拼接，室内巡检机器人viorb-slam2算法通过对所有点云的拼接能够建立全局的稠密地图。

实验结果展示

1、数据集实验结果

实验需要的有图像数据和imu数据，所以采用euroc数据集进行实验。euroc数据集苏黎世联邦理工学院的一个公开数据集，其使用单目相机和imu进行数据采集，包含了苏黎世联邦理工学院实验室大厅和普通房间两个场景。

本实验使用了orb-slam2算法和室内巡检机器人viorb-slam2算法运行euroc的所有11个数据集，在其中的v203difficult数据集中，orb-slam2算法由于特征点缺少导致相机无法提取特征点，出现了相机跟踪丢失的问题，导致接下来的无法进行相机的定位和地图构建。室内巡检机器人viorb-slam2算法运行该数据集时在该帧并未出现特征点丢失，室内巡检机器人viorb-slam2算法能完整的运行该数据集，得到全局的相机的运行轨迹。

原orb-slam2运行该数据集时得到的地图为稀疏地图，得到的稀疏地图只能够看到一些稀疏点，无法根据地图知道实际中物体的形状，而本发明提出的算法通过改进地图构建方式最终能够得到全局的稠密地图，能够从地图中清晰的看到地图中物体的具体形状。

下表为orb-slam2算法和本发明算法的误差，定位精度采用均方误差根rmse进行衡量，其表达式如下所示：

其中，表示第i个关键帧的位置，表示基准图像的位置，m表示关键帧的数目。均方误差跟越小表示算法的精度越高。

表中x表示orb-slam2运行数据集时跟踪失败无法计算出rmse。通过上表可以看出，本发明的室内巡检机器人viorb-slam2算法在解决原orb-slam2跟踪丢失问题和尺度问题的情况下依然具有良好的精度，最终能够得到巡检机器人的定位信息并且能够建立室内环境的稠密地图。