一种未知环境下排爆机器人多阶自主导航探测系统及方法

摘要

本发明公开了一种未知环境下排爆机器人多阶自主导航探测系统，包括机器人本体，在机器人上设置有控制器、TNT气体浓度传感器、RGB‑D相机和激光雷达和多阶探测方法。发明提出两步走的SLAM方法实现排爆机器人的自主导航：第一步为粗略路径规划阶段，即采用RGB‑D相机和激光雷达融合的方法，使用SLAM算法实现机器人的自主定位和环境地图构建，基于嗅觉技术检测TNT分子信息，提出了规划类似“Ω”形的路径检测TNT分子存在区域；继而根据TNT分子浓度的渐增，提出圆形法粗略规划路径，到达接近未爆物的区域。第二步为精细路径规划阶段，即采用SLAM算法精确规划路径到达未爆物所在位置，完成爆炸物排爆工作。

说明书

技术领域

本发明属于特种机器人自主导航及探测领域，具体涉及一种未知环境下排爆机器人多阶自主导航探测系统及方法。

背景技术

随着全球化的发展和反恐工作的深入，与恐怖分子的矛盾进一步激化，各国都存在不法分子安放炸弹的威胁。通过放置炸弹来制造爆炸恐怖活动，是恐怖主义进行恐怖主义活动的惯用手段，据统计，目前世界上遗留地雷有一亿一千万枚。虽然现在排爆技术在不断提升，但大多数还是靠人力排爆，且很多未爆物被放置于未知环境下，这无疑为排爆工作增加了难度，更严重威胁着排爆人员和社会人民的生命安全。

近年来，机器人行业发展迅速，机器人涉及的领域在日益增多，功能也在日趋完善，但移动机器人的自主性仍是机器人界的一大难题。排爆机器人作为一种安全可靠的排爆工具越来越受到各国的重视，多种新型排爆机器人也在陆续问世。尽管如此，如今的排爆机器人仍有很大的局限性：一是当工作环境已知时，排爆机器人能够自主完成排爆任务；二是当工作环境未知时，排爆机器人会迷失方向，无法自主导航到达目标物，完成排爆任务。基于现实状况，未爆物更多处于未知的复杂环境中，操作人员和排爆机器人难以到达未爆物所处位置并完成排爆，从而给社会留下了巨大的安全隐患。

发明内容

针对上述现有排爆机器人功能的不足，本发明提出了一种未知环境下排爆机器人多阶自主导航探测系统及方法，本发明采用一种类似“Ω”形和圆形的未爆物搜索方法，并利用RGB-D相机和激光融合实现排爆机器人在未知复杂环境下到达未爆物的自主导航，从而解决排除“隐蔽未爆物”难的问题。

为解决上述技术问题，本发明所采用的技术方案如下：

一种未知环境下排爆机器人多阶自主导航探测系统，包括机器人，在机器人上设置有控制器、TNT气体浓度传感器、RGB-D相机和激光雷达；TNT气体浓度传感器，用于实时检测机器人所处位置的TNT气体的浓度信息并将检测信息传输至控制器内；控制器根据浓度信息得到机器人的前进方向；RGB-D相机用于采集机器人所处位置的环境图像并将采集图像传输至控制器内；控制器将将接收到的图像信息进行处理后构建地图和确认未爆物；激光雷达用于检测机器人所处环境信息并将检测信息传输给控制器，控制器处理后判断机器人所处环境是否存在障碍物，控制器将接收到的信息综合处理后机器人发送移动命令，机器人接收命令后进行避障导航移动直至找到未爆物。

一种未知环境下排爆机器人多阶自主导航探测方法，步骤如下：

S1，粗略路径规划。

S1.1，RGB-D相机实时采集机器人所处位置的rgb图像和depth图像并传输至控制器内，控制器运用OpenCV库提取rgb图像的特征点。

S1.1.1，从rgb图像中提取粗略特征点。

根据rgb图像像素点亮度，从rgb图像中选择满足正态分布G(I)的像素点作为粗略特征点；正态分布G(I)的公式为：

其中，I为像素点亮度，(μ，σ²)为像素点的像素坐标，μ表示位置参数，σ²表示尺度参数。

S1.1.2，在rgb图像中从粗略特征点中选取像素点P，并设定像素点的亮度为I，阈值为T。

S1.1.3，以像素点P为中心，构建正方形图像块，并从粗略特征点中选取正方形4个顶点的像素点。

S1.1.4，判断像素点P是否为精准特征点；若正方形4个顶点的像素点中有3个同时大于I+T或小于I-T，则像素点p为精确特征点并执行下一步，执行步骤S1.1.5，否则剔除像素点P点并重复步骤S1.1.2-S1.1.3。

S1.1.5，计算精确特征点p对应正方形图像块的质心C。

C＝(x_c,y_c)(3)；

其中，p,q表示矩阶；N表示像素点个数；f(x,y)表示像素点的灰度值。

S1.1.6，连接图像块的中心点P和质心C，得到一个方向向量得到精准特征点的方向；

θ＝arctan(m01/m10)(5)。

S1.1.7，重复步骤S1.1.2-S1.1.6直至得到所有的精准特征点。

S1.2，将当前时刻的rgb图像与下一时刻的rgb图像通过堆排序进行特征匹配得到匹配点对。

S1.2.1，设定当前时刻的rgb图像为图像I_t，提取的精准特征点为m＝1,2,...,m；下一时刻的rgb图像为图像I_t+1，提取的精准特征点为

S1.2.2，对每一个精准特征点和所有的精准特征点分别测量描述子距离。

S1.2.3，以描述子距离为记录数构建小顶堆。

S1.2.4，以测得堆顶数据的精准特征点为精准特征点的匹配点。

S1.2.5，依次循环，匹配所有的精准特征点和精准特征点

S1.3，将步骤S1.2得到的匹配点对通过tf坐标转换得到三维坐标匹配点对。

S1.4，采用OpenCV库中的RANSAC函数构建运动变换模型R,t，运动变换模型R,t为：

S1.5，通过最小二乘法求解运动变换模型R,t，得到相机位姿序列和相应的相机运动序列，构建相机位姿图，并并通过闭环检测rgb图像及其对应的depth图像，以获得相邻两帧rgb图像间运动距离过大或提取到的特征点过少为约束条件，对图像进行筛选以优化相机位姿图；求解公式为：

式中p_i，q_i表示两帧图像一一对应的特征点，R表示旋转向量，t表示平移向量。

S1.6，采用基于g2o库的图优化方法全局优化位姿图，位姿图边为相机位姿间的相对运动估计，继而获得到相机运动轨迹，构建三维点云地图，并通过Octomap库将点云地图转化为三维栅格地图。

S1.7，获得障碍物的轮廓。

S1.7.1，在构建地图时，激光雷达采集机器人所处环境中障碍物的距离信息和角度信息并传输至控制器内，控制器进行处理并划分区域。

S1.7.1.1，设定一个区域阈值T。

S1.7.1.2，计算相邻扫描点(x_i，x_i+1)间距D。

S1.7.1.3，比较间距D与区域阈值T；如果D≦T，则这两个扫描点源于同一障碍物；如果D>T，则这两个扫描点源于不同障碍物，并将x_i作为当前区域的终点，x_i+1作为下一区域的起点。

S1.7.1.4，重复步骤S1.7.1.2-S1.7.1.3，直至将所有扫描点循环完毕，完成区域的划分。

S1.7.2，筛选有效区域。

对步骤S1.7.1中得到的多个数据区域，将一些扫描点很少，或扫描点很密集的区域视为噪声区域剔除，剩余区域为合格数据区域。

S1.7.3，在合格数据区域内得到障碍物的轮廓形状。

S1.7.3.1，设定阈值L；

S1.7.3.2，根据合格数据区域内扫描点的坐标(x_i,y_i)并结合合最小二乘法拟合直线；

y＝kx+b(11)；

S1.7.3.3，计算合格数据区域内各扫描点距离拟合直线的距离d；

S1.7.3.4，将距离d与阈值L一一进行比较；若d大于L，则障碍物某一平面轮廓是折线，距离d对应的扫描点为障碍物的角点；反之，则障碍物某一平面轮廓是直线；

S1.7.3.5，连接该合格数据区域内的所有角点连接在一起，得到障碍物的轮廓形状；

S1.7.3.6，重复步骤S1.7.3.2-S1.7.3.5，直至所有合格数据区域都循环一遍。

S1.8，根据步骤S1.7得到的障碍物的轮廓形状结合RGD-D相机采集的环境障碍物坐标信息，得到障碍物的位置信息，将障碍物的位置信息添加到步骤S1.6得到的三维栅格地图中，并将被占据的栅格和与被占据栅格相邻的栅格放入不可通过序列，未被占据栅格放入可通过序列，得到完善的三维格栅地图。

S1.9，在完善三维格栅地图的前提下，检测TNT分子的存在区域。

S1.9.1，在完善三维格栅地图内，以机器人当前位置为原点A，水平方向为x轴，纵向为y轴，竖直方向为z轴的三维坐标系。

S1.9.2，控制器从完善三维格栅地图中获得原点A的坐标位置，从TNT气体浓度传感器处得到原点A处的TNT分子浓度C。

S1.9.3，控制器将TNT分子浓度C与0作比较，若C＝0，设定阈值M，则控制器控制机器人以范围每次增加M米的Ω形行进，直至找到TNT分子的存在区域。

S1.10，找到TNT分子的存在区域后，控制器控制机器人朝TNT分子浓度增大的方向搜索未爆物的存在区域。

S1.10.1，构建以机器人当前位置B_j点为圆心，a为半径的圆M_j，圆M_j为机器人取样路径，以π为间隔距离在圆M_j上进行采样，得到2n个采样点的坐标，坐标集合A为：A＝{A_i(x_i,y_i,z)},i＝1,2,3,...,2n，以及2n个采样点的TNT分子浓度，TNT分子浓度集合C为：C＝{C_i},i＝1,2,3,...2n。

S1.10.2，对2n个采样点进行仿真，判断是否满足高斯分布；若满足高斯图像则停止采样，进行建模；若不满足高斯分布，则从TNT分子浓度集合C中选择最大值，以TNT分子浓度最大值对应的采样点A_i(x_i,y_i,z)和当前位置B_j点所在直线为前进方向，机器人沿着前进方向前进大于当前半径的距离到达另一点B_j+1。

S1.10.3，以另一点B_j+1为机器人的当前位置，并令a＝a+g,g为常数；重复步骤S1.10.1-S1.10.3，直至采样点满足高斯分布，此时机器人当前位置为B_m。

S1.10.4，对满足高斯分布的采样点进行整理并建立TNT分子的扩散高斯模型；

其中，C(x_i,y_i,z)为采样点A(x_i,y_i,z)处的TNT分子浓度，Q为未爆物泄露TNT分子速率，μ表示平均风速，σ_y为TNT分子水平方向的扩散参数，σ_z为TNT分子竖直方向的扩散参数。

S1.10.5，计算TNT分子浓度递增梯度，并采用预测浓度的方法逆推未爆物存在区域；

式中表示采样点A_i和采样点A_i+1两点间的浓度比值，S表示采样点A_i和采样点A_i+1两点间的距离；假设未爆物在距离机器人S处，则未爆物在以机器人所在位置B_m为圆心，S为半径的圆D上。

S1.10.6，构建以机器人当前位置B_m为圆心，S为半径的圆D。

S1.10.7，从B_m点出发，在圆D上搜索满足TNT分子浓度递增梯度的采样点A_i，若不存在，则以圆D上浓度最大的点为A_i点，机器人以B_m，A_i点所在直线为前进方向并前进S米达到B_m+1点。

S1.10.8，更新机器人当前位置，并重复步骤S1.10.6-S1.10.7，直至有多个圆D相交于一点，交点视为未爆物疑似存在区域，粗SLAM结束。

S2，精细路径规划。

S2.1，在完善三维格栅地图内的未爆物疑似存在区域，构建以机器人为根节点的单叉树。

S2.2，根据障碍物被检测到的顺序，将障碍物依次作为上一障碍物的叶子结点和下一障碍物的根节点，并增加到增加在单叉树上。

S2.3，遍历单叉树上的叶子节点，并探测节点是否为未爆物，若不是，则避开障碍物，并对不可通过栅格和可通过栅格的改变，并采用A*算法得到节点之间的最优路径到达下一叶子节点并继续探测，直至探测到未爆物。

本发明提出两步走的SLAM方法实现排爆机器人的自主导航：第一步为粗略路径规划阶段,即采用RGB-D相机和激光雷达融合的方法，使用SLAM算法实现机器人的自主定位和环境地图构建，基于嗅觉技术检测TNT分子信息，提出了规划类似“Ω”形的路径检测TNT分子存在区域；继而根据TNT分子浓度的渐增，提出圆形法粗略规划路径，到达接近未爆物的区域。第二步为精细路径规划阶段，即采用SLAM算法精确规划路径到达未爆物所在位置，完成排爆工作。通过RGB-D相机采集外界环境信息，采用RGB-D SLAM实现机器人的自主导航和地图构建，再通过激光雷达获取环境中障碍物的位置信息，最后规划路径并实现避障。本发明探测准确，提出的多阶探测方法，不仅节省了一定的探测时间，还通过先完成目标物的模糊范围定位，再完成目标物具体位置的精确定位的方法，实现了目标物的精确定位，最终实现了在耗时短的基础上精确定位的目标，弥补了导航界定位精确和耗时短不能同时满足的缺陷。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明地图构建流程图。

图2为本发明未爆物搜索过程图。

图3为本发明“Ω”形行走路线图。

图4为本发明圆形法找寻未爆物的路线图。

图5为本发明避障路径图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种未知环境下排爆机器人多阶自主导航探测系统，包括机器人，在机器人上设置有控制器、TNT气体浓度传感器、RGB-D相机和激光雷达；TNT气体浓度传感器，用于实时检测机器人所处位置的TNT气体的浓度信息并将检测信息传输至控制器内；控制器根据浓度信息得到机器人的前进方向；RGB-D相机用于采集机器人所处位置的环境图像并将采集图像传输至控制器内；控制器将将接收到的图像信息进行处理后构建地图和确认未爆物；激光雷达用于检测机器人所处环境信息并将检测信息传输给控制器，控制器处理后判断机器人所处环境是否存在障碍物，控制器将接收到的信息综合处理后机器人发送移动命令，机器人接收命令后进行避障导航移动直至找到未爆物。

并提供一种未知环境下排爆机器人多阶自主导航探测方法，步骤如下：

S1，粗略路径规划，如图1所示。

S1.1，RGB-D相机实时采集机器人所处位置的rgb图像和depth图像并传输至控制器内，控制器运用OpenCV库提取rgb图像的特征点。

S1.1.1，从rgb图像中提取粗略特征点。

根据rgb图像像素点亮度，从rgb图像中选择满足正态分布G(I)的像素点作为粗略特征点；正态分布G(I)的公式为：

其中，I为像素点亮度，(μ，σ²)为像素点的像素坐标，μ表示位置参数，σ²表示尺度参数。

S1.1.2，在rgb图像中从粗略特征点中选取像素点P，并设定像素点的亮度为I，阈值为T。

S1.1.3，以像素点P为中心，构建正方形图像块，并从粗略特征点中选取正方形4个顶点的像素点。

S1.1.4，判断像素点P是否为精准特征点；若正方形4个顶点的像素点中有3个同时大于I+T或小于I-T，则像素点p为精确特征点并执行下一步，执行步骤S1.1.5，否则剔除像素点P点并重复步骤S1.1.2-S1.1.3。

S1.1.5，计算精确特征点p对应正方形图像块的质心C。

C＝(x_c,y_c)(3)；

其中，p,q表示矩阶；N表示像素点个数；f(x,y)表示像素点的灰度值。

S1.1.6，连接图像块的中心点P和质心C，得到一个方向向量得到精准特征点的方向；

θ＝arctan(m01/m10)(5)。

S1.1.7，重复步骤S1.1.2-S1.1.6直至得到所有的精准特征点。

S1.2，将当前时刻的rgb图像与下一时刻的rgb图像通过堆排序进行特征匹配得到匹配点对。

S1.2.1，设定当前时刻的rgb图像为图像I_t，提取的精准特征点为m＝1,2,...,m；下一时刻的rgb图像为图像I_t+1，提取的精准特征点为

S1.2.2，对每一个精准特征点和所有的精准特征点分别测量描述子距离。

S1.2.3，以描述子距离为记录数构建小顶堆。

S1.2.4，以测得堆顶数据的精准特征点为精准特征点的匹配点。

S1.2.5，依次循环，匹配所有的精准特征点和精准特征点

S1.3，将步骤S1.2得到的匹配点对通过tf坐标转换得到三维坐标匹配点对。

S1.4，采用OpenCV库中的RANSAC函数构建运动变换模型R,t，运动变换模型R,t为：

S1.5，通过最小二乘法求解运动变换模型R,t，得到相机位姿序列和相应的相机运动序列，构建相机位姿图，并并通过闭环检测rgb图像及其对应的depth图像，以获得相邻两帧rgb图像间运动距离过大或提取到的特征点过少为约束条件，对图像进行筛选以优化相机位姿图；求解公式为：

式中p_i，q_i表示两帧图像一一对应的特征点，R表示旋转向量，t表示平移向量。

S1.6，采用基于g2o库的图优化方法全局优化位姿图，位姿图边为相机位姿间的相对运动估计，继而获得到相机运动轨迹，构建三维点云地图，并通过Octomap库将点云地图转化为三维栅格地图。

S1.7，获得障碍物的轮廓。

S1.7.1，在构建地图时，激光雷达采集机器人所处环境中障碍物的距离信息和角度信息并传输至控制器内，控制器进行处理并划分区域。

S1.7.1.1，设定一个区域阈值T。

S1.7.1.2，计算相邻扫描点(x_i,x_i+1)间距D。

S1.7.1.3，比较间距D与区域阈值T；如果D≦T，则这两个扫描点源于同一障碍物；如果D>T，则这两个扫描点源于不同障碍物，并将x_i作为当前区域的终点，x_i+1作为下一区域的起点。

S1.7.1.4，重复步骤S1.7.1.2-S1.7.1.3，直至将所有扫描点循环完毕，完成区域的划分。

S1.7.2，筛选有效区域。

对步骤S1.7.1中得到的多个数据区域，将一些扫描点很少，或扫描点很密集的区域视为噪声区域剔除，剩余区域为合格数据区域。

S1.7.3，在合格数据区域内得到障碍物的轮廓形状。

S1.7.3.1，设定阈值L；

S1.7.3.2，根据合格数据区域内扫描点的坐标(x_i,y_i)并结合合最小二乘法拟合直线；

y＝kx+b(11)；

S1.7.3.3，计算合格数据区域内各扫描点距离拟合直线的距离d；

S1.7.3.4，将距离d与阈值L一一进行比较；若d大于L，则障碍物某一平面轮廓是折线，距离d对应的扫描点为障碍物的角点；反之，则障碍物某一平面轮廓是直线；

S1.7.3.5，连接该合格数据区域内的所有角点连接在一起，得到障碍物的轮廓形状；

S1.7.3.6，重复步骤S1.7.3.2-S1.7.3.5，直至所有合格数据区域都循环一遍。

S1.8，根据步骤S1.7得到的障碍物的轮廓形状结合RGD-D相机采集的环境障碍物坐标信息，得到障碍物的位置信息，将障碍物的位置信息添加到步骤S1.6得到的三维栅格地图中，并将被占据的栅格和与被占据栅格相邻的栅格放入不可通过序列，未被占据栅格放入可通过序列，得到完善的三维格栅地图。

S1.9，在完善三维格栅地图的前提下，检测TNT分子的存在区域。

如图2所示，S1.9.1，在完善三维格栅地图内，以机器人当前位置为原点A，水平方向为x轴，纵向为y轴，竖直方向为z轴的三维坐标系。

S1.9.2，控制器从完善三维格栅地图中获得原点A的坐标位置，从TNT气体浓度传感器处得到原点A处的TNT分子浓度C。

S1.9.3，控制器将TNT分子浓度C与0作比较，若C＝0，设定阈值M，则控制器控制机器人以范围每次增加M米的Ω形行进，如图3所示，直至找到TNT分子的存在区域。

S1.10，找到TNT分子的存在区域后，控制器控制机器人朝TNT分子浓度增大的方向搜索未爆物的存在区域，如图4所示。

S1.10.1，构建以机器人当前位置B_j点为圆心，a为半径的圆M_j，圆M_j为机器人取样路径，以π为间隔距离在圆M_j上进行采样，得到2n个采样点的坐标，坐标集合A为：A＝{A_i(x_i,y_i,z)},i＝1,2,3,...,2n，以及2n个采样点的TNT分子浓度，TNT分子浓度集合C为：C＝{C_i},i＝1,2,3,...2n。

S1.10.2，对2n个采样点进行仿真，判断是否满足高斯分布；若满足高斯图像则停止采样，进行建模；若不满足高斯分布，则从TNT分子浓度集合C中选择最大值，以TNT分子浓度最大值对应的采样点A_i(x_i,y_i,z)和当前位置B_j点所在直线为前进方向，机器人沿着前进方向前进大于当前半径的距离到达另一点B_j+1。

S1.10.3，以另一点B_j+1为机器人的当前位置，并令a＝a+g,g为常数；重复步骤S1.10.1-S1.10.3，直至采样点满足高斯分布，此时机器人当前位置为B_m。

S1.10.4，对满足高斯分布的采样点进行整理并建立TNT分子的扩散高斯模型；

其中，C(x_i,y_i,z)为采样点A(x_i,y_i,z)处的TNT分子浓度，Q为未爆物泄露TNT分子速率，μ表示平均风速，σ_y为TNT分子水平方向的扩散参数，σ_z为TNT分子竖直方向的扩散参数。

S1.10.5，计算TNT分子浓度递增梯度，并采用预测浓度的方法逆推未爆物存在区域；

式中表示采样点A_i和采样点A_i+1两点间的浓度比值，S表示采样点A_i和采样点A_i+1两点间的距离；假设未爆物在距离机器人S处，则未爆物在以机器人所在位置B_m为圆心，S为半径的圆D上。

S1.10.6，构建以机器人当前位置B_m为圆心，S为半径的圆D。

S1.10.7，从B_m点出发，在圆D上搜索满足TNT分子浓度递增梯度的采样点A_i，若不存在，则以圆D上浓度最大的点为A_i点，机器人以B_m，A_i点所在直线为前进方向并前进S米达到B_m+1点。

S1.10.8，更新机器人当前位置，并重复步骤S1.10.6-S1.10.7，直至有多个圆D相交于一点，交点视为未爆物疑似存在区域，粗SLAM结束。

S2，精细路径规划。

S2.1，在完善三维格栅地图内的未爆物疑似存在区域，构建以机器人为根节点的单叉树。

S2.2，根据障碍物被检测到的顺序，将障碍物依次作为上一障碍物的叶子结点和下一障碍物的根节点，并增加到增加在单叉树上。

S2.3，遍历单叉树上的叶子节点，并探测节点是否为未爆物，若不是，则避开障碍物，并对不可通过栅格和可通过栅格的改变，如图5所示，并采用A*算法得到节点之间的最优路径到达下一叶子节点并继续探测，直至探测到未爆物。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。