CN113359718B - 移动机器人全局路径规划与局部路径规划融合方法及设备 - Google Patents
移动机器人全局路径规划与局部路径规划融合方法及设备 Download PDFInfo
- Publication number
- CN113359718B CN113359718B CN202110579208.3A CN202110579208A CN113359718B CN 113359718 B CN113359718 B CN 113359718B CN 202110579208 A CN202110579208 A CN 202110579208A CN 113359718 B CN113359718 B CN 113359718B
- Authority
- CN
- China
- Prior art keywords
- path planning
- global path
- algorithm
- node
- path
- 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 36
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 148
- 230000004927 fusion Effects 0.000 claims abstract description 56
- 238000011156 evaluation Methods 0.000 claims abstract description 36
- 238000009499 grossing Methods 0.000 claims abstract description 24
- 238000004088 simulation Methods 0.000 claims description 16
- 238000005457 optimization Methods 0.000 claims description 15
- 238000011478 gradient descent method Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 14
- 239000000126 substance Substances 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000007500 overflow downdraw method Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 11
- 238000012360 testing method Methods 0.000 description 4
- 238000013459 approach Methods 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 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/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
-
- 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公开了移动机器人全局路径规划与局部路径规划融合方法及设备，利用改进后的全局路径规划A*算法在栅格地图上规划全局路径；改进后的全局路径规划A*算法为在全局路径规划A*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；采用平滑策略对全局划路径进行平滑处理，得到平滑后的全局路径；利用融合算法从平滑后的全局路径上选取局部路径规划的目标，使得规划路径接***滑后的全局路径，完成全局路径规划与局部路径规划融合；融合算法为改进后的全局路径规划A*算法与局部路径规划动态窗口法融合得到的算法。本发明能够解决现有路径规划方法中移动机器人规划路径效率低、拐点频繁和无法躲避未知障碍物的问题。
Description
技术领域
本发明属于移动机器人路径规划技术领域，具体涉及移动机器人全局路径规划与局部路径规划融合方法及设备。
背景技术
在智能机器人领域，路径规划问题占据着重要地位。路径规划问题指在有障碍物的环境中，在满足距离、计算时间、通信延迟和能量消耗等优化条件的前提下，寻找从机器人初始位置到期望位置的安全路径。在真实场景下，机器人进行路径规划，首先要明白本体处于环境中的什么位置，同时在规划好路径后，设置机器人角速度和线速度，机器人会顺利完成任务。全局路径规划前环境已经确定好，而局部路径规划则不确定，需要利用传感器对环境进行实时扫描，掌握环境完整信息。
目前，A*算法是常用的全局路径规划算法。全局路径规划通常是面向目的地，根据已知环境，要求机器人找到连接起点和终点的完整路径。但是传统的方法存在实时性差和路径转折点较多的问题，在实际环境中难以避免出现未知障碍物，机器人要在未知环境中仍能顺利到达终点，则需要涉及局部路径规划，达到避开障碍物的目的。
发明内容
针对现有技术中存在的问题，本发明提供了移动机器人全局路径规划与局部路径规划融合方法及设备，其目的在于解决现有路径规划方法中移动机器人规划路径效率低、拐点频繁和无法躲避未知障碍物的问题。
为了解决上述技术问题，本发明通过以下技术方案予以实现：
一种移动机器人全局路径规划与局部路径规划融合方法，包括：
利用改进后的全局路径规划A*算法在栅格地图上规划全局路径；所述改进后的全局路径规划A*算法为在全局路径规划A*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；
采用平滑策略对所述全局划路径进行平滑处理，得到平滑后的全局路径；
利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得所述规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合；所述融合算法为所述改进后的全局路径规划A*算法与局部路径规划动态窗口法融合得到的算法。
进一步地，所述改进后的全局路径规划A*算法的表达式为：
其中，
公式中，x和y分别代表横坐标和纵坐标；f(n)为当前位置n的成本评价函数；g(n)为机器人从初始位置到当前位置n的实际成本；h(n)为机器人从当前位置n到目标位置的估计成本；dnt是当前节点n与目标之间的长度；dst是起点与目标之间的长度；h(p)是当前节点n的父节点的启发式函数；β为节点方向角，表示起始点到当前节点n的向量与节点n指向目标点的向量之间的夹角。
进一步地，所述利用改进后的全局路径规划A*算法在栅格地图上规划全局路径，具体为：
步骤1.1，设定open列表和close列表，分别存储没有访问过的节点信息和已经访问过的节点信息；
步骤1.2，开始搜索时，close列表为空，访问过开始节点后，将所述开始节点从open列表中删除，将所述开始节点加入close列表中，利用所述改进后的全局路径规划A*算法反复搜索，寻找目标点；
步骤1.3，在到达目标点之前，继续搜索所述开始节点的相邻节点，判断搜索到的新节点是否在open列表中，如果不在open列表中，则将该新节点加入open列表中，采用成本评价函数计算该新节点的代价值f，将该新节点设置为当前搜索点的父节点并加入close列表中；如果在open列表中，则执行步骤1.4；
步骤1.4，采用成本评价函数计算该新节点的代价值f，并与前一次计算出来的相同节点的代价值f进行对比，选取代价值f最小的节点作为下一次搜索点的父节点，并将所述代价值f最小的节点加入close列表中，然后继续更新open列表中的节点信息；
步骤1.5，循环步骤1.1至步骤1.4，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径。
进一步地，所述平滑策略为梯度下降法。
进一步地，所述采用平滑策略对所述全局划路径进行平滑处理，得到平滑的全局路径，具体包括：
步骤2.1，假设有原始全局路径v和平滑的全局路径s，最小化式d1和d2；
式中，i为路径中的点；d1为平滑后的全局路径与原平滑的全局滑路径的偏移量；d2是平滑后的全局路径一个节点到下一个节点的偏移量；平滑后的全局路径的起点与终点与原始全局路径相同，即起点v1＝s1和指定目标vn＝sn；
步骤2.2，设定误差阈值e，利用梯度下降法更新
和
从而达到优化的目的，在梯度下降法优化过程中，
式中，α和β为在梯度方向上的搜索步长；
步骤2.3，考虑与前一状态偏差，则有：
步骤2.4，更新
和
直到梯度下降的距离小于误差阈值，则停止更新，得到更加平滑的全局路径。
进一步地，所述融合算法的评价函数的表达式为：
G(v,w)＝GoalCost(v,w)-ObstacleCost(v,w)
其中，
GoalCost(v,w)＝k1·[Dtarget(t-1)-Dtarget(t)]
ObstacleCost(v,w)＝max(k2·[Dsafe-Dobs(t)],0)
式中，k1为前向增益；Dtarget(t-1)为t-1时刻机器人与目标之间的距离；Dtarget(t)是t时刻机器人与局部目标点之间的距离；Dtarget(t-1)-Dtarget(t)用于测量机器人与目标之间的距离增量；k2为障碍增益；Dsafe为制动距离，即机器人在最大加速度下将最大线速度减速为零所需的欧氏距离；Dobs(t)是t时刻机器人到最近障碍物的距离。
进一步地，所述利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得所述规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合，具体为：
步骤3.1，对平滑后的全局路径进行初始化；
步骤3.2，机器人在速度空间(v,w)中进行采样，生成模拟轨迹；
步骤3.3，在考虑全局最优路径的基础上，在步骤3.2生成的模拟轨迹中，根据所述融合算法的评价函数选择最优路径；
步骤3.4，机器人沿着步骤3.3中得到的最优路径移动；
步骤3.5，判断是否移动到终点，如果移动到终点则结束，如果不是终点，则返回步骤3.2。
一种移动机器人全局路径规划与局部路径规划融合设备，包括：
路径规划模块，用于利用改进后的全局路径规划A*算法在栅格地图上规划全局路径；所述改进后的全局路径规划A*算法为在全局路径规划A*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；
平滑处理模块，用于采用平滑策略对所述全局划路径进行平滑处理，得到平滑后的全局路径；
路径融合模块，用于利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得所述规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合；所述融合算法为所述改进后的全局路径规划A*算法与局部路径规划动态窗口法融合得到的算法。
与现有技术相比，本发明至少具有以下有益效果：本发明提供的一种移动机器人全局路径规划与局部路径规划融合方法，解决了传统全局路径规划A*算法存在路径优化不足、搜索效率低以及拐点多的问题，将节点距离信息集成到以指数衰减函数为系数的启发式函数中，以加快全局寻优的搜索速度，同时还考虑了上一代父节点的影响，降低了节点往返搜索概率。获取初始路径后，采用梯度下降法平滑路径。融合改进全局路径规划A*算法与局部路径规划算法，将评价函数改为考虑全局最优，解决了局部路径容易陷入局部最优性的问题，采用全局路径规划与局部路径规划融合算法进行路径规划不仅能满足全局最优性，而且能躲避未知障碍物。仿真实验和真实场景下Turtlrbot机器人试验验证了改进后的全局路径规划A*算法规划路径搜索时间、遍历节点数量、拐点数量相较于原始全局路径规划A*算法明显减少，在不同特征的环境中，全局路径规划与局部路径规划融合算法都能在全局最优的基础上成功规划出无碰撞路径，验证了全局路径规划与局部路径规划融合算法的全局最优性能和实时避障能力。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明具体实施方式中的技术方案，下面将对具体实施方式描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明中改进全局路径规划A*算法的流程示意图；
图2(a)是本发明实施例中30*30的栅格地图中原始全局路径规划A*算法规划路径仿真图，图2(b)是本发明实施例中30*30的栅格地图中改进全局路径规划A*算法规划路径仿真图；
图3(a)是本发明实施例中50*50的栅格地图中原始全局路径规划A*算法规划路径仿真图，图3(b)是本发明实施例中50*50的栅格地图中改进全局路径规划A*算法规划路径仿真图；
图4(a)是本发明实施例中30*30的栅格地图中全局与局部路径规划融合算法规划路径仿真图，图4(b)是本发明实施例中50*50的栅格地图中全局与局部路径规划融合算法规划路径仿真图；
图5(a)是本发明实施例中室内电梯口真实场景，图5(b)是本发明实施例中走廊真实场景；
图6(a)是本发明实施例中室外电梯口场景下原始全局路径规划A*算法路径规划，图6(b)是本发明实施例中室外电梯口场景下改进全局路径规划A*算法路径规划，图6(c)是本发明实施例中室外电梯口场景下移动机器人沿规划路径移动到终点示意图；
图7(a)是本发明实施例中走廊场景下原始全局路径规划A*算法路径规划，图7(b)是本发明实施例中走廊场景下改进全局路径规划A*算法路径规划，图7(c)是本发明实施例中走廊场景下机器人沿规划路径移动到终点示意图；
图8是本发明移动机器人全局路径规划与局部路径规划融合算法的流程示意图；
图9(a)是本发明实施例中走廊场景添加未知行人，图9(b)是本发明实施例中空旷场景添加未知障碍物；
图10是走廊添加未知行人环境全局路径规划与局部路径规划融合算法路径规划结果；
图11是电梯口添加未知障碍物场景下全局路径规划与局部路径规划融合算法路径规划过程，图11(a)是本发明实施例中已知地图上路径规划初始结果，图11(b)是本发明实施例中机器人第一次遇到未知障碍物规划路径，图11(c)是本发明实施例中机器人第二次遇到未知障碍物规划路径，图11(d)是本发明实施例中机器人沿规划路径移动到终点。
具体实施方式
作为本发明的某一具体实施方式，一种移动机器人全局路径规划与局部路径规划融合方法，具体包括如下步骤：
步骤1，利用改进后的全局路径规划A*算法在栅格地图上规划全局路径；改进后的全局路径规划A*算法为在全局路径规划A*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法，改进后的全局路径规划A*算法的表达式为：
其中，
公式中，x和y分别代表横坐标和纵坐标，f(n)为当前位置n的成本评价函数；g(n)为机器人从初始位置到当前位置n的实际成本；h(n)为机器人从当前位置n到目标位置的估计成本；dnt是当前节点n与目标之间的长度；dst是起点与目标之间的长度；h(p)是当前节点n的父节点的启发式函数；β为节点方向角，表示起始点到当前节点n的向量与节点n指向目标点的向量之间的夹角。
具体地说，利用改进后的全局路径规划A*算法在栅格地图上规划全局路径，具体流程图如图1所示，包括以下步骤：
步骤1.1，设定open列表和close列表，分别存储没有访问过的节点信息和已经访问过的节点信息；
步骤1.2，开始搜索时，close列表为空，访问过开始节点后，将开始节点从open列表中删除，将开始节点加入close列表中，利用改进后的全局路径规划A*算法反复搜索，寻找目标点；
步骤1.3，在到达目标点之前，继续搜索开始节点的相邻节点，判断搜索到的新节点是否在open列表中，如果不在open列表中，则将该新节点加入open列表中，采用成本评价函数计算该新节点的代价值f，将该新节点设置为当前搜索点的父节点并加入close列表中；如果在open列表中，则执行步骤1.4；
步骤1.4，采用成本评价函数计算该新节点的代价值f，并与前一次计算出来的相同节点的代价值f进行对比，选取代价值f最小的节点作为下一次搜索点的父节点，并将代价值f最小的节点加入close列表中，然后继续更新open列表中的节点信息；
步骤1.5，循环步骤1.1至步骤1.4，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径。
步骤2，采用平滑策略对全局划路径进行平滑处理，得到平滑后的全局路径；作为优选的实施方式，平滑策略采用梯度下降法，具体包括以下步骤：
步骤2.1，假设有原始全局路径v和平滑的全局路径s，最小化式d1和d2；
式中，d1为平滑后的全局路径与原平滑的全局滑路径的偏移量；d2是平滑后的全局路径一个节点到下一个节点的偏移量；平滑后的全局路径的起点与终点与原始全局路径相同，即起点v1＝s1和指定目标vn＝sn；
步骤2.2，设定误差阈值e，利用梯度下降法更新
和
从而达到优化的目的。在梯度下降法优化过程中，
式中，α和β为在梯度方向上的搜索步长；
步骤2.3，考虑与前一状态偏差，则有：
步骤2.4，更新
和
直到梯度下降的距离小于误差阈值，则停止更新，得到更加平滑的全局路径。
步骤3，利用融合算法从平滑后的全局路径上选取局部路径规划的目标，使得规划路径接***滑后的全局路径，完成全局路径规划与局部路径规划融合；
融合算法为改进后的全局路径规划A*算法与局部路径规划动态窗口法融合得到的算法，融合算法的评价函数的表达式为：
G(v,w)＝GoalCost(v,w)-ObstacleCost(v,w)
其中，
GoalCost(v,w)＝k1·[Dtarget(t-1)-Dtarget(t)]
ObstacleCost(v,w)＝max(k2·[Dsafe-Dobs(t)],0)
式中，k1为前向增益；Dtarget(t-1)为t-1时刻机器人与目标之间的距离；Dtarget(t)是t时刻机器人与局部目标点之间的距离；Dtarget(t-1)-Dtarget(t)用于测量机器人与目标之间的距离增量；k2为障碍增益；Dsafe为制动距离，即机器人在最大加速度下将最大线速度减速为零所需的欧氏距离；Dobs(t)是t时刻机器人到最近障碍物的距离。
具体地说，利用融合算法从平滑后的全局路径上选取局部路径规划的目标，使得规划路径接***滑后的全局路径，完成全局路径规划与局部路径规划融合，具体包括以下步骤：
步骤3.1，对平滑后的全局路径进行初始化；
步骤3.2，机器人在速度空间(v,w)中进行采样，生成模拟轨迹；
步骤3.3，在考虑全局最优路径的基础上，在步骤3.2生成的模拟轨迹中，根据融合算法的评价函数选择最优路径；
步骤3.4，机器人沿着步骤3.3中得到的最优路径移动；
步骤3.5，判断是否移动到终点，如果移动到终点则结束，如果不是终点，则返回步骤3.2。
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
步骤1，根据改进全局路径规划A*算法在栅格地图上规划路径。
图1为本发明改进全局路径规划A*方法的流程图，具体步骤如下：
步骤1.1，设定open列表和close列表，分别存储没有访问和已经访问过的节点信息。
步骤1.2，开始搜索时，close列表为空，访问过开始节点后，将其从open列表删除，加入close列表中，A*算法反复搜索，寻找目标点。
步骤1.3，在到达目标点之前，继续搜索其相邻节点，判断搜索到的新节点是否在open列表中，如果没有，将新节点加入open列表，计算这个节点的代价值f，将此节点设置为当前搜索点的父节点加入close列表中。
A*算法中，节点的代价值采用成本评价函数计算。
A*算法是启发式搜索算法，用于在静态二维构型空间中计算最优路径。依据成本评价函数进行搜索，搜索到满足成本评价函数的点作为下一次要搜索的点，并重复此过程，直到找到目标点，形成最优路径。成本评价函数如下：
f(n)＝h(n)+g(n) (1)
其中，f(n)为当前位置n的成本评价函数，g(n)为机器人从初始位置到当前位置n的实际成本，h(n)为机器人从当前位置n到目标位置的估计成本。h(n)的选择直接影响到A*算法的成功率和准确率。
本发明使用四轮驱动机器人，移动方位不受限制，并且欧几里得距离可以直接反映两点之间的距离，选用欧几里得距离作为估价函数，更加接近于实际距离。欧几里得距离表示为当前节点与目标节点之间的距离：
当机器人的移动方位不受限制时，使用欧几里得距离。
在现有全局路径规划技术A*算法中，实际代价和估计代价的权重均为1。网格地图的实际成本远远大于估计成本，这将扩大搜索的范围节点，导致算法的效率显著降低。另一方面，A*算法的启发函数中只考虑了当前节点到目标节点的距离。针对上述问题，我们将节点距离信息集成到以指数衰减函数为系数的启发式函数中，以加快全局寻优的搜索速度，同时考虑了上一代父节点的影响，降低了节点往返搜索的概率。假设当前临时标记节点为n，节点的父节点设为p，则改进后的启发函数如下：
其中，dnt是当前节点n与目标之间的长度，dst是起点与目标之间的长度，h(p)是当前节点n的父节点的启发式函数。节点方向角β为起始点到当前节点n的向量与节点n指向目标点的向量之间的夹角。
综上所述，改进后的A*算法评价函数可写成式(4):
改进后的A*算法评价函数将节点角度信息和距离信息集成到评价函数中，使A*算法在算法搜索时具有方向性和明确的目的，避免了不必要的节点搜索，遍历点减少，从而路径搜索时间减少，提高了算法的效率。
步骤1.4，如果搜索到的新节点已经存在于open列表中，就计算这些节点的代价值f，与前一次计算出来的相同节点的f值进行对比，选取f值最小的那一点作为下一次搜索点的父节点，并将此节点加入close列表中。然后继续更新open列表的信息，
步骤1.5，循环以上过程，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束。如果此时open列表为空，则表示没有路径。
步骤2，对步骤1中获取的路径采用平滑策略。
获取路径后，采用梯度下降法对路径进行平滑处理，在保证安全性的同时减少了路径的冗余拐点和大角度转弯，同时缩短了路径长度。
在梯度下降法迭代中，如果步长太长，可能会错过最优解，如果步长过小，速度过慢，算法循环会持续很长时间。因此在机器人研究中，采用递归性地逼近最小偏差模型。目标函数f(x)的梯度可表示为：
其基本思想为沿梯度下降的方向求解目标函数的最小值，迭代公式为：
yk＝yk-1-βgk (6)
其中gk表示函数的梯度，搜索方向为梯度负方向，β表示搜索步长，即搜索到使函数值最小的一点。
步骤2.1，假设有原始路径v和平滑路径s，平滑算法核心思想是最小化式(7)和式(8)：：
其中d1为平滑后路径与原平滑路径的偏移量，同时，d2是平滑后路径一个节点到下一个节点的偏移量。如前所述，需要最小化d1和d2，以实现平滑路径。平滑后路径的起点与终点与原始路径相同，即起点v1＝s1和指定目标vn＝sn。
步骤2.2，设定误差阈值e，利用梯度下降法更新式(9)和式(10)，从而达到优化的目的。在梯度下降法优化过程中，
其中，α和β为在梯度方向上的搜索步长。
步骤2.3，除此之外，还可以考虑与前一状态偏差，则有：
步骤2.4，更新式(9)和(11)，直到梯度下降的距离小于误差阈值，则停止更新，得到更加平滑的路径。
步骤3，如图1所示，将步骤1和步骤2改进的全局路径A*算法与局部路径规划动态窗口法算法融合：融合算法考虑了改进A*算法获得的全局路径信息，引入了全局路径规划和局部目标，从全局路径上选取局部路径规划的目标，使得规划结果接近全局路径。通过对障碍物碰撞可能性的权重因子进行分段设计，将避障目标和到目标时间最短的目标分离出来。针对避障目标，在安全距离项中给出一个很大的增益，使评估函数对可能发生的碰撞非常敏感。
步骤3.1，对步骤2中得到的平滑后的路径进行初始化。
步骤3.2，机器人在速度空间(v,w)中进行采样，生成模拟轨迹。
步骤3.3，在考虑全局最优路径的基础上，在步骤3.2生成的模拟轨迹中，根据评价函数选择最优路径。
为了使机器人到达目标时间最短，在评价函数中提出了一个显式的动态项来表示到目标的距离。在新的评价函数中考虑了最大前向速度对应的安全制动距离，如式(12)所示：
G(v,w)＝GoalCost(v,w)-ObstacleCost(v,w) (12)
其中，
GoalCost(v,w)＝k1·[Dtarget(t-1)-Dtarget(t)] (13)
ObstacleCost(v,w)＝max(k2·[Dsafe-Dobs(t)],0) (14)
其中k1为前向增益，Dtarget(t-1)为t-1时刻机器人与目标之间的距离。Dtarget(t)是t时刻机器人与局部目标点之间的距离。Dtarget(t-1)-Dtarget(t)用于测量机器人与目标之间的距离增量。k2为障碍增益，Dsafe为制动距离，即机器人在最大加速度下将最大线速度减速为零所需的欧氏距离，Dobs(t)是t时刻机器人到最近障碍物的距离。
当所选择的速度可能使机器人无法避障时，这意味着ObstacleCost为正(Dsafe大于Dobs(t))，较大的障碍增益会使得G成为一个较大的负数。在这种情况下，GoalCost被忽略，所以算法会选择最快的速度避开障碍物，保证机器人的安全。由于ObstacleCost的分段权值设计，使得该方法对障碍物的碰撞极为敏感，从而提高了避障率。
步骤3.4，机器人沿着步骤3.3中得到的最优路径移动。
步骤3.5，判断是否移动到终点，如果移动到终点则结束，如果不是终点，则返回步骤3.2。
图1为本发明所述改进全局路径规划A*算法的流程图。为验证本发明改进全局路径规划A*算法和全局路径规划与局部路径规划融合算法的有效性，设置两种不同大小、障碍物不同的两种地图，将改进全局路径规划A*算法与原始全局路径规划A*算法进行对比，分别从路径长度、遍历节点数、搜索时间、拐点数量这四个方面对算法进行了性能仿真分析，在全局路径地图中，设置未知障碍物，验证了融合算法的可行性。
在两种不同大小、障碍物不同的栅格地图上，对本发明所提改进全局路径规划A*算法和原始全局路径规划A*算法性能进行仿真实验，图2为30*30栅格的地图，图2中黑色方块为障碍物，起点和目标点为箭头所标注位置，灰色阴影部分为算法在搜索过程中所遍历节点数，图2(a)为原始全局路径规划A*算法规划结果，图2(b)为本发明改进全局路径规划A*算法规划结果，对比2(a)和2(b)可以看出，本发明改进全局路径规划A*算法遍历节点和拐点数量减少。
图3为50*50栅格地图，图3(a)为原始全局路径规划A*算法规划结果，图3(b)为本发明改进全局路径规划A*算法规划结果，对比图3(a)和(b)可以看出，在更加复杂的场景中，改进算法仍然可行。表1为两种地图路径规划仿真实验参数对比，由表1可以看出，在不同大小的栅格地图中，在路径长度基本保持一致的情况下，本发明改进算法规划路径所用搜索时间、遍历节点数量、拐点数量相较于原始全局路径规划A*算法明显减少，进一步验证了本发明改进全局路径规划A*算法的有效性。
表1两种栅格地图路径规划仿真实验参数
为了验证本发明所述的全局路径规划与局部路径规划融合算法有效性，在全局路径地图上添加未知障碍物，对全局路径规划与局部路径规划融合算法进行仿真。图4为不同地图上全局路径规划与局部路径规划融合算法路径规划结果，图中箭头标注为未知障碍物，虚线为改进全局路径规划A*算法规划结果，曲线为全局路径规划与局部路径规划融合算法路径规划结果，全局路径上的标注出来的点为全局路径规划与局部路径规划融合算法规划路径的局部目标点。图4(a)和(b)分别为30*30和50*50的地图上全局路径规划与局部路径规划融合算法路径规划结果，由图4(a)和(b)可以看出，无论如何改变地图大小、障碍物的位置、数量和大小，融合算法都能在全局最优的基础上成功规划一条无碰撞路径。从而验证了全局路径规划与局部路径规划融合算法的全局最优性能和实时避障能力。
在实际情况下，机器人的工作环境比仿真环境更加复杂，环境特征更加丰富，存在行人等不确定因素，为了验证改进算法在这些情况下的有效性，分别采用传统全局路径规划A*算法、本发明改进全局路径规划A*算法、本发明所提改进全局路径规划A*算法和局部路径规划融合算法，基于Turtlebot机器人对西安理工大学教五楼不同环境分别进行了路径规划试验。
室外电梯口和走廊场景如图5(a)和(b)所示，图6为图5(a)所示室外电梯口的路径规划实验结果，图6(a)为原始全局路径规划A*算法路径规划结果，图6(b)为本发明改进全局路径规划A*算法路径规划结果，图6(c)为移动机器人沿着规划好的路径移动到终点。图6(a)中，原始A*算法规划出来的路径存在较多拐点，会导致路径长度过长，转折点不平滑，实际导航中移动机器人难以顺利移动到终点，移动过程中耗时较长，而图6(b)中本发明改进全局路径规划A*算法规划出来的路径则缓解了路径中拐点较多、转折点不平滑和路径耗费时间较长的问题，并在实际操作中移动机器人能顺利、快速地沿着规划好的路径从起点移动到终点。
图7为图5(b)所示走廊场景下路径规划结果，图7(a)为原始全局路径规划A*算法路径规划结果，图7(b)为本发明改进全局路径规划A*算法路径规划结果，图5(b)中障碍物呈S形状摆放，在已构建好的地图上手动为机器人指定起点与终点，图7(a)中所规划路径存在较多拐点，路径不平滑，而图7(b)中所规划路径更为平滑，机器人能顺利、快速的从起点移动到终点。由此可见，本发明改进全局路径规划A*算法不论在较为空旷的环境中还是在较窄的环境中，都能顺利躲避障碍物，规划出一条更优的路径顺利达到终点。
此外，不同真实环境机器人路径规划试验参数如表2所示，可以看出，改进路径规划算法在路径搜索时间和路径规划长度两方面都得到了优化。
表2不同真实环境路径规划试验参数
图8为本发明所述的全局路径规划与局部路径规划融合算法流程图。在真实场景中，除了已知障碍物，还会出现未知障碍物，因此本发明在已知场景中添加未知障碍物，对本发明所述的全局路径规划与局部路径规划融合算法进行路径规划试验。首先在图5(b)所示走廊场景中添加已知地图中未见的行人，场景如图9(a)所示。
在已知地图上为移动机器人设定好起点与终点，图10为走廊添加未知行人环境下机器人移动过程中规划路径结果。由图10可看出，本发明所提出的全局路径规划与局部融合算法在路径规划过程中不仅能保证全局最优性，而且在机器人移动过程中能实时躲避行人，成功到达终点。
图11为在图9(b)所示空旷场景添加未知障碍物的情况下利用本发明所述全局路径规划与局部路径规划融合算法进行路径规划的过程，图9(b)中所摆放三个障碍物为未知障碍物，图11(a)为机器人在已知地图上路径规划初始结果，图11(b)为机器人第一次遇到未知障碍物规划路径，图11(c)为机器人第二次遇到未知障碍物规划路径，图11(d)为移动机器人移动到终点。由图11可以看出，当未知障碍物较多时，机器人也能成功躲避，顺利的移动到终点，从而验证了本发明所提全局与局部路径规划融合算法的有效性。
最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。
Claims (2)
1.一种移动机器人全局路径规划与局部路径规划融合方法，其特征在于，包括：
利用改进后的全局路径规划A*算法在栅格地图上规划全局路径；所述改进后的全局路径规划A*算法为在全局路径规划A*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；
采用平滑策略对所述全局路径进行平滑处理，得到平滑后的全局路径；所述平滑策略为梯度下降法；
利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合；所述融合算法为所述改进后的全局路径规划A*算法与局部路径规划动态窗口法融合得到的算法；
所述改进后的全局路径规划A*算法的评价函数表达式为：
其中，
公式中，x和y分别代表横坐标和纵坐标；f(n)为当前位置n的成本评价函数；g(n)为机器人从初始位置到当前位置n的实际成本；h(n)为机器人从当前位置n到目标位置g的估计成本；dnt是当前节点n与目标之间的长度；dst是起点与目标之间的长度；h(p)是当前节点n的父节点的启发式函数；β为节点方向角，表示起始点到当前节点n的向量与节点n指向目标点的向量之间的夹角；
所述利用改进后的全局路径规划A*算法在栅格地图上规划全局路径，具体为：
步骤1.1，设定open列表和close列表，分别存储没有访问过的节点信息和已经访问过的节点信息；
步骤1.2，开始搜索时，close列表为空，访问过开始节点后，将所述开始节点从open列表中删除，将所述开始节点加入close列表中，利用所述改进后的全局路径规划A*算法反复搜索，寻找目标点；
步骤1.3，在到达目标点之前，继续搜索所述开始节点的相邻节点，判断搜索到的新节点是否在open列表中，如果不在open列表中，则将该新节点加入open列表中，采用成本评价函数计算该新节点的代价值f，将该新节点设置为当前搜索点的父节点并加入close列表中；如果在open列表中，则执行步骤1.4；
步骤1.4，采用成本评价函数计算该新节点的代价值f，并与前一次计算出来的相同节点的代价值f进行对比，选取代价值f最小的节点作为下一次搜索点的父节点，并将所述代价值f最小的节点加入close列表中，然后继续更新open列表中的节点信息；
步骤1.5，循环步骤1.1至步骤1.4，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径；
所述采用平滑策略对所述全局路径进行平滑处理，得到平滑的全局路径，具体包括：
步骤2.1，假设有原始全局路径v和平滑的全局路径s，最小化式d1和d2；
式中，i为路径中的点；d1为平滑后的全局路径与原平滑的全局路径的偏移量；d2是平滑后的全局路径一个节点到下一个节点的偏移量；平滑后的全局路径的起点与终点与原始全局路径相同，即起点v1＝s1和指定目标vn＝sn；
步骤2.2，设定误差阈值e，利用梯度下降法更新
和
从而达到优化的目的，在梯度下降法优化过程中，
式中，α和β为在梯度方向上的搜索步长；
步骤2.3，考虑与前一状态偏差，则有：
步骤2.4，更新
和
直到梯度下降的距离小于误差阈值，则停止更新，得到更加平滑的全局路径；
所述融合算法的评价函数的表达式为：
G(v,w)＝GoalCost(v,w)-ObstacleCost(v,w)
其中，
GoalCost(v,w)＝k1·[Dtarget(t-1)-Dtarget(t)]
ObstacleCost(v,w)＝max(k2·[Dsafe-Dobs(t)],0)
式中，k1为前向增益；Dtarget(t-1)为t-1时刻机器人与目标之间的距离；Dtarget(t)是t时刻机器人与局部目标点之间的距离；Dtarget(t-1)-Dtarget(t)用于测量机器人与目标之间的距离增量；k2为障碍增益；Dsafe为制动距离，即机器人在最大加速度下将最大线速度减速为零所需的欧氏距离；Dobs(t)是t时刻机器人到最近障碍物的距离；
所述利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得所述规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合，具体为：
步骤3.1，对平滑后的全局路径进行初始化；
步骤3.2，机器人在速度空间(v,w)中进行采样，生成模拟轨迹；
步骤3.3，在考虑全局最优路径的基础上，在步骤3.2生成的模拟轨迹中，根据所述融合算法的评价函数选择最优路径；
步骤3.4，机器人沿着步骤3.3中得到的最优路径移动；
步骤3.5，判断是否移动到终点，如果移动到终点则结束，如果不是终点，则返回步骤3.2。
2.一种移动机器人全局路径规划与局部路径规划融合设备，其特征在于，基于权利要求1所述的一种移动机器人全局路径规划与局部路径规划融合方法，包括：
路径规划模块，用于利用改进后的全局路径规划A*算法在栅格地图上规划全局路径；所述改进后的全局路径规划A*算法为在全局路径规划A*算法的成本评价函数中集成有节点角度信息和节点距离信息的算法；
平滑处理模块，用于采用平滑策略对所述全局路径进行平滑处理，得到平滑后的全局路径；
路径融合模块，用于利用融合算法从所述平滑后的全局路径上选取局部路径规划的目标，使得规划路径接近所述平滑后的全局路径，完成全局路径规划与局部路径规划融合；所述融合算法为所述改进后的全局路径规划A*算法与局部路径规划动态窗口法融合得到的算法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110579208.3A CN113359718B (zh) | 2021-05-26 | 2021-05-26 | 移动机器人全局路径规划与局部路径规划融合方法及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110579208.3A CN113359718B (zh) | 2021-05-26 | 2021-05-26 | 移动机器人全局路径规划与局部路径规划融合方法及设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113359718A CN113359718A (zh) | 2021-09-07 |
| CN113359718B true CN113359718B (zh) | 2022-11-22 |
Family
ID=77527708
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110579208.3A Active CN113359718B (zh) | 2021-05-26 | 2021-05-26 | 移动机器人全局路径规划与局部路径规划融合方法及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113359718B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114167872A (zh) * | 2021-12-08 | 2022-03-11 | 江西省智能产业技术创新研究院 | 机器人避障方法、***、计算机及机器人 |
| CN114510056A (zh) * | 2022-02-21 | 2022-05-17 | 上海机器人产业技术研究院有限公司 | 室内移动机器人的平稳移动全局路径规划方法 |
| CN115326057A (zh) * | 2022-08-31 | 2022-11-11 | 深圳鹏行智能研究有限公司 | 路径规划方法、装置、机器人以及可读存储介质 |
| CN115562255A (zh) * | 2022-09-13 | 2023-01-03 | 中国安全生产科学研究院 | 基于空地协同的多智能消防机器人消防水带防缠绕方法 |
| CN115562265B (zh) * | 2022-09-29 | 2024-01-05 | 哈尔滨理工大学 | 一种基于改进a*算法的移动机器人路径规划方法 |
| CN116013074A (zh) * | 2023-01-05 | 2023-04-25 | 北京清丰智行科技有限公司 | 一种园区基于车路云协同的智慧出行*** |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108983780A (zh) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | 一种基于改进rrt*算法的移动机器人路径规划方法 |
| CN110487295A (zh) * | 2019-09-06 | 2019-11-22 | 中国计量大学 | 一种时间优化的平滑a*算法 |
| CN111694364A (zh) * | 2020-06-30 | 2020-09-22 | 山东交通学院 | 一种应用于智能车路径规划的基于改进蚁群算法与动态窗口法的混合算法 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6857145B2 (ja) * | 2018-03-09 | 2021-04-14 | 株式会社日立製作所 | 軌道計画装置、軌道計画方法、及び生産システム |
| KR101990878B1 (ko) * | 2018-11-29 | 2019-06-19 | 주식회사 트위니 | 시간상태 영역에서의 온라인 양방향 경로 계획 방법, 이를 구현하기 위한 프로그램이 저장된 기록매체 및 이를 구현하기 위해 매체에 저장된 컴퓨터프로그램 |
| CN109798909A (zh) * | 2019-02-01 | 2019-05-24 | 安徽达特智能科技有限公司 | 一种全局路径规划的方法 |
| US20210103286A1 (en) * | 2019-10-04 | 2021-04-08 | Hong Kong Applied Science And Technology Research Institute Co., Ltd. | Systems and methods for adaptive path planning |
| CN111174798A (zh) * | 2020-01-17 | 2020-05-19 | 长安大学 | 一种足式机器人路径规划方法 |
| CN112631294B (zh) * | 2020-12-16 | 2022-06-28 | 上海应用技术大学 | 一种移动机器人智能路径规划方法 |
-
2021
- 2021-05-26 CN CN202110579208.3A patent/CN113359718B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108983780A (zh) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | 一种基于改进rrt*算法的移动机器人路径规划方法 |
| CN110487295A (zh) * | 2019-09-06 | 2019-11-22 | 中国计量大学 | 一种时间优化的平滑a*算法 |
| CN111694364A (zh) * | 2020-06-30 | 2020-09-22 | 山东交通学院 | 一种应用于智能车路径规划的基于改进蚁群算法与动态窗口法的混合算法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于改进A*算法机器人路径规划研究;王小红等;《计算机测量与控制》;20180725(第07期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113359718A (zh) | 2021-09-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113359718B (zh) | 移动机器人全局路径规划与局部路径规划融合方法及设备 | |
| CN112325884B (zh) | 一种基于dwa的ros机器人局部路径规划方法 | |
| CN106371445B (zh) | 一种基于拓扑地图的无人车规划控制方法 | |
| CN113821029B (zh) | 一种路径规划方法、装置、设备及存储介质 | |
| CN110228069A (zh) | 一种机械臂在线避障运动规划方法 | |
| CN115079705A (zh) | 基于改进a星融合dwa优化算法的巡检机器人路径规划方法 | |
| CN110509279A (zh) | 一种仿人机械臂的运动路径规划方法及*** | |
| CN108444490B (zh) | 基于可视图和a*算法深度融合的机器人路径规划方法 | |
| CN112683275B (zh) | 一种栅格地图的路径规划方法 | |
| CN113359768A (zh) | 一种基于改进的a*算法的路径规划方法 | |
| CN112114584A (zh) | 一种球形两栖机器人的全局路径规划方法 | |
| Gao et al. | Multi-mobile robot autonomous navigation system for intelligent logistics | |
| Wang | Path planning of mobile robot based on a* algorithm | |
| CN115167398A (zh) | 一种基于改进a星算法的无人船路径规划方法 | |
| CN114596360B (zh) | 一种基于图拓扑的双阶段主动即时定位与建图算法 | |
| CN113359714B (zh) | 基于粒子滤波算法的巡检机器人动态路径规划方法及装置 | |
| Cheng et al. | Research on path planning of mobile robot based on dynamic environment | |
| CN113296504A (zh) | 基于rgbd深度相机的移动机器人建图与路径规划方法 | |
| Zhang et al. | A multi-goal global dynamic path planning method for indoor mobile robot | |
| CN115826586A (zh) | 一种融合全局算法和局部算法的路径规划方法及*** | |
| Li et al. | An enhanced direction calibration based on reinforcement learning for indoor localization system | |
| Yan et al. | A study of improved global path planning algorithm for parking robot based on Ros | |
| CN111912411B (zh) | 一种机器人导航定位方法、***及存储介质 | |
| CN114310904A (zh) | 一种适用于机械臂关节空间路径规划的新型双向rrt*方法 | |
| Li et al. | Research on AUV path planning based on improved ant colony algorithm |
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 |