CN113311862B - 机器人规避障碍物的运动规划方法 - Google Patents
机器人规避障碍物的运动规划方法 Download PDFInfo
- Publication number
- CN113311862B CN113311862B CN202110550658.XA CN202110550658A CN113311862B CN 113311862 B CN113311862 B CN 113311862B CN 202110550658 A CN202110550658 A CN 202110550658A CN 113311862 B CN113311862 B CN 113311862B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- robot
- mode
- window
- threshold
- 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
- 238000000034 method Methods 0.000 title claims abstract description 22
- 230000003044 adaptive effect Effects 0.000 claims description 19
- 230000007246 mechanism Effects 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 7
- 230000004888 barrier function Effects 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims description 3
- 238000010187 selection method Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 abstract 1
- 230000008859 change Effects 0.000 description 4
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000009191 jumping Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 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/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种机器人规避障碍物的运动规划方法,基于向量场直方图算法的进行了改进,应用一种可自动调整范围的动态窗口,根据设置条件选择模式一或者模式二,并在不同的窗口模式下设置自适应阈值,使用阈值评价函数,同时考虑移动机器人硬件特性、运动特性确定阈值的选取范围,使机器人在快速适应不同环境的同时可以无碰撞且平滑到达目标点。本发明的一种机器人规避障碍物的运动规划方法,规划的路径更短,而且安全有效。
Description
技术领域
本发明涉及机器人领域,尤其涉及一种机器人规避障碍物的运动规划方法。
背景技术
移动机器人的发展是目前科学研究的热门领域之一,随着现在科技的不断进步,机器人领域也在不断发展,自主避障能力和路径规划作为机器人的最基本的能力,也在快速发展当中,国内外学者对机器人的路径规划做了大量的研究,其中有人工势场法、bug算法、虚拟势场算法VFF等,向量场直方图法(VFH)也是其中之一。
但是目前的VFH算法还是存在一些问题,例如受阈值、栅格、窗口范围影响较大,因此不能很好的完成运动路线的规划。
发明内容
针对上述现有技术中的不足,本发明提供一种机器人规避障碍物的运动规划方法,规划的路径更短,而且安全有效。
为了实现上述目的,本发明提供一种机器人规避障碍物的运动规划方法,包括步骤:
S1:设置初始窗口大小为W*W,地图类型为栅格地图,将所述地图的每一个栅格作为一个障碍物向量,所述障碍物向量包括大小mi,j和方向βi,j;
S2:计算所述初始窗口内的障碍物总密度值pn;
S3:根据所述障碍物总密度值选择窗口模式,当满足预设条件时,切换不同的所述窗口模式;
S4:根据角分辨率f,将正方形所述初始窗口分为若干个扇区,计算所述扇区个数n及障碍物所在扇区k;
S5:计算每个所述扇区的障碍物密度值,生成二维障碍物向量直方图;
S6:根据所述初始窗口的自适应阈值选择可行进的方向作为候选区域;
S7:所述障碍物密度值大于阈值的为不可行的方向,所述障碍物密度值低于阈值的可行驶的方向,将连续可通行的所述扇区作为候选山谷;
S8:构造代价函数,选择代价最小的所述候选区域作为行进方向。
优选地,所述S1步骤中,所述障碍物向量的大小mi,j和方向βi,j满足公式:
优选地,所述S2步骤中,所述障碍物总密度值pn的计算公式为:
pn=∑i,jmi,j。
优选地,所述S3步骤中,所述窗口模式包括模式一和模式二;根据所述障碍物总密度值判断障碍物密集情况,所述碍物密集情况包括障碍物稀疏和障碍物密集;当障碍物稀疏时,使用模式一的第一动态窗口,所述第一动态窗口的大小为W*W,W=14;当障碍物密集时,使用模式二的第二动态窗口,所述第二动态窗口的大小为Q*Q,Q计算公式为:
Q=int(((pmax-p1)*W)/pmax);
其中pmax为最大障碍物总密度值,p1为模式一下障碍物总密度值,W为模式一窗口大小范围;Q为模式二窗口大小范围;
所述S3步骤中最大障碍物总密度值pmax计算公式为:
对2种窗口模式设置不同的跳转条件,其跳转条件如下所示:
在所述模式一下:
ω为模式一临界跳转条件,当p1<ω时,模式保持不变,当p1≥ω时,切换模式为模式二,其中ω为经验值;
在所述模式二下:
q为模式二下临界跳转条件,当p2>q时,模式保持不变,当p2≤q时,切换模式为模式一;
其中μ为修正系数。
优选地,所述S4步骤中,所述扇区个数n及障碍物所在扇区k满足公式:
优选地,所述S5步骤中,每个所述扇区的障碍物密度值满足公式:
其中,hk表示每个所述扇区的障碍物密度值。
优选地,所述S6步骤中,当机器人未找到目标点时,
所述自适应阈值的选择根据所述扇区内障碍物密度来进行选择;不同所述窗口模式下所述自适应阈值选择方法不同;
所述模式一下所述自适应阈值采用第一阈值T1:
其中,τhigh表示第一高阈值,pi表示第i个扇区的障碍物总密度值,δ1表示第一临界条件,τlow表示第一低阈值;
所述模式二下所述自适应阈值采用第二阈值T2:
其中,ηhigh表示第二高阈值,ηlow表示第二低阈值,δ2表示第二临界条件;
所述自适应阈值的选择还考虑机器人的运动特性和硬件特性;机器人速度为v,加速度为α,则机器人速度减速到0的距离S为:
考虑到雷达的探测距离和机器人减速安全距离,雷达到机器人边缘的距离为R,其减速距离为S:
所述模式一下还满足:
τhigh<d1;
τlow>(S+R);
其中,d1表示雷达扫描的最大距离;
所述S6步骤中,当机器人找到目标点时,所述自适应阈值采用第三阈值T3:
其中,Tn表示选定阈值,在模式一下Tn为T1,在模式二下Tn为T2;γ表示自定义值,drt表示机器人距离目标点的距离。
优选地,所述S7步骤中,将最靠近障碍物的所述扇区边界记为右边界kn;则左边界kf满足:
kf=kn+smax;
其中,smax表示最大扇区数;
所述S7步骤中,候选方向c满足公式:
优选地,所述S8步骤中,所述代价函数g(c)为:
优选地,还包括一循环跳出机制,设置触发条件,当***陷入死循环,***启动所述循环跳出机制,自动选择一个可行进的方向,跳出当前循环。
本发明由于采用了以上技术方案,使其具有以下有益效果:
本发明是基于VFH的改进算法,地图类型为栅格地图,当雷达扫描到障碍物时会给更新单元格的确信值C,因为原算法使用一个固定的窗口,在障碍物较多的情况下,会产生较大的障碍物密度值,会导致机器人无法通过或难以搜索可行进路径,本发明改进的算法中,根据当前窗口障碍物总密度值选择不同的窗口模式,当障碍物密度较小时,会选择窗口较大的模式,当障碍物密度较大时,会选择窗口较小的模式,从而使得机器人可以在密集障碍物中穿行。设置跳出机制,当机器人在选择行进方向时陷入死循环,根据其触发条件,启动跳出机制,跳出机制运行后,机器人选择任何可行进的方向作为前进方向,逃离当前位置后,再进行路径规划。
附图说明
图1为本发明实施例的机器人规避障碍物的运动规划方法的流程图;
图2为本发明实施例的第一运行轨迹图;
图3为本发明实施例的第二运行轨迹图;
图4为本发明实施例的窗口模式选择流程图。
具体实施方式
下面根据附图图1~图4,给出本发明的较佳实施例,并予以详细描述,使能更好地理解本发明的功能、特点。
请参阅图1~图4,本发明实施例的一种机器人规避障碍物的运动规划方法,包括步骤:
S1:机器人首先会根据位姿信息和里程计信息确定自己的位姿;
雷达开始扫描,扫描到障碍物时为障碍物所在的栅格会更新确信值C。
根据雷达信息构建栅格地图,再初始点和目标点开始进行路径规划,地图为栅格地图;
本实施例采用VFH算法,采用栅格法建立地图,每个栅格都有一个信任值C,以二维直角直方图网格作为世界模型;以机器人为中心,构建一个Ws×Ws的正方形窗口作为初始窗口,正方形窗口随着机器人的移动而不断移动,模型通过机载距离传感器采样的距离数据不断更新栅格中的信任值C;
S1步骤中,障碍物向量的大小mi,j和方向βi,j满足公式:
S2:计算初始窗口内的障碍物总密度值pn;
pn=∑i,jmi,j。
S3:根据障碍物总密度值选择窗口模式,当满足预设条件时,切换不同的窗口模式;
S3步骤中,窗口模式包括模式一和模式二;根据障碍物总密度值判断障碍物密集情况,碍物密集情况包括障碍物稀疏和障碍物密集;当障碍物稀疏时,使用模式一的第一动态窗口,第一动态窗口的大小为14*14;当障碍物密集时,使用模式二的第二动态窗口,第二动态窗口的大小为Q*Q;可表示为公式:
Q=int(((pmax-p1)*W)/pmax)
其中pmax为最大障碍物总密度值,p1为模式一下障碍物总密度值,W为模式一窗口大小范围;Q为模式二窗口大小范围;
S3步骤中最大障碍物总密度值pmax计算公式为:
对2种窗口模式设置不同的跳转条件,其跳转条件如下所示:
在模式一下:
ω为模式一临界跳转条件,当p1<ω时,模式保持不变,当p1≥ω时,切换模式为模式二,其中ω为经验值;
在模式二下:
q为模式二下临界跳转条件,当p2>q时,模式保持不变,当p2≤q时,切换模式为模式一;
其中μ为修正系数。
S4:根据角分辨率f,将正方形初始窗口分为若干个扇区,计算扇区个数n及障碍物所在扇区k;
扇区个数n及障碍物所在扇区k满足公式:
S5:计算每个扇区的障碍物密度值,生成二维障碍物向量直方图;
每个扇区的障碍物密度值满足公式:
其中,hk表示每个扇区的障碍物密度值。
S6:根据初始窗口的自适应阈值选择可行进的方向作为候选区域;分为未找到目标点和找到目标点2种情况;
当机器人未找到目标点时,模式一和模式二下均应用自适应阈值,让机器人可以适应不同的障碍物密集环境;
自适应阈值的选择根据扇区内障碍物密度来进行选择;不同窗口模式下自适应阈值选择方法不同;
模式一下自适应阈值采用第一阈值T1:
其中,τhigh表示第一高阈值,pi表示第i个扇区的障碍物总密度值,δ1表示第一临界条件,τlow表示第一低阈值;
模式二下自适应阈值采用第二阈值T2:
其中,ηhigh表示第二高阈值,ηlow表示第二低阈值,δ2表示第二临界条件;
自适应阈值的选择还考虑机器人的运动特性和硬件特性;机器人速度为v,加速度为α,则机器人速度减速到0的距离S为:
考虑到雷达的探测距离和机器人减速安全距离,雷达到机器人边缘的距离为R,其减速距离为S:
模式一下还满足:
τhigh<d1;
τlow>(S+R);
其中,d1表示雷达最大的扫描距离;
S6步骤中,当机器人找到目标点时,若目标点附近没有障碍物,机器人按上述阈值可顺利到达目标点,若目标点距离障碍物太近,算法会检测到较大的障碍物强度,这样会导致机器人无法接近目标点,因此当机器人找到目标点时,无论窗口在模式一还是模式二情况下,均用第三阈值T3:
其中,Tn表示选定阈值,在模式一下Tn为T1,在模式二下Tn为T2;γ表示自定义值,本实施例中,γ=0.5,drt表示机器人距离目标点的距离。
S7:障碍物密度值大于阈值的为不可行的方向,障碍物密度值低于阈值的可行驶的方向,将连续可通行的扇区作为候选山谷;
将最靠近障碍物的扇区边界记为右边界kn;则左边界kf满足:
kf=kn+smax;
其中,smax表示最大扇区数;
连续扇区个数小于或者等于smax,其左边界均为kf,kf为一个通行区域距离最远的边界,若可通行连续扇区超过smax,则将超过的扇区作为其他候选区域。
S7步骤中,候选方向c满足公式:
S8:构造代价函数,选择代价最小的候选区域作为行进方向。
原VFH算法选择方向没有考虑到选择的方向机器人是否能达到,而且方向转变较大的情况下可能会造成机器人震荡,偏离规划好的路线,因此应用代价函数,让机器人根据代价函数选择行进的方向。
代价函数g(c)为:
λ1>λ2;
选择代价最小的一组作为行进的方向。完成一次路径规划。
下一轮路径规划首先根据窗口内障碍物总密度值选择窗口模式,再进行窗口内障碍物密度的计算,在进行上述步骤。
还包括一循环跳出机制,设置触发条件,当***陷入死循环,***启动循环跳出机制,自动选择一个可行进的方向,跳出当前循环。其中一种跳出机制是根据障碍物总密度值,如障碍物总密度值在一定时间内没有任何改变或者改变的程度较小,启动跳出机制,机器人沿着任何可行进的方向前进一段距离后,重新规划路径。
请参阅图2和图3,机器人到达目标点。
以上结合附图实施例对本发明进行了详细说明,本领域中普通技术人员可根据上述说明对本发明做出种种变化例。因而,实施例中的某些细节不应构成对本发明的限定,本发明将以所附权利要求书界定的范围作为本发明的保护范围。
Claims (8)
1.一种机器人规避障碍物的运动规划方法,包括步骤:
S1:设置初始窗口大小为W*W,地图类型为栅格地图,将所述地图的每一个栅格作为一个障碍物向量,所述障碍物向量包括大小mi,j和方向βi,j;
S2:计算所述初始窗口内的障碍物总密度值pn;
S3:根据所述障碍物总密度值选择窗口模式,当满足预设条件时,切换不同的所述窗口模式;
S4:根据角分辨率f,将正方形所述初始窗口分为若干个扇区,计算所述扇区个数n及障碍物所在扇区k;
S5:计算每个所述扇区的障碍物密度值,生成二维障碍物向量直方图;
S6:根据所述初始窗口的自适应阈值选择可行进的方向作为候选区域;
S7:所述障碍物密度值大于阈值的为不可行的方向,所述障碍物密度值低于阈值的可行驶的方向,将连续可通行的所述扇区作为候选山谷;
S8:构造代价函数,选择代价最小的所述候选区域作为行进方向;
所述S3步骤中,所述窗口模式包括模式一和模式二;根据所述障碍物总密度值判断障碍物密集情况,所述碍物密集情况包括障碍物稀疏和障碍物密集;当障碍物稀疏时,使用模式一的第一动态窗口,所述第一动态窗口的大小为W*W,W=14;碍物密集时,使用模式二的第二动态窗口,所述第二动态窗口的大小为Q*Q,Q计算公式为:
Q=int(((pmax-p1)*W)/pmax)
其中pmax为最大障碍物总密度值,p1为模式一下障碍物总密度值,W为模式一窗口大小范围;Q为模式二窗口大小范围;
所述S3步骤中最大障碍物总密度值pmax计算公式为:
对2种窗口模式设置不同的跳转条件,其跳转条件如下所示:
在所述模式一下:
ω为模式一临界跳转条件,当p1<ω时,模式保持不变,当p1≥ω时,切换模式为模式二,其中ω为经验值;
在所述模式二下:
q为模式二下临界跳转条件,当p2>q时,模式保持不变,当p2≤q时,切换模式为模式一;
其中μ为修正系数;
所述S6步骤中,当机器人未找到目标点时,
所述自适应阈值的选择根据所述扇区内障碍物密度来进行选择;不同所述窗口模式下所述自适应阈值选择方法不同;
所述模式一下所述自适应阈值采用第一阈值T1:
其中,τhigh表示第一高阈值,pi表示第i个扇区的障碍物总密度值,δ1表示第一临界条件,τlow表示第一低阈值;
所述模式二下所述自适应阈值采用第二阈值T2:
其中,ηhigh表示第二高阈值,ηlow表示第二低阈值,δ2表示第二临界条件;
所述自适应阈值的选择还考虑机器人的运动特性和硬件特性;机器人速度为v,加速度为α,则机器人速度减速到0的距离S为:
考虑到雷达的探测距离和机器人减速安全距离,雷达到机器人边缘的距离为R,其减速距离为S:
所述模式一下还满足:
τhigh<d1;
τlow>(S+R);
其中,d1表示雷达扫描最大距离;
所述S6步骤中,当机器人找到目标点时,所述自适应阈值采用第三阈值T3:
其中,Tn表示选定阈值,在模式一下Tn为T1,在模式二下Tn为T2;γ表示自定义值,drt表示机器人距离目标点的距离。
3.根据权利要求2所述的机器人规避障碍物的运动规划方法,其特征在于,所述S2步骤中,所述障碍物总密度值pn的计算公式为:
pn=∑i,jmi,j。
8.根据权利要求7所述的机器人规避障碍物的运动规划方法,其特征在于,还包括一种循环跳出机制,设置触发条件,当***陷入死循环,***启动所述循环跳出机制,自动选择一个可行进的方向,跳出当前循环。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110550658.XA CN113311862B (zh) | 2021-05-17 | 2021-05-17 | 机器人规避障碍物的运动规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110550658.XA CN113311862B (zh) | 2021-05-17 | 2021-05-17 | 机器人规避障碍物的运动规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113311862A CN113311862A (zh) | 2021-08-27 |
CN113311862B true CN113311862B (zh) | 2022-05-27 |
Family
ID=77373952
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110550658.XA Active CN113311862B (zh) | 2021-05-17 | 2021-05-17 | 机器人规避障碍物的运动规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113311862B (zh) |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8744196B2 (en) * | 2010-11-26 | 2014-06-03 | Hewlett-Packard Development Company, L.P. | Automatic recognition of images |
KR101987274B1 (ko) * | 2017-12-29 | 2019-09-27 | 한국과학기술연구원 | 장애물 재배치 위치 생성방법 |
CN109919026B (zh) * | 2019-01-30 | 2023-06-30 | 华南理工大学 | 一种水面无人艇局部路径规划方法 |
-
2021
- 2021-05-17 CN CN202110550658.XA patent/CN113311862B/zh active Active
Non-Patent Citations (1)
Title |
---|
基于势场的移动机器人自主导航研究综述;张捍东等;《自动化与仪表》;20070715(第04期);正文全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN113311862A (zh) | 2021-08-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108549385B (zh) | 一种结合a*算法和vfh避障算法的机器人动态路径规划方法 | |
CN113341984A (zh) | 基于改进rrt算法的机器人路径规划方法和装置 | |
CN112631294A (zh) | 一种移动机器人智能路径规划方法 | |
CN110632931A (zh) | 动态环境下基于深度强化学习的移动机器人避碰规划方法 | |
CN110928295A (zh) | 一种融合人工势场与对数蚁群算法的机器人路径规划方法 | |
CN111694364A (zh) | 一种应用于智能车路径规划的基于改进蚁群算法与动态窗口法的混合算法 | |
CN109597425B (zh) | 基于强化学习的无人机导航和避障方法 | |
CN112925315A (zh) | 一种基于改进蚁群算法和a*算法的履带车路径规划方法 | |
CN112731916A (zh) | 融合跳点搜索法和动态窗口法的全局动态路径规划方法 | |
CN110908377A (zh) | 一种机器人导航空间约简方法 | |
CN116804879B (zh) | 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 | |
CN111736592A (zh) | 路径决定装置、机器人以及路径决定方法 | |
CN115933648A (zh) | 一种机器人动态避障方法及*** | |
CN116360457A (zh) | 一种基于自适应栅格和改进a*-dwa融合算法的路径规划方法 | |
CN113311862B (zh) | 机器人规避障碍物的运动规划方法 | |
Miura et al. | Adaptive robot speed control by considering map and motion uncertainty | |
CN116448134B (zh) | 基于风险场与不确定分析的车辆路径规划方法及装置 | |
CN117029846A (zh) | 一种复杂环境下移动机器人的广义激光测距路径规划算法 | |
CN115690343A (zh) | 一种基于视觉跟随的机器人激光雷达扫描建图方法 | |
CN113341975B (zh) | 基于变目标函数萤火虫优化路径的机器人移动方法 | |
CN114995400A (zh) | 基于混合微粒群算法的移动机器人路径规划方法及*** | |
CN114740868A (zh) | 一种基于深度强化学习的移动机器人路径规划方法 | |
Wang et al. | AGRNav: Efficient and Energy-Saving Autonomous Navigation for Air-Ground Robots in Occlusion-Prone Environments | |
CN114167856A (zh) | 一种基于人工情感的服务机器人局部路径规划方法 | |
Dong et al. | Path Planning Research for Outdoor Mobile Robot |
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 | ||
TR01 | Transfer of patent right |
Effective date of registration: 20221220 Address after: 317600 Floor 2 and 3, No. 15, Huihai Road, Shamen Town, Yuhuan City, Taizhou City, Zhejiang Province Patentee after: Zhejiang Yisheng Intelligent Technology Co.,Ltd. Address before: 200240 No. 690, Jiangchuan Road, Shanghai, Minhang District Patentee before: SHANGHAI DIANJI University |
|
TR01 | Transfer of patent right |