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

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

Info

Publication number
CN107526360A
CN107526360A CN201710881953.7A CN201710881953A CN107526360A CN 107526360 A CN107526360 A CN 107526360A CN 201710881953 A CN201710881953 A CN 201710881953A CN 107526360 A CN107526360 A CN 107526360A
Authority
CN
China
Prior art keywords
point
robot
points
tnt
controller
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201710881953.7A
Other languages
English (en)
Other versions
CN107526360B (zh
Inventor
蔡磊
焦红伟
蔡晨
程静
李国厚
赵明富
余周
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Henan Institute of Science and Technology
Original Assignee
Henan Institute of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Henan Institute of Science and Technology filed Critical Henan Institute of Science and Technology
Priority to CN201710881953.7A priority Critical patent/CN107526360B/zh
Publication of CN107526360A publication Critical patent/CN107526360A/zh
Application granted granted Critical
Publication of CN107526360B publication Critical patent/CN107526360B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar

Abstract

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

Description

一种未知环境下排爆机器人多阶自主导航探测系统及方法
技术领域
本发明属于特种机器人自主导航及探测领域，具体涉及一种未知环境下排爆机器人多阶自主导航探测系统及方法。
背景技术
随着全球化的发展和反恐工作的深入，与恐怖分子的矛盾进一步激化，各国都存在不法分子安放炸弹的威胁。通过放置炸弹来制造爆炸恐怖活动，是恐怖主义进行恐怖主义活动的惯用手段，据统计，目前世界上遗留地雷有一亿一千万枚。虽然现在排爆技术在不断提升，但大多数还是靠人力排爆，且很多未爆物被放置于未知环境下，这无疑为排爆工作增加了难度，更严重威胁着排爆人员和社会人民的生命安全。
近年来，机器人行业发展迅速，机器人涉及的领域在日益增多，功能也在日趋完善，但移动机器人的自主性仍是机器人界的一大难题。排爆机器人作为一种安全可靠的排爆工具越来越受到各国的重视，多种新型排爆机器人也在陆续问世。尽管如此，如今的排爆机器人仍有很大的局限性：一是当工作环境已知时，排爆机器人能够自主完成排爆任务；二是当工作环境未知时，排爆机器人会迷失方向，无法自主导航到达目标物，完成排爆任务。基于现实状况，未爆物更多处于未知的复杂环境中，操作人员和排爆机器人难以到达未爆物所处位置并完成排爆，从而给社会留下了巨大的安全隐患。
发明内容
针对上述现有排爆机器人功能的不足，本发明提出了一种未知环境下排爆机器人多阶自主导航探测系统及方法，本发明采用一种类似“Ω”形和圆形的未爆物搜索方法，并利用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为像素点亮度，(μ，σ2)为像素点的像素坐标，μ表示位置参数，σ2表示尺度参数。
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＝(xc,yc) (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图像为图像It，提取的精准特征点为m＝1,2,...,m；下一时刻的rgb图像为图像It+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图像间运动距离过大或提取到的特征点过少为约束条件，对图像进行筛选以优化相机位姿图；求解公式为：
式中pi，qi表示两帧图像一一对应的特征点，R表示旋转向量，t表示平移向量。
S1.6，采用基于g2o库的图优化方法全局优化位姿图，位姿图边为相机位姿间的相对运动估计，继而获得到相机运动轨迹，构建三维点云地图，并通过Octomap库将点云地图转化为三维栅格地图。
S1.7，获得障碍物的轮廓。
S1.7.1，在构建地图时，激光雷达采集机器人所处环境中障碍物的距离信息和角度信息并传输至控制器内，控制器进行处理并划分区域。
S1.7.1.1，设定一个区域阈值T。
S1.7.1.2，计算相邻扫描点(xi，xi+1)间距D。
S1.7.1.3，比较间距D与区域阈值T；如果D≦T，则这两个扫描点源于同一障碍物；如果D>T，则这两个扫描点源于不同障碍物，并将xi作为当前区域的终点，xi+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，根据合格数据区域内扫描点的坐标(xi,yi)并结合合最小二乘法拟合直线；
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，构建以机器人当前位置Bj点为圆心，a为半径的圆Mj，圆Mj为机器人取样路径，以π为间隔距离在圆Mj上进行采样，得到2n个采样点的坐标，坐标集合A为：A＝{Ai(xi,yi,z)},i＝1,2,3,...,2n，以及2n个采样点的TNT分子浓度，TNT分子浓度集合C为：C＝{Ci},i＝1,2,3,...2n。
S1.10.2，对2n个采样点进行仿真，判断是否满足高斯分布；若满足高斯图像则停止采样，进行建模；若不满足高斯分布，则从TNT分子浓度集合C中选择最大值，以TNT分子浓度最大值对应的采样点Ai(xi,yi,z)和当前位置Bj点所在直线为前进方向，机器人沿着前进方向前进大于当前半径的距离到达另一点Bj+1。
S1.10.3，以另一点Bj+1为机器人的当前位置，并令a＝a+g,g为常数；重复步骤S1.10.1-S1.10.3，直至采样点满足高斯分布，此时机器人当前位置为Bm。
S1.10.4，对满足高斯分布的采样点进行整理并建立TNT分子的扩散高斯模型；
其中，C(xi,yi,z)为采样点A(xi,yi,z)处的TNT分子浓度，Q为未爆物泄露TNT分子速率，μ表示平均风速，σy为TNT分子水平方向的扩散参数，σz为TNT分子竖直方向的扩散参数。
S1.10.5，计算TNT分子浓度递增梯度，并采用预测浓度的方法逆推未爆物存在区域；
式中表示采样点Ai和采样点Ai+1两点间的浓度比值，S表示采样点Ai和采样点Ai+1两点间的距离；假设未爆物在距离机器人S处，则未爆物在以机器人所在位置Bm为圆心，S为半径的圆D上。
S1.10.6，构建以机器人当前位置Bm为圆心，S为半径的圆D。
S1.10.7，从Bm点出发，在圆D上搜索满足TNT分子浓度递增梯度的采样点Ai，若不存在，则以圆D上浓度最大的点为Ai点，机器人以Bm，Ai点所在直线为前进方向并前进S米达到Bm+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为像素点亮度，(μ，σ2)为像素点的像素坐标，μ表示位置参数，σ2表示尺度参数。
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＝(xc,yc) (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图像为图像It，提取的精准特征点为m＝1,2,...,m；下一时刻的rgb图像为图像It+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图像间运动距离过大或提取到的特征点过少为约束条件，对图像进行筛选以优化相机位姿图；求解公式为：
式中pi，qi表示两帧图像一一对应的特征点，R表示旋转向量，t表示平移向量。
S1.6，采用基于g2o库的图优化方法全局优化位姿图，位姿图边为相机位姿间的相对运动估计，继而获得到相机运动轨迹，构建三维点云地图，并通过Octomap库将点云地图转化为三维栅格地图。
S1.7，获得障碍物的轮廓。
S1.7.1，在构建地图时，激光雷达采集机器人所处环境中障碍物的距离信息和角度信息并传输至控制器内，控制器进行处理并划分区域。
S1.7.1.1，设定一个区域阈值T。
S1.7.1.2，计算相邻扫描点(xi,xi+1)间距D。
S1.7.1.3，比较间距D与区域阈值T；如果D≦T，则这两个扫描点源于同一障碍物；如果D>T，则这两个扫描点源于不同障碍物，并将xi作为当前区域的终点，xi+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，根据合格数据区域内扫描点的坐标(xi,yi)并结合合最小二乘法拟合直线；
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，构建以机器人当前位置Bj点为圆心，a为半径的圆Mj，圆Mj为机器人取样路径，以π为间隔距离在圆Mj上进行采样，得到2n个采样点的坐标，坐标集合A为：A＝{Ai(xi,yi,z)},i＝1,2,3,...,2n，以及2n个采样点的TNT分子浓度，TNT分子浓度集合C为：C＝{Ci},i＝1,2,3,...2n。
S1.10.2，对2n个采样点进行仿真，判断是否满足高斯分布；若满足高斯图像则停止采样，进行建模；若不满足高斯分布，则从TNT分子浓度集合C中选择最大值，以TNT分子浓度最大值对应的采样点Ai(xi,yi,z)和当前位置Bj点所在直线为前进方向，机器人沿着前进方向前进大于当前半径的距离到达另一点Bj+1。
S1.10.3，以另一点Bj+1为机器人的当前位置，并令a＝a+g,g为常数；重复步骤S1.10.1-S1.10.3，直至采样点满足高斯分布，此时机器人当前位置为Bm。
S1.10.4，对满足高斯分布的采样点进行整理并建立TNT分子的扩散高斯模型；
其中，C(xi,yi,z)为采样点A(xi,yi,z)处的TNT分子浓度，Q为未爆物泄露TNT分子速率，μ表示平均风速，σy为TNT分子水平方向的扩散参数，σz为TNT分子竖直方向的扩散参数。
S1.10.5，计算TNT分子浓度递增梯度，并采用预测浓度的方法逆推未爆物存在区域；
式中表示采样点Ai和采样点Ai+1两点间的浓度比值，S表示采样点Ai和采样点Ai+1两点间的距离；假设未爆物在距离机器人S处，则未爆物在以机器人所在位置Bm为圆心，S为半径的圆D上。
S1.10.6，构建以机器人当前位置Bm为圆心，S为半径的圆D。
S1.10.7，从Bm点出发，在圆D上搜索满足TNT分子浓度递增梯度的采样点Ai，若不存在，则以圆D上浓度最大的点为Ai点，机器人以Bm，Ai点所在直线为前进方向并前进S米达到Bm+1点。
S1.10.8，更新机器人当前位置，并重复步骤S1.10.6-S1.10.7，直至有多个圆D相交于一点，交点视为未爆物疑似存在区域，粗SLAM结束。
S2，精细路径规划。
S2.1，在完善三维格栅地图内的未爆物疑似存在区域，构建以机器人为根节点的单叉树。
S2.2，根据障碍物被检测到的顺序，将障碍物依次作为上一障碍物的叶子结点和下一障碍物的根节点，并增加到增加在单叉树上。
S2.3，遍历单叉树上的叶子节点，并探测节点是否为未爆物，若不是，则避开障碍物，并对不可通过栅格和可通过栅格的改变，如图5所示，并采用A*算法得到节点之间的最优路径到达下一叶子节点并继续探测，直至探测到未爆物。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种未知环境下排爆机器人多阶自主导航探测系统，其特征在于：包括机器人，在机器人上设置有控制器、TNT气体浓度传感器、RGB-D相机和激光雷达；TNT气体浓度传感器，用于实时检测机器人所处位置的TNT气体的浓度信息并将检测信息传输至控制器内；控制器根据浓度信息得到机器人的前进方向；RGB-D相机用于采集机器人所处位置的环境图像并将采集图像传输至控制器内；控制器将将接收到的图像信息进行处理后构建地图和确认未爆物；激光雷达用于检测机器人所处环境信息并将检测信息传输给控制器，控制器处理后判断机器人所处环境是否存在障碍物，控制器将接收到的信息综合处理后机器人发送移动命令，机器人接收命令后进行避障导航移动直至找到未爆物。
2.一种未知环境下排爆机器人多阶自主导航探测方法，其特征在于，步骤如下：S1，粗略路径规划；
S1.1，RGB-D相机实时采集机器人所处位置的rgb图像和depth图像并传输至控制器内，控制器运用OpenCV库提取rgb图像的特征点；
S1.2，将当前时刻的rgb图像与下一时刻的rgb图像通过堆排序进行特征匹配得到匹配点对；
S1.3，将步骤S1.2得到的匹配点对通过tf坐标转换得到三维坐标匹配点对；
S1.4，采用OpenCV库中的RANSAC函数构建运动变换模型R,t，运动变换模型R,t为：
<mrow> <mo>&amp;ForAll;</mo> <mi>i</mi> <mo>,</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mo>=</mo> <msub> <mi>Rq</mi> <mi>i</mi> </msub> <mo>+</mo> <mi>t</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
S1.5，通过最小二乘法求解运动变换模型R,t，得到相机位姿序列和相应的相机运动序列，构建相机位姿图，并通过闭环检测rgb图像及其对应的depth图像，以获得相邻两帧rgb图像间运动距离过大或提取到的特征点过少为约束条件，对图像进行筛选以优化相机位姿图；求解公式为：
<mrow> <munder> <mrow> <mi>m</mi> <mi>i</mi> <mi>n</mi> </mrow> <mrow> <mi>R</mi> <mo>,</mo> <mi>t</mi> </mrow> </munder> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mo>|</mo> <mo>|</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mo>-</mo> <mrow> <mo>(</mo> <msub> <mi>Rq</mi> <mi>i</mi> </msub> <mo>+</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mo>(</mo> <mn>7</mn> <mo>)</mo> <mo>;</mo> </mrow>
式中pi，qi表示两帧图像一一对应的特征点，R表示旋转向量，t表示平移向量。
S1.6，采用基于g2o库的图优化方法全局优化位姿图，位姿图边为相机位姿间的相对运动估计，继而获得到相机运动轨迹，构建三维点云地图，并通过Octomap库将点云地图转化为三维栅格地图；
S1.7，获得障碍物的轮廓；
S1.8，根据步骤S1.7得到的障碍物的轮廓形状结合RGD-D相机采集的环境障碍物坐标信息，得到障碍物的位置信息，将障碍物的位置信息添加到步骤S1.6得到的三维栅格地图中，并将被占据的栅格和与被占据栅格相邻的栅格放入不可通过序列，未被占据栅格放入可通过序列，得到完善的三维格栅地图；
S1.9，在完善三维格栅地图的前提下，检测TNT分子的存在区域；
S1.10，找到TNT分子的存在区域后，控制器控制机器人朝TNT分子浓度增大的方向搜索未爆物的存在区域；
S2，精细路径规划；
S2.1，在完善三维格栅地图内的未爆物疑似存在区域，构建以机器人为根节点的单叉树；
S2.2，根据障碍物被检测到的顺序，将障碍物依次作为上一障碍物的叶子结点和下一障碍物的根节点，并增加到增加在单叉树上；
S2.3，遍历单叉树上的叶子节点，并探测节点是否为未爆物，若不是，则避开障碍物，并对不可通过栅格和可通过栅格的改变，并采用A*算法得到节点之间的最优路径到达下一叶子节点并继续探测，直至探测到未爆物。
3.根据权利要求2所述的未知环境下排爆机器人多阶自主导航探测方法，其特征在于，在步骤S1.1中，具体步骤为：S1.1.1，从rgb图像中提取粗略特征点；
根据rgb图像像素点亮度，从rgb图像中选择满足正态分布G(I)的像素点作为粗略特征点；正态分布G(I)的公式为：
<mrow> <mi>G</mi> <mrow> <mo>(</mo> <mi>I</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <msqrt> <mrow> <mn>2</mn> <mi>&amp;pi;</mi> </mrow> </msqrt> <mi>&amp;sigma;</mi> </mrow> </mfrac> <mi>exp</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <mi>&amp;mu;</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mrow> <mn>2</mn> <msup> <mi>&amp;sigma;</mi> <mn>2</mn> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，I为像素点亮度，(μ，σ2)为像素点的像素坐标，μ表示位置参数，σ2表示尺度参数；
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；
<mrow> <msub> <mi>m</mi> <mrow> <mi>p</mi> <mi>q</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>y</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>x</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msup> <mi>x</mi> <mi>p</mi> </msup> <msup> <mi>y</mi> <mi>q</mi> </msup> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>,</mo> <mi>p</mi> <mo>,</mo> <mi>q</mi> <mo>=</mo> <mo>{</mo> <mn>0</mn> <mo>,</mo> <mn>1</mn> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
C＝(xc,yc) (3)；
<mrow> <msub> <mi>x</mi> <mi>c</mi> </msub> <mo>=</mo> <mfrac> <msub> <mi>m</mi> <mn>10</mn> </msub> <msub> <mi>m</mi> <mn>00</mn> </msub> </mfrac> <mo>,</mo> <msub> <mi>y</mi> <mi>c</mi> </msub> <mo>=</mo> <mfrac> <msub> <mi>m</mi> <mn>01</mn> </msub> <msub> <mi>m</mi> <mn>00</mn> </msub> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，p,q表示矩阶；N表示像素点个数；f(x,y)表示像素点的灰度值；
S1.1.6，连接图像块的中心点P和质心C，得到一个方向向量得到精准特征点的方向；
θ＝arctan(m01/m10) (5)；
S1.1.7，重复步骤S1.1.2-S1.1.6直至得到所有的精准特征点。
4.根据权利要求2所述的未知环境下排爆机器人多阶自主导航探测方法，其特征在于，在步骤S1.2中，具体步骤为：S1.2.1，设定当前时刻的rgb图像为图像It，提取的精准特征点为下一时刻的rgb图像为图像It+1，提取的精准特征点为
S1.2.2，对每一个精准特征点和所有的精准特征点分别测量描述子距离；
S1.2.3，以描述子距离为记录数构建小顶堆；
S1.2.4，以测得堆顶数据的精准特征点为精准特征点的匹配点；
S1.2.5，依次循环，匹配所有的精准特征点和精准特征点
5.根据权利要求2所述的未知环境下排爆机器人多阶自主导航探测方法，其特征在于，在步骤S1.7中，具体步骤为：S1.7.1，在构建地图时，激光雷达采集机器人所处环境中障碍物的距离信息和角度信息并传输至控制器内，控制器进行处理并划分区域；
S1.7.2，筛选有效区域；
对步骤S1.7.1中得到的多个数据区域，将一些扫描点很少，或扫描点很密集的区域视为噪声区域剔除，剩余区域为合格数据区域；
S1.7.3，在合格数据区域内得到障碍物的轮廓形状。
6.根据权利要求5所述的未知环境下排爆机器人多阶自主导航探测方法，其特征在于，在步骤S1.7.1中，具体步骤为：S1.7.1.1，设定一个区域阈值T；
S1.7.1.2，计算相邻扫描点(xi,xi+1)间距D；
<mrow> <mi>D</mi> <mo>=</mo> <msqrt> <mrow> <msup> <msub> <mi>x</mi> <mi>i</mi> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>2</mn> <mo>&amp;times;</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>&amp;times;</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;times;</mo> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </msqrt> <mo>,</mo> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mi>n</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
S1.7.1.3，比较间距D与区域阈值T；如果D≦T，则这两个扫描点源于同一障碍物；如果D>T，则这两个扫描点源于不同障碍物，并将xi作为当前区域的终点，xi+1作为下一区域的起点；
S1.7.1.4，重复步骤S1.7.1.2-S1.7.1.3，直至将所有扫描点循环完毕，完成区域的划分。
7.根据权利要求5所述的未知环境下排爆机器人多阶自主导航探测方法，其特征在于，在步骤S1.7.3中，具体步骤为：S1.7.3.1，设定阈值L；
S1.7.3.2，根据合格数据区域内扫描点的坐标(xi,yi)并结合合最小二乘法拟合直线；
<mrow> <mi>arg</mi> <mi> </mi> <mi>m</mi> <mi>i</mi> <mi>n</mi> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>|</mo> <mo>|</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mi>k</mi> <mo>-</mo> <msub> <mi>y</mi> <mi>i</mi> </msub> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <mi>b</mi> <mo>=</mo> <mo>(</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>y</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> <mo>/</mo> <mi>N</mi> <mo>-</mo> <mi>k</mi> <mo>(</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> <mo>/</mo> <mi>N</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
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，直至所有合格数据区域都循环一遍。
8.根据权利要求2所述的未知环境下排爆机器人多阶自主导航探测方法，其特征在于，在步骤S1.9中，具体步骤为：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分子的存在区域。
9.根据权利要求2所述的未知环境下排爆机器人多阶自主导航探测方法，其特征在于，在步骤S1.10中，具体步骤为：S1.10.1，构建以机器人当前位置Bj点为圆心，a为半径的圆Mj，圆Mj为机器人取样路径，以π为间隔距离在圆Mj上进行采样，得到2n个采样点的坐标，坐标集合A为：A＝{Ai(xi,yi,z)},i＝1,2,3,...,2n，以及2n个采样点的TNT分子浓度，TNT分子浓度集合C为：C＝{Ci},i＝1,2,3,...2n；
S1.10.2，对2n个采样点进行仿真，判断是否满足高斯分布；若满足高斯图像则停止采样，进行建模；若不满足高斯分布，则从TNT分子浓度集合C中选择最大值，以TNT分子浓度最大值对应的采样点Ai(xi,yi,z)和当前位置Bj点所在直线为前进方向，机器人沿着前进方向前进大于当前半径的距离到达另一点Bj+1；
S1.10.3，以另一点Bj+1为机器人的当前位置，并令a＝a+g,g为常数；重复步骤S1.10.1-S1.10.3，直至采样点满足高斯分布，此时机器人当前位置为Bm；
S1.10.4，对满足高斯分布的采样点进行整理并建立TNT分子的扩散高斯模型；
<mrow> <mi>C</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>,</mo> <msub> <mi>y</mi> <mi>i</mi> </msub> <mo>,</mo> <mi>z</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mi>Q</mi> <mrow> <mn>2</mn> <msub> <mi>&amp;pi;&amp;mu;&amp;sigma;</mi> <mi>y</mi> </msub> <msub> <mi>&amp;sigma;</mi> <mi>z</mi> </msub> </mrow> </mfrac> <mi>exp</mi> <mo>&amp;lsqb;</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>y</mi> <mi>i</mi> </msub> <mn>2</mn> </msup> <msup> <mi>z</mi> <mn>2</mn> </msup> </mrow> <mrow> <msup> <msub> <mi>&amp;sigma;</mi> <mi>y</mi> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>&amp;sigma;</mi> <mi>z</mi> </msub> <mn>2</mn> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，C(xi,yi,z)为采样点A(xi,yi,z)处的TNT分子浓度，Q为未爆物泄露TNT分子速率，μ表示平均风速，σy为TNT分子水平方向的扩散参数，σz为TNT分子竖直方向的扩散参数；
S1.10.5，计算TNT分子浓度递增梯度，并采用预测浓度的方法逆推未爆物存在区域；
<mrow> <mfrac> <msub> <mi>C</mi> <mi>i</mi> </msub> <msub> <mi>C</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mfrac> <mo>=</mo> <mi>exp</mi> <mo>&amp;lsqb;</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <mi>S</mi> <mn>2</mn> </msup> <msup> <mi>z</mi> <mn>2</mn> </msup> </mrow> <mrow> <msup> <msub> <mi>&amp;sigma;</mi> <mi>y</mi> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>&amp;sigma;</mi> <mi>z</mi> </msub> <mn>2</mn> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>=</mo> <msubsup> <mi>S</mi> <mi>i</mi> <mn>2</mn> </msubsup> <mo>,</mo> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mn>1</mn> <mo>,</mo> <mn>2..</mn> <mo>,</mo> <mn>2</mn> <mi>n</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
式中表示采样点Ai和采样点Ai+1两点间的浓度比值，S表示采样点Ai和采样点Ai+1两点间的距离；假设未爆物在距离机器人S处，则未爆物在以机器人所在位置Bm为圆心，S为半径的圆D上；
S1.10.6，构建以机器人当前位置Bm为圆心，S为半径的圆D；
S1.10.7，从Bm点出发，在圆D上搜索满足TNT分子浓度递增梯度的采样点Ai，若不存在，则以圆D上浓度最大的点为Ai点，机器人以Bm，Ai点所在直线为前进方向并前进S米达到Bm+1点；
S1.10.8，更新机器人当前位置，并重复步骤S1.10.6-S1.10.7，直至有多个圆D相交于一点，交点视为未爆物疑似存在区域。
CN201710881953.7A 2017-09-26 2017-09-26 一种未知环境下排爆机器人多阶自主导航探测系统及方法 Active CN107526360B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710881953.7A CN107526360B (zh) 2017-09-26 2017-09-26 一种未知环境下排爆机器人多阶自主导航探测系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710881953.7A CN107526360B (zh) 2017-09-26 2017-09-26 一种未知环境下排爆机器人多阶自主导航探测系统及方法

Publications (2)

Publication Number Publication Date
CN107526360A true CN107526360A (zh) 2017-12-29
CN107526360B CN107526360B (zh) 2020-08-21

Family

ID=60736236

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710881953.7A Active CN107526360B (zh) 2017-09-26 2017-09-26 一种未知环境下排爆机器人多阶自主导航探测系统及方法

Country Status (1)

Country Link
CN (1) CN107526360B (zh)

Cited By (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108628318A (zh) * 2018-06-28 2018-10-09 广州视源电子科技股份有限公司 拥堵环境检测方法、装置、机器人及存储介质
CN108638065A (zh) * 2018-05-15 2018-10-12 河南科技学院 一种排爆机器人双臂协同控制系统
CN109079738A (zh) * 2018-08-24 2018-12-25 北京秘塔网络科技有限公司 一种自适应agv机器人及自适应导航方法
CN109282822A (zh) * 2018-08-31 2019-01-29 北京航空航天大学 构建导航地图的存储介质、方法和设备
CN109461179A (zh) * 2018-10-17 2019-03-12 河南科技学院 一种排爆子母机器人协作探测系统
CN109491383A (zh) * 2018-11-06 2019-03-19 上海应用技术大学 多机器人定位与建图系统及方法
CN109634286A (zh) * 2019-01-21 2019-04-16 深圳市傲基电子商务股份有限公司 割草机器人视觉避障方法、割草机器人和可读存储介质
CN110032187A (zh) * 2019-04-09 2019-07-19 清华大学 无人摩托静态障碍避障路径规划计算方法
CN110554687A (zh) * 2018-05-30 2019-12-10 中国北方车辆研究所 一种面向未知环境的多机器人自适应探测方法
CN110625308A (zh) * 2019-09-27 2019-12-31 哈尔滨理工大学 一种基于焊接机器人的橡胶桥梁支座焊接方法
CN111290388A (zh) * 2020-02-25 2020-06-16 苏州科瓴精密机械科技有限公司 路径追踪方法、系统，机器人及可读存储介质
CN111347426A (zh) * 2020-03-26 2020-06-30 季华实验室 一种基于3d视觉的机械臂精确放置轨迹规划方法
CN112123343A (zh) * 2020-11-25 2020-12-25 炬星科技（深圳）有限公司 点云匹配方法、设备及存储介质
CN112987763A (zh) * 2021-05-11 2021-06-18 南京理工大学紫金学院 一种基于ros的自主导航机器人控制系统的智能小车
CN113075933A (zh) * 2021-03-30 2021-07-06 北京布科思科技有限公司 机器人通行控制方法和装置及设备
CN113791610A (zh) * 2021-07-30 2021-12-14 河南科技大学 一种移动机器人全局路径规划方法
CN114415652A (zh) * 2021-11-09 2022-04-29 南京南自信息技术有限公司 一种轮式机器人路径规划方法
CN114859942A (zh) * 2022-07-06 2022-08-05 北京云迹科技股份有限公司 一种机器人运动控制方法、装置、电子设备及存储介质
TWI808210B (zh) * 2018-08-23 2023-07-11 日商日本精工股份有限公司 自走裝置、自走裝置的行駛控制方法以及行駛控制程式

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120121161A1 (en) * 2010-09-24 2012-05-17 Evolution Robotics, Inc. Systems and methods for vslam optimization
US20120185115A1 (en) * 2007-10-05 2012-07-19 Jason Dean Laserbot: programmable robotic apparatus with laser
CN203557388U (zh) * 2013-10-29 2014-04-23 中国人民解放军总装备部军械技术研究所 一种排爆机器人的目标位姿获取机构及目标抓取系统
CN103914068A (zh) * 2013-01-07 2014-07-09 中国人民解放军第二炮兵工程大学 一种基于栅格地图的服务机器人自主导航方法
CN103941750A (zh) * 2014-04-30 2014-07-23 东北大学 基于小型四旋翼无人机的构图装置及方法
CN104690733A (zh) * 2015-02-17 2015-06-10 公安部上海消防研究所 一种防爆型消防侦检机器人
CN104848991A (zh) * 2015-06-05 2015-08-19 天津理工大学 基于视觉的主动式泄漏气体检测方法
CN105823478A (zh) * 2016-03-14 2016-08-03 武汉卓拔科技有限公司 一种自主避障导航信息共享和使用方法
CN106052674A (zh) * 2016-05-20 2016-10-26 青岛克路德机器人有限公司 一种室内机器人的slam方法和系统

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120185115A1 (en) * 2007-10-05 2012-07-19 Jason Dean Laserbot: programmable robotic apparatus with laser
US20120121161A1 (en) * 2010-09-24 2012-05-17 Evolution Robotics, Inc. Systems and methods for vslam optimization
CN103914068A (zh) * 2013-01-07 2014-07-09 中国人民解放军第二炮兵工程大学 一种基于栅格地图的服务机器人自主导航方法
CN203557388U (zh) * 2013-10-29 2014-04-23 中国人民解放军总装备部军械技术研究所 一种排爆机器人的目标位姿获取机构及目标抓取系统
CN103941750A (zh) * 2014-04-30 2014-07-23 东北大学 基于小型四旋翼无人机的构图装置及方法
CN104690733A (zh) * 2015-02-17 2015-06-10 公安部上海消防研究所 一种防爆型消防侦检机器人
CN104848991A (zh) * 2015-06-05 2015-08-19 天津理工大学 基于视觉的主动式泄漏气体检测方法
CN105823478A (zh) * 2016-03-14 2016-08-03 武汉卓拔科技有限公司 一种自主避障导航信息共享和使用方法
CN106052674A (zh) * 2016-05-20 2016-10-26 青岛克路德机器人有限公司 一种室内机器人的slam方法和系统

Cited By (33)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108638065B (zh) * 2018-05-15 2021-04-16 河南科技学院 一种排爆机器人双臂协同控制系统
CN108638065A (zh) * 2018-05-15 2018-10-12 河南科技学院 一种排爆机器人双臂协同控制系统
CN110554687A (zh) * 2018-05-30 2019-12-10 中国北方车辆研究所 一种面向未知环境的多机器人自适应探测方法
CN110554687B (zh) * 2018-05-30 2023-08-22 中国北方车辆研究所 一种面向未知环境的多机器人自适应探测方法
CN108628318A (zh) * 2018-06-28 2018-10-09 广州视源电子科技股份有限公司 拥堵环境检测方法、装置、机器人及存储介质
TWI808210B (zh) * 2018-08-23 2023-07-11 日商日本精工股份有限公司 自走裝置、自走裝置的行駛控制方法以及行駛控制程式
CN109079738A (zh) * 2018-08-24 2018-12-25 北京秘塔网络科技有限公司 一种自适应agv机器人及自适应导航方法
CN109079738B (zh) * 2018-08-24 2022-05-06 北京密塔网络科技有限公司 一种自适应agv机器人及自适应导航方法
CN109282822B (zh) * 2018-08-31 2020-05-05 北京航空航天大学 构建导航地图的存储介质、方法和设备
CN109282822A (zh) * 2018-08-31 2019-01-29 北京航空航天大学 构建导航地图的存储介质、方法和设备
CN109461179A (zh) * 2018-10-17 2019-03-12 河南科技学院 一种排爆子母机器人协作探测系统
CN109461179B (zh) * 2018-10-17 2021-07-09 河南科技学院 一种排爆子母机器人协作探测系统
CN109491383A (zh) * 2018-11-06 2019-03-19 上海应用技术大学 多机器人定位与建图系统及方法
CN109634286A (zh) * 2019-01-21 2019-04-16 深圳市傲基电子商务股份有限公司 割草机器人视觉避障方法、割草机器人和可读存储介质
CN110032187A (zh) * 2019-04-09 2019-07-19 清华大学 无人摩托静态障碍避障路径规划计算方法
CN110032187B (zh) * 2019-04-09 2020-08-28 清华大学 无人摩托静态障碍避障路径规划计算方法
CN110625308A (zh) * 2019-09-27 2019-12-31 哈尔滨理工大学 一种基于焊接机器人的橡胶桥梁支座焊接方法
CN111290388B (zh) * 2020-02-25 2022-05-13 苏州科瓴精密机械科技有限公司 路径追踪方法、系统，机器人及可读存储介质
CN111290388A (zh) * 2020-02-25 2020-06-16 苏州科瓴精密机械科技有限公司 路径追踪方法、系统，机器人及可读存储介质
CN111347426A (zh) * 2020-03-26 2020-06-30 季华实验室 一种基于3d视觉的机械臂精确放置轨迹规划方法
CN111347426B (zh) * 2020-03-26 2021-06-04 季华实验室 一种基于3d视觉的机械臂精确放置轨迹规划方法
CN112123343B (zh) * 2020-11-25 2021-02-05 炬星科技（深圳）有限公司 点云匹配方法、设备及存储介质
CN112123343A (zh) * 2020-11-25 2020-12-25 炬星科技（深圳）有限公司 点云匹配方法、设备及存储介质
CN113075933B (zh) * 2021-03-30 2023-08-29 北京布科思科技有限公司 机器人通行控制方法和装置及设备
CN113075933A (zh) * 2021-03-30 2021-07-06 北京布科思科技有限公司 机器人通行控制方法和装置及设备
CN112987763B (zh) * 2021-05-11 2021-09-17 南京理工大学紫金学院 一种基于ros的自主导航机器人控制系统的智能小车
CN112987763A (zh) * 2021-05-11 2021-06-18 南京理工大学紫金学院 一种基于ros的自主导航机器人控制系统的智能小车
CN113791610A (zh) * 2021-07-30 2021-12-14 河南科技大学 一种移动机器人全局路径规划方法
CN113791610B (zh) * 2021-07-30 2024-04-26 河南科技大学 一种移动机器人全局路径规划方法
CN114415652A (zh) * 2021-11-09 2022-04-29 南京南自信息技术有限公司 一种轮式机器人路径规划方法
CN114415652B (zh) * 2021-11-09 2024-03-26 南京南自信息技术有限公司 一种轮式机器人路径规划方法
CN114859942A (zh) * 2022-07-06 2022-08-05 北京云迹科技股份有限公司 一种机器人运动控制方法、装置、电子设备及存储介质
CN114859942B (zh) * 2022-07-06 2022-10-04 北京云迹科技股份有限公司 一种机器人运动控制方法、装置、电子设备及存储介质

Also Published As

Publication number Publication date
CN107526360B (zh) 2020-08-21

Similar Documents

Publication Publication Date Title
CN107526360B (zh) 一种未知环境下排爆机器人多阶自主导航探测系统及方法
CN111337941B (zh) 一种基于稀疏激光雷达数据的动态障碍物追踪方法
CN108875911B (zh) 一种泊车位检测方法
Chen et al. Gaussian-process-based real-time ground segmentation for autonomous land vehicles
CN112184736B (zh) 一种基于欧式聚类的多平面提取方法
Prieto et al. As-is building-structure reconstruction from a probabilistic next best scan approach
Liu et al. Point cloud segmentation based on Euclidean clustering and multi-plane extraction in rugged field
CN114998276B (zh) 一种基于三维点云的机器人动态障碍物实时检测方法
Nielsen et al. Survey on 2d lidar feature extraction for underground mine usage
Wang et al. 3D-LIDAR based branch estimation and intersection location for autonomous vehicles
Fukuda et al. Navigation system based on ceiling landmark recognition for autonomous mobile robot-landmark detection based on fuzzy template matching (ftm)
CN112945196A (zh) 一种基于点云数据的露天矿台阶线提取和边坡监测的方法
Song et al. Critical rays self-adaptive particle filtering SLAM
Gao et al. A novel local path planning method considering both robot posture and path smoothness
Yang et al. Enhanced visual SLAM for construction robots by efficient integration of dynamic object segmentation and scene semantics
Hu et al. A modified particle filter for simultaneous robot localization and landmark tracking in an indoor environment
Kwon et al. Elevation moment of inertia: A new feature for Monte Carlo localization in outdoor environment with elevation map
Kaleci et al. Plane segmentation of point cloud data using split and merge based method
Gao et al. A new method for repeated localization and matching of tunnel lining defects
Wang et al. Obstacle recognition method for ship based on 3D lidar
Kiddee et al. A geometry based feature detection method of V-groove weld seams for thick plate welding robots
Velasco-Sánchez et al. Obstacle Detection with Differences of Normals in Unorganized Point Clouds for Mobile Robotics
Wang et al. Mobile robot SLAM methods improved for adapting to search and rescue environments
Jun Markerless Point Cloud Matching Algorithm Based on 3D Feature Extraction
Díaz-Vilariño et al. Autonomous Point Cloud Acquisition of Unknown Indoor Scenes.

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant