移动机器人全局路径规划与局部路径规划融合方法及设备

1.本发明属于移动机器人路径规划技术领域，具体涉及移动机器人全局路径规划与局部路径规划融合方法及设备。


背景技术：

2.在智能机器人领域，路径规划问题占据着重要地位。路径规划问题指在有障碍物的环境中，在满足距离、计算时间、通信延迟和能量消耗等优化条件的前提下，寻找从机器人初始位置到期望位置的安全路径。在真实场景下，机器人进行路径规划，首先要明白本体处于环境中的什么位置，同时在规划好路径后，设置机器人角速度和线速度，机器人会顺利完成任务。全局路径规划前环境已经确定好，而局部路径规划则不确定，需要利用传感器对环境进行实时扫描，掌握环境完整信息。
3.目前，a*算法是常用的全局路径规划算法。全局路径规划通常是面向目的地，根据已知环境，要求机器人找到连接起点和终点的完整路径。但是传统的方法存在实时性差和路径转折点较多的问题，在实际环境中难以避免出现未知障碍物，机器人要在未知环境中仍能顺利到达终点，则需要涉及局部路径规划，达到避开障碍物的目的。


技术实现要素：

4.针对现有技术中存在的问题，本发明提供了移动机器人全局路径规划与局部路径规划融合方法及设备，其目的在于解决现有路径规划方法中移动机器人规划路径效率低、拐点频繁和无法躲避未知障碍物的问题。
5.为了解决上述技术问题，本发明通过以下技术方案予以实现：
6.一种移动机器人全局路径规划与局部路径规划融合方法，包括：
7.利用改进后的全局路径规划a*算法在栅格地图上规划全局路径；所述改进后的全局路径规划a*算法为在全局路径规划a*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；
8.采用平滑策略对所述全局划路径进行平滑处理，得到平滑后的全局路径；
9.利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得所述规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合；所述融合算法为所述改进后的全局路径规划a*算法与局部路径规划动态窗口法融合得到的算法。
10.进一步地，所述改进后的全局路径规划a*算法的表达式为：
[0011][0012]
其中，
[0013][0014]
公式中，x和y分别代表横坐标和纵坐标；f(n)为当前位置n的成本评价函数；g(n)为机器人从初始位置到当前位置n的实际成本；h(n)为机器人从当前位置n到目标位置的估
计成本；d
nt
是当前节点n与目标之间的长度；d
st
是起点与目标之间的长度；h(p)是当前节点n的父节点的启发式函数；β为节点方向角，表示起始点到当前节点n的向量与节点n指向目标点的向量之间的夹角。
[0015]
进一步地，所述利用改进后的全局路径规划a*算法在栅格地图上规划全局路径，具体为：
[0016]
步骤1.1，设定open列表和close列表，分别存储没有访问过的节点信息和已经访问过的节点信息；
[0017]
步骤1.2，开始搜索时，close列表为空，访问过开始节点后，将所述开始节点从open列表中删除，将所述开始节点加入close列表中，利用所述改进后的全局路径规划a*算法反复搜索，寻找目标点；
[0018]
步骤1.3，在到达目标点之前，继续搜索所述开始节点的相邻节点，判断搜索到的新节点是否在open列表中，如果不在open列表中，则将该新节点加入open列表中，采用成本评价函数计算该新节点的代价值f，将该新节点设置为当前搜索点的父节点并加入close列表中；如果在open列表中，则执行步骤1.4；
[0019]
步骤1.4，采用成本评价函数计算该新节点的代价值f，并与前一次计算出来的相同节点的代价值f进行对比，选取代价值f最小的节点作为下一次搜索点的父节点，并将所述代价值f最小的节点加入close列表中，然后继续更新open列表中的节点信息；
[0020]
步骤1.5，循环步骤1.1至步骤1.4，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径。
[0021]
进一步地，所述平滑策略为梯度下降法。
[0022]
进一步地，所述采用平滑策略对所述全局划路径进行平滑处理，得到平滑的全局路径，具体包括：
[0023]
步骤2.1，假设有原始全局路径v和平滑的全局路径s，最小化式d1和d2；
[0024][0025][0026]
式中，i为路径中的点；d1为平滑后的全局路径与原平滑的全局滑路径的偏移量；d2是平滑后的全局路径一个节点到下一个节点的偏移量；平滑后的全局路径的起点与终点与原始全局路径相同，即起点v1＝s1和指定目标v
n
＝s
n
；
[0027]
步骤2.2，设定误差阈值e，利用梯度下降法更新和从而达到优化的目的，在梯度下降法优化过程中，
[0028][0029][0030]
式中，α和β为在梯度方向上的搜索步长；
[0031]
步骤2.3，考虑与前一状态偏差，则有：
[0032][0033]
步骤2.4，更新和直到梯度下降的距离小于误差阈值，则停止更新，得到更加平滑的全局路径。
[0034]
进一步地，所述融合算法的评价函数的表达式为：
[0035]
g(v,w)＝goalcost(v,w)
‑
obstaclecost(v,w)
[0036]
其中，
[0037]
goalcost(v,w)＝k1·
[d
target
(t
‑
1)
‑
d
target
(t)]
[0038]
obstaclecost(v,w)＝max(k2·
[d
safe
‑
do
bs
(t)],0)
[0039]
式中，k1为前向增益；d
target
(t
‑
1)为t
‑
1时刻机器人与目标之间的距离；d
target
(t)是t时刻机器人与局部目标点之间的距离；d
target
(t
‑
1)
‑
d
target
(t)用于测量机器人与目标之间的距离增量；k2为障碍增益；d
safe
为制动距离，即机器人在最大加速度下将最大线速度减速为零所需的欧氏距离；d
obs
(t)是t时刻机器人到最近障碍物的距离。
[0040]
进一步地，所述利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得所述规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合，具体为：
[0041]
步骤3.1，对平滑后的全局路径进行初始化；
[0042]
步骤3.2，机器人在速度空间(v,w)中进行采样，生成模拟轨迹；
[0043]
步骤3.3，在考虑全局最优路径的基础上，在步骤3.2生成的模拟轨迹中，根据所述融合算法的评价函数选择最优路径；
[0044]
步骤3.4，机器人沿着步骤3.3中得到的最优路径移动；
[0045]
步骤3.5，判断是否移动到终点，如果移动到终点则结束，如果不是终点，则返回步骤3.2。
[0046]
一种移动机器人全局路径规划与局部路径规划融合设备，包括：
[0047]
路径规划模块，用于利用改进后的全局路径规划a*算法在栅格地图上规划全局路径；所述改进后的全局路径规划a*算法为在全局路径规划a*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；
[0048]
平滑处理模块，用于采用平滑策略对所述全局划路径进行平滑处理，得到平滑后的全局路径；
[0049]
路径融合模块，用于利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得所述规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合；所述融合算法为所述改进后的全局路径规划a*算法与局部路径规划动态窗口法融合得到的算法。
[0050]
与现有技术相比，本发明至少具有以下有益效果：本发明提供的一种移动机器人全局路径规划与局部路径规划融合方法，解决了传统全局路径规划a*算法存在路径优化不足、搜索效率低以及拐点多的问题，将节点距离信息集成到以指数衰减函数为系数的启发式函数中，以加快全局寻优的搜索速度，同时还考虑了上一代父节点的影响，降低了节点往返搜索概率。获取初始路径后，采用梯度下降法平滑路径。融合改进全局路径规划a*算法与
局部路径规划算法，将评价函数改为考虑全局最优，解决了局部路径容易陷入局部最优性的问题，采用全局路径规划与局部路径规划融合算法进行路径规划不仅能满足全局最优性，而且能躲避未知障碍物。仿真实验和真实场景下turtlrbot机器人试验验证了改进后的全局路径规划a*算法规划路径搜索时间、遍历节点数量、拐点数量相较于原始全局路径规划a*算法明显减少，在不同特征的环境中，全局路径规划与局部路径规划融合算法都能在全局最优的基础上成功规划出无碰撞路径，验证了全局路径规划与局部路径规划融合算法的全局最优性能和实时避障能力。
[0051]
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
[0052]
为了更清楚地说明本发明具体实施方式中的技术方案，下面将对具体实施方式描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0053]
图1是本发明中改进全局路径规划a*算法的流程示意图；
[0054]
图2a)是本发明实施例中30*30的栅格地图中原始全局路径规划a*算法规划路径仿真图，图2b)是本发明实施例中30*30的栅格地图中改进全局路径规划a*算法规划路径仿真图；
[0055]
图3a)是本发明实施例中50*50的栅格地图中原始全局路径规划a*算法规划路径仿真图，图3b)是本发明实施例中50*50的栅格地图中改进全局路径规划a*算法规划路径仿真图；
[0056]
图4a)是本发明实施例中30*30的栅格地图中全局与局部路径规划融合算法规划路径仿真图，图4b)是本发明实施例中50*50的栅格地图中全局与局部路径规划融合算法规划路径仿真图；
[0057]
图5a)是本发明实施例中室内电梯口真实场景，图5b)是本发明实施例中走廊真实场景；
[0058]
图6a)是本发明实施例中室外电梯口场景下原始全局路径规划a*算法路径规划，图6b)是本发明实施例中室外电梯口场景下改进全局路径规划a*算法路径规划，图6c)是本发明实施例中室外电梯口场景下移动机器人沿规划路径移动到终点示意图；
[0059]
图7a)是本发明实施例中走廊场景下原始全局路径规划a*算法路径规划，图7b)是本发明实施例中走廊场景下改进全局路径规划a*算法路径规划，图7c)是本发明实施例中走廊场景下机器人沿规划路径移动到终点示意图；
[0060]
图8是本发明移动机器人全局路径规划与局部路径规划融合算法的流程示意图；
[0061]
图9a)是本发明实施例中走廊场景添加未知行人，图9b)是本发明实施例中空旷场景添加未知障碍物；
[0062]
图10是走廊添加未知行人环境全局路径规划与局部路径规划融合算法路径规划结果；
[0063]
图11是电梯口添加未知障碍物场景下全局路径规划与局部路径规划融合算法路
径规划过程，图11a)是本发明实施例中已知地图上路径规划初始结果，图11b)是本发明实施例中机器人第一次遇到未知障碍物规划路径，图11c)是本发明实施例中机器人第二次遇到未知障碍物规划路径，图11d)是本发明实施例中机器人沿规划路径移动到终点。
具体实施方式
[0064]
作为本发明的某一具体实施方式，一种移动机器人全局路径规划与局部路径规划融合方法，具体包括如下步骤：
[0065]
步骤1，利用改进后的全局路径规划a*算法在栅格地图上规划全局路径；改进后的全局路径规划a*算法为在全局路径规划a*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法，改进后的全局路径规划a*算法的表达式为：
[0066][0067]
其中，
[0068][0069]
公式中，x和y分别代表横坐标和纵坐标，f(n)为当前位置n的成本评价函数；g(n)为机器人从初始位置到当前位置n的实际成本；h(n)为机器人从当前位置n到目标位置的估计成本；d
nt
是当前节点n与目标之间的长度；d
st
是起点与目标之间的长度；h(p)是当前节点n的父节点的启发式函数；β为节点方向角，表示起始点到当前节点n的向量与节点n指向目标点的向量之间的夹角。
[0070]
具体地说，利用改进后的全局路径规划a*算法在栅格地图上规划全局路径，具体流程图如图1所示，包括以下步骤：
[0071]
步骤1.1，设定open列表和close列表，分别存储没有访问过的节点信息和已经访问过的节点信息；
[0072]
步骤1.2，开始搜索时，close列表为空，访问过开始节点后，将开始节点从open列表中删除，将开始节点加入close列表中，利用改进后的全局路径规划a*算法反复搜索，寻找目标点；
[0073]
步骤1.3，在到达目标点之前，继续搜索开始节点的相邻节点，判断搜索到的新节点是否在open列表中，如果不在open列表中，则将该新节点加入open列表中，采用成本评价函数计算该新节点的代价值f，将该新节点设置为当前搜索点的父节点并加入close列表中；如果在open列表中，则执行步骤1.4；
[0074]
步骤1.4，采用成本评价函数计算该新节点的代价值f，并与前一次计算出来的相同节点的代价值f进行对比，选取代价值f最小的节点作为下一次搜索点的父节点，并将代价值f最小的节点加入close列表中，然后继续更新open列表中的节点信息；
[0075]
步骤1.5，循环步骤1.1至步骤1.4，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径。
[0076]
步骤2，采用平滑策略对全局划路径进行平滑处理，得到平滑后的全局路径；作为优选的实施方式，平滑策略采用梯度下降法，具体包括以下步骤：
[0077]
步骤2.1，假设有原始全局路径v和平滑的全局路径s，最小化式d1和d2；
[0078][0079][0080]
式中，d1为平滑后的全局路径与原平滑的全局滑路径的偏移量；d2是平滑后的全局路径一个节点到下一个节点的偏移量；平滑后的全局路径的起点与终点与原始全局路径相同，即起点v1＝s1和指定目标v
n
＝s
n
；
[0081]
步骤2.2，设定误差阈值e，利用梯度下降法更新和从而达到优化的目的。在梯度下降法优化过程中，
[0082][0083][0084]
式中，α和β为在梯度方向上的搜索步长；
[0085]
步骤2.3，考虑与前一状态偏差，则有：
[0086][0087]
步骤2.4，更新和直到梯度下降的距离小于误差阈值，则停止更新，得到更加平滑的全局路径。
[0088]
步骤3，利用融合算法从平滑后的全局路径上选取局部路径规划的目标，使得规划路径接近平滑后的全局路径，完成全局路径规划与局部路径规划融合；
[0089]
融合算法为改进后的全局路径规划a*算法与局部路径规划动态窗口法融合得到的算法，融合算法的评价函数的表达式为：
[0090]
g(v,w)＝goalcost(v,w)
‑
obstaclecost(v,w)
[0091]
其中，
[0092]
goalcost(v,w)＝k1·
[d
target
(t
‑
1)
‑
d
target
(t)]
[0093]
obstaclecost(v,w)＝max(k2·
[d
safe
‑
d
obs
(t)],0)
[0094]
式中，k1为前向增益；d
target
(t
‑
1)为t
‑
1时刻机器人与目标之间的距离；d
target
(t)是t时刻机器人与局部目标点之间的距离；d
target
(t
‑
1)
‑
d
target
(t)用于测量机器人与目标之间的距离增量；k2为障碍增益；d
safe
为制动距离，即机器人在最大加速度下将最大线速度减速为零所需的欧氏距离；d
obs
(t)是t时刻机器人到最近障碍物的距离。
[0095]
具体地说，利用融合算法从平滑后的全局路径上选取局部路径规划的目标，使得规划路径接近平滑后的全局路径，完成全局路径规划与局部路径规划融合，具体包括以下步骤：
[0096]
步骤3.1，对平滑后的全局路径进行初始化；
[0097]
步骤3.2，机器人在速度空间(v,w)中进行采样，生成模拟轨迹；
[0098]
步骤3.3，在考虑全局最优路径的基础上，在步骤3.2生成的模拟轨迹中，根据融合算法的评价函数选择最优路径；
[0099]
步骤3.4，机器人沿着步骤3.3中得到的最优路径移动；
[0100]
步骤3.5，判断是否移动到终点，如果移动到终点则结束，如果不是终点，则返回步骤3.2。
[0101]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0102]
步骤1，根据改进全局路径规划a*算法在栅格地图上规划路径。
[0103]
图1为本发明改进全局路径规划a*方法的流程图，具体步骤如下：
[0104]
步骤1.1，设定open列表和close列表，分别存储没有访问和已经访问过的节点信息。
[0105]
步骤1.2，开始搜索时，close列表为空，访问过开始节点后，将其从open列表删除，加入close列表中，a*算法反复搜索，寻找目标点。
[0106]
步骤1.3，在到达目标点之前，继续搜索其相邻节点，判断搜索到的新节点是否在open列表中，如果没有，将新节点加入open列表，计算这个节点的代价值f，将此节点设置为当前搜索点的父节点加入close列表中。
[0107]
a*算法中，节点的代价值采用成本评价函数计算。
[0108]
a*算法是启发式搜索算法，用于在静态二维构型空间中计算最优路径。依据成本评价函数进行搜索，搜索到满足成本评价函数的点作为下一次要搜索的点，并重复此过程，直到找到目标点，形成最优路径。成本评价函数如下：
[0109]
f(n)＝h(n)+g(n)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0110]
其中，f(n)为当前位置n的成本评价函数，g(n)为机器人从初始位置到当前位置n的实际成本，h(n)为机器人从当前位置n到目标位置的估计成本。h(n)的选择直接影响到a*算法的成功率和准确率。
[0111]
本发明使用四轮驱动机器人，移动方位不受限制，并且欧几里得距离可以直接反映两点之间的距离，选用欧几里得距离作为估价函数，更加接近于实际距离。欧几里得距离表示为当前节点与目标节点之间的距离：
[0112][0113]
当机器人的移动方位不受限制时，使用欧几里得距离。
[0114]
在现有全局路径规划技术a*算法中，实际代价和估计代价的权重均为1。网格地图的实际成本远远大于估计成本，这将扩大搜索的范围节点，导致算法的效率显著降低。另一方面，a*算法的启发函数中只考虑了当前节点到目标节点的距离。针对上述问题，我们将节点距离信息集成到以指数衰减函数为系数的启发式函数中，以加快全局寻优的搜索速度，同时考虑了上一代父节点的影响，降低了节点往返搜索的概率。假设当前临时标记节点为n，节点的父节点设为p，则改进后的启发函数如下：
[0115][0116]
其中，d
nt
是当前节点n与目标之间的长度，d
st
是起点与目标之间的长度，h(p)是当前节点n的父节点的启发式函数。节点方向角β为起始点到当前节点n的向量与节点n指向目
标点的向量之间的夹角。
[0117]
综上所述，改进后的a*算法评价函数可写成式(4):
[0118][0119]
改进后的a*算法评价函数将节点角度信息和距离信息集成到评价函数中，使a*算法在算法搜索时具有方向性和明确的目的，避免了不必要的节点搜索，遍历点减少，从而路径搜索时间减少，提高了算法的效率。
[0120]
步骤1.4，如果搜索到的新节点已经存在于open列表中，就计算这些节点的代价值f，与前一次计算出来的相同节点的f值进行对比，选取f值最小的那一点作为下一次搜索点的父节点，并将此节点加入close列表中。然后继续更新open列表的信息，
[0121]
步骤1.5，循环以上过程，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束。如果此时open列表为空，则表示没有路径。
[0122]
步骤2，对步骤1中获取的路径采用平滑策略。
[0123]
获取路径后，采用梯度下降法对路径进行平滑处理，在保证安全性的同时减少了路径的冗余拐点和大角度转弯，同时缩短了路径长度。
[0124]
在梯度下降法迭代中，如果步长太长，可能会错过最优解，如果步长过小，速度过慢，算法循环会持续很长时间。因此在机器人研究中，采用递归性地逼近最小偏差模型。目标函数f(x)的梯度可表示为：
[0125][0126]
其基本思想为沿梯度下降的方向求解目标函数的最小值，迭代公式为：
[0127]
y
k
＝y
k
‑1‑
βg
k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0128]
其中g
k
表示函数的梯度，搜索方向为梯度负方向，β表示搜索步长，即搜索到使函数值最小的一点。
[0129]
步骤2.1，假设有原始路径v和平滑路径s，平滑算法核心思想是最小化式(7)和式(8)：：
[0130][0131][0132]
其中d1为平滑后路径与原平滑路径的偏移量，同时，d2是平滑后路径一个节点到下一个节点的偏移量。如前所述，需要最小化d1和d2，以实现平滑路径。平滑后路径的起点与终点与原始路径相同，即起点v1＝s1和指定目标v
n
＝s
n
。
[0133]
步骤2.2，设定误差阈值e，利用梯度下降法更新式(9)和式(10)，从而达到优化的目的。在梯度下降法优化过程中，
[0134][0135]
[0136]
其中，α和β为在梯度方向上的搜索步长。
[0137]
步骤2.3，除此之外，还可以考虑与前一状态偏差，则有：
[0138][0139]
步骤2.4，更新式(9)和(11)，直到梯度下降的距离小于误差阈值，则停止更新，得到更加平滑的路径。
[0140]
步骤3，如图1所示，将步骤1和步骤2改进的全局路径a*算法与局部路径规划动态窗口法算法融合：融合算法考虑了改进a*算法获得的全局路径信息，引入了全局路径规划和局部目标，从全局路径上选取局部路径规划的目标，使得规划结果接近全局路径。通过对障碍物碰撞可能性的权重因子进行分段设计，将避障目标和到目标时间最短的目标分离出来。针对避障目标，在安全距离项中给出一个很大的增益，使评估函数对可能发生的碰撞非常敏感。
[0141]
步骤3.1，对步骤2中得到的平滑后的路径进行初始化。
[0142]
步骤3.2，机器人在速度空间(v,w)中进行采样，生成模拟轨迹。
[0143]
步骤3.3，在考虑全局最优路径的基础上，在步骤3.2生成的模拟轨迹中，根据评价函数选择最优路径。
[0144]
为了使机器人到达目标时间最短，在评价函数中提出了一个显式的动态项来表示到目标的距离。在新的评价函数中考虑了最大前向速度对应的安全制动距离，如式(12)所示：
[0145]
g(v,w)＝goalcost(v,w)
‑
obstaclecost(v,w)
ꢀꢀꢀꢀꢀ
(12)
[0146]
其中，
[0147]
goalcost(v,w)＝k1·
[d
target
(t
‑
1)
‑
d
target
(t)]
ꢀꢀꢀꢀꢀ
(13)
[0148]
obstaclecost(v,w)＝max(k2·
[d
safe
‑
d
obs
(t)],0)
ꢀꢀ
(14)
[0149]
其中k1为前向增益，d
target
(t
‑
1)为t
‑
1时刻机器人与目标之间的距离。d
target
(t)是t时刻机器人与局部目标点之间的距离。d
target
(t
‑
1)
‑
d
target
(t)用于测量机器人与目标之间的距离增量。k2为障碍增益，d
safe
为制动距离，即机器人在最大加速度下将最大线速度减速为零所需的欧氏距离，d
obs
(t)是t时刻机器人到最近障碍物的距离。
[0150]
当所选择的速度可能使机器人无法避障时，这意味着obstaclecost为正(d
safe
大于d
obs
(t))，较大的障碍增益会使得g成为一个较大的负数。在这种情况下，goalcost被忽略，所以算法会选择最快的速度避开障碍物，保证机器人的安全。由于obstaclecost的分段权值设计，使得该方法对障碍物的碰撞极为敏感，从而提高了避障率。
[0151]
步骤3.4，机器人沿着步骤3.3中得到的最优路径移动。
[0152]
步骤3.5，判断是否移动到终点，如果移动到终点则结束，如果不是终点，则返回步骤3.2。
[0153]
图1为本发明所述改进全局路径规划a*算法的流程图。为验证本发明改进全局路径规划a*算法和全局路径规划与局部路径规划融合算法的有效性，设置两种不同大小、障碍物不同的两种地图，将改进全局路径规划a*算法与原始全局路径规划a*算法进行对比，分别从路径长度、遍历节点数、搜索时间、拐点数量这四个方面对算法进行了性能仿真分析，在全局路径地图中，设置未知障碍物，验证了融合算法的可行性。
[0154]
在两种不同大小、障碍物不同的栅格地图上，对本发明所提改进全局路径规划a*
算法和原始全局路径规划a*算法性能进行仿真实验，图2为30*30栅格的地图，图2中黑色方块为障碍物，起点和目标点为箭头所标注位置，灰色阴影部分为算法在搜索过程中所遍历节点数，图2(a)为原始全局路径规划a*算法规划结果，图2(b)为本发明改进全局路径规划a*算法规划结果，对比2(b)和(c)可以看出，本发明改进全局路径规划a*算法遍历节点和拐点数量减少。
[0155]
图3为50*50栅格地图，图3(a)为原始全局路径规划a*算法规划结果，图3(b)为本发明改进全局路径规划a*算法规划结果，对比图3(a)和(b)可以看出，在更加复杂的场景中，改进算法仍然可行。表1为两种地图路径规划仿真实验参数对比，由表1可以看出，在不同大小的栅格地图中，在路径长度基本保持一致的情况下，本发明改进算法规划路径所用搜索时间、遍历节点数量、拐点数量相较于原始全局路径规划a*算法明显减少，进一步验证了本发明改进全局路径规划a*算法的有效性。
[0156]
表1两种栅格地图路径规划仿真实验参数
[0157][0158]
为了验证本发明所述的全局路径规划与局部路径规划融合算法有效性，在全局路径地图上添加未知障碍物，对全局路径规划与局部路径规划融合算法进行仿真。图4为不同地图上全局路径规划与局部路径规划融合算法路径规划结果，图中箭头标注为未知障碍物，虚线为改进全局路径规划a*算法规划结果，曲线为全局路径规划与局部路径规划融合算法路径规划结果，全局路径上的标注出来的点为全局路径规划与局部路径规划融合算法规划路径的局部目标点。图4(a)和(b)分别为30*30和50*50的地图上全局路径规划与局部路径规划融合算法路径规划结果，由图4(a)和(b)可以看出，无论如何改变地图大小、障碍物的位置、数量和大小，融合算法都能在全局最优的基础上成功规划一条无碰撞路径。从而验证了全局路径规划与局部路径规划融合算法的全局最优性能和实时避障能力。
[0159]
在实际情况下，机器人的工作环境比仿真环境更加复杂，环境特征更加丰富，存在行人等不确定因素，为了验证改进算法在这些情况下的有效性，分别采用传统全局路径规划a*算法、本发明改进全局路径规划a*算法、本发明所提改进全局路径规划a*算法和局部路径规划融合算法，基于turtlebot机器人对西安理工大学教五楼不同环境分别进行了路径规划试验。
[0160]
室外电梯口和走廊场景如图5(a)和(b)所示，图6为图5(a)所示室外电梯口的路径规划实验结果，图6(a)为原始全局路径规划a*算法路径规划结果，图6(b)为本发明改进全局路径规划a*算法路径规划结果，图6(c)为移动机器人沿着规划好的路径移动到终点。图6(a)中，原始a*算法规划出来的路径存在较多拐点，会导致路径长度过长，转折点不平滑，实际导航中移动机器人难以顺利移动到终点，移动过程中耗时较长，而图6(b)中本发明改进全局路径规划a*算法规划出来的路径则缓解了路径中拐点较多、转折点不平滑和路径耗费时间较长的问题，并在实际操作中移动机器人能顺利、快速地沿着规划好的路径从起点移动到终点。
[0161]
图7为图5(b)所示走廊场景下路径规划结果，图7(a)为原始全局路径规划a*算法路径规划结果，图7(b)为本发明改进全局路径规划a*算法路径规划结果，图5(b)中障碍物呈s形状摆放，在已构建好的地图上手动为机器人指定起点与终点，图7(a)中所规划路径存在较多拐点，路径不平滑，而图7(b)中所规划路径更为平滑，机器人能顺利、快速的从起点移动到终点。由此可见，本发明改进全局路径规划a*算法不论在较为空旷的环境中还是在较窄的环境中，都能顺利躲避障碍物，规划出一条更优的路径顺利达到终点。
[0162]
此外，不同真实环境机器人路径规划试验参数如表2所示，可以看出，改进路径规划算法在路径搜索时间和路径规划长度两方面都得到了优化。
[0163]
表2不同真实环境路径规划试验参数
[0164][0165]
图8为本发明所述的全局路径规划与局部路径规划融合算法流程图。在真实场景中，除了已知障碍物，还会出现未知障碍物，因此本发明在已知场景中添加未知障碍物，对本发明所述的全局路径规划与局部路径规划融合算法进行路径规划试验。首先在图5(b)所示走廊场景中添加已知地图中未见的行人，场景如图9(a)所示。
[0166]
在已知地图上为移动机器人设定好起点与终点，图10为走廊添加未知行人环境下机器人移动过程中规划路径结果。由图10可看出，本发明所提出的全局路径规划与局部融合算法在路径规划过程中不仅能保证全局最优性，而且在机器人移动过程中能实时躲避行人，成功到达终点。
[0167]
图11为在图9(b)所示空旷场景添加未知障碍物的情况下利用本发明所述全局路径规划与局部路径规划融合算法进行路径规划的过程，图9(b)中所摆放三个障碍物为未知障碍物，图11(a)为机器人在已知地图上路径规划初始结果，图11(b)为机器人第一次遇到未知障碍物规划路径，图11(c)为机器人第二次遇到未知障碍物规划路径，图11(d)为移动机器人移动到终点。由图11可以看出，当未知障碍物较多时，机器人也能成功躲避，顺利的移动到终点，从而验证了本发明所提全局与局部路径规划融合算法的有效性。
[0168]
最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。