一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法与流程

本发明涉及巡检机器人定位与地图构建领域，尤其涉及一种双目相机和惯导融合的巡检机器人定位与三维地图构建方法。

背景技术：

随着科技不断进步，机器人已在各个领域得到应用，机器人的智能化发展也早已成为一种趋势并得到越来越多的研究者关注。从室内到室外，从空中到海底，众多危险、不适宜人类活动的环境下，机械、重复性高的岗位上，我们都可以看到智能机器人正逐步替代人类完成工作要求。

与传统机器人不同，智能移动机器人的智能化主要体现在其具备感知能力的特点。自主巡检机器人是常见的具备巡检能力的智能机器人，主要实现从某一位置寻找合适路径运行至目标位置点并完成特定检测任务的功能，检测内容包括设备巡检、图表识别、数据读取等。自主巡检机器人当前主要应用于变电站、油田、煤矿、仓库、化工厂等环境，替代人力从而解决人工巡检方式所存在的作业效率低、劳动成本高、维护风险大等问题。在未知、动态复杂、变化的环境下，巡检机器人如何自主确定自身位姿、感知所处周围环境、三维空间信息仍然是一个技术难点。

巡检机器人现在采用的定位方式主要有电磁定位、惯性定位、uwb定位、gnss卫星定位。电磁定位在机器人行驶的路径上铺设金属线，并在金属性上加载导引频率，通过识别导引频率来实现机器人的导引。它主要的优点是引线隐蔽性较好，系统稳定和可靠，抗干扰能力好。但是系统定位误差大，并且要在地面铺设磁条，从而导致机器人灵活性差，维护成本也比较高。惯性定位是一种不要依赖任何外部信息的航迹推算导航方法。机器人主要利用电子罗盘、加速度计或陀螺仪等传感器来测量自身的加速度和方位角，从而推算下一点的位置。但是数据是经过积分得到的，常量误差会随着时间变化而被积累放大，长时间导航就会导致定位精度下降。惯性定位是一种航迹推算方法，和里程计定位方法具有相同的缺点，当机器人沿着重复轨迹进行工作时，误差会不断累计。uwb是一种无载波通信技术，利用纳秒至微微秒级的非正弦波窄脉冲传输数据。但是作为伪卫星定位方法的一种，uwb在复杂动态环境下定位效果不是很理想。gnss卫星定位(导航)系统能够向全球导航通信设备提供实时的位置、速度等定位信息服务，但是定位精度只能达到几米，一般用于室外导航并且信号容易受到树木、楼宇等遮挡影响，并且室内无法使用。

为此，本发明现提出一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法。

技术实现要素：

为了克服现有技术中存在的不足，本发明提供一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，通过此方法进行高精度的位姿估计和构建三维环境地图，能够应用于自主巡检机器人。

为实现上述目的，本发明采用的技术方案为：

一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，定位与三维地图构建方法如下：惯导数据在去噪之后进行预计分处理，左、右摄像头在去雾处理之后进行特征提取和光流跟踪；惯导数据和双目相机数据进行初始化之后进入定位步骤，通过非线性优化估计巡检机器人位姿并遴选关键帧进入回环检测和地图构建步骤；在地图构建步骤对关键帧对应的双目相机rgb图像采用分割树的方法进行深度恢复，对点云去除地面构建三维地图；在回环检测步骤，当回环检测判断为是时，进行全局ba优化，优化巡检机器人位姿和地图。

进一步：去雾处理方法如下：利用暗通道来判断巡检机器人所处环境中是否有雾霾，雾图模型表示为：

i(x)＝j(x)t(x)+a(1-t(x))，

其中i(x)为待去雾图像，j(x)为要恢复的无雾图像，a为全球大气光成分，t(x)为透射率；

图像每个通道的关系进一步表示为：

其中c表示图像的每个通道；对于任意的图像j，其暗通道定义为：

式中j^c表示彩色图像的每个通道，ω(x)表示每个窗口；透射率的估计值求解为：

式中w为恒定参数，为保留一定程度的雾使图像具有景深，w常取小于1的值。

进一步：图像恢复方法如下：

式中t0为阈值；对图像的暗通道t(x)像素进行概率统计，当像素大于阈值ζ为总像素的η比例时，图像作为雾图处理；确定ζ为192，η为40％。

进一步：双目相机数据和惯导数据如果能够进行初始化就直接初始化，若不能进行初始化便采用双目先初始化在融合惯导数据。

进一步：生成一个关键帧必须满足如下几个条件：

1)从上一个关键帧生成起至少经过了15帧图像；

2)跟踪到至少50个特征点；

3)与上一关键帧的距离超过了设定的阈值。

进一步：当巡检机器人在旋转时增加两条强制的约束：

1)当机器人短时间旋转角度超过15°时必须要产生一个关键帧；

2)当机器人静止时，每3秒必须要产生一个关键帧。

进一步：在地图构建步骤将摄像头图像i看作连通无向图g＝(v,e)；对于连通像素s和r的边e权重表示为：

we＝w(s,r)＝|i(s)-i(r)|；

非局部聚合代价c^ad(p)表示加权的cd的和：

其中q表示图像i中的每个像素，s(p,q)表示在和函数中像素q对像素对p在贡献的加权函数；

在树结构中，s(p,q)被定义为：

其中σ为用于距离调整的参数；

像素点p在每一个视差水平d下的代价值：

进一步：在地图构建步骤中深度为zc的像素(u,v)对应三维点pw为:

该点云对应的法向量为：

初始误差范围的半径为：

r＝zc/f；

初始置信度为：

c＝exp(-γ²/2σ²)，

其中，γ为归一化后的深度数据；σ一般取为0.6。

进一步：在地图构建步骤中采用随机采样一致性计算内点和外点，来求解地面平面方程：

ax+by+cz+d＝0，

其中，a，b，c为hessian范式中法向量的坐标，d为常数。

进一步：在回环检测步骤定义图像两帧之间的相对位姿：

第i帧和第j帧的残差定义为：

所有序列边和闭环边的整体目标函数定义为：

其中s为序列边，l为闭环边，h为鲁棒核函数。

进一步：在定位和回环检测环节，使用的图像为灰度图，定义灰度图和rgb图的像素转换为：灰度图＝0.29*r+0.58*g+0.11*b；在地图构建步骤使用的是rgb图像。

本发明有益效果：

通过本发明能够对自主巡检机器人进行精确的定位，并且构建三维环境地图应用于进一步的路径规划；通过此方法进行高精度的位姿估计和构建三维环境地图，能够应用于自主巡检机器人。

附图说明

图1为本发明的定位与三维地图构建方法流程图；

图2为本发明的雾图原图；

图3为本发明的雾图的暗通道图；

图4为本发明的雾图去雾效果图；

图5为本发明的左摄像头图像；

图6为本发明的右摄像头图像；

图7为本发明的图像中每个子树根节点分布图；

图8为本发明的图像分割效果图；

图9为本发明的双目估计深度恢复图；

图10为本发明的带有地面的三维点云；

图11为本发明的去除地面的三维点云；

图12为本发明的三维地图。

具体实施方式

为使本发明实施的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行更加详细的描述。在附图中，自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。所描述的实施例是本发明一部分实施例，而不是全部的实施例。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。下面结合附图对本发明的实施例进行详细说明。

如图1所示，一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，包括以下内容：整个方法框架分为数据处理、初始化、定位、地图构建和回环检测五个步骤。惯导数据在去噪之后进行预计分处理，左、右摄像头在去雾处理之后进行特征提取和光流跟踪；惯导数据和双目相机数据进行初始化之后进入定位步骤，通过非线性优化估计巡检机器人位姿并遴选关键帧进入回环检测和地图构建步骤；在地图构建步骤对关键帧对应的双目相机rgb图像采用分割树的方法进行深度恢复，对点云去除地面构建三维地图；在回环检测步骤，当回环检测判断为是时，进行全局ba优化，优化巡检机器人位姿和地图。

在数据处理步骤中：利用暗通道来判断巡检机器人所处环境中是否有雾霾，雾图模型表示为：

i(x)＝j(x)t(x)+a(1-t(x))，

其中i(x)为待去雾图像，j(x)为要恢复的无雾图像，a为全球大气光成分，t(x)为透射率。图像每个通道的关系可以进一步表示为：

其中c表示图像的每个通道；对于任意的图像j，其暗通道定义为：

式中j^c表示彩色图像的每个通道，ω(x)表示每个窗口；透射率的估计值求解为：

式中w为恒定参数，为保留一定程度的雾使图像具有景深，w常取小于1的值。最终的图像恢复可以表示为：

式中t0为阈值；对图像的暗通道t(x)像素进行概率统计，当像素大于阈值ζ为总像素的η比例时，图像作为雾图处理；确定ζ为192，η为40％。

需要说明的是，图2为本发明的雾图原图，图3为本发明的雾图的暗通道图，图4为本发明的雾图去雾效果图；由此可知，通过本发明的去雾处理方法完全实现了去雾效果。

在初始化步骤中：双目相机数据和惯导数据如果能够进行初始化就直接初始化，若不能进行初始化便采用双目先初始化在融合惯导数据。

在定位步骤：生成一个关键帧必须满足如下几个条件：1)从上一个关键帧生成起至少经过了15帧图像；2)跟踪到至少50个特征点；3)与上一关键帧的距离超过了设定的阈值；当巡检机器人在旋转时增加两条强制的约束：1)当机器人短时间旋转角度超过15°时必须要产生一个关键帧；2)当机器人静止时，每3秒必须要产生一个关键帧。这样会保证跟踪不会丢失，并且提高了跟踪的精度。关键帧在用于构建三维地图时将会进一步进行筛选，剔除相似度较大的帧。

在地图构建步骤：采用分割树进行深度恢复，将摄像头图像i看作连通无向图g＝(v,e)。其中v表示图像i中像素点集合，e是像素点与像素点之间边的集合。对于连通像素s和r的边e权重可以表示为：

we＝w(s,r)＝|i(s)-i(r)|，

一个树t可以由一系列的边e组成，边之间的权重可以看成是树的深度代价。对于像素p和q，在树t中他们之间存在唯一路径，路径中边权重的总和决定他们之间的距离d(p,q)。cd(p)表示像素p在深度层d上的匹配代价。非局部聚合代价c^ad(p)表示加权的cd的和。

其中q表示图像i中的每个像素，s(p,q)表示在和函数中像素q对像素对p在贡献的加权函数；在树结构中，s(p,q)被定义为：

其中σ为用于距离调整的参数；在初次代价聚合中，当像素p的所有子节点都被访问后，它的代价值表示为：

在从根节点到叶节点进行的最终代价聚合中，像素p的最终代价聚合代价由其父节点pr(p)决定：

重新计算图像代价，让d表示左视差图，则像素点p在每一个视差水平d下的代价值：

需要说明的是，图5为本发明的左摄像头图像；图6为本发明的右摄像头图像；图7为本发明的图像中每个子树根节点分布图；图8为本发明的图像分割效果图；图9为本发明的双目估计深度恢复图；由此可知，通过本发明的上述方法实现了深度恢复图像。

在地图构建步骤中，当前关键帧的位姿为t，对应的李代数为ξ，可以得到已知深度为zc的像素(u,v)对应三维点pw为:

该点云对应的法向量为：

初始误差范围的半径为：

r＝zc/f，

初始置信度为：

c＝exp(-γ²/2σ²)，

其中，γ为归一化后的深度数据；σ一般取为0.6。

采用随机采样一致性计算内点和外点，将内点定义为在距离水平面一定范围内的三维点，反之为外点。对每个关键帧筛选出地面点云后，利用这些点云的集合pi(i＝1,…,n)来求解地面平面方程：

ax+by+cz+d＝0，

其中为a，b，c为hessian范式中法向量的坐标，d为常数；从点云中分割提取的内点都处在估计参数对应的平面上或与平面距离在一定的范围内。

在回环检测步骤：当回环检测判断为是时，使用全局ba进行优化；定义图像两帧之间的相对位姿：

当检测到当前帧与数据库中的帧构成闭环时，对数据库中的所有帧进行闭环优化。第i帧和第j帧的残差定义为：

所有序列边和闭环边的整体目标函数定义为：

其中s为序列边，l为闭环边，h为鲁棒核函数。

在定位和回环检测环节，使用的图像为灰度图，定义灰度图和rgb图的像素转换为：灰度图＝0.29*r+0.58*g+0.11*b；在地图构建步骤使用的是rgb图像。

需要说明的是，图10为本发明的带有地面的三维点云；图11为本发明的去除地面的三维点云；图12为本发明的三维地图。由此可知，通过本发明的上述方法实现了三维地图。

综上可得，本发明公开了一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，对惯导数据进行去噪和预计分，对左、右摄像头进行去雾处理之后进行特征提取和光流跟踪。采用非线性优化估计相机位姿，筛选关键帧进行回环检测和地图构建。地图构建步骤能够通过关键帧对应的双目相机左、右摄像头图像进行深度恢复，求出已知深度像素的三维坐标。采用随机采样一致性去除点云中的地面部分，构建三维地图。通过本发明能够对自主巡检机器人进行精确的定位，并且构建三维环境地图应用于进一步的路径规划。

最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制；尽管参照较佳实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者对部分技术特征进行等同替换；而不脱离本发明技术方案的精神，其均应涵盖在本发明请求保护的技术方案范围当中。