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

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

A Method of Location and Three-dimensional Map Construction for Autonomous Patrol Robot Fused with Binocular Camera and Inertial Navigation System

The invention discloses a positioning and three-dimensional map building method for an autonomous inspection robot fused with binocular camera and inertial navigation system, which denoises and predicts inertial navigation data, extracts features and traces optical flow after defogging left and right cameras. Nonlinear optimization is used to estimate the camera pose, and key frames are screened for loop detection and map construction. In the map building step, the left and right camera images of binocular camera corresponding to the key frames can be depth restored, and the three-dimensional coordinates of the known depth pixels can be obtained. Random sampling consistency is used to remove the ground part of point cloud and build a three-dimensional map. The invention can accurately locate the autonomous inspection robot and construct a three-dimensional environment map for further path planning.

【技术实现步骤摘要】
一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法
本专利技术涉及巡检机器人定位与地图构建领域，尤其涉及一种双目相机和惯导融合的巡检机器人定位与三维地图构建方法。
技术介绍
随着科技不断进步，机器人已在各个领域得到应用，机器人的智能化发展也早已成为一种趋势并得到越来越多的研究者关注。从室内到室外，从空中到海底，众多危险、不适宜人类活动的环境下，机械、重复性高的岗位上，我们都可以看到智能机器人正逐步替代人类完成工作要求。与传统机器人不同，智能移动机器人的智能化主要体现在其具备感知能力的特点。自主巡检机器人是常见的具备巡检能力的智能机器人，主要实现从某一位置寻找合适路径运行至目标位置点并完成特定检测任务的功能，检测内容包括设备巡检、图表识别、数据读取等。自主巡检机器人当前主要应用于变电站、油田、煤矿、仓库、化工厂等环境，替代人力从而解决人工巡检方式所存在的作业效率低、劳动成本高、维护风险大等问题。在未知、动态复杂、变化的环境下，巡检机器人如何自主确定自身位姿、感知所处周围环境、三维空间信息仍然是一个技术难点。巡检机器人现在采用的定位方式主要有电磁定位、惯性定位、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，其暗通道定义为：式中Jc表示彩色图像的每个通道，Ω(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)|；非局部聚合代价CAd(p)表示加权的Cd的和：其中q表示图像I中的每个像素，S(p,q)表示在和函数中像素q对像素对p在贡献的加权函数；在树结构中，S(p,q)被定义为：其中σ为用于距离调整的参数；像素点p在每一个视差水平d下的代价值：进一步：在地图构建步骤中深度为zc的像素(u,v)对应三维点Pw为:该点云对应的法向量为：初始误差范围的半径为：r＝zc/f；初始置信度为：c＝exp(-γ2/2σ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所示，一种双目相本文档来自技高网...

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

【技术特征摘要】
1.一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，其特征在于，定位与三维地图构建方法如下：惯导数据在去噪之后进行预计分处理，左、右摄像头在去雾处理之后进行特征提取和光流跟踪；惯导数据和双目相机数据进行初始化之后进入定位步骤，通过非线性优化估计巡检机器人位姿并遴选关键帧进入回环检测和地图构建步骤；在地图构建步骤对关键帧对应的双目相机RGB图像采用分割树的方法进行深度恢复，对点云去除地面构建三维地图；在回环检测步骤，当回环检测判断为是时，进行全局BA优化，优化巡检机器人位姿和地图。2.根据权利要求1所述的一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，其特征在于，去雾处理方法如下：利用暗通道来判断巡检机器人所处环境中是否有雾霾，雾图模型表示为：I(x)＝J(x)t(x)+A(1-t(x))，其中I(x)为待去雾图像，J(x)为要恢复的无雾图像，A为全球大气光成分，t(x)为透射率；图像每个通道的关系进一步表示为：其中C表示图像的每个通道；对于任意的图像J，其暗通道定义为：式中Jc表示彩色图像的每个通道，Ω(x)表示每个窗口；透射率的估计值求解为：式中w为恒定参数，为保留一定程度的雾使图像具有景深，w常取小于1的值。3.根据权利要求2所述的一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，其特征在于，图像恢复方法如下：式中t0为阈值；对图像的暗通道t(x)像素进行概率统计，当像素大于阈值ζ为总像素的η比例时，图像作为雾图处理；确定ζ为192，η为40％。4.根据权利要求1所述的一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，其特征在于：双目相机数据和惯导数据如果能够进行初始化就直接初始化，若不能进行初始化便采用双目先初始化在融合惯导数据。5.根据权利要求1所述的一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法，其特征在于，生成一个关键帧必须满足如下几个条件：1)从上一个关键帧生成...

【专利技术属性】
技术研发人员：朱华，陈常，陈子文，刘宇，张征，汪雷，李猛钢，由韶泽，杨汪庆，
申请(专利权)人：中国矿业大学，
类型：发明
国别省市：江苏,32