CN113485375B - 一种基于启发式偏置采样的室内环境机器人探索方法 - Google Patents
一种基于启发式偏置采样的室内环境机器人探索方法 Download PDFInfo
- Publication number
- CN113485375B CN113485375B CN202110929934.3A CN202110929934A CN113485375B CN 113485375 B CN113485375 B CN 113485375B CN 202110929934 A CN202110929934 A CN 202110929934A CN 113485375 B CN113485375 B CN 113485375B
- Authority
- CN
- China
- Prior art keywords
- point
- boundary points
- heuristic
- robot
- points
- 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.)
- Active
Links
- 238000005070 sampling Methods 0.000 title claims abstract description 50
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 37
- 238000001514 detection method Methods 0.000 claims description 17
- 238000012360 testing method Methods 0.000 claims description 7
- 238000000605 extraction Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 5
- 230000006872 improvement Effects 0.000 description 9
- 230000006870 function Effects 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 238000012959 renal replacement therapy Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 241000282414 Homo sapiens Species 0.000 description 2
- 230000009471 action Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 208000037170 Delayed Emergence from Anesthesia Diseases 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 229910052739 hydrogen Inorganic materials 0.000 description 1
- 239000001257 hydrogen Substances 0.000 description 1
- 125000004435 hydrogen atom Chemical class [H]* 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000005065 mining Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011176 pooling Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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
- G05D1/0251—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 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于启发式偏置采样的室内环境机器人探索方法,其包括:A、初始化;B、进行定位和建图;C、利用所述两棵快速生长随机树进行边界点的提取,记为RRT边界点;在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点,记为房间边界点;剔除RRT边界点和房间边界点中的无效边界点;D、机器人优先选择收益值最大的房间边界点作为目标点进行探索,并在所有房间边界点探索完成后优先选择收益值最大的RRT边界点作为目标点进行探索;E、引导机器人向目标点导航并更新地图;F、当先验区域内检测不到边界点时,销毁先验区域;G、循环步骤A‑F,直至完成整个环境探索。本发明可有效减少探索过程中的回溯现象,提升探索效率。
Description
技术领域
本发明涉及机器人技术领域,特别涉及一种基于启发式偏置采样的室内环境机器人探索方法。
背景技术
随着计算机软硬件技术的发展以及人工智能技术的突破,服务型移动机器人的研究取得了瞩目的成绩,智能公共服务机器人应用场景和服务模式不断拓展,带动我国服务机器人市场规模高速增长。移动型机器人自主探索是指机器人在没有任何先验知识的情况下,在一个新的环境中通过移动而建立完整环境地图的过程。
目前,主流的自主探索算法是基于边界点的探索方法,边界将探测空间分为已知区域和未知区域,引导机器人向未知区域探索可以逐步完成对整个环境的建模。在火灾救援、矿井探测等时间要求苛刻的场合,快速完成对整个环境的建模,可以有效帮助施救、开采等任务的进行。
为了更快速的完成环境建模任务,广大学者的研究重心聚焦于如何探测和选择边界点,边界点的选择直接影响探索效率。然而这类探索策略仅仅考量的是当前时刻的探索路径成本和更新该边界点周围小部分地图带来的的信息收益,而忽略环境结构的潜在收益。比如,在室内环境下,机器人在进入一个房间探索后,一般会有两种动作:1、将房间区域探索完成再转向房间外探索;2、房间区域还未探索完成就转向了房间外,之后再次进入房间探索。显然,第二种动作更浪费时间,效率更低。
基于快速搜索随机树(RRT)的自主探索策略的核心思想是一种贪婪策略,且边界点的信息增益模型也不设计对周围环境的考量,那么机器人在每一时刻会无视周围已探索的环境结构而选取收益值最大的边界点作为目标点。比如,机器人在一个房间区域探索时,如果房间外的边界点的收益值较大,机器人就转向房间外探索,之后绕回把房间区域探索完毕,这便产生了回溯现象;其次,由于RRT算法本身的随机性,在对边界点的提取时,往往不能及时的将机器人前进区域的边界点提取出去,那么机器人也会出现选择另一个区域探索,又回头探索的现象。导致了机器人探索慢、效率低的问题。
发明内容
本发明要解决的技术问题是提供一种探索快、探索效率高的基于启发式偏置采样的室内环境机器人探索方法。
为了解决上述问题,本发明提供了基于启发式偏置采样的室内环境机器人探索方法,其包括:
A、初始化;
B、进行定位和建图;
C、在地图的空闲区域生长两棵快速搜索随机树,利用所述两棵快速生长随机树进行边界点的提取,记为RRT边界点;进行启发式物体的识别并获取启发式物体的位置信息,以启发式物体的位置构造先验区域,在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点,记为房间边界点;剔除RRT边界点和房间边界点中的无效边界点;
D、当房间边界点存在时,机器人优先选择收益值最大的房间边界点作为目标点进行探索,并在所有房间边界点探索完成后对RRT边界点进行探索,且优先选择收益值最大的RRT边界点作为目标点进行探索;
E、引导机器人向目标点导航并更新地图;
F、当先验区域内检测不到边界点时,销毁先验区域;
G、循环步骤A-F,直至完成整个环境探索。
作为本发明的进一步改进,步骤B具体包括:通过SLAM模块接收传感器数据,更新部分地图为已知的区域,同时通过SLAM模块构建的地图反过来修正机器人的位姿。
作为本发明的进一步改进,所述两棵快速搜索随机树分为全局树和局部树,全局树和局部树均通过以下步骤进行RRT边界点的提取;具体包括:
C11、将起点添加到树结构中作为根节点,两棵快速搜索随机树的起点都是人为在地图的空闲区域内设定的;
C12、在地图区域内随机撒点作为候选点;
C13、若该候选点在已知区域内,则遍历树结构上的所有已有的节点,选取距离候选点最近的节点作为最邻近点,以最邻近点到候选节点的连线作为生长方向;如果最邻近点与候选节点的距离超过预先设定的步长,则由最邻近点沿着生长方向生长一个步长,到达的点作为生长点;如果距离不超过步长,则该候选点作为生长点;
若候选点在未知区域内,则先找到该候选点的最邻近节点,最邻近点到候选点的连线作为生长方向,然后由最邻近点沿着生长方向向前生长,到达边界的地方作为边界点;
C14、将生长点和候选节点的连线在地图上做碰撞检测,通过遍历这条直线上所有的栅格点,然后判断该栅格点的栅格状态,如果该栅格点的状态是被障碍物占据的,则碰撞检测不通过,返回步骤C12重新进行采样;如果连线没有碰到障碍物,就将该候选点、生长点和候选节点的连线添加到树结构中;
其中,当局部树探测到边界点后,局部树会被清除并在机器人当前位置重新生长。
作为本发明的进一步改进,在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点,记为房间边界点;具体包括:
C21、将启发式物体的位置添加到树结构中作为根节点;
C22、偏置采样快速搜索随机树算法每次使用一个随机函数来决定采用哪一种采样方式;所述采样方式包括均匀随机采样和偏置采样;
C23、遍历树结构上的所有已有的节点,选取距离候选点最近的节点作为最邻近点,最邻近点到候选节点的连线作为生长方向;如果最邻近点与候选节点的距离超过预先设定的步长,则由最邻近点沿着生长方向生长一个步长,到达的点作为生长点;如果距离不超过步长,则该候选点作为生长点;
C24、将生长点和候选节点的连线在地图上做碰撞检测,碰撞测试通过则进入步骤C25,否则返回步骤C22;
C25、判断通过碰撞测试的生长点是否为边界点,并改变部分边界点的栅格状态;
C26、根据当前时刻的边界点数目与总边界点数目之比计算覆盖率。
作为本发明的进一步改进,步骤C24具体包括:遍历生长点和候选节点的连线所在直线上的所有的栅格点,然后判断该栅格点的栅格状态,如果该栅格点的状态是被占据的或者是未知的,则碰撞检测不通过,返回步骤C22重新进行采样;如果连线没有碰到障碍物和未知区域,则将该候选点、生长点和候选节点的连线添加到树结构中。
作为本发明的进一步改进,所述剔除RRT边界点和房间边界点中的无效边界点,具体包括:
C31、将所述RRT边界点和房间边界点通过mean-shift聚类算法聚类后得到质心点;
C32、实时检测该边界点的栅格状态以及在costmap中的值,若该栅格状态是空闲的且costmap中值超过某一阈值,则该栅格点也作为无效点剔除。
作为本发明的进一步改进,所述以启发式物体的位置构造先验区域,具体包括:
当机器人识别到启发式物体时,若机器人位置在启发式物体的下方,则所述先验区域在启发式物体的位置的上方;当机器人位置在启发式物体的上方时,则所述先验区域在启发式物体的位置的下方。
作为本发明的进一步改进,所述先验区域的长度分别为启发式物体的位置向两侧延伸长度a,先验区域的宽度为启发式物体的位置向后延伸长度2b;其中,2≤a≤10,2≤b≤10。
作为本发明的进一步改进,步骤E具体包括:使用A*全局路径规划算法在已知的环境中规划出一条由机器人当前位置到目标点的路径,并结合DWA局部路径规划算法使机器人很好的利用局部环境信息完成避障,将两者相结合引导机器人向目标点导航并更新地图。
作为本发明的进一步改进,在步骤A中,整个环境都是未知的,机器人处于环境中的某一位置,机器人通过自身携带的传感器获取周围环境信息的数据。
本发明的有益效果:
本发明基于启发式偏置采样的室内环境机器人探索方法通过采样启发式先验信息探索,当机器人识别到启发式物体后,优先探索先验区域内的环境,可以使机器人优先探索完一个房间区域再转向其它区域,有效减少探索过程中的回溯现象,提升探索效率。
上述说明仅是本发明技术方案的概述,为了能够更清楚了解本发明的技术手段,而可依照说明书的内容予以实施,并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂,以下特举较佳实施例,并配合附图,详细说明如下。
附图说明
图1是本发明优选实施例中基于启发式偏置采样的室内环境机器人探索方法的流程图;
图2是本发明优选实施例中构造的先验区域的示意图;
图3是本发明优选实施例中实验场景一的示意图;
图4是本发明优选实施例中实验场景二的示意图;
图5是本发明优选实施例中实验场景三的示意图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明,以使本领域的技术人员可以更好地理解本发明并能予以实施,但所举实施例不作为对本发明的限定。
如图1所示,为本发明优选实施例中的基于启发式偏置采样的室内环境机器人探索方法,包括以下步骤:
A、初始化;整个环境都是未知的,机器人处于环境中的某一位置,机器人通过自身携带的传感器获取周围环境信息的数据。
B、进行定位和建图;具体包括:通过SLAM模块接收传感器数据,更新部分地图为已知的区域,同时通过SLAM模块构建的地图反过来修正机器人的位姿。
C、在地图的空闲区域生长两棵快速搜索随机树,利用所述两棵快速生长随机树进行边界点的提取,记为RRT边界点;进行启发式物体的识别并获取启发式物体的位置信息,以启发式物体的位置构造先验区域,在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点,记为房间边界点;剔除RRT边界点和房间边界点中的无效边界点。
其中,所述两棵快速搜索随机树分为全局树和局部树,全局树和局部树均通过以下步骤进行RRT边界点的提取;具体包括:
C11、将起点添加到树结构中作为根节点,两棵快速搜索随机树的起点都是人为在地图的空闲区域内设定的;
C12、在地图区域内随机撒点作为候选点;
C13、若该候选点在已知区域内,则遍历树结构上的所有已有的节点,选取距离候选点最近的节点作为最邻近点,以最邻近点到候选节点的连线作为生长方向;如果最邻近点与候选节点的距离超过预先设定的步长,则由最邻近点沿着生长方向生长一个步长,到达的点作为生长点;如果距离不超过步长,则该候选点作为生长点;
若候选点在未知区域内,则先找到该候选点的最邻近节点,最邻近点到候选点的连线作为生长方向,然后由最邻近点沿着生长方向向前生长,到达边界的地方作为边界点,这一过程在算法方面的体现是从最邻近点沿生长方向开始遍历,栅格状态为未知的作为边界点;
C14、将生长点和候选节点的连线在地图上做碰撞检测,通过遍历这条直线上所有的栅格点,然后判断该栅格点的栅格状态,如果该栅格点的状态是被障碍物占据的,则碰撞检测不通过,返回步骤C12重新进行采样;如果连线没有碰到障碍物,就将该候选点、生长点和候选节点的连线添加到树结构中;
其中,当局部树探测到边界点后,局部树会被清除并在机器人当前位置重新生长。
可选的,进行启发式物体的识别并获取启发式物体的位置信息,具体包括:
S1、使用一种基于深度学习的目标检测方法进行启发式物体的识别,它是在YOLOv4_tiny的基础上改进的轻量级网络。这个轻量级网络主要由卷积层、逆残差块、池化层和SPP(Spatial Pyramid Pooling)层组成,共42层,输出简化为两层。与YOLOv4_tiny相比,这一轻量级网络的检测准确率提高了19.2%,接近YOLOv4,但速度几乎是YOLOv4的4倍。总的来说,这一轻量级网络在速度和精度方面都是非常有效的,它适合于启发式目标识别。
S2、当使用以上网络检测到启发式物体后,就可以获取到启发式物体在像素平面下的坐标信息。接下来实现从图像坐标系到相机坐标系再到世界坐标系的转换,假设输出的启发式物体的中心点P在图像坐标系下的坐标为(u,v),其在相机坐标系下的坐标为(xc,yc,zc),在世界坐标系下的坐标为(x,y,z),它们之间存在如下映射关系:
为了方便实际转换计算,本发明用齐次坐标表示相机参数,所以二维点到三维点的转换可表示为:
其中,相机内参(fx,fy,cx,cy)可通过订阅深度相机在ROS下发布的话题得到。
在本实施例中,以启发式物体的位置为基准构造先验区域,此区域是遵从人类的感知习惯,门后所在的区域是一个房间,区域的大小是人为经验设定的,且保证在大多数场合略大于实际区域大小。具体包括:当机器人识别到启发式物体时,若机器人位置在启发式物体的下方,则预估的先验区域在启发式物体的位置的上方;同理,当机器人位置在启发式物体的上方时,预估的先验区域在启发式物体的位置的下方。可选的,预估的先验区域的长度分别为启发式物体的位置向两侧延伸长度a,预估区域的宽度为启发式物体的位置向后延伸长度2b。本实施例中,其中,2≤a≤10,2≤b≤10;可选的,a和b均设置为6。参照图2。
其中,在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点,记为房间边界点;具体包括:
C21、将启发式物体的位置添加到树结构中作为根节点;
C22、偏置采样快速搜索随机树算法(bias_RRT)每次使用一个随机函数来决定采用哪一种采样方式;所述采样方式包括均匀随机采样和偏置采样;其中,所述均匀随机采样即在给定的地图区域每一个位置采样的概率相同;所述偏置采样只选择地图中的边界位置采样。地图的边界位置包含空闲区域与障碍物区域的交界,空闲区域和未知区域的交界,可以直接使用OpenCV(计算机视觉中经典的专用库)中图像轮廓的提取方法提取出这些边界,并计算出此时的边界点数目,记为总边界点数目。本发明bias_RRT算法每次都使用一个随机函数来决定采用哪一种采样方式;
C23、遍历树结构上的所有已有的节点,选取距离候选点最近的节点作为最邻近点,最邻近点到候选节点的连线作为生长方向;如果最邻近点与候选节点的距离超过预先设定的步长,则由最邻近点沿着生长方向生长一个步长,到达的点作为生长点;如果距离不超过步长,则该候选点作为生长点;
C24、将生长点和候选节点的连线在地图上做碰撞检测,碰撞测试通过则进入步骤C25,否则返回步骤C22;其中,碰撞测试的原理是遍历这条直线上所有的栅格点,然后判断该栅格点的栅格状态,如果该栅格点的状态是被占据的或者是未知的,则碰撞检测不通过,返回步骤C22重新进行采样;如果连线没有碰到障碍物和未知区域,就将该候选点、生长点和候选节点的连线添加到树结构中;(对比RRT算法提取边界点的过程,本发明bias_RRT算法在碰撞检测时将被占据的和未知的栅格状态都看成障碍物,这样可以快速进行树结构的建立);
C25、判断通过碰撞测试的生长点是否为边界点,并改变部分边界点的栅格状态;
具体的,判定方法为检查在以生长点为中心的正方形区域内是否有栅格状态为被占据的栅格点和未知的栅格点,若正方形区域内有被占据的栅格点,则说明树生长到了障碍物附近,同时改变这些正方形区域内栅格点的状态为已知的,表明这一块区域已被树结构覆盖,那么步骤C22中的总边界点数目经过此处理将会减少;若正方形区域内有未知的栅格点,则说明树生长到了未知区域附近,同时改变正方形区域内栅格点的状态为已知的,表明这一块区域已被树结构覆盖,并将该点作为房间边界点发布出去,这些房间边界点将作为目标点引导机器人向着房间区域探索。同样的,因为改变了正方形区域内栅格点的状态,步骤C22中的总边界点数目会再次减少;
C26、根据当前时刻的边界点数目与总边界点数目之比计算覆盖率。
具体的,覆盖率计算公式为:
其中,Nnow为当前时刻的边界点数目,Ntotal为总边界点数目。每次覆盖率计算都会和预先设定的阈值进行比较,如果小于预先设定的阈值,则表示树结构已经覆盖完整个先验区域,这一标志会作为先验区域是否探索完成的重要指标,本实施例中阈值设置为0.1。由于RRT算法本身的随机性,在某些极端情况,不能及时提取所有的边界点,所以将覆盖率和先验区域没有边界点两者共同作为先验区域探索完成的标志,可以有效使机器人优先探索完先验区域,避免回溯现象。
其中,所述剔除RRT边界点和房间边界点中的无效边界点,具体包括:
C31、将所述RRT边界点和房间边界点通过mean-shift聚类算法聚类后得到质心点;
C32、实时检测该边界点的栅格状态以及在costmap中的值,若该栅格状态是空闲的且costmap中值超过某一阈值,则该栅格点也作为无效点剔除。
具体的,将探测到的边界点通过mean-shift聚类算法聚类后得到质心点,这样可以过滤掉一部分边界点,减少计算消耗。同时在每一时刻,都检测该边界点的栅格状态以及在costmap(代价地图)中的值(costmap将每一栅格值划分在0~255之间,白色值为255,代表空闲状态;黑色值为0,代表障碍物;之间的值是灰色,代表未知的),若该栅格状态是空闲的(表明该栅格点已被探索)且costmap中值超过某一阈值,则表明该栅格点所在区域已被探索,该栅格点也应该作为无效点剔除。
D、当房间边界点存在时,机器人优先选择收益值最大的房间边界点作为目标点进行探索,并在所有房间边界点探索完成后对RRT边界点进行探索,且优先选择收益值最大的RRT边界点作为目标点进行探索。
具体的,机器人优先选择房间边界点探索,使机器人在识别到启发式物体后,优先进入先验区域探索。当房间边界点存在时,表示先验区域还未探索完成,然后将房间边界点探索完后才会选择RRT边界点进行探索。这样机器人便能按照人为的设想,将一个区域探索完毕再转向别的区域探索;
当有多个边界点存在时,我们为这些边界点设计了收益函数,房间边界点收益函数的计算公式同RRT边界点相同,但是这两类边界点分开比较,即当房间边界点存在时,选择收益值最大的房间边界点作为目标点;当没有房间边界点存在时,则选取最大的RRT边界点作为目标点;边界点的收益函数如下:
R1f=w1*If-w2*Nf
其中,If为信息增益:即在质心点的信息增益半径r=1内未知的栅格的个数;Nf为路径成本:即机器人当前位置与质心点位置的欧式距离;本实施例中w1设定为3,w2设定为1。
E、引导机器人向目标点导航并更新地图;具体包括:使用A*全局路径规划算法在已知的环境中规划出一条由机器人当前位置到目标点的路径,并结合DWA局部路径规划算法使机器人很好的利用局部环境信息完成避障,将两者相结合引导机器人向目标点导航并更新地图。
F、当先验区域内检测不到边界点时,销毁先验区域;具体的,当先验区域内检测不到边界点时,表示该区域已经探索完成,则销毁先验区域模型以便下一个启发式区域的形成。
G、循环步骤A-F,直至完成整个环境探索。
为充分证明本发明的有效性,本发明与基于快速搜索随机树的自主探索算法(以下用“RRTs算法”表示)在两个仿真场景下进行了对比实验。对于每一个实验环境,总共进行了20次实验。包括10次使用RRTs算法,10次我们改进后的算法。实验对比的指标是探索整个环境所使用的时间以及行驶的路径长度。
实验数据如表1、表2和表3所示,分别对应实验场景一、二和三。在第一个实验场景中(如图3),本发明的方法相比于RRTs算法,探索时间减少了31.2%,探索路径长度减少了21.8%。在第二个实验场景中(如图4),我们的方法相比于RRTs算法,探索时间减少了13.4%,探索路径长度减少了17.2%。在第三个实验场景中(如图5),我们的方法相比于RRTs算法,探索时间减少了21.9%,探索路径长度减少了22.7%。
表一
表二
表三
本发明基于启发式偏置采样的室内环境机器人探索方法通过采样启发式先验信息探索,当机器人识别到启发式物体后,优先探索先验区域内的环境,可以使机器人优先探索完一个房间区域再转向其它区域,有效减少探索过程中的回溯现象,提升探索效率。
以上实施例仅是为充分说明本发明而所举的较佳的实施例,本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换,均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。
Claims (9)
1.基于启发式偏置采样的室内环境机器人探索方法,其特征在于,包括:
A、初始化;
B、进行定位和建图;
C、在地图的空闲区域生长两棵快速搜索随机树,利用所述两棵快速生长随机树进行边界点的提取,记为RRT边界点;进行启发式物体的识别并获取启发式物体的位置信息,以启发式物体的位置构造先验区域,在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点,记为房间边界点;剔除RRT边界点和房间边界点中的无效边界点;
D、当房间边界点存在时,机器人优先选择收益值最大的房间边界点作为目标点进行探索,并在所有房间边界点探索完成后对RRT边界点进行探索,且优先选择收益值最大的RRT边界点作为目标点进行探索;
E、引导机器人向目标点导航并更新地图;
F、当先验区域内检测不到边界点时,销毁先验区域;
G、循环步骤A-F,直至完成整个环境探索;
在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点,记为房间边界点;具体包括:
C21、将启发式物体的位置添加到树结构中作为根节点;
C22、偏置采样快速搜索随机树算法每次使用一个随机函数来决定采用哪一种采样方式;所述采样方式包括均匀随机采样和偏置采样;
C23、遍历树结构上的所有已有的节点,选取距离候选点最近的节点作为最邻近点,最邻近点到候选节点的连线作为生长方向;如果最邻近点与候选节点的距离超过预先设定的步长,则由最邻近点沿着生长方向生长一个步长,到达的点作为生长点;如果距离不超过步长,则该候选点作为生长点;
C24、将生长点和候选节点的连线在地图上做碰撞检测,碰撞测试通过则进入步骤C25,否则返回步骤C22;
C25、判断通过碰撞测试的生长点是否为边界点,并改变部分边界点的栅格状态;
C26、根据当前时刻的边界点数目与总边界点数目之比计算覆盖率。
2.如权利要求1所述的基于启发式偏置采样的室内环境机器人探索方法,其特征在于,步骤B具体包括:通过SLAM模块接收传感器数据,更新部分地图为已知的区域,同时通过SLAM模块构建的地图反过来修正机器人的位姿。
3.如权利要求1所述的基于启发式偏置采样的室内环境机器人探索方法,其特征在于,所述两棵快速搜索随机树分为全局树和局部树,全局树和局部树均通过以下步骤进行RRT边界点的提取;具体包括:
C11、将起点添加到树结构中作为根节点,两棵快速搜索随机树的起点都是人为在地图的空闲区域内设定的;
C12、在地图区域内随机撒点作为候选点;
C13、若该候选点在已知区域内,则遍历树结构上的所有已有的节点,选取距离候选点最近的节点作为最邻近点,以最邻近点到候选节点的连线作为生长方向;如果最邻近点与候选节点的距离超过预先设定的步长,则由最邻近点沿着生长方向生长一个步长,到达的点作为生长点;如果距离不超过步长,则该候选点作为生长点;
若候选点在未知区域内,则先找到该候选点的最邻近节点,最邻近点到候选点的连线作为生长方向,然后由最邻近点沿着生长方向向前生长,到达边界的地方作为边界点;
C14、将生长点和候选节点的连线在地图上做碰撞检测,通过遍历这条直线上所有的栅格点,然后判断该栅格点的栅格状态,如果该栅格点的状态是被障碍物占据的,则碰撞检测不通过,返回步骤C12重新进行采样;如果连线没有碰到障碍物,就将该候选点、生长点和候选节点的连线添加到树结构中;
其中,当局部树探测到边界点后,局部树会被清除并在机器人当前位置重新生长。
4.如权利要求1所述的基于启发式偏置采样的室内环境机器人探索方法,其特征在于,步骤C24具体包括:遍历生长点和候选节点的连线所在直线上的所有的栅格点,然后判断该栅格点的栅格状态,如果该栅格点的状态是被占据的或者是未知的,则碰撞检测不通过,返回步骤C22重新进行采样;如果连线没有碰到障碍物和未知区域,则将该候选点、生长点和候选节点的连线添加到树结构中。
5.如权利要求1所述的基于启发式偏置采样的室内环境机器人探索方法,其特征在于,所述剔除RRT边界点和房间边界点中的无效边界点,具体包括:
C31、将所述RRT边界点和房间边界点通过mean-shift聚类算法聚类后得到质心点;
C32、实时检测该边界点的栅格状态以及在costmap中的值,若该栅格状态是空闲的且costmap中值超过某一阈值,则该栅格点也作为无效点剔除。
6.如权利要求1所述的基于启发式偏置采样的室内环境机器人探索方法,其特征在于,所述以启发式物体的位置构造先验区域,具体包括:
当机器人识别到启发式物体时,若机器人位置在启发式物体的下方,则所述先验区域在启发式物体的位置的上方;当机器人位置在启发式物体的上方时,则所述先验区域在启发式物体的位置的下方。
7.如权利要求6所述的基于启发式偏置采样的室内环境机器人探索方法,其特征在于,所述先验区域的长度分别为启发式物体的位置向两侧延伸长度a,先验区域的宽度为启发式物体的位置向后延伸长度2b;其中,2≤a≤10,2≤b≤10。
8.如权利要求1所述的基于启发式偏置采样的室内环境机器人探索方法,其特征在于,步骤E具体包括:使用A*全局路径规划算法在已知的环境中规划出一条由机器人当前位置到目标点的路径,并结合DWA局部路径规划算法使机器人很好的利用局部环境信息完成避障,将两者相结合引导机器人向目标点导航并更新地图。
9.如权利要求1所述的基于启发式偏置采样的室内环境机器人探索方法,其特征在于,在步骤A中,整个环境都是未知的,机器人处于环境中的某一位置,机器人通过自身携带的传感器获取周围环境信息的数据。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110929934.3A CN113485375B (zh) | 2021-08-13 | 2021-08-13 | 一种基于启发式偏置采样的室内环境机器人探索方法 |
PCT/CN2022/100861 WO2023016101A1 (zh) | 2021-08-13 | 2022-06-23 | 一种基于启发式偏置采样的室内环境机器人探索方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110929934.3A CN113485375B (zh) | 2021-08-13 | 2021-08-13 | 一种基于启发式偏置采样的室内环境机器人探索方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113485375A CN113485375A (zh) | 2021-10-08 |
CN113485375B true CN113485375B (zh) | 2023-03-24 |
Family
ID=77945221
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110929934.3A Active CN113485375B (zh) | 2021-08-13 | 2021-08-13 | 一种基于启发式偏置采样的室内环境机器人探索方法 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN113485375B (zh) |
WO (1) | WO2023016101A1 (zh) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113485375B (zh) * | 2021-08-13 | 2023-03-24 | 苏州大学 | 一种基于启发式偏置采样的室内环境机器人探索方法 |
CN114153210B (zh) * | 2021-12-01 | 2024-03-19 | 苏州盈科电子有限公司 | 机器人的移动控制方法及*** |
CN114253272A (zh) * | 2021-12-23 | 2022-03-29 | 北京理工大学重庆创新中心 | 一种无人车室内自主探索方法 |
CN114740866A (zh) * | 2022-05-10 | 2022-07-12 | 山东大学 | 基于深度学习的机器人自主探索方法及*** |
CN114812539B (zh) * | 2022-06-21 | 2022-11-18 | 杭州涂鸦信息技术有限公司 | 地图探索、地图使用方法、装置、机器人和存储介质 |
CN115167433B (zh) * | 2022-07-22 | 2024-03-19 | 华南理工大学 | 融合全局视觉的机器人自主探索环境信息的方法及*** |
CN115469662B (zh) * | 2022-09-13 | 2023-07-07 | 苏州大学 | 一种环境探索方法、装置及应用 |
CN116382309A (zh) * | 2023-06-06 | 2023-07-04 | 康复大学(筹) | 一种基于圆柱启发采样的机器人路径规划算法 |
CN117406771B (zh) * | 2023-10-17 | 2024-04-30 | 武汉大学 | 一种基于四旋翼无人机的高效自主探索方法、***及设备 |
CN117405178B (zh) * | 2023-12-15 | 2024-03-15 | 成都电科星拓科技有限公司 | 一种自动化检测室内环境参数的移动监测方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108983780A (zh) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | 一种基于改进rrt*算法的移动机器人路径规划方法 |
CN110188696A (zh) * | 2019-05-31 | 2019-08-30 | 华南理工大学 | 一种水面无人装备多源感知方法及*** |
CN112286202A (zh) * | 2020-11-11 | 2021-01-29 | 福州大学 | 一种非均匀采样fmt*的移动机器人路径规划方法 |
CN112327852A (zh) * | 2020-11-09 | 2021-02-05 | 苏州大学 | 融合路径信息丰富度的移动机器人自主探索方法 |
Family Cites Families (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5998816B2 (ja) * | 2012-10-04 | 2016-09-28 | セイコーエプソン株式会社 | 経路探索方法、経路探索装置、ロボット制御装置、ロボット及びプログラム |
GB201803292D0 (en) * | 2018-02-28 | 2018-04-11 | Five Ai Ltd | Efficient computation of collision probabilities for safe motion planning |
CN108762270B (zh) * | 2018-06-01 | 2021-04-27 | 上海理工大学 | 变概率双向快速搜索随机树改进路径规划算法 |
CN109341707B (zh) * | 2018-12-03 | 2022-04-08 | 南开大学 | 未知环境下移动机器人三维地图构建方法 |
EP3748455B1 (en) * | 2019-06-07 | 2022-03-16 | Tata Consultancy Services Limited | A method and a system for hierarchical network based diverse trajectory proposal |
CN110221614B (zh) * | 2019-06-14 | 2021-06-01 | 福州大学 | 一种基于快速探索随机树的多机器人地图探索方法 |
CN110308740B (zh) * | 2019-06-28 | 2022-02-22 | 天津大学 | 一种面向移动目标追踪的无人机群动态任务分配方法 |
JP2021033685A (ja) * | 2019-08-26 | 2021-03-01 | 株式会社デンソー | 学習プログラム及び学習方法 |
CN112833904B (zh) * | 2021-01-05 | 2024-06-04 | 北京航空航天大学 | 一种基于自由空间与快速搜索随机树算法的无人车动态路径规划方法 |
CN113050632B (zh) * | 2021-03-11 | 2022-06-14 | 珠海一微半导体股份有限公司 | 用于机器人探索未知区域的地图探索方法、芯片及机器人 |
CN113064426B (zh) * | 2021-03-17 | 2022-03-15 | 安徽工程大学 | 一种改进双向快速搜索随机树算法的智能车路径规划方法 |
CN113110482B (zh) * | 2021-04-29 | 2022-07-19 | 苏州大学 | 基于先验信息启发式的室内环境机器人探索方法及*** |
CN113110522B (zh) * | 2021-05-27 | 2022-07-08 | 福州大学 | 一种基于复合式边界检测的机器人自主探索方法 |
CN113485375B (zh) * | 2021-08-13 | 2023-03-24 | 苏州大学 | 一种基于启发式偏置采样的室内环境机器人探索方法 |
-
2021
- 2021-08-13 CN CN202110929934.3A patent/CN113485375B/zh active Active
-
2022
- 2022-06-23 WO PCT/CN2022/100861 patent/WO2023016101A1/zh unknown
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108983780A (zh) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | 一种基于改进rrt*算法的移动机器人路径规划方法 |
CN110188696A (zh) * | 2019-05-31 | 2019-08-30 | 华南理工大学 | 一种水面无人装备多源感知方法及*** |
CN112327852A (zh) * | 2020-11-09 | 2021-02-05 | 苏州大学 | 融合路径信息丰富度的移动机器人自主探索方法 |
CN112286202A (zh) * | 2020-11-11 | 2021-01-29 | 福州大学 | 一种非均匀采样fmt*的移动机器人路径规划方法 |
Non-Patent Citations (1)
Title |
---|
基于稀疏节点快速扩展随机树的移动机械臂运动规划;李耀仲;《中国机械工程》;20201112;1462-1470 * |
Also Published As
Publication number | Publication date |
---|---|
CN113485375A (zh) | 2021-10-08 |
WO2023016101A1 (zh) | 2023-02-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113485375B (zh) | 一种基于启发式偏置采样的室内环境机器人探索方法 | |
CN113110482B (zh) | 基于先验信息启发式的室内环境机器人探索方法及*** | |
WO2020134082A1 (zh) | 一种路径规划方法、装置和移动设备 | |
Jebari et al. | Multi-sensor semantic mapping and exploration of indoor environments | |
CN109163722B (zh) | 一种仿人机器人路径规划方法及装置 | |
CN108303096B (zh) | 一种视觉辅助激光定位***及方法 | |
WO2012155121A2 (en) | Systems and methods for estimating the geographic location at which image data was captured | |
JP2020038660A (ja) | CNN(Convolutional Neural Network)を利用して車線を検出するための学習方法及び学習装置そしてこれを利用したテスト方法及びテスト装置{LEARNING METHOD, LEARNING DEVICE FOR DETECTING LANE USING CNN AND TEST METHOD, TEST DEVICE USING THE SAME} | |
CN110146080B (zh) | 一种基于移动机器人的slam回环检测方法及装置 | |
Li et al. | Improving autonomous exploration using reduced approximated generalized voronoi graphs | |
CN115145289A (zh) | 多智能体协同围捕方法、***、设备及存储介质 | |
CN115469662A (zh) | 一种环境探索方法、装置及应用 | |
Nasr et al. | Landmark recognition for autonomous mobile robots | |
Zuo et al. | An improved autonomous exploration framework for indoor mobile robotics using reduced approximated generalized voronoi graphs | |
CN116740635A (zh) | 一种实现社交距离检测的嵌入式*** | |
CN114740867A (zh) | 基于双目视觉的智能避障方法、装置、机器人及介质 | |
CN114217641B (zh) | 一种非结构环境下无人机送变电设备巡检方法及*** | |
Jebari et al. | Combined vision and frontier-based exploration strategies for semantic mapping | |
CN114397894A (zh) | 一种模仿人类记忆的移动机器人目标搜索方法 | |
CN113836251A (zh) | 一种认知地图构建方法、装置、设备及介质 | |
Xu et al. | Hybrid frontier detection strategy for autonomous exploration in multi-obstacles environment | |
Tian et al. | Autonomous exploration of RRT robot based on seeded region growing | |
Fu et al. | A Lightweight Autonomous Vehicle System Based On Pure Visual Navigation | |
Rojo et al. | Using global appearance descriptors to solve topological visual slam | |
CN117237561B (zh) | 一种人形机器人封闭环境下三维点云地图重建方法 |
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 |