一种未知动态环境下机器人智能路径规划方法与流程

1.本发明涉及机器人动态路径规划技术领域，更具体地，涉及一种未知动态环境下机器人智能路径规划方法。


背景技术：

2.路径规划是指在未知障碍物的环境中，为机器人规划出一条连接起点到终点的无碰撞的可行路径，同时还应该满足某个或某些优化指标，如路径长度、连续性、规划时间等。
3.当前已有的动态环境路径规划方法包括混合式算法及局部路径规划方法。混合规划算法首先将环境中的障碍物分为静态障碍与动态障碍，然后把规划任务分为全局规划阶段与再规划阶段。全局规划阶段不考虑环境中的动态障碍，已有的a*算法、神经网络及大量的智能优化算法等均可用于全局路径规划；再规划阶段主要是当机器人运动过程中遇到动态障碍时，利用优化算法重新规划出一条避开动态障碍的可行路径，二者相互配合引导机器人向目标点运动。局部路径规划方法主要有人工势场法、行为分解法、动态窗口规划法等，但由于缺乏全局信息引导，这些局部规划方法无法保证规划路径能够到达目标点，也无法保证规划路径的最优性。此外，局部规划侧重于能否成功避障，在复杂环境下易陷入局部最优，规划路径的完整性无法保证。上述方法在面对复杂环境时都存在有鲁棒性差、效率低、稳定性差等不足。


技术实现要素：

4.为解决上述技术问题，本发明提供一种未知动态环境下机器人智能路径规划方法，它实时性强，稳定性好，对环境具有很强的自适应性，大大提高路径规划方法的执行效率。
5.本技术技术方案如下：
6.一种未知动态环境下机器人智能路径规划方法，其特征在于，包括如下步骤：
7.步骤s1.地图环境建模，依据环境中障碍物的分布将栅格分为自由栅格与障碍栅格，在访问栅格前，先对栅格进行标识；
8.步骤s2.在栅格环境中，机器人进行路径规划，计算最短路径，判断是否到达终点，若否则执行步骤s3，若是则执行步骤s5；
9.步骤s3.找到花费代价最小的邻居，判断其是否在动态障碍物集合中，若是则原地等待，若否则行进至该邻居，
10.步骤s4.检测障碍物是否发生变化，若是则更新估计代价的修正值km并设置新的起点，更新多阶邻居的一步超前目标函数值rhs(s)，并加入到优先队列中，重新计算最短距离，返回步骤s2；
11.步骤s5.判断是否有障碍物碰撞，若是则执行步骤s6，若否则结束；
12.步骤s6.判断动态障碍物碰撞坐标是否相同，若是则设置该坐标为暂时不可达并更新地图，若否则执行步骤s7；
13.步骤s7.判断之前动态障碍物是否还在，若是，则回到碰撞前一时间步，返回步骤s2；若否则设置该坐标可完全到达更新地图。
14.进一步地，所述步骤s2中，机器人在下一步行走前，需要对周围邻居栅格进行优先级评估，利用估价函数对其周边栅格进行评估，并选择估价值最小的栅格为下一前进栅格，表达式为f(s)＝g(s) h(s)。
15.g(s)表示终点栅格到当前位置s实际的花费代价，h(s)表示当前位置s到起点栅格的估计代价,其中，h(s)使用两栅格间曼哈顿距离来表示，表达式为：
16.d(s,s')＝|x
s-x
s'
| |y
s-y
s'
|
17.其中(xs,ys)和(x
s'
,y
s'
)分别是栅格s和栅格s’的行列坐标。
18.进一步地，所述步骤s2具体为设置地图环境中所有栅格初始rhs(s)值和g(s)值都为正无穷；设置终点rhs(s)＝0；设置km＝0，动态障碍物约束集合d＝0，规划路径集合p＝0；设置优先队列u＝0，将终点插入优先队列u，计算终点的下步栅格优先函数值key值值；
19.扫描多阶内邻居，如果检测到有障碍物变化，修改km值，根据上一个起点和当前点的启发值，并且将当前点设置为上一个起点；继续执行步骤s3和s5。
20.进一步地，所述步骤s3中，取当前位置s所有邻居点中最小的rhs(s)为s的rhs值；如果当前位置s在优先队列中，将s从优先队列u中移除；如果g(s)！＝rhs(s)，将s和它的下步栅格优先函数值key值插入到优先队列中。
21.进一步地，当优先队列u不为空，且优先队列最小值小于当前机器人所在的栅格节点的下步栅格优先函数值key值，或者当前机器人所在的栅格节点的下步栅格优先函数值key值不一致时，执行下面循环：取队列u中最优先点的key值k_old；队列u中下步栅格优先函数值key值最小的栅格点u出栈；如果u的下步栅格优先函数值key值大于k_old，u重新加入队列；如果g(u)＞rhs(u)，令g＝rhs，执行步骤s3；否则令g(u)变为正无穷，将u重新加入队列，执行步骤s3。
22.进一步地，所述步骤s4中，当中途有障碍物出现改变地图环境时，g(s)需要rhs(s)函数来实时更新，rhs(s)记录栅格s的父节点的g(s)函数值加上这两点之间的代价中的最小值，相当于一个点从父代栅格到达自身的最小代价，rhs(s)函数表达式为：
23.其中，g(s’)表示终点栅格到栅格s’实际的花费代价，s
start
为当前机器人所在的栅格节点，neighbors为当前机器人所在的栅格节点的邻居节点。
24.进一步地，所述步骤s4中，当栅格可能需要被检测时，需要加入待检测队列u，里面按下步栅格优先函数值key值大小存放待检测更新的结点；对同一栅格，不同情况下会产生不同的key，key将成为第一级规划器寻路时结点检测先后顺序的依据，分别包括第一优先计算函数k1(s)和第二优先计算函数k2(s)，先判断第一优先计算函数k1(s)值，如果第一优先计算函数k1(s)相等再判断第二优先计算函数k2(s)，具体公式如下：
25.k1(s)＝min(g(s),rhs(s)) h(s
start
,s) km；
26.k2(s)＝min(g(s),rhs(s))
27.其中
[0028][0029]km
是对h(s
start
,s)的修正，h(s
start
,s)是事先计算的估计代价，每当机器人发现有与事先环境不一致的障碍物变化时，会基于当前地图环境中邻居栅格的实际情况计算h(s,s')并对km进行累加，修正后计算出的k1(s)使得队列u中栅格优先级顺序更合理。由于s
start
是当前机器人所处位置，是不断变化的，所以是不断变化的，事先环境下的key值最优值为k_old，当环境更新改变时，为了计算效率更快，可以直接用km来修正k_old。
[0030]
进一步地，对于队列u中的栅格，按照下步栅格优先函数值key值由小到大的顺序依次对它们进行状态检测，检测完毕都会对该栅格多阶内邻居状态进行更新以判断是否需要再次进行队列u，具体状态如下：
[0031]
(1).当一个栅格g(s)＝rhs(s)时处于该状态的结点不会被纳入队列u；
[0032]
(2).当一个栅格g(s)＞rhs(s)时，令g(s)＝rhs(s)，并将其从队列u中移除；
[0033]
(3).当一个栅格g(s)＜rhs(s)时，令g(s)＝∞并更新其多阶内邻居,同时将其再次加入队列u中以待作进一步判断。
[0034]
进一步地，所述步骤s5中，具体为当当前节点不是终点时，找到起点的邻居节点集合；找到邻居中最小rhs值对应的节点；如果该点不在集合d内，则将该点作为新的起点；将该点加入集合p中；执行步骤s2。
[0035]
进一步地，所述步骤s6和步骤s7中具体为，获得步骤s5中出现冲突的时间和坐标，加入到集合d中；如果d＝0，则结束退出并返回集合p；如果存在冲突，将d中第一次冲突栅格坐标与上一次冲突集合d中所有冲突坐标进行比对，如果相等，设置该坐标为暂时不可达；设置d-d中的坐标为完全可达；从第一次冲突的时间步前一步开始，继续执行步骤s5，最终集合p保存的就是规划路径集合。
[0036]
与现有技术相比，本发明具有如下有益效果：
[0037]
本方法采用两级规划器来规划路径，第一级规划器为基于d*lite改进的规划算法，于当前环境下增量式寻路，一旦机器人预判到碰撞，不需要从头开始重新规划，在此基础上，增加了局部环境下机器人的等待和迂回决策；第二级规划器借鉴基于冲突搜索思想，增加了机器人对大量未知状态的移动障碍物的判断与避障。实现了机器人在未知动态环境下进行自适应智能实时寻路，通过两级规划器，增强了机器人对各类未知动态环境的自适应性，大幅提高了动态路径规划方法的实时性和成功率。
附图说明
[0038]
图1是本发明流程图。
[0039]
图2是本发明格栅标识示意图。
[0040]
图3是本发明机器人3阶邻居示意图。
具体实施方式
[0041]
下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
[0042]
如图1所示，本实施例一种未知动态环境下机器人智能路径规划方法，其特征在于，包括如下步骤：
[0043]
步骤s1.地图环境建模，依据环境中障碍物的分布将栅格分为自由栅格与障碍栅格，在访问栅格前，先对栅格进行标识；
[0044]
步骤s2.在栅格环境中，机器人进行路径规划，计算最短路径，判断是否到达终点，若否则执行步骤s3，若是则执行步骤s5；
[0045]
步骤s3.找到花费代价最小的邻居，判断其是否在动态障碍物集合中，若是则原地等待，若否则行进至该邻居，
[0046]
步骤s4.检测障碍物是否发生变化，若是则更新估计代价的修正值km并设置新的起点，更新多阶邻居的一步超前目标函数值rhs(s)，并加入到优先队列中，重新计算最短距离，返回步骤s2；
[0047]
步骤s5.判断是否有障碍物碰撞，若是则执行步骤s6，若否则结束；
[0048]
步骤s6.判断动态障碍物碰撞坐标是否相同，若是则设置该坐标为暂时不可达并更新地图，若否则执行步骤s7；
[0049]
步骤s7.判断之前动态障碍物是否还在，若是，则回到碰撞前一时间步，返回步骤s2；若否则设置该坐标可完全到达更新地图。
[0050]
进一步优选的实施例中，步骤s1中具体为：以图论理论为基础，使用固定大小的栅格将机器人工作空间划分为多个简单的工作区域，再将这些栅格组成一个连通图。依据环境中障碍物的分布可将栅格分为自由栅格与障碍栅格两种。自由栅格表示机器人可以在该栅格自由行走，障碍栅格表示该栅格被障碍物所占据，不可到达。对于形状不规则的障碍物，先将其按照栅格大小进行分割处理，障碍物可以占据一个或者多个栅格，如果分割后的障碍物没有完全占据一个栅格，为了路径规划方便，也视作一个完整的障碍栅格。
[0051]
在访问栅格前，需要先对栅格进行标识。栅格地图包括可行栅格(可完全通行)与障碍栅格(禁止通行)。如图2所示，以左下角为原点，建立二维直角坐标系，黑色为障碍栅格，白色为自由栅格，s代表起点，g代表终点，可以根据栅格所在行与列坐标对栅格进行标号。
[0052]
如图3所示，假设机器人o位于栅格中心点，限定机器人为8向自由度运动，则下一时间节点可能有左上、上、右上、左、右、左下、下以及右下等8个运动方向，以机器人o为栅格中心点，由内向外依次为o的一阶邻居、二阶邻居和三阶邻居。机器人在栅格间行走会产生花费，由于栅格大小一致，设定相邻两个栅格行走时机器人需要花费1个单位代价，对于左上、右上、左下、右下对角线栅格行走机器人则会花费1.4个单位代价，如果邻居是障碍栅格，则花费代价为无穷大。
[0053]
进一步优选的实施例中，步骤s2中，机器人在下一步行走前，需要对周围邻居栅格进行优先级评估，利用估价函数对其周边栅格进行评估，并选择估价值最小的栅格为下一前进栅格，表达式为f(s)＝g(s) h(s)。
[0054]
g(s)表示终点栅格到当前位置s实际的花费代价，h(s)表示当前位置s到起点栅格的估计代价,其中，h(s)使用两栅格间曼哈顿距离来表示，表达式为：
[0055]
d(s,s')＝|x
s-x
s'
| |y
s-y
s'
|
[0056]
其中(xs,ys)和(x
s'
,y
s'
)分别是栅格s和栅格s’的行列坐标。
[0057]
当中途有障碍物出现改变地图环境时，g(s)需要rhs(s)函数来实时更新，rhs(s)记录栅格s的父节点的g(s)函数值加上这两点之间的代价中的最小值，相
[0058]
当于一个点从父代栅格到达自身的最小代价，rhs(s)函数表达式为：
[0059][0060]
其中，g(s’)表示终点栅格到栅格s’实际的花费代价，s
start
为当前机器人所在的栅格节点，neighbors为当前机器人所在的栅格节点的邻居节点。s
start
为，neighbors为
[0061]
当栅格可能需要被检测时，需要加入待检测队列u，里面按下步栅格优先函数值key值大小存放待检测更新的结点；对同一栅格，不同情况下会产生不同的key，key将成为第一级规划器寻路时结点检测先后顺序的依据，分别包括第一优先计算函数k1(s)和第二优先计算函数k2(s)，先判断第一优先计算函数k1(s)值，如果第一优先计算函数k1(s)相等再判断第二优先计算函数k2(s)，具体公式如下：
[0062]
k1(s)＝min(g(s),rhs(s)) h(s
start
,s) km；
[0063]
k2(s)＝min(g(s),rhs(s))
[0064]
其中
[0065][0066]
其中,goal为终点，otherwise为除终点外的其他点，km是对h(s
start
,s)的修正，h(s
start
,s)是事先计算的估计代价，每当机器人发现有与事先环境不一致的障碍物变化时，会基于当前地图环境中邻居栅格的实际情况计算h(s,s')并对km进行累加，修正后计算出的k1(s)使得队列u中栅格优先级顺序更合理。
[0067]
对于队列u中的栅格，算法会按照下步栅格优先函数值key值由小到大的顺序依次对它们进行状态检测，检测完毕都会对该栅格三阶内邻居状态进行更新以判断是否需要再次进行队列u。具体状态如下：
[0068]
1.当一个栅格g(s)＝rhs(s)时表示该栅格处于预估与实际没有差错的稳定状态。如外界环境没有变化将一直保持此状态。处于该状态的结点不会被纳入队列u。
[0069]
2.当一个栅格g(s)＞rhs(s)时说明边代价函数值变小，代表网格上障碍物清除或者算法能搜索到更优路径。当检测到某栅格为该状态时，令g(s)＝rhs(s)，并将其从队列u中移除。
[0070]
3.当一个栅格g(s)＜rhs(s)时表示该栅格附近可能存在新增障碍物，代价花费变大。当检测到某栅格为该状态时，令g(s)＝∞并更新其三阶内邻居,同时将其再次加入u中以待作进一步判断。
[0071]
第一级规划器会生成初步规划路径，路径中可能会出现动态障碍物碰撞，对于这些动态障碍物，第二级规划器会建立动态障碍物约束集合，将这些动态障碍物碰撞的时间步和坐标一并加入集合中，并且会在碰撞前一步开始，以约束集合为限制条件，增量调用第一级规划器继续进行路径规划，如果出现新的动态障碍物碰撞，将其时间步和坐标加入集合，如果时间步已过，且障碍物坐标不是机器人当前坐标的三阶内邻居，则将此动态障碍物约束从集合中剔除，如此循环执行，直到动态障碍物集合中元素清零为止。此时，第二级规划器会生成一条无碰撞的规划路径。
[0072]
具体流程如下：
[0073]
设置地图环境中所有栅格初始rhs(s)值和g(s)值都为正无穷；设置终点rhs(s)＝0；设置km＝0，动态障碍物约束集合d＝0，规划路径集合p＝0；设置优先队列u＝0，将终点插入优先队列u，计算终点的下步栅格优先函数值key值；
[0074]
扫描3阶内邻居，如果检测到有障碍物变化，修改km值，根据上一个起点和当前点的启发值，并且将当前点设置为上一个起点；继续执行步骤s3和s5。
[0075]
步骤s3中，取当前位置s所有邻居点中最小的rhs(s)为s的rhs值；如果当前位置s在优先队列中，将s从优先队列u中移除；如果g(s)！＝rhs(s)，将s和它的下步栅格优先函数值key值插入到优先队列中。
[0076]
当优先队列u不为空，且优先队列最小值小于当前机器人所在的栅格节点的下步栅格优先函数值key值，或者当前机器人所在的栅格节点的下步栅格优先函数值key值不一致时，执行下面循环：取队列u中最优先点的key值k_old；队列u中下步栅格优先函数值key值最小的栅格点u出栈；如果u的下步栅格优先函数值key值大于k_old，u重新加入队列；如果g(u)＞rhs(u)，令g＝rhs，执行步骤s3；否则令g(u)变为正无穷。
[0077]
步骤s5中，具体为当当前节点不是终点时，找到起点的邻居节点集合；找到邻居中最小rhs值对应的节点；如果该点不在集合d内，则将该点作为新的起点；将该点加入集合p中；执行步骤s2。
[0078]
步骤s6和步骤s7中具体为，获得步骤s5中出现冲突的时间和坐标，加入到集合d中；如果d＝0，则结束退出并返回集合p；如果存在冲突，将d中第一次冲突栅格坐标与上一次冲突集合d中所有冲突坐标进行比对，如果相等，设置该坐标为暂时不可达；设置d-d中的坐标为完全可达；从第一次冲突的时间步前一步开始，继续执行步骤s5，最终集合p保存的就是规划路径集合。
[0079]
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。