CN112066976B - 一种自适应膨胀处理方法、系统、机器人及存储介质 - Google Patents
一种自适应膨胀处理方法、系统、机器人及存储介质 Download PDFInfo
- Publication number
- CN112066976B CN112066976B CN202010929771.4A CN202010929771A CN112066976B CN 112066976 B CN112066976 B CN 112066976B CN 202010929771 A CN202010929771 A CN 202010929771A CN 112066976 B CN112066976 B CN 112066976B
- Authority
- CN
- China
- Prior art keywords
- node
- obstacle
- current processing
- expansion
- robot
- 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
- 238000003672 processing method Methods 0.000 title claims abstract description 13
- 238000012545 processing Methods 0.000 claims abstract description 114
- 238000000034 method Methods 0.000 claims abstract description 51
- 230000004888 barrier function Effects 0.000 claims abstract description 26
- 230000008569 process Effects 0.000 claims abstract description 17
- 230000003044 adaptive effect Effects 0.000 claims description 26
- 230000006870 function Effects 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 7
- 230000009467 reduction Effects 0.000 claims description 4
- 230000004044 response Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 12
- 238000004422 calculation algorithm Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 4
- 230000010339 dilation Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000010485 coping Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000007726 management method Methods 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 230000033228 biological regulation Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种自适应膨胀处理方法、系统、机器人及存储介质,其包括:获取机器人所处环境的栅格地图,对障碍点坐标进行初始标记;获得障碍点的坐标后,填充机器人传感器位置到障碍点之间的栅格以及该障碍点栅格的初始代价值;对障碍点进行自适应膨胀处理,根据栅格代价值填充障碍点周围膨胀区栅格的代价值,完成对代价地图的膨胀处理。本发明提升了移动机器人自主导航过程中应对复杂环境的安全性和鲁棒性。
Description
技术领域
本发明涉及一种移动机器人地图创建技术领域,特别是关于一种基于代价地图的自适应膨胀处理方法、系统、机器人及存储介质。
背景技术
移动机器人自主导航技术作为各项高新技术综合应用的载体,在各个行业均展现了巨大的应用价值。在移动机器人进行自主导航前,首先需要完成机器人在未知环境中的地图构建。目前主要应用在移动机器人自主导航的地图类型主要是栅格地图。栅格地图易于实现建模、更新与处理,具有结构简单、便于存储计算等优点,它把环境信息离散划分成一系列栅格,用概率值表征该栅格位置下的障碍信息。一般情况下只含简单的布尔信息,因此在复杂环境下寻求更优路径有一定局限性。
代价地图对传统栅格地图划分新的区域,根据传感器数据实时更新障碍物信息,机器人移动时在障碍物周围设置一层安全缓冲区。相比于其他种类地图,代价地图更有助于机器人的定位与导航。如今随着移动机器人自主导航应用范围的不断扩大,代价地图也逐渐表露出了应对复杂环境时障碍物划分不灵活,路径规划不理想等不足。
发明内容
针对上述问题,本发明的目的是提供一种基于代价地图的自适应膨胀处理方法,其能优化移动机器人自主导航中的地图构建,提升了移动机器人自主导航过程中应对复杂环境的安全性和鲁棒性。
为实现上述目的,本发明采取以下技术方案:一种自适应膨胀处理方法,该方法基于代价地图实现,其包括以下步骤:1)获取机器人所处环境的栅格地图,对障碍点坐标进行初始标记;2)获得障碍点的坐标后,填充机器人传感器位置到障碍点之间的栅格以及该障碍点栅格的初始代价值;3)对障碍点进行自适应膨胀处理,根据栅格代价值填充障碍点周围膨胀区栅格的代价值,完成对代价地图的膨胀处理。
进一步,所述步骤3)中,自适应膨胀处理包括以下步骤:
3.1)由初始障碍点构造膨胀点优先队列inflation_cells;
3.2)将队列inflation_cells的第一个节点作为当前处理节点,计算当前处理节点的父节点的衰减系数coef;
3.3)若当前处理节点为障碍点,则以其为中心在栅格地图上构建七宫图;反之,若当前处理节点不是障碍点,则继承其当前处理障碍点On的衰减因子sp,并根据计算其衰减系数coef和衰减因子sp的乘积coef·sp,进入步骤3.4);
3.4)判断d(qn)<coef·sp·rc是否成立,若成立,则将当前处理障碍点On的上下左右四个子节点加入队列inflation_cells,否则不做处理;
3.5)由自适应膨胀半径栅格代价值计算函数f(qn)计算出当前处理节点的代价值,与该处理节点的原始代价值比较,取最大值作为当前处理节点的代价值;
3.6)从队列inflation_cells中移除当前处理节点即障碍点,返回步骤3.2)对下一个节点进行处理。
进一步,所述步骤3.2)中,衰减系数coef计算方法为:
3.2.1)判断该当前处理节点是否为障碍点,若是,则将该当前处理节点自身作为父节点,此时的父节点即为当前处理障碍点On;若不是障碍点,则记离该当前处理节点最近的障碍点作为父节点;
进一步,当前处理障碍点On的衰减系数coef为:
式中,ri表示机器人地面投影轮廓的内切圆半径;rc表示外接圆半径;qn为栅格;Dmax为距机器人最远障碍点的距离;n表示障碍点的个数;e为自然对数函数的底数;为当前处理障碍点On与机器人之间的栅格距离。
进一步,所述步骤3.3)中,衰减因子sp的计算方法为:依次得到七宫图中的其余48个栅格代价值,累计得出值为FREE_SPACE的栅格数量nF和值为LETHAL_OBSTACLE的栅格数量nL,则衰减因子sp为:
进一步,所述步骤3.4)中,对加入队列inflation_cells的子节点进行如下处理:
将四个子节点依次作为当前处理节点,判断其与父节点的距离是否在膨胀半径内,在内则继续将该点的子节点加入膨胀队列,此时记其父节点仍然为之前的当前处理障碍点On,在外则不做处理;每处理完一个节点后计算并填充该节点的代价值,最后从膨胀队列中移除这个节点。
进一步,所述自适应膨胀半径栅格代价值计算函数f(qn)为:
式中,ra表示每个栅格的边长;rm表示人为规定的代价地图膨胀半径;w表示代价值下降权重;d(qn)为当前节点与其父节点之间的距离;ri表示机器人地面投影轮廓的内切圆半径;rc表示外接圆半径。
一种自适应膨胀处理系统,其包括障碍点坐标初始标记模块、初始代价值填充模块和膨胀处理模块;所述障碍点坐标初始标记模块获取机器人所处环境的栅格地图,对障碍点坐标进行初始标记;所述初始代价值填充模块获得障碍点的坐标后,填充机器人传感器位置到障碍点之间的栅格以及该障碍点栅格的初始代价值;所述膨胀处理模块对障碍点进行自适应膨胀处理,根据栅格代价值填充障碍点周围膨胀区栅格的代价值,完成对代价地图的膨胀处理。
一种机器人,其包括至少一个处理器;以及与所述至少一个处理器通信连接的存储器;其中,所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行上述各实施例中任一项所述膨胀处理方法。
一种计算机可读存储介质,存储有计算机程序,所述计算机程序被处理器执行时实现上述各实施例中任一项所述膨胀处理方法。
本发明由于采取以上技术方案,其具有以下优点:本发明的自适应膨胀半径代价地图建立在全局路径规划时避开复杂障碍物群,局部规划时增加可行驶空间,提升了移动机器人自主导航过程中应对复杂环境的安全性和鲁棒性。
附图说明
图1是迭代节点关系图。
图2是七宫图示意图。
图3是实施例中使用距离自适应膨胀半径示意图。
图4是实施例中未使用距离自适应膨胀半径示意图。
图5是实施例中使用轮廓自适应膨胀半径示意图。
图6是实施例中未使用轮廓自适应膨胀半径示意图。
图7是实施例中使用自适应膨胀半径示意图。
图8是实施例中未使用自适应膨胀半径示意图。
图9是实施例中自适应膨胀半径导航仿真及效果图。
图10是实施例中的Gazebo中简单障碍环境示意图。
图11是实施例中简单障碍环境时未使用自适应膨胀半径示意图。
图12是实施例中简单障碍环境时使用自适应膨胀半径示意图。
图13是实施例中的Gazebo中复杂障碍环境示意图。
图14是实施例中复杂障碍环境时未使用自适应膨胀半径示意图。
图15是实施例中复杂障碍环境时使用自适应膨胀半径示意图。
图16是实施例中机器人结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例的附图,对本发明实施例的技术方案进行清楚、完整地描述。显然,所描述的实施例是本发明的一部分实施例,而不是全部的实施例。基于所描述的本发明的实施例,本领域普通技术人员所获得的所有其他实施例,都属于本发明保护的范围。
本发明第一实施方式涉及一种基于代价地图的自适应膨胀处理方法,代价地图对障碍物边缘的膨胀是栅格地图上的障碍点依据与机器人的栅格距离,对其子节点迭代填充代价值的过程。具体的,本发明包括以下步骤:
1)获取机器人所处环境的栅格地图,对障碍点坐标进行初始标记;优选的,采用Bresenham算法对障碍点坐标进行标记。
2)获得障碍点的坐标后,填充机器人传感器位置到障碍点之间的栅格以及该障碍点栅格的初始代价值;
代价地图中的一个栅格在计算机内存中占用一个字节,可以表示0-255中的任意数字,称为栅格代价值。
3)对障碍点进行自适应膨胀处理,根据栅格代价值填充障碍点周围膨胀区栅格的代价值,完成对代价地图的膨胀处理。
上述步骤3)中,自适应膨胀处理包括以下步骤:
3.1)由初始障碍点构造膨胀点优先队列inflation_cells;
3.2)将队列inflation_cells的第一个节点作为当前处理节点,计算当前处理节点的父节点的衰减系数coef;
具体方法为:
3.2.1)判断该当前处理节点是否为障碍点,若是,则将该当前处理节点自身作为父节点,此时的父节点即为当前处理障碍点On;若不是障碍点,则记离该当前处理节点最近的障碍点作为父节点;
当前处理障碍点On的衰减系数coef为:
式中,ri表示机器人地面投影轮廓的内切圆半径;rc表示外接圆半径;qn为栅格;Dmax为距机器人最远障碍点的距离;n表示障碍点的个数;e为自然对数函数的底数。
3.3)若当前处理节点为障碍点,则以其为中心在栅格地图上构建七宫图;反之,若当前处理节点不是障碍点,则继承其当前处理障碍点On的衰减因子sp,并计算其衰减系数coef和衰减因子sp的乘积coef·sp,进入步骤3.4);
衰减因子sp的计算方法为:
依次得到七宫图中的其余48个栅格代价值,累计得出值为FREE_SPACE的栅格数量nF和值为LETHAL_OBSTACLE的栅格数量nL,则衰减因子sp为:
3.4)判断d(qn)<coef·sp·rc是否成立,若成立,则将当前处理障碍点On的上下左右四个子节点加入队列inflation_cells,否则不做处理。
对加入队列inflation_cells的子节点进行如下处理:
将四个子节点依次作为当前处理节点,判断其与父节点的距离是否在膨胀半径内,在内则继续将该点的子节点加入膨胀队列(记其父节点仍然为之前的当前处理障碍点On),在外则不做处理。每处理完一个节点后计算并填充该节点的代价值,最后从膨胀队列中移除这个节点。
例如,如图1所示,首先把某一障碍点On作为父节点,计算子节点(O1、O2、O3、O4)与On的距离d(qn);然后判断每个子节点的d(qn)是否符合d(qn)<coef·sp·rc,若成立则将其子节点作为父节点进行迭代,否则不做处理;最后由当前节点的d(qn)计算并填充当前节点的代价值。
需要注意的是,膨胀范围内非障碍点的父节点都是距离它最近的一个障碍点;
初始障碍点为中心的膨胀半径范围内的栅格点都会进入膨胀队列进行处理,是一个不断迭代加入新点和移除已处理点的过程。
3.5)由自适应膨胀半径栅格代价值计算函数f(qn)计算出当前处理节点的代价值,与该处理节点的原始代价值比较,取最大值作为当前处理节点的代价值;
其中,原始代价值是指未进行膨胀处理前的栅格地图上的代价值,其含有简单信息,即障碍点,无障碍,未知区域。
3.6)从队列inflation_cells中移除当前处理节点即障碍点,返回步骤3.2)对下一个节点进行处理;
当所有与父节点距离在膨胀半径内的节点都加入膨胀半径经过处理后,膨胀队列会为逐渐为空(因为在膨胀半径外的点不会再加入,处理完一个点后会移除这个点,所以最后膨胀队列里面只有没有点的增加,只有点的移除),即完成了一次对一个障碍点的膨胀操作。
上述实施例中,自适应膨胀半径栅格代价值计算函数f(qn)为:
式中,ra表示每个栅格的边长;rm表示人为规定的代价地图膨胀半径;w表示代价值下降权重。
设栅格地图分辨率为Δ,机器人与任一栅格的距离l转换成栅格距离d为:
上述实施例中,针对机器人紧贴障碍物边缘行驶的情况,进行自适应调整膨胀半径的方法为:在未紧贴障碍物行驶时保留更多的安全行驶空间,在紧贴障碍物行驶时再给出最低限度安全行驶空间。
如图2所示,七宫图为一个7行7列的平面方阵。开始时,把当前子栅格放入七宫图的中心位置,即第4行第4列,记为Pn,然后以这一点为中心,检测其余栅格的概率值。
无人车靠近某个障碍物点Pn时,周围非占据的栅格数量减少,被占据栅格数量增加,然后计算衰减因子sp进行判断。
实施例:
为了检验该算法的效果,设计实验利用husky_robot机器人在Gazebo中搭建仿真环境对该方法进行测试。采用ROS Navigation导航框架,使用的SLAM方法为Gmapping,采用不使用静态地图的方式,在未知环境中进行实时建图导航。全局规划算法选取经典的A*算法,局部规划算法选取DWA动态窗口法。在相同的障碍物分布情况下对比膨胀半径以及路径规划情况。本仿真实验所有程序都运行在Ubuntu操作系统中,运行内存为8GB,主频为2.3GHz。
如图3、图4所示,将使用距离自适应膨胀半径前后进行对比,可以看出,在使用距离自适应膨胀半径后远处障碍物与近处障碍物各自的膨胀半径大小跟与无人车的距离成正比。
如图5、图6所示,将轮廓自适应膨胀半径使用前后对比,可以看出,在贴近障碍物行驶时,在小车靠近的轮廓区域,膨胀半径减少,进一步增加了小车的灵活性。
如图7、图8所示,自适应膨胀使用前后对比,可以看出,在相同障碍物分布环境下可以得出,距离较近的障碍物膨胀半径较小,并在紧贴障碍物区域膨胀半径在安全范围内进一步缩小。无人车面对近距离障碍物时有更多的局部规划空间如图9所示,同样,处于远处的障碍物则具有较大的膨胀半径,能在全局规划中规避掉更多的狭窄空间,避免不安全的路径规划出现。
路径规划情况对照:
如图10所示,为Gzebo障碍物设置情景一,通过简单障碍物环境。相同起始点和目标点下使用两种膨胀半径算法的路径导航效果如图11、图12所示:在场景一下,原始膨胀半径算法给出相等的膨胀半径,改进后的膨胀算法根据无人车靠近障碍物的位置自适应的调整膨胀半径大小,给予局部规划更多的空间。
如图13所示,为Gzebo障碍物设置情景二,通过复杂障碍物环境。如图14、图15所示,相同起始点和目标点下使用两种膨胀半径算法的路径导航效果。
综上,本发明在使用时,机器人紧靠障碍物行驶时,机器人在保证安全的前提下能预留出更多的局部规划空间。复杂障碍物情景下,使用传统的膨胀半径算法,全剧规划路径从障碍物之中穿过,最终机器人进入障碍物群中,与障碍物发生碰撞被困,导航失败;使用本发明的膨胀半径处理后,机器人在遇到复杂障碍物情况时,全局路径规划成功绕过障碍物进行导航,最终到达目标点,导航成功。基于本发明的代价地图建立能全局路径规划时避开复杂障碍物群,局部规划时增加可行驶空间,提升了移动机器人自主导航过程中应对复杂环境的安全性和鲁棒性。
本发明第二实施方式涉及一种自适应膨胀处理系统,其包括障碍点坐标初始标记模块、初始代价值填充模块和膨胀处理模块;
障碍点坐标初始标记模块获取机器人所处环境的栅格地图,对障碍点坐标进行初始标记;
初始代价值填充模块获得障碍点的坐标后,填充机器人传感器位置到障碍点之间的栅格以及该障碍点栅格的初始代价值;
膨胀处理模块对障碍点进行自适应膨胀处理,根据栅格代价值填充障碍点周围膨胀区栅格的代价值,完成对代价地图的膨胀处理。
本发明第三实施方式涉及一种机器人,该机器人10的结构如图16所示,包括:至少一个处理器101;以及,与至少一个处理器101通信连接的存储器102;其中,存储器102存储有可被至少一个处理器101执行的指令,指令被至少一个处理器101执行,以使至少一个处理器101能够执行上述膨胀区域的确定方法。
其中,存储器102和处理器101采用总线方式连接,总线可以包括任意数量的互联的总线和桥,总线将一个或多个处理器101和存储器102的各种电路链接在一起。总线还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路链接在一起,这些都是本领域所公知的,因此,本发明不再对其进行进一步描述。总线接口在总线和收发机之间提供接口。收发机可以是一个元件,也可以是多个元件,比如多个接收器和发送器,提供用于在传输介质上与各种其他装置通信的单元。经处理器101处理的数据通过天线在无线介质上进行传输,进一步,天线还接收数据并将数据传送给处理器。
处理器负责管理总线和通常的处理,还可以提供各种功能,包括定时,外围接口,电压调节、电源管理以及其他控制功能。而存储器可以被用于存储处理器在执行操作时所使用的数据。
本发明第四实施方式涉及一种计算机可读存储介质,存储有计算机程序,计算机程序被处理器执行时实现第一实施方式中的膨胀处理方法。
本发明第五实施方式涉及一种计算机程序,其包括有计算机程序指令,该程序指令被处理器执行时用于实现本发明上述实施例提供的任一膨胀处理方法的步骤。
上述实施例中,计算机可读存储介质和计算机程序用于实现上述方法实施例中相应的数据存储方法,并具有相应的方法实施例的有益效果,在此不再赘述。
需要指出,根据实施的需要,可将本发明实施例中描述的各个部件/步骤拆分为更多部件/步骤,也可将两个或多个部件/步骤或者部件/步骤的部分操作组合成新的部件/步骤,以实现本发明实施例的目的。
上述根据本发明实施例的方法可在硬件、固件中实现,或者被实现为可存储在记录介质(诸如CD ROM、RAM、软盘、硬盘或磁光盘)中的软件或计算机代码,或者被实现通过网络下载的原始存储在远程记录介质或非暂时机器可读介质中并将被存储在本地记录介质中的计算机代码,从而在此描述的方法可被存储在使用通用计算机、专用处理器或者可编程或专用硬件(诸如ASIC或FPGA)的记录介质上的这样的软件处理。可以理解,计算机、处理器、微处理器控制器或可编程硬件包括可存储或接收软件或计算机代码的存储组件(例如,RAM、ROM、闪存等),当所述软件或计算机代码被计算机、处理器或硬件访问且执行时,实现在此描述的处理方法。此外,当通用计算机访问用于实现在此示出的处理的代码时,代码的执行将通用计算机转换为用于执行在此示出的处理的专用计算机。
本领域普通技术人员可以意识到,结合上述所公开的实施例描述的各示例的单元及方法步骤,能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明实施例的范围。
以上实施方式仅用于说明本发明实施例,而并非对本发明实施例的限制,有关技术领域的普通技术人员,在不脱离本发明实施例的精神和范围的情况下,还可以做出各种变化和变型,因此所有等同的技术方案也属于本发明实施例的范畴,本发明实施例的专利保护范围应由权利要求限定。
Claims (4)
1.一种自适应膨胀处理方法,该方法基于代价地图实现,其特征在于,包括以下步骤:
1)获取机器人所处环境的栅格地图,对障碍点坐标进行初始标记;
2)获得障碍点的坐标后,填充机器人传感器位置到障碍点之间的栅格以及该障碍点栅格的初始代价值;
3)对障碍点进行自适应膨胀处理,根据栅格代价值填充障碍点周围膨胀区栅格的代价值,完成对代价地图的膨胀处理;
所述步骤3)中,自适应膨胀处理包括以下步骤:
3.1)由初始障碍点构造膨胀点优先队列inflation_cells;
3.2)将队列inflation_cells的第一个节点作为当前处理节点,计算当前处理节点的父节点的衰减系数coef;
3.3)若当前处理节点为障碍点,则以其为中心在栅格地图上构建七宫图;反之,若当前处理节点不是障碍点,则继承其当前处理障碍点On的衰减因子sp,并根据计算其衰减系数coef和衰减因子sp的乘积coef·sp,进入步骤3.4);
3.4)判断d(qn)<coef·sp·rc是否成立,若成立,则将当前处理障碍点On的上下左右四个子节点加入队列inflation_cells,否则不做处理;
3.5)由自适应膨胀半径栅格代价值计算函数f(qn)计算出当前处理节点的代价值,与该处理节点的原始代价值比较,取最大值作为当前处理节点的代价值;
3.6)从队列inflation_cells中移除当前处理节点即障碍点,返回步骤3.2)对下一个节点进行处理;
所述步骤3.2)中,衰减系数coef计算方法为:
3.2.1)判断该当前处理节点是否为障碍点,若是,则将该当前处理节点自身作为父节点,此时的父节点即为当前处理障碍点On;若不是障碍点,则记离该当前处理节点最近的障碍点作为父节点;
当前处理障碍点On的衰减系数coef为:
式中,ri表示机器人地面投影轮廓的内切圆半径;rc表示外接圆半径;qn为栅格;Dmax为距机器人最远障碍点的距离;n表示障碍点的个数;e为自然对数函数的底数;为当前处理障碍点On与机器人之间的栅格距离;
所述步骤3.3)中,衰减因子sp的计算方法为:依次得到七宫图中的其余48个栅格代价值,累计得出值为FREE_SPACE的栅格数量nF和值为LETHAL_OBSTACLE的栅格数量nL,则衰减因子sp为:
所述步骤3.4)中,对加入队列inflation_cells的子节点进行如下处理:
将四个子节点依次作为当前处理节点,判断其与父节点的距离是否在膨胀半径内,在内则继续将该点的子节点加入膨胀队列,此时记其父节点仍然为之前的当前处理障碍点On,在外则不做处理;每处理完一个节点后计算并填充该节点的代价值,最后从膨胀队列中移除这个节点;
所述自适应膨胀半径栅格代价值计算函数f(qn)为:
式中,ra表示每个栅格的边长;rm表示人为规定的代价地图膨胀半径;w表示代价值下降权重;d(qn)为当前节点与其父节点之间的距离;ri表示机器人地面投影轮廓的内切圆半径;rc表示外接圆半径。
2.一种自适应膨胀处理系统,其特征在于,包括:障碍点坐标初始标记模块、初始代价值填充模块和膨胀处理模块;
所述障碍点坐标初始标记模块获取机器人所处环境的栅格地图,对障碍点坐标进行初始标记;
所述初始代价值填充模块获得障碍点的坐标后,填充机器人传感器位置到障碍点之间的栅格以及该障碍点栅格的初始代价值;
所述膨胀处理模块对障碍点进行自适应膨胀处理,根据栅格代价值填充障碍点周围膨胀区栅格的代价值,完成对代价地图的膨胀处理;
所述膨胀处理模块中,自适应膨胀处理包括以下步骤:
3.1)由初始障碍点构造膨胀点优先队列inflation_cells;
3.2)将队列inflation_cells的第一个节点作为当前处理节点,计算当前处理节点的父节点的衰减系数coef;
3.3)若当前处理节点为障碍点,则以其为中心在栅格地图上构建七宫图;反之,若当前处理节点不是障碍点,则继承其当前处理障碍点On的衰减因子sp,并根据计算其衰减系数coef和衰减因子sp的乘积coef·sp,进入步骤3.4);
3.4)判断d(qn)<coef·sp·rc是否成立,若成立,则将当前处理障碍点On的上下左右四个子节点加入队列inflation_cells,否则不做处理;
3.5)由自适应膨胀半径栅格代价值计算函数f(qn)计算出当前处理节点的代价值,与该处理节点的原始代价值比较,取最大值作为当前处理节点的代价值;
3.6)从队列inflation_cells中移除当前处理节点即障碍点,返回步骤3.2)对下一个节点进行处理;
在3.2)中,衰减系数coef计算方法为:
3.2.1)判断该当前处理节点是否为障碍点,若是,则将该当前处理节点自身作为父节点,此时的父节点即为当前处理障碍点On;若不是障碍点,则记离该当前处理节点最近的障碍点作为父节点;
当前处理障碍点On的衰减系数coef为:
式中,ri表示机器人地面投影轮廓的内切圆半径;rc表示外接圆半径;qn为栅格;Dmax为距机器人最远障碍点的距离;n表示障碍点的个数;e为自然对数函数的底数;为当前处理障碍点On与机器人之间的栅格距离;
在3.3)中,衰减因子sp的计算方法为:依次得到七宫图中的其余48个栅格代价值,累计得出值为FREE_SPACE的栅格数量nF和值为LETHAL_OBSTACLE的栅格数量nL,则衰减因子sp为:
在3.4)中,对加入队列inflation_cells的子节点进行如下处理:
将四个子节点依次作为当前处理节点,判断其与父节点的距离是否在膨胀半径内,在内则继续将该点的子节点加入膨胀队列,此时记其父节点仍然为之前的当前处理障碍点On,在外则不做处理;每处理完一个节点后计算并填充该节点的代价值,最后从膨胀队列中移除这个节点;
所述自适应膨胀半径栅格代价值计算函数f(qn)为:
式中,ra表示每个栅格的边长;rm表示人为规定的代价地图膨胀半径;w表示代价值下降权重;d(qn)为当前节点与其父节点之间的距离;ri表示机器人地面投影轮廓的内切圆半径;rc表示外接圆半径。
3.一种机器人,其特征在于,包括:
至少一个处理器;以及,
与所述至少一个处理器通信连接的存储器;其中,
所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行如权利要求1中所述膨胀处理方法。
4.一种计算机可读存储介质,存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1中所述膨胀处理方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010929771.4A CN112066976B (zh) | 2020-09-07 | 2020-09-07 | 一种自适应膨胀处理方法、系统、机器人及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010929771.4A CN112066976B (zh) | 2020-09-07 | 2020-09-07 | 一种自适应膨胀处理方法、系统、机器人及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112066976A CN112066976A (zh) | 2020-12-11 |
CN112066976B true CN112066976B (zh) | 2023-06-16 |
Family
ID=73663835
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010929771.4A Active CN112066976B (zh) | 2020-09-07 | 2020-09-07 | 一种自适应膨胀处理方法、系统、机器人及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112066976B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113171041B (zh) * | 2021-05-18 | 2022-08-23 | 上海高仙自动化科技发展有限公司 | 一种目标路径生成方法、装置、设备及存储介质 |
CN113721611B (zh) * | 2021-08-20 | 2024-09-06 | 河南牧原智能科技有限公司 | 用于清粪机器人的覆盖式路径规划方法、装置和存储介质 |
CN115711624A (zh) * | 2022-10-18 | 2023-02-24 | 中国科学院半导体研究所 | 运动代价地图构建方法、装置、无人设备及存储介质 |
CN115629612A (zh) * | 2022-12-19 | 2023-01-20 | 科大讯飞股份有限公司 | 一种避障方法、装置、设备及存储介质 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106774347A (zh) * | 2017-02-24 | 2017-05-31 | 安科智慧城市技术(中国)有限公司 | 室内动态环境下的机器人路径规划方法、装置和机器人 |
CN107091642A (zh) * | 2017-05-15 | 2017-08-25 | 东南大学 | 一种基于异平面锚节点映射及栅格化纠偏的室内定位方法 |
CN108908330A (zh) * | 2018-06-28 | 2018-11-30 | 中国人民解放军国防科技大学 | 一种基于虚拟现实的机器人行为控制方法 |
CN109708636A (zh) * | 2017-10-26 | 2019-05-03 | 广州极飞科技有限公司 | 导航图配置方法、避障方法以及装置、终端、无人飞行器 |
CN109916393A (zh) * | 2019-03-29 | 2019-06-21 | 电子科技大学 | 一种基于机器人位姿的多重栅格值导航方法及其应用 |
CN109947118A (zh) * | 2019-04-19 | 2019-06-28 | 南京大学 | 一种使用gpu加速的代价地图快速更新方法 |
CN110470308A (zh) * | 2019-09-06 | 2019-11-19 | 北京云迹科技有限公司 | 一种避障系统及方法 |
CN111026131A (zh) * | 2019-12-30 | 2020-04-17 | 深圳前海达闼云端智能科技有限公司 | 一种膨胀区域的确定方法、确定装置、机器人和存储介质 |
CN111338384A (zh) * | 2019-12-17 | 2020-06-26 | 北京化工大学 | 一种仿蛇机器人自适应路径跟踪方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2014011990A1 (en) * | 2012-07-13 | 2014-01-16 | International Electronic Machines Corp. | Straight line path planning |
-
2020
- 2020-09-07 CN CN202010929771.4A patent/CN112066976B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106774347A (zh) * | 2017-02-24 | 2017-05-31 | 安科智慧城市技术(中国)有限公司 | 室内动态环境下的机器人路径规划方法、装置和机器人 |
CN107091642A (zh) * | 2017-05-15 | 2017-08-25 | 东南大学 | 一种基于异平面锚节点映射及栅格化纠偏的室内定位方法 |
CN109708636A (zh) * | 2017-10-26 | 2019-05-03 | 广州极飞科技有限公司 | 导航图配置方法、避障方法以及装置、终端、无人飞行器 |
CN108908330A (zh) * | 2018-06-28 | 2018-11-30 | 中国人民解放军国防科技大学 | 一种基于虚拟现实的机器人行为控制方法 |
CN109916393A (zh) * | 2019-03-29 | 2019-06-21 | 电子科技大学 | 一种基于机器人位姿的多重栅格值导航方法及其应用 |
CN109947118A (zh) * | 2019-04-19 | 2019-06-28 | 南京大学 | 一种使用gpu加速的代价地图快速更新方法 |
CN110470308A (zh) * | 2019-09-06 | 2019-11-19 | 北京云迹科技有限公司 | 一种避障系统及方法 |
CN111338384A (zh) * | 2019-12-17 | 2020-06-26 | 北京化工大学 | 一种仿蛇机器人自适应路径跟踪方法 |
CN111026131A (zh) * | 2019-12-30 | 2020-04-17 | 深圳前海达闼云端智能科技有限公司 | 一种膨胀区域的确定方法、确定装置、机器人和存储介质 |
Non-Patent Citations (6)
Title |
---|
A novel obstacle avoidance method based on multi-information inflation map;Yuan,RP等;INDUSTRIAL ROBOT-THE INTERNATIONAL JOURNAL OF ROBOTICS RESARCH AND APPLICATION;253-265 * |
Visual navigation for mobile robot with Kinect camera in dynamic environment;Jing Xin等;2016 35th Chinese Control Conference (CCC);4757-4764 * |
利用七宫格的遮挡车辆凹性检测与分割;刘万军等;中国图象图形学报;第19卷(第1期);45-53 * |
基于ROS的自主移动机器人系统设计与实现;沈俊;中国优秀硕士学位论文全文数据库 (信息科技辑);I140-823 * |
基于三维建图与虚拟现实的人-机器人交互技术研究;王盼;中国优秀硕士学位论文全文数据库 (信息科技辑);I138-2110 * |
基于车辆轮廓凹陷区域的分割算法;张栩华等;电子设计工程;第27卷(第24期);157-160+166 * |
Also Published As
Publication number | Publication date |
---|---|
CN112066976A (zh) | 2020-12-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112066976B (zh) | 一种自适应膨胀处理方法、系统、机器人及存储介质 | |
CN112859859B (zh) | 一种基于三维障碍物体素对象映射的动态栅格地图更新方法 | |
CN111536964B (zh) | 机器人定位方法及装置、存储介质 | |
CN109059924B (zh) | 基于a*算法的伴随机器人增量路径规划方法及系统 | |
CN108280866B (zh) | 道路点云数据处理方法及系统 | |
CN110986953B (zh) | 路径规划方法、机器人及计算机可读存储介质 | |
CN112033413B (zh) | 一种基于结合环境信息的改进a*算法的路径规划方法 | |
EP3470786B1 (en) | A computer-implemented method and a system for generating a 3d path to a landing location for an aerial vehicle | |
CN111680747B (zh) | 用于占据栅格子图的闭环检测的方法和装置 | |
CN113110455B (zh) | 一种未知初始状态的多机器人协同探索方法、装置及系统 | |
CN112329789B (zh) | 点云的提取方法、装置、计算机设备和存储介质 | |
CN113433937B (zh) | 基于启发式探索的分层导航避障系统、分层导航避障方法 | |
JP7047576B2 (ja) | 地図作成装置 | |
CN110940341B (zh) | 路径规划方法、机器人及计算机可读存储介质 | |
CN111045433B (zh) | 一种机器人的避障方法、机器人及计算机可读存储介质 | |
CN114460965A (zh) | 一种基于改进人工势场法的无人机三维避障方法 | |
CN112612267A (zh) | 自动驾驶的路径规划方法和装置 | |
EP4258162A1 (en) | Placement position acquisition method, model training method, and related devices | |
CN116147653B (zh) | 一种面向无人驾驶车辆的三维参考路径规划方法 | |
CN116772846A (zh) | 一种无人机航迹规划方法、装置、设备及介质 | |
CN112132951A (zh) | 一种基于视觉的网格语义地图的构建方法 | |
CN110716547A (zh) | 一种基于波前算法的3d探索的方法 | |
US20220300002A1 (en) | Methods and systems for path planning in a known environment | |
CN112883128A (zh) | 一种封闭环境下机器人自主建图方法 | |
KR102698858B1 (ko) | 강화학습 기반 건축 설계 방법, 시스템 및 비일시성의 컴퓨터 판독 가능 기록 매체 |
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 |