CN112947434A - 一种基于局部最优卷积评价的机器人路径规划方法 - Google Patents
一种基于局部最优卷积评价的机器人路径规划方法 Download PDFInfo
- Publication number
- CN112947434A CN112947434A CN202110153481.XA CN202110153481A CN112947434A CN 112947434 A CN112947434 A CN 112947434A CN 202110153481 A CN202110153481 A CN 202110153481A CN 112947434 A CN112947434 A CN 112947434A
- Authority
- CN
- China
- Prior art keywords
- node
- robot
- path planning
- now
- mobile 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.)
- Granted
Links
- 238000011156 evaluation Methods 0.000 title claims abstract description 99
- 238000000034 method Methods 0.000 title claims abstract description 58
- 239000013598 vector Substances 0.000 claims abstract description 79
- 230000007613 environmental effect Effects 0.000 claims abstract description 6
- 230000000007 visual effect Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000006467 substitution reaction Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 45
- 238000010586 diagram Methods 0.000 description 7
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000010845 search algorithm Methods 0.000 description 4
- 230000007547 defect Effects 0.000 description 3
- 238000011478 gradient descent method Methods 0.000 description 3
- 238000007689 inspection Methods 0.000 description 3
- 230000007423 decrease Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000005094 computer simulation Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000002699 waste material Substances 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- 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)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Numerical Control (AREA)
Abstract
本发明公开了一种基于局部最优卷积评价的机器人路径规划方法,包括:移动机器人建立工作环境的二维网格化地图,确定出发点与目标点位置;移动机器人感知当前位置周围的环境信息,从候选移动方向中筛选出可行移动方向;计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影,获取局部最优移动方向;利用卷积评价指标检验局部最优移动方向的合理性;在初次路径规划未能找到可行路径时,进行二次路径规划,并取消对局部最优移动方向合理性的检验环节。本发明能够实现移动机器人在未知环境下的路径规划,决策思路符合移动机器人的实际运行特点,具有运算量小、遍历性强、运行效率高的优点。
Description
技术领域
本发明属于机器人学及导航学的技术领域,尤其涉及一种基于局部最优卷积评价的机器人路径规划方法。
背景技术
移动机器人的路径规划问题是机器人技术在消防救灾、医疗服务、工业巡检等众多领域投入应用所必须攻破的难点。目前广泛应用于移动机器人路径规划的方法主要有A*算法、蚁群算法、人工势场法等。A*算法建立在机器人拥有全局环境地图的基础上,如果直接应用于未知环境中,将因规划过程中路径的跳变使规划效率变得极低;蚁群算法需要生成大量虚拟个体对地图进行探索,一定程度上仅适合在实验仿真中使用,难以具备实际操作性;人工势场法存在易陷入局部平衡点的问题,尽管目前针对人工势场法存在众多优化方案,但在复杂大环境下的路径规划鲁棒性仍然存在缺陷。
经过对上述及其它路径规划方法的研究,发现深度优先搜索算法从行为上符合移动机器人在未知环境中的工作特点,但传统深度优先搜索算法不具备启发性,且仅能进行四个方向的移动,遍历性强但效率低,效果也不理想。
所以,需要一个新的技术方案来解决这个问题。
发明内容
发明目的:为了克服现有技术中存在的不足,提供一种基于局部最优卷积评价的机器人路径规划方法,以解决现有方法对移动机器人在未知环境中的路径规划运行效率低、鲁棒性差、难以投入实际应用的问题。
技术方案:为实现上述目的,本发明提供一种基于局部最优卷积评价的机器人路径规划方法,包括如下步骤:
S1:建立移动机器人工作环境的二维网格化地图,确定出发点与目标点位置;
S2:移动机器人感知相邻节点的环境信息,并从8个候选移动方向中筛选出可行移动方向;
S3:若存在可行移动方向,则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值,选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向作为局部最优移动方向;若不存在可行移动方向,则移动机器人认定是否需要返回父节点;
S4:通过局部最优卷积评价方法判断局部最优移动方向是否具备合理性,若具备合理性,移动机器人向局部最优移动方向移动至相邻节点;若不具备合理性,移动机器人认定需要返回父节点;
S5:当移动机器人认定需要返回父节点时,若当前节点不是出发点,即当前节点存在父节点,则移动机器人正常返回父节点,并称机器人进行了一次“后退”操作。;若当前节点是出发点,即当前节点不存在父节点,如果当前阶段为初次路径规划,则初次路径规划结束,进行二次路径规划;如果当前阶段为二次路径规划,则表明不存在可行路径,路径规划失败;
S6:路径规划开始时移动机器人处于初次路径规划阶段。移动机器人在进行初次路径规划时,重复上述步骤S2至步骤S4,直至到达目标点或进入二次路径规划阶段;移动机器人在进行二次路径规划时,重复上述步骤S2至步骤S3,并且不检验局部最优移动方向的合理性,直接向局部最优移动方向进行移动,直至到达目标点或路径规划失败。
进一步地,所述步骤S1具体为:
A1:建立移动机器人工作环境的二维网格化地图,地图中的每一网格为地图的一个节点,节点形状为正方形,取正右方为横轴正方向,正上方为纵轴正方向;
A2:为各节点添加位置属性Location=(m,n),m为节点横坐标,n为节点纵坐标;由于每个节点与它的位置属性一一对应,因此若节点N的位置属性为(m1,n1),则节点N亦可基于它的位置属性称为为节点(m1,n1);m与n的取值范围为整数集,出发点的位置属性为(x0,y0),目标点的位置属性为(0,0);
A3:为各节点添加类型属性Type,节点N的相邻节点定义为与节点N存在边接触或顶点接触的节点,则每个节点共有8个相邻节点;类型属性用于在移动机器人进行决策时,判断8个相邻节点对应的移动方向是否为可行移动方向,类型属性Type的取值范围为{Unknown,Unvisited,Visited,Obstacle},其中Unknown代表节点尚未被探索过,Unvisited代表节点不存在障碍物且未到达过,Visited代表节点不存在障碍物且已到达过,Obstacle代表节点存在障碍物;初始时刻出发点的类型属性为Visited,地图内部所有其他节点的类型属性均为Unknown,地图边界的所有节点的类型属性均为Obstacle;
A4:为各节点添加父节点属性Father;节点N的父节点定义为机器人在工作过程中通过“前进”操作进入节点N所处的上一个节点,父节点属性用于记录节点的父节点,父节点属性Father的取值范围为{0,1,2,3,4,5,6,7,8},其中0代表节点不存在父节点,1、2、3、4、5、6、7、8分别代表父节点为正右方、右上方、正上方、左上方、正左方、左下方、正下方、右下方的相邻节点,初始时刻所有节点的父节点属性均为0。
进一步地,所述步骤S2中候选移动方向的数量为8个,可行移动方向的帅选包括:
式中,i∈{1,2,3,4,5,6,7,8},当前节点Now共有8个相邻节点,且每个候选移动方向向量分别指向1个相邻节点的中心,因此候选移动方向向量与相邻节点一一对应;
B2:记当前节点Now在候选移动方向向量方向上的相邻节点为Ni,当前节点Now在候选移动方向向量顺时针旋转45°方向上的相邻节点为Ni-,当前节点Now在候选移动方向向量逆时针旋转45°方向上的相邻节点为Ni+,它们的类型属性分别为Ni.Type、Ni-.Type、Ni+.Type;
B3:机器人对当前节点Now的8个相邻节点中类型属性为Unknown的所有节点N',根据感知到的环境信息将节点N'的类型属性N'.Type赋为Unvisited或Obstacle;
B4:机器人根据以下规则,计算可行性判断函数Feasible(i)的值:
当i为奇数时,若满足
Ni.Type=Unvisited
则
Feasible(i)=True
否则
Feasible(i)=Flase
当i为偶数时,若同时满足以下三个条件
Ni.Type=Unvisited
Ni-.Type≠Obstacle
Ni+.Type≠Obstacle
则
Feasible(i)=True
否则
Feasible(i)=Flase
B5:在当前节点Now处,从集合{1,2,3,4,5,6,7,8}中选取能使
Feasible(o)=True
进一步地,所述步骤S3具体为:若存在可行移动方向,则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值,选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向为局部最优移动方向;若不存在可行移动方向,则移动机器人认定是否需要返回父节点。
进一步地,所述步骤S3具体包括如下步骤:
C1:若集合D为空集,则移动机器人认定需要返回父节点;
C2:若集合D为非空集,设当前节点Now的位置属性Now.Location为(x1,y1),即
Now.Location=(x1,y1)
定义目标点距离函数Distance为
目标点距离函数Distance的值即为当前节点Now与目标点之间的距离;
C3:根据以下公式(3),计算目标点距离函数的梯度ΔDistance:
C5:机器人在当前节点Now处,对于集合D中的每个元素o,分别计算相应的单位移动得分Score(o)后,取使Score(o)最大的元素o,记为Im,将Im带入步骤B1中式(1)计算得到的向量为局部最优移动方向向量,向量的方向为局部最优移动方向,将Im带入步骤C4中式(5)计算得到局部最优单位移动得分Sm,即
进一步地,所述步骤S4具体为:
D1:定义合理性阈值ET,ET是一个可调参数,取值范围为-1到1之间的全体实数,不含-1和1;定义卷积评价指标E,初始时刻E=1;
D2:当机器人在当前节点Now处计算出局部最优单位移动得分Sm后,根据以下公式(6)计算预期卷积评价指标Ep:
Ep=(1-Kp)·E+Kp·Sm (6)
式中Kp为实时评价权重,是一个常数,根据以下公式(7)确定
Kp=1-e-p (7)
式中e为自然常数;p为评价聚焦度,是一个可调参数,取值范围为全体正实数;
D3:若预期卷积评价指标Ep小于合理性阈值ET,即
Ep<ET
则认定局部最优移动方向不具备合理性,机器人认定需要返回父节点;
D4:若预期卷积评价指标Ep大于或等于合理性阈值,即
Ep≥ET
进一步地,所述步骤C1中移动机器人认定需要返回父节点的方法为:
定义有效移动步数Step,初始时刻Step=0;定义有效移动得分函数s(x),初始时刻对全体负实数x有s(x)=1、对全体非负实数x有s(x)=0;定义预期移动得分函数sp(x),当移动机器人在当前节点Now处计算出局部最优单位移动得分Sm后,预期移动得分函数sp(x)的函数值为在有效移动得分函数s(x)的基础上,将x∈[Step,Step+1)的部分赋为Sm,即依次进行以下两步赋值操作:
sp(x)←s(x),x∈R
sp(x)←Sm,x∈[Step,Step+1)
其中R为实数集;
记当前节点Now的父节点属性Now.Father为F,即
F=Now.Father
机器人在认定需要返回父节点时,若当前节点Now不为出发点,即
F≠0
E1:将有效移动得分函数s(x)在x∈[Step-1,Step)上的函数值赋为0,即
s(x)←0,x∈[Step-1,Step)
E2:将有效移动步数Step自减1,即
Step←Step-1
E3:设父节点NF的位置属性NF.Location为(x',y'),即
NF.Location=(x',y')
并根据以下公式(9)为卷积评价指标E赋值:
式中
E4:将当前节点Now赋为父节点NF,即
Now←NF
机器人在认定需要返回父节点时,若当前节点Now为出发点,即
F=0
如果当前阶段为初次路径规划,则结束初次路径规划,所有数据恢复至初始时刻,并进行二次路径规划性;如果当前阶段为二次路径规划,则表明没有可行路径,路径规划失败。
进一步地,所述步骤D4具体为:
mod表示前者对后者进行取模运算;
F3:将有效移动步数Step自增1,即
Step←Step+1
F4:将卷积评价指标E赋为预期卷积评价指标Ep,即
E←Ep
F5:将有效移动得分函数s(x)的函数值赋为预期移动得分函数sp(x),即
s(x)←sp(x),x∈R
进一步地,所述步骤D2具体为:
根据以下公式(11)定义评价权重函数k(x),其中p为评价聚焦度
卷积评价指标E的定义式为:
E=(k(x)*s(x))|x=step (12)
预期卷积评价指标Ep的定义式为:
Ep=(k(x)*sp(x))|x=Step+1 (13)
即卷积评价指标E是评价权重函数k(x)与有效移动得分函数s(x)的卷积在Step处的值,预期卷积评价指标Ep是评价权重函数k(x(与预期移动得分函数sp(x)的卷积在Step+1处的值;
又由于
因此
由此得到步骤D2中的迭代计算式即式(6),因此移动机器人在实际工作中无需根据定义式记录有效移动得分函数s(x)、预期移动得分函数sp(x)与评价权重函数k(x),再进行卷积计算从而得到卷积评价指标E与预期卷积评价指标Ep,而仅需按照步骤D2,根据评价聚焦度p计算出实时评价权重Kp,并利用实时评价权重Kp结合局部最优单位移动得分Sm对卷积评价指标E与预期卷积评价指标Ep进行反复迭代。
卷积评价指标E有以下特点:
①值域为(-1,1];
②机器人进行的每一次有效移动的局部最优单位移动得分都会对E产生影响,且越早的有效移动影响权重越小,越晚的有效移动影响权重越大;
③当机器人进行的每一次有效移动的局部最优单位移动得分均为1时,有
E=1
④评价聚焦度p越大,早期有效移动的影响权重越小,晚期有效移动的影响权重越大,即机器人更多地聚焦于最近几次有效移动的局部最优单位移动得分。
预期卷积评价指标Ep的特点与卷积评价指标E类似,区别在于会将试图进行的下一次移动的局部最优单位移动得分也纳入考虑,且影响权重最大。
采用局部最优卷积评价方法检验局部最优移动方向的合理性的意义在于,由于当某一步前期的决策中可行移动方向较少且均背离目标点方向时,会导致得到的局部最优单位移动得分极低,将机器人导向歧途,从而导致后续路径规划效率降低,因此需要对后期局部最优单位移动得分施加更大的权重进行考虑,并适时选择返回。
采用此方法检验局部最优移动方向的合理性的缺点在于当可行路径较少,且均存在部分路段局部最优单位移动得分极低时,机器人会在半途时将所有可行路径均舍弃,因此需要进行二次路径规划并取消局部最优卷积评价环节,对全局地图进行遍历式的探索。
本发明在深度优先搜索算法的基础上,引入梯度下降方法使其具备启发性,并将移动方向拓展至八个方向,同时创造了局部最优卷积评价方法使移动机器人能够自行判断路径合理性并主动纠正,从而提出了一种基于局部最优卷积评价的机器人路径规划方法。本发明具有运算量小、遍历性强、运行效率高的优点,对实现移动机器人在未知环境中的导航功能具有较好的理论价值及意义。
有益效果:本发明与现有技术相比,在深度优先搜索算法的基础上,引入梯度下降方法使其具备启发性,并将移动方向拓展至八个方向,同时创造了局部最优卷积评价方法使移动机器人能够自行判断路径合理性并主动纠正;在未知环境中,用目标点距离函数负梯度方向向量做启发式引导,按照深度优先的探索规则提高遍历性,同时创造了局部最优卷积评价方法避免机器人因前期的局部最优而陷入后期歧途,并且对传感器要求低,能适应极其狭窄的感知范围,极大地提高了移动机器人在未知环境中进行路径规划的效率、鲁棒性与可操作性,对于移动机器人在消防救灾、医疗服务、工业巡检等众多领域的实际应用具有很好的工程价值及意义。
附图说明
图1是本发明实施例提供的一种基于局部最优卷积评价的机器人路径规划方法的工作流程示意图;
图2是本发明实施例提供的全局地图;
图3是本发明实施例提供的机器人视角地图;
图4是本发明实施例提供的移动机器人工作过程中单步决策结果示意图;
图5是本发明实施例提供的单步决策过程中所生成的所有候选移动方向示意图;
图6是本发明实施例提供的单步决策过程中所生成的可行移动方向示意图;
图7是本发明实施例提供的单步决策过程中目标点距离函数负梯度方向向量示意图;
图8是本发明实施例提供的机器人在包含卷积评价阶段情况下完成路径规划的最终结果图;
图9是本发明实施例提供的机器人在不包含卷积评价阶段情况下完成路径规划的最终结果图。
具体实施方式
下面结合附图和具体实施例,进一步阐明本发明,应理解这些实施例仅用于说明本发明而不用于限制本发明的范围,在阅读了本发明之后,本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
本发明提供一种基于局部最优卷积评价的机器人路径规划方法,如图1所示,包括以下步骤:
步骤1:建立移动机器人工作环境的二维网格化地图,确定出发点与目标点位置。
步骤2:移动机器人感知相邻节点的环境信息,并从8个候选移动方向中筛选出可行移动方向。
步骤3:若存在可行移动方向,则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值,选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向为局部最优移动方向;若不存在可行移动方向,则移动机器人认定需要返回父节点。
步骤4:通过局部最优卷积评价方法判断局部最优移动方向是否具备合理性,具体实施方法为:计算局部最优移动方向的预期卷积评价指标,再与合理性阈值进行比较,若预期卷积评价指标大于或等于合理性阈值,则认定局部最优移动方向具备合理性,移动机器人向局部最优移动方向移动至相邻节点,并称机器人进行了一次“前进”操作;若预期卷积评价指标小于合理性阈值,则认定局部最优移动方向不具备合理性,移动机器人认定需要返回父节点。
步骤5:当移动机器人认定需要返回父节点时,若当前节点不是出发点,即当前节点存在父节点,则移动机器人正常返回父节点,并称机器人进行了一次“后退”操作。若当前节点是出发点,即当前节点不存在父节点,如果当前阶段为初次路径规划,则初次路径规划结束,进行二次路径规划;如果当前阶段为二次路径规划,则表明不存在可行路径,路径规划失败。
步骤6:路径规划开始时移动机器人处于初次路径规划阶段。移动机器人在进行初次路径规划时,重复上述步骤2至步骤4,直至到达目标点或进入二次路径规划阶段;移动机器人在进行二次路径规划时,重复上述步骤2至步骤3,并且不检验局部最优移动方向的合理性,直接向局部最优移动方向进行移动,直至到达目标点或路径规划失败。
基于上述技术方案,本实施例将该基于局部最优卷积评价的机器人路径规划方法进行实例应用,本实施例中使用的IDE为Visual Studio 2017,基于C++语言进行仿真,生成102*102的网格化地图(地图最外侧网格为边界),在地图内部随机生成密度为30%的障碍物,设定右上角节点为出发点,左下角节点为目标点,并使用openCV库将地图可视化。本方法可应用于消防救灾、医疗服务、工业巡检等众多领域,结合附图2~9,以下对具体的过程进行说明:
在本发明实施例中,随机生成的全局地图如图2所示,移动机器人将在该全局地图中完成路径规划工作,机器人感知地图如图3所示,其中地图内部右上角节点为出发点,地图内部左下角节点为目标点,浅灰色节点为未探索过的节点,黑色节点为存在障碍物的节点,白色节点为无障碍物且未到达过的节点,深灰色节点为无障碍物且已到达过的节点。
步骤1包括:
步骤1.1:建立移动机器人工作环境的二维网格化地图,地图中的每一网格为地图的一个节点,节点形状为正方形,取正右方为横轴正方向,正上方为纵轴正方向。
步骤1.2:为各节点添加位置属性Location=(m,n),m为节点横坐标,n为节点纵坐标。由于每个节点与它的位置属性一一对应,因此若节点N的位置属性为(m1,n1),则节点N亦可基于它的位置属性称为为节点(m1,n1)。m与n的取值范围为整数集,出发点的位置属性为(99,99),目标点的位置属性为(0,0)。
步骤1.3∶为各节点添加类型属性Type。节点N的相邻节点定义为与节点N存在边接触或顶点接触的节点,则每个节点共有8个相邻节点。类型属性用于在移动机器人进行决策时,判断8个相邻节点对应的移动方向是否为可行移动方向。类型属性Type的取值范围为{Unknown,Unvisited,Visited,Obstacle},其中Unknown代表节点尚未被探索过,Unvisited代表节点不存在障碍物且未到达过,Visited代表节点不存在障碍物且已到达过,Obstacle代表节点存在障碍物。初始时刻出发点的类型属性为Visited,地图内部所有其他节点的类型属性均为Unknown,地图边界的所有节点的类型属性均为Obstacle。
步骤1.4:为各节点添加父节点属性Father。节点N的父节点定义为机器人在工作过程中通过“前进”操作进入节点N所处的上一个节点。父节点属性用于记录节点的父节点,父节点属性Father的取值范围为{0,1,2,3,4,5,6,7,8},其中0代表节点不存在父节点,1、2、3、4、5、6、7、8分别代表父节点为正右方、右上方、正上方、左上方、正左方、左下方、正下方、右下方的相邻节点,初始时刻所有节点的父节点属性均为0。
参照图4箭头所示的决策过程中,步骤2,包括:
步骤2.1:当前节点为节点(4,9)(即箭头起点所在的深灰色节点),取横轴正方向的单位向量为向量,按逆时针方向将向量每次旋转45°,依次得 向量,这8个向量为候选移动方向向量,如图5箭头所示,它们的方向为候选移动方向,数值可根据以下公式计算:
式中i∈{1,2,3,4,5,6,7,8}。当前节点Now共有8个相邻节点,且每个候选移动方向向量分别指向1个相邻节点的中心,因此候选移动方向向量与相邻节点一一对应。
步骤2.2:记当前节点Now在候选移动方向向量方向上的相邻节点为Ni,当前节点Now在候选移动方向向量顺时针旋转45°方向上的相邻节点为Ni-,当前节点Now在候选移动方向向量逆时针旋转45°方向上的相邻节点为Ni+,它们的类型属性分别为Ni.Type、Ni-.Type、Ni+.Type。
步骤2.3:机器人对当前节点Now的8个相邻节点中类型属性为Unknown的所有节点N',根据感知到的环境信息将节点N'的类型属性N'.Type赋为Unvisited或Obstacle。
步骤2.4:机器人根据以下规则,计算可行性判断函数Feasible(i)的值:
(1)当i为奇数时,若满足
Ni.Type=Unvisited
则
Feasible(i)=True
否则
Feasible(i)=Flase
(2)当i为偶数时,若同时满足以下三个条件
Ni.Type=Unvisited
Ni-.Type≠Obstacle
Ni+.Type≠Obstacle
则
Feasible(i)=True
否则
Feasible(i)=Flase
步骤2.5:在当前节点Now处,从集合{1,2,3,4,5,6,7,8}中选取能使
Feasible(o)=True
的元素o,构成集合D,将o带入步骤2.1中式1计算得到的向量为当前节点Now处的可行移动方向向量,向量的方向为当前节点Now处的可行移动方向。对于本实施例的决策过程,D={1,5,6,7},对应的可行移动方向如图6箭头所示。
参照图4箭头所示的决策过程中,步骤3包括:
步骤3.1:由于不满足集合D为空集,故移动机器人无需返回父节点;
步骤3.2:由于满足集合D非空集,设当前节点Now的位置属性Now.Location为(x1,y1),即
Now.Location=(x1,y1)
定义目标点距离函数Distance为
目标点距离函数Distance的值即为当前节点Now与目标点之间的距离。
如图7中箭头所示。
步骤3.5:机器人在当前节点Now处,对于集合D中的每个元素o,分别计算相应的单位移动得分Score(o)后,取使Score(o)最大的元素o,记为Im。将Im带入步骤2.1中式1计算得到的向量为局部最优移动方向向量,向量的方向为局部最优移动方向,将Im带入步骤3.4中式5计算得到局部最优单位移动得分Sm,即
对于本实施例的决策过程,有
Im=6
Sm=0.933
参照图4箭头所示的决策过程中,步骤4包括:
步骤4.1:对于本实施例的决策过程,合理性阈值ET设置为0,卷积评价指标E在实施移动前为0.721。
步骤4.2:当机器人在当前节点Now处计算出局部最优单位移动得分Sm=0.933后,根据以下公式计算预期卷积评价指标Ep:
Ep=(1-Kp)·E+Kp·Sm (6)
式中,Kp为实时评价权重,是一个常数,根据以下公式确定:
Kp=1-e-p (7)
式中,e为自然常数;p为评价聚焦度,是一个可调参数,取值范围为全体正实数。
对于本实施例的决策过程,评价聚焦度p设置为0.1,则
Kp=1-e-p=0.09516
Ep=(1-Kp)·E+Kp·Sm=0.741
步骤4.3:由于不满足预期卷积评价指标小于合理性阈值,即
Ep<ET
不成立,故机器人不认定需要返回父节点。
步骤4.4:由于满足预期卷积评价指标大于或等于合理性阈值,即
Ep≥ET
重复上述过程,最终机器人到达目标点。
通过上述技术方案的实施,本发明的优点是:(1)提供了移动机器人在未知环境下的路径规划方法;(2)对机器人感知范围要求低,降低机器人传感器所需成本;(3)采用二维网格化地图,数据量小,运算速度快,降低机器人处理器所需成本;(4)创造了局部最优卷积评价方法,提高了移动机器人进行路径规划的总体效率;(5)决策流程符合机器人实际运行状态,不仅可实现计算机仿真,且易于投入实际应用。
为展示优点(4)的具体表现,本发明还提供了与上述实施例全局地图相同但机器人决策过程中不含局部最优卷积评价方法的路径规划结果对比。图8所示为机器人决策过程包含局部最优卷积评价方法的最终路径规划结果,机器人花费的总路程为1372.2;图9所示为机器人决策过程不含局部最优卷积评价方法的最终路径规划结果,机器人花费的总路程为2155.2。由此可见,在上述实施例中,局部最优卷积评价方法有效地避免了移动机器人在地图底部错误地向右侧探索而浪费大量的路程,从而较大幅度地提高了路径规划的效率。
Claims (9)
1.一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,包括如下步骤:
S1:建立移动机器人工作环境的二维网格化地图,确定出发点与目标点位置;
S2:移动机器人感知相邻节点的环境信息,从候选移动方向中筛选出可行移动方向;
S3:若存在可行移动方向,则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值,选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向作为局部最优移动方向;若不存在可行移动方向,则移动机器人认定是否需要返回父节点;
S4:通过局部最优卷积评价方法判断局部最优移动方向是否具备合理性,若具备合理性,移动机器人向局部最优移动方向移动至相邻节点;若不具备合理性,移动机器人认定需要返回父节点;
S5:当移动机器人认定需要返回父节点时,若当前节点不是出发点,即当前节点存在父节点,则移动机器人正常返回父节点;若当前节点是出发点,即当前节点不存在父节点,如果当前阶段为初次路径规划,则初次路径规划结束,进行二次路径规划;如果当前阶段为二次路径规划,则表明不存在可行路径,路径规划失败;
S6:路径规划开始时移动机器人处于初次路径规划阶段。移动机器人在进行初次路径规划时,重复上述步骤S2至步骤S4,直至到达目标点或进入二次路径规划阶段;移动机器人在进行二次路径规划时,重复上述步骤S2至步骤S3,并且不检验局部最优移动方向的合理性,直接向局部最优移动方向进行移动,直至到达目标点或路径规划失败。
2.根据权利要求1所述的一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,所述步骤S1具体为:
A1:建立移动机器人工作环境的二维网格化地图,地图中的每一网格为地图的一个节点,节点形状为正方形,取正右方为横轴正方向,正上方为纵轴正方向;
A2:为各节点添加位置属性Location=(m,n),m为节点横坐标,n为节点纵坐标;由于每个节点与它的位置属性一一对应,因此若节点N的位置属性为(m1,n1),则节点N亦可基于它的位置属性称为为节点(m1,n1);m与n的取值范围为整数集,出发点的位置属性为(x0,y0),目标点的位置属性为(0,0);
A3:为各节点添加类型属性Type,节点N的相邻节点定义为与节点N存在边接触或顶点接触的节点,则每个节点共有8个相邻节点;类型属性用于在移动机器人进行决策时,判断8个相邻节点对应的移动方向是否为可行移动方向,类型属性Type的取值范围为{Unknown,Unvisited,Visited,Obstacle},其中Unknown代表节点尚未被探索过,Unvisited代表节点不存在障碍物且未到达过,Visited代表节点不存在障碍物且已到达过,Obstacle代表节点存在障碍物;初始时刻出发点的类型属性为Visited,地图内部所有其他节点的类型属性均为Unknown,地图边界的所有节点的类型属性均为Obstacle;
A4:为各节点添加父节点属性Father;节点N的父节点定义为机器人在工作过程中进入节点N所处的上一个节点,父节点属性用于记录节点的父节点,父节点属性Father的取值范围为{0,1,2,3,4,5,6,7,8},其中0代表节点不存在父节点,1、2、3、4、5、6、7、8分别代表父节点为正右方、右上方、正上方、左上方、正左方、左下方、正下方、右下方的相邻节点,初始时刻所有节点的父节点属性均为0。
3.根据权利要求1所述的一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,所述步骤S2中候选移动方向的数量为8个,可行移动方向的帅选包括:
式中,i∈{1,2,3,4,5,6,7,8},当前节点Now共有8个相邻节点,且每个候选移动方向向量分别指向1个相邻节点的中心,因此候选移动方向向量与相邻节点一一对应;
B2:记当前节点Now在候选移动方向向量方向上的相邻节点为Ni,当前节点Now在候选移动方向向量顺时针旋转45°方向上的相邻节点为Ni-,当前节点Now在候选移动方向向量逆时针旋转45°方向上的相邻节点为Ni+,它们的类型属性分别为Ni.Type、Ni-.Type、Ni+.Type;
B3:机器人对当前节点Now的8个相邻节点中类型属性为Unknown的所有节点N′,根据感知到的环境信息将节点N′的类型属性N′.Type赋为Unvisited或Obstacle;
B4:机器人根据以下规则,计算可行性判断函数Feasible(i)的值:
当i为奇数时,若满足
Ni.Type=Unvisited
则
Feasible(i)=True
否则
Feasible(i)=Flase
当i为偶数时,若同时满足以下三个条件
Ni.Type=Unvisited
Ni-.Type≠Obstacle
Ni+.Type≠Obstacle
则
Feasible(i)=True
否则
Feasible(i)=Flase
B5:在当前节点Now处,从集合{1,2,3,4,5,6,7,8}中选取能使
4.根据权利要求1所述的一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,所述步骤S3具体为:若存在可行移动方向,则计算各可行移动方向上的单位向量在目标点距离函数负梯度方向上的投影值,选取单位方向向量在目标点距离函数负梯度方向上的投影值最大的可行移动方向为局部最优移动方向;若不存在可行移动方向,则移动机器人认定是否需要返回父节点。
5.根据权利要求4所述的一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,所述步骤S3具体包括如下步骤:
C1:若集合D为空集,则移动机器人认定需要返回父节点;
C2:若集合D为非空集,设当前节点Now的位置属性Now.Location为(x1,y1),即
Now.Location=(x1,y1)
定义目标点距离函数Distance为
目标点距离函数Distαnce的值即为当前节点Now与目标点之间的距离;
C5:机器人在当前节点Now处,对于集合D中的每个元素o,分别计算相应的单位移动得分Score(o)后,取使Score(o)最大的元素o,记为Im,将Im带入步骤B1中式(1)计算得到的向量为局部最优移动方向向量,向量的方向为局部最优移动方向,将Im带入步骤C4中式(5)计算得到局部最优单位移动得分Sm,即
6.根据权利要求1所述的一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,所述步骤S4具体为:
D1:定义合理性阈值ET,ET是一个可调参数,取值范围为-1到1之间的全体实数,不含-1和1;定义卷积评价指标E,初始时刻E=1;
D2:当机器人在当前节点Now处计算出局部最优单位移动得分Sm后,根据以下公式(6)计算预期卷积评价指标Ep:
Ep=(1-Kp)·E+Kp·Sm (6)
式中Kp为实时评价权重,是一个常数,根据以下公式(7)确定
Kp=1-e-p (7)
式中e为自然常数;p为评价聚焦度,是一个可调参数,取值范围为全体正实数;
D3:若预期卷积评价指标Ep小于合理性阈值ET,即
Ep<ET
则认定局部最优移动方向不具备合理性,机器人认定需要返回父节点;
D4:若预期卷积评价指标Ep大于或等于合理性阈值,即
Ep≥ET
7.根据权利要求5所述的一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,所述步骤C1中移动机器人认定需要返回父节点的方法为:
定义有效移动步数Step,初始时刻Step=0;定义有效移动得分函数s(x),初始时刻对全体负实数x有s(x)=1、对全体非负实数x有s(x)=0;定义预期移动得分函数sp(x),当移动机器人在当前节点Now处计算出局部最优单位移动得分Sm后,预期移动得分函数sp(x)的函数值为在有效移动得分函数s(x)的基础上,将x∈[Step,Step+1)的部分赋为Sm,即依次进行以下两步赋值操作:
sp(x)←s(x),x∈R
sp(x)←Sm,x∈[Step,Step+1)
其中R为实数集;
记当前节点Now的父节点属性Now.Father为F,即
F=Now.Father
机器人在认定需要返回父节点时,若当前节点Now不为出发点,即
F≠0
E1:将有效移动得分函数s(x)在x∈[Step-1,Step)上的函数值赋为0,即
s(x)←0,x∈[Step-1,Step)
E2:将有效移动步数Step自减1,即
Step←Step-1
E3:设父节点NF的位置属性NF.Location为(x′,y′),即
NF.Location=(x′,y′)
并根据以下公式(9)为卷积评价指标E赋值:
式中
E4:将当前节点Now赋为父节点NF,即
Now←NF
机器人在认定需要返回父节点时,若当前节点Now为出发点,即
F=0
如果当前阶段为初次路径规划,则结束初次路径规划,所有数据恢复至初始时刻,并进行二次路径规划性;如果当前阶段为二次路径规划,则表明没有可行路径,路径规划失败。
8.根据权利要求6所述的一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,所述步骤D4具体为:
mod表示前者对后者进行取模运算;
F3:将有效移动步数Step自增1,即
Step←Step+1
F4:将卷积评价指标E赋为预期卷积评价指标Ep,即
E←Ep
F5:将有效移动得分函数s(x)的函数值赋为预期移动得分函数sp(x),即
s(x)←sp(x),x∈R
9.根据权利要求6所述的一种基于局部最优卷积评价的机器人路径规划方法,其特征在于,所述步骤D2具体为:
根据以下公式(11)定义评价权重函数k(x),其中p为评价聚焦度
卷积评价指标E的定义式为:
E=(k(x)*s(x))|x=Step (12)
预期卷积评价指标Ep的定义式为:
Ep=(k(x)*sp(x))|x=Step+1 (13)
即卷积评价指标E是评价权重函数k(x)与有效移动得分函数s(x)的卷积在Step处的值,预期卷积评价指标Ep是评价权重函数k(x)与预期移动得分函数sp(x)的卷积在Step+1处的值;
又由于
因此
由此得到步骤D2中的迭代计算式即式(6),因此移动机器人在实际工作中无需根据定义式记录有效移动得分函数s(x)、预期移动得分函数sp(x)与评价权重函数k(x),再进行卷积计算从而得到卷积评价指标E与预期卷积评价指标Ep,而仅需按照步骤D2,根据评价聚焦度p计算出实时评价权重Kp,并利用实时评价权重Kp结合局部最优单位移动得分Sm对卷积评价指标E与预期卷积评价指标Ep进行反复迭代。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110153481.XA CN112947434B (zh) | 2021-02-04 | 2021-02-04 | 一种基于局部最优卷积评价的机器人路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110153481.XA CN112947434B (zh) | 2021-02-04 | 2021-02-04 | 一种基于局部最优卷积评价的机器人路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112947434A true CN112947434A (zh) | 2021-06-11 |
CN112947434B CN112947434B (zh) | 2024-02-27 |
Family
ID=76243683
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110153481.XA Active CN112947434B (zh) | 2021-02-04 | 2021-02-04 | 一种基于局部最优卷积评价的机器人路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112947434B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103365293A (zh) * | 2013-05-08 | 2013-10-23 | 北京大学深圳研究生院 | 一种基于动态区域划分的机器人安全路径规划方法 |
CN104159265A (zh) * | 2014-07-24 | 2014-11-19 | 镇江智拓智能科技发展有限公司 | 一种面向低速无线自组网的数据传输方法 |
CN106444769A (zh) * | 2016-10-31 | 2017-02-22 | 湖南大学 | 一种室内移动机器人增量式环境信息采样的最优路径规划方法 |
CN107764264A (zh) * | 2016-08-19 | 2018-03-06 | 德韧营运有限责任公司 | 用于机动车辆的自动辅助泊车系统的最优路径规划器 |
CN109239659A (zh) * | 2018-08-31 | 2019-01-18 | 平安科技(深圳)有限公司 | 室内导航方法、装置、计算机设备及存储介质 |
-
2021
- 2021-02-04 CN CN202110153481.XA patent/CN112947434B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103365293A (zh) * | 2013-05-08 | 2013-10-23 | 北京大学深圳研究生院 | 一种基于动态区域划分的机器人安全路径规划方法 |
CN104159265A (zh) * | 2014-07-24 | 2014-11-19 | 镇江智拓智能科技发展有限公司 | 一种面向低速无线自组网的数据传输方法 |
CN107764264A (zh) * | 2016-08-19 | 2018-03-06 | 德韧营运有限责任公司 | 用于机动车辆的自动辅助泊车系统的最优路径规划器 |
CN106444769A (zh) * | 2016-10-31 | 2017-02-22 | 湖南大学 | 一种室内移动机器人增量式环境信息采样的最优路径规划方法 |
CN109239659A (zh) * | 2018-08-31 | 2019-01-18 | 平安科技(深圳)有限公司 | 室内导航方法、装置、计算机设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN112947434B (zh) | 2024-02-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110887484B (zh) | 基于改进遗传算法的移动机器人路径规划方法及存储介质 | |
CN111060109B (zh) | 一种基于改进a星算法的无人艇全局路径规划方法 | |
CN111504325A (zh) | 一种基于扩大搜索邻域的加权a*算法的全局路径规划方法 | |
CN110319837A (zh) | 一种服务机器人室内复杂状况路径规划方法 | |
CN114119920A (zh) | 三维点云地图构建方法、系统 | |
CN110744543A (zh) | 基于ur3机械臂的改进式prm避障运动规划方法 | |
CN112033413A (zh) | 一种结合环境信息的改进a*算法 | |
CN109163722A (zh) | 一种仿人机器人路径规划方法及装置 | |
CN114705196A (zh) | 一种用于机器人的自适应启发式全局路径规划方法与系统 | |
CN115145284A (zh) | 一种爆破环境下验孔机器人路径规划方法 | |
Xie et al. | 2.5 D navigation graph and improved a-star algorithm for path planning in ship inside virtual environment | |
CN114779788A (zh) | 一种改进a*算法的路径规划方法 | |
CN114185354B (zh) | 一种基于dqn的agv全局路径规划方法和系统 | |
CN115933637A (zh) | 一种变电设备巡检机器人路径规划方法、设备及存储介质 | |
CN114371711B (zh) | 一种机器人编队避障路径规划方法 | |
Bouman et al. | Adaptive coverage path planning for efficient exploration of unknown environments | |
Liu et al. | Automated clash free rebar design in precast concrete exterior wall via generative adversarial network and multi-agent reinforcement learning | |
Zhou et al. | An approach for solving the three-objective arc welding robot path planning problem | |
CN112947434A (zh) | 一种基于局部最优卷积评价的机器人路径规划方法 | |
Tsardoulias et al. | Construction of minimized topological graphs on occupancy grid maps based on GVD and sensor coverage information | |
Saigol et al. | Efficient search for known objects in unknown environments using autonomous indoor robots | |
KR102248977B1 (ko) | 플랜트엔지니어링의 3차원 배관경로 자동 생성방법 | |
CN112182704A (zh) | 一种地图中建筑物群移位优化方法及装置 | |
Yan | Research on the A Star Algorithm for Finding Shortest Path | |
Broch et al. | Cognitive maps for mobile robot navigation: A hybrid representation using reference systems |
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 |