一种危险化工事故中移动机器人定位与建图方法与流程

本发明属于移动机器人定位导航技术领域，涉及到一种对烟雾进行预处理的基于单目视觉和imu传感器的移动机器人在危险化工事故环境下的定位与建图方法。

背景技术：

近年来，危化品环境事故频发，危化品环境等特殊环境的安全问题引起越来越大的关注。危化品事故突发环境具有复杂，危险系数高的特点，并且危化品爆炸或泄露造成光照和烟雾污染。考虑到环境的复杂性和危险性，本方法移动机器人通过自主定位与建图对环境进行探索，并且一定程度上提高了该环境下定位与建图的鲁棒性和精度。

即时定位与地图构建(slam，simultaneouslocalizationandmapping)是使机器人在未知环境中自动创建地图，并根据对自身状态的估计和地图进行自定位的技术。高精度的自定位信息是移动机器人拥有智能，进行路径规划、地图绘制等任务的前提条件。对于危险化工事故现场的复杂环境，现有的slam方法显然无法正常工作。

基于激光雷达的slam相对精确，误差模型简单，在强光条件下也能稳定运行。同时，激光雷达slam价格昂贵，无法进行重定位的问题会造成累积误差。无法在危险化工事故情况下进行定位和建图。

惯性导航算法具有定位精度高、自适应性强和适用环境广泛的优点，同时视觉具有传感器价格低廉、易于维护、所利用的纹理信息丰富的特点，在重定位和场景分类上具有巨大优势。结合多种传感器的优势，多传感器融合技术成为slam领域的研究热点。

对于基于视觉和imu的危险化工事故环境下定位与建图，难点在于解决随机噪声，爆炸或者危化品泄露条件下光照不均匀和烟雾影响。因此，如何解决以上问题，是解决机器人应用在危化品事故环境中定位与建图的关键问题。

技术实现要素：

以下给出一个或多个方面的主要概述以应对这些方面的基本理解。此概述不能将所有构想的方面进行详述，其唯一的目的是要以简化形式给出一个或多个方面的一些概念以为稍后给出的更加详细的描述之序。

针对移动机器人在危化品事故环境下随机噪声多，光照不均匀和烟雾情况，如图3所示，采用结合图像去雾预处理的单目视觉和imu联合定位与建图方法。对比surf，orb和sift特征点的性能，采用速度较快的orb特征点，使用一种基于时空的马尔科夫随机场算法进行图像去雾预处理，随后采用单目视觉结合imu的方法进行危化品事故环境下移动机器人的定位和建图，提高了机器人在危化品事故环境中定位和建图的鲁棒性。

本发明提出一种应用于危化品事故环境下的移动机器人定位与建图方法，包括：

步骤一：采用kinect相机采集图像信息，采用基于时空的马尔科夫随机场方法对图像进行去雾操作。考虑到危化品爆炸造成的烟雾，其对特征匹配的影响如图6所示，会降低特征正确匹配的数量。采用基于时空的马尔科夫随机场算法进行图像去雾预处理。

步骤二：在相机采集的图像进行去雾操作的基础上，采用单目视觉和imu联合初始化，先进行单目视觉初始化，然后利用单目slam测量的每个关键帧的位置和旋转来初始化imu，得到较为准确的尺度，重力加速度的方向，速度，陀螺仪和加速度的偏置。

步骤三：在系统初始化完成后，构建单目视觉和imu融合slam系统。前端采用光照鲁棒性较强的orb特征点进行提取和匹配，采用对极几何约束和pnp算法估计相机位姿。融合imu数据跟踪局部地图，后端优化重投影误差项和imu的时间误差项。而对于slam系统建图，从初始的关键帧开始增量的构建一个生成树。对于每一个新的关键帧进行闭环搜索，使用视觉词袋进行回环检测，为确保全局地图的一致性，利用相似性约束对位姿图进行优化。

一种基于单目视觉和imu传感器的危化品事故中移动机器人定位与建图方法。

步骤一包括以下步骤：

步骤1：从单目相机中获取图像帧数据。

步骤2：采用dcp算法估计烟雾浓度，计算暗通道图d(x)。

步骤3：基于ivp算法，构建时间和空间马尔科夫随机场方法求解相关优化随机域参数，计算得到去雾的图像。

根据本发明的一种危化品事故环境的定位与建图方法，在步骤二中，包括以下步骤：

步骤1：单目视觉的初始化，根据匹配关键帧之间的位姿，三角化测量初始的特征点云深度，进而获得点云地图，ba优化初始化点云。

步骤2：根据两个连续帧进行陀螺仪的偏置估计，计算尺度、重力方向、速度、加速度和陀螺仪的偏置。

根据本发明的一种基于单目视觉和imu传感器的危险化工事故环境下移动机器人定位与建图方法，在步骤三中，包括以下步骤：

步骤1：orb特征点的提取和匹配。

步骤2：通过特征点匹配，采用对极几何约束和pnp算法估计相机位姿并跟踪局部地图，决策是否添加关键帧。

步骤3：如果有新的关键帧插入，利用视觉惯性融合imu数据采用局部ba算法优化相机位姿，融合关键帧数据。其中imu和视觉传感器融合的是重投影误差项和imu的时间误差项。

步骤4：如果有新的关键帧插入，在完成步骤三之后进行回环检测，纠正相机累计误差，为了确保全局地图的一致性，利用相似性约束对位姿图进行优化，最后启动启动第四个线程执行全局ba算法，来计算整个系统最优结构和运动的结果。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1为系统框架示意图；

图2为本发明中整体系统的流程图；

图3为危化品环境；

图4为帧到帧特征点的提取和匹配流程图；

图5为orb特征点提取和匹配实验仿真结果；

图6为图像去雾流程图。

具体实施方式

以下结合附图和具体实施例对本发明作详细描述。注意，以下结合附图和具体实施例描述的诸方面仅是示例性的，而不应被理解为对本发明的保护范围进行任何限制。

步骤一：针对危化品事故发生时造成的烟雾现象，烟雾会使得图片亮度提高而饱和度降低。采用一种基于时空的马尔科夫随机场算法进行图像去雾处理：

基于大气光散射模型：i(x)＝j(x)t(x)+a(1-t(x))

其中i(x)是观察到的图像，j(x)是真实场景图像，t(x)是中值透射率，a是全局大气光，x是像素值。采用暗通道先验算法得到暗通道图，其与透射率之间的关系为t(x)＝1-wd(x)/a，其中系数w设定为0.7。后建立时空mrf下以微调烟雾浓度d(x)，首先考虑的是暗通道图。

步骤1：获取单目相机获取的图片帧。

步骤2：采用dcp算法估计烟雾浓度，其中其中i^c的含义是图像的颜色通道，r、g、b分别代表红绿蓝三个通道，选择通道c亮度最低的像素值绘制暗通道图。

步骤4：基于像素亮度值先验算法，像素的亮度值因雾浓度不同而不同，基于时间和空间构建马尔科夫随机场。为了保证处理图片自然，时间上和空间上满足连续性，将图片的强度值函数v(x)线性关联到烟雾浓度d(x)，其关于参数w和b的空间s似然函数ps(w，b)：

其中参数ω(x)是以x为中心的r×r的图像块，σs是空间参数。

其关于时间t的似然函数p：

在第t帧，τ代表帧差，f是相邻帧的数量，这里将f设为7，是时间参数。

其联合的似然函数表述为：

其中σs被省略。

取log函数得到最大似然估计：

求导使得函数取极值，计算得像素优化随机域参数w和b。

步骤4：由上步得到的参数w和b，带入求得图像的d(x)。

步骤5：根据大气光散射模型计算得到相应的改善后的图像。

步骤二：单目视觉和imu联合初始化。

步骤1：单目视觉的初始化，采用针孔相机模型，相机坐标系到世界坐标系的对应关系如下，其中像素坐标系原点平移[cx，cy]^t，设焦距为f，fx和fy的单位是像素，中间量组成的矩阵k为相机内参数矩阵，现实世界中的空间点p的坐标[x，y，z]^t与对应的像素坐标[u，v]^t：

其中相机坐标系下坐标puv世界坐标系下位姿pw的变换，由其外参数旋转矩阵r和平移向量t来描述：

系统采用基于模型选择的自动初始化方法，对平面场景算法选择单应矩阵来计算，而对于非平面矩阵，选择基础矩阵计算。通过匹配选取两个可以作为起始两帧的初始帧，根据匹配计算两帧之间的位姿，三角化测量初始的特征点云深度，进而获得点云地图，ba优化初始化点云。

步骤2：运行纯视觉slam15s，利用单目slam测量的每个关键帧的位姿来初始化imu，计算尺度、重力方向、速度、加速度和陀螺仪的偏置。imu坐标系通常由b来表示，以一定的时间间隔δt来采样，测量imu的加速度ab和角速度ωb，imu测量模型：

其中表示在坐标系b下b相对于w的瞬时加速度真值，其中b^g(t)为随时间t缓慢变化的imu内部误差，η^g(t)为测量白噪声，wa(t)∈r³表示w坐标下imu的瞬时加速度，gw表示w坐标系下重力矢量。

imu的初始化包括以下几个步骤：

步骤2.1.利用多个连续关键帧进行陀螺仪偏置估计，假设在一个常量bg的基础上加上一个微小的的改变，通过最小化旋转误差来计算陀螺仪偏置的更新量：

其中n是关键帧的个数，由相机旋转乘上刚体到相机的旋转变换矩阵rcb，δri，i+1是两个连续关键帧预积分旋转测量量。给以零初始值，上述函数通过高斯牛顿迭代，求解得到最小目标函数对应的为陀螺仪的初始偏置。

步骤2.2.尺度和重力加速度方向粗略估计，为了解决单目slam尺度不确定性，在相机坐标系和imu坐标系变换之间加入一个尺度因子s：wpb＝swpc+rwccpb，wpb，wpc，cpb分别表示相机坐标系到世界坐标系，imu坐标系到世界坐标系，imu坐标系到相机坐标系的平移，，rwc代表相机到世界坐标系的旋转，带回预积分公式。结合所有的三个连续关键帧构造方程a3(n-2)×4x4×1＝b3(n-2)×1，然后通过svd求解非齐次方程，得到尺度因子s^*和重力加速度

步骤2.3.通过计算连续帧间的线性方程解加速度偏置估计，尺度和重力方向精确估计；

步骤2.4.速度估计，在已知尺度，重力加速度方向和偏置，带入方程计算出初始化关键帧的旋转rwb、位置wpb和速度wvb量，完成imu初始化。

步骤三：单目视觉和imu融合slam系统构建。

步骤1：视觉orb特征点提取和匹配。采用具有旋转不变性和尺度不变性的orb特征点进行特征点的提取和描述子计算，brief描述子进行描述子的计算，特征点的提取和匹配的对比如图4图5所示；

步骤1.1.关键点提取，利用fast特征点检测的方法来检测角点，然后利用harris角点的度量方法，从fast特征点中挑选出harris角点响应值最大的n个特征点。其中harris角点的响应函数r定义为：

r＝detm-α(tracem)²

其中detm为矩阵的行列式，tracem为矩阵m的直迹，α为常数，取值范围为0.04～0.06。

步骤1.2.特征点描述，选择brief作为特征描述方法，但在此基础上加上了旋转不变性，对于任何一个特征点来说，它的brief描述子是一个长度为n的二值码串，这个二值串是由通过特征点周围n个点对比较(2n个点)生成的0-1二值串binarray：

binarray＝[p1，p2，…，pm，」，pi∈{0，1}

步骤1.3.帧到帧的特征点匹配，使用使用gms(网格算法)进行orb特征点的误匹配消除。将运动平滑度封装起来，作为局域对之间的具有一定数量的特征匹配的统计可能性。进一步消除高噪声条件下特征点误匹配。

步骤2：进行相机的跟踪并并估计相机位姿。基于单目视觉传感器，采用pnp算法进行相机位姿估计。如果跟踪失败启动重定位，并利用连续帧数据重新估计imu的陀螺仪偏置和加速度偏置，最后决策是否添加关键帧。

步骤2.1.如果上一帧图像跟踪成功，获得当前帧和局部地图的匹配。如果不成功则搜索附近的地图点云中的匹配点，根据寻找到的对应匹配点估计当前相机位姿。

步骤2.2.如果无法在扩大范围的情况下找到匹配点，计算当前帧词袋向量，匹配关键帧，采用pnp算法估计相机位姿。

步骤2.3：当系统跟踪失败，成功完成相机重定位之后，利用20个连续帧的定位使用纯视觉信息，已知尺度和重力加速度情况下根据imu初始化相关公式重新估计加速度偏置和陀螺仪偏置。

步骤2.4.跟踪局部地图，融合imu数据，根据地图点是否被更新，采用不同的优化方式，如果地图点更新，imu预积分误差项建立在当前帧和最近一个关键帧i之间，最小化重投影误差；如果地图点未更新，优化方程建立在当前帧和上一帧。

步骤2.5.关键帧决策，判断是否加入新的关键帧。判定条件包括当前帧跟踪的地图点少于50个，当前帧跟踪少于参考关键帧点云的90％，或者局部地图处于空闲状态。

步骤3：视觉惯性局部地图紧耦合优化，运用局部的ba算法。

步骤3.1.更新关键帧和地图点的关系，保留共视程度高的地图点，剔除新添加的质量不高的地图点。

步骤3.2.计算imu预积分，需要处理三件事。首先更新当前的预积分量，然后更新imu残差的协方差矩阵，最后更新imu残差对于bias的雅可比矩阵。其中imu的预积分公式如下：

其中i和i+1是帧数，是在i+1帧在世界坐标系下的旋转、速度和平移，δti，i+1为两帧之间的时间差，δri，i+1，δvi，i+1，δpi，i+1是两帧之间的旋转、速度和平移差量，是旋转相对角速度的雅克比矩阵，和是速度相对角速度和加速度的雅克比矩阵，和是平移相对于角速度和加速度的雅克比矩阵，和表示i帧的陀螺仪偏置和加速度计偏置，gw是重力加速度，w代表世界坐标系，b是imu坐标系。

步骤3.3.视觉惯性局部地图耦合优化，运用局部的ba算法，通过最小化重投影误差和imu时间误差来优化相机位姿。imu+视觉优化的是重投影误差项+imu的时间误差项：

其中θ包含当前帧的旋转，平移，速度，偏置信息。

步骤4.关键帧剔除，剔除90％以上的mappoints能被其它共视关键帧(至少3个)观测到的关键帧。

步骤四：在完成步骤三之后采用视觉词袋方法进行回环检测，通过执行位姿图的优化来更正累计误差：

步骤1：首先，计算当前帧的bow，并设定初始位姿为上一帧的位姿；其次，根据位姿和bow词典来寻找特征匹配进行回环检测；

步骤2：回环融合，融合重复的地图点云。为了有效的闭合回环，通过essentialgraph优化位姿图。优化过后，每个地图点云根据关键帧的校正进行变换。

步骤3：在位姿优化之后，会启动第四个线程来执行全局ba算法优化，来计算整个系统最优结构和运动的结果。

为了使得文章的解释更简单化，已上述的图文描述为一系列步骤，但是应该理解并领会，这些方法不受操作的次序所限制，因为按照一个或多个步骤进行实施，一些动作可按不同的顺序发生，但本领域技术人员可以理解其动作发生的原理。

尽管已对本发明说明性的具体实施方式逐步进行了描述，以便于本技术领域的技术人员能够进行领会，但是本发明不仅限于具体实施方式的范围，本领域技术人员可以在权利要求的范围内做出各种变形或修改，只要各种变化只要在所附的权利要求限定和确定的本发明精神和范围内。