CN115826586B - 一种融合全局算法和局部算法的路径规划方法及系统 - Google Patents

Abstract

本申请公开了一种融合全局算法和局部算法的路径规划方法及系统，其中方法包括：基于A‑star算法进行初步路径规划，得到基础路线；对基础路线进行优化，得到改进后算法；在改进后算法中融合动态窗口法，得到最终算法；利用最终算法来对机器人路径进行规划。本申请在A‑star算法的基础上，增加动态加权系数、子节点选择和二次规划，提高了规划效率，增加了路径平滑度，使规划轨迹在确保安全的前提下更贴近全局最优。本申请增加动、静态障碍物子函数，可通过调节子函数的权重大小，减少静态障碍物对轨迹规划的影响，增强移动机器人对动态障碍物的避障灵敏度。

Description

一种融合全局算法和局部算法的路径规划方法及系统

技术领域

本申请涉及机器人路径规划领域，具体涉及一种融合全局算法和局部算法的路径规划方法及系统。

背景技术

近些年来，无人车、无人飞行器、移动机器人等移动智能体逐渐进入人们的视野。它不仅被应用到医疗、物流、工业生产等领域帮助人们完成枯燥、繁重的工作，还可以用于航天，完成月球探索计划。路径规划是移动机器人实现自主导航的关键技术之一，也是重点难点。

路径规划的任务就是移动机器人在已知、未知或部分未知的环境中以最小成本规划出一条从初始位置到目标位置的无碰撞路径，路径应满足搜索时间短、安全性高、长度短、路径平滑等一系列要求。

全局路径规划按照优化算法可以分为随机采样类算法、群体智能优化算法、图搜索类算法等。随机采样类算法主要包括快速扩展随机树算法和概率路线图算法，在高维状态空间应用广泛，其搜索能力强，但该类算法运算成本高、随机性强。群体智能优化算法主要包括遗传算法、蚁群算法、粒子群算法等，适用于复杂问题的求解和优化，能同时处理多个个体，易于实现并行性，但是这类算法对参数的依赖性较强，导致方法的优化性能与人的经验密切相关，很难使算法性能最优化，且进化速度慢，运算效率低。在路径规划算法的各类应用中，基于图搜索的方法是最为成熟的。相较于基于采样的算法，分辨率更完备，能够快速得到全局最优路径，但生成的路径连续性差，不适合直接应用于机器人运动，且该类算法在高维空间搜索性能不可靠，原因在于在二维空间中，图搜索算法是基于当前点向四周扩散，计算四周扩展节点的累加成本，而在三维或者更高维的空间中，其四周扩展节点数量迅速增加，这大大增加了每一次节点扩展的计算成本。常用的基于图搜索的路径规划算法例如A-star算法，Dijkstra算法，广度优先遍历（Breadth First Search，BFS），深度优先遍历（Depth First Search，DFS）等。局部路径规划算法有动态窗口法(Dynamic windowapproach，DWA)、Timed Elastic Band算法、人工势能法等等。局部路径规划算法是通过结合传感器的数据进行在线实时局部路径规划，具有良好的避障能力，适用于动态环境中的机器人路径规划，但前瞻性不足，容易陷入U型障碍，难以保证路径最优性。随着移动机器人应用场景日益复杂，环境建模成本较高，而在未知、复杂环境中找到一条最优或次优路径是不容易的，单一算法无法长时间满足实际需求。

发明内容

本申请通过融合算法来进行路径规划，能在保证实时性和高效率的前提下保证路径的最优性。

为实现上述目的，本申请提供了一种融合全局算法和局部算法的路径规划方法，步骤包括：

基于A-star算法进行初步路径规划，得到基础路线；

对所述基础路线进行优化，得到改进后算法；

在所述改进后算法中融合动态窗口法，得到最终算法；利用所述最终算法来对机器人路径进行规划。

优选的，进行所述初步路径规划的方法包括：

在Dijkstra算法的基础上添加启发式函数；

评价当前节点到起始点的距离与所述启发式函数之间的大小关系，来进行所述初步路径规划。

优选的，对所述基础路线进行优化的方法包括：增加子节点选择规则和删除冗余节点。

优选的，所述增加子节点选择规则的方法包括：生成子节点时，判断所述子节点与障碍物的位置关系，来避免斜穿障碍物顶点的不安全行为，提高搜索效率，使运行更平稳。

优选的，所述删除冗余节点的方法包括：设置路径点从起始点到目标点排序集合closelist=[S,N₁,…N_n,G]，定义N₁，N₂，…，N_n；其中，S表示节点；G表示目标节点；连接SN₂，若SN₂不经过障碍物，且线段SN₂到障碍物的距离小于预设阈值，则继续连接SN₃，直到SN_m(m=3,4,…,n)到障碍物的距离大于预设阈值，向后退一个单元格，将S与N_m-1连接起来，同时删除中间冗余点，更新路径；重复步骤直至路径中没有冗余转折点。

优选的，所述动态窗口法包括：根据机器人运动模型在速度空间中进行多组速度的采样，确定由所有可行速度组成的动态窗口，预测时间内每组不同速度下机器人可能的运动轨迹，同时使用预设的评价函数评估集合的轨迹并选择具有最高评估的集合执行直到下一个执行时间。

优选的，融合所述动态窗口法的方法包括：

设定keylist=[S,P₁,…P_n,G]中的每一个路径点作为局部路径规划的子目标点；

当机器人从起始点S移动时，以P₁为子目标G_min，即设置G_min=P₁；

当机器人与子目标Gmin = P_i的距离小于阈值时，子目标将切换到下一个关键路径点，即设置G_min= P_{i + 1}；

当Gmin= P_i且关键路径点P_{i + j}，j>0，出现在规划窗口的路径中时，子目标切换到Gmin=P_{i + j}。

本申请还提供了一种融合全局算法和局部算法的路径规划系统，包括：初始规划模块、优化模块和融合模块；

所述初始规划模块用于基于A-star算法进行初步路径规划，得到基础路线；

所述优化模块用于对所述基础路线进行优化，得到改进后算法；

所述融合模块用于在所述改进后算法中融合动态窗口法，得到最终算法；利用所述最终算法来对机器人路径进行规划。

与现有技术相比，本申请的有益效果如下：

在A-star算法的基础上，增加动态加权系数、子节点选择和二次规划，提高了规划效率，增加了路径平滑度，使规划轨迹在确保安全的前提下更贴近全局最优。本申请改进的A-star算法具有最优性、高效率、高安全等优点。增加动、静态障碍物子函数，可通过调节子函数的权重大小，减少静态障碍物对轨迹规划的影响，增强移动机器人对动态障碍物的避障灵敏度。本申请既考虑到环境中的未知障碍物，又能保证路径规划的完备性、实时性，满足机器人的运动约束。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本申请邻居栅格节点示意图；

图2为本申请初步路径规划前的风险路径示意图；

图3为本申请初步路径规划后的优化后路径示意图；

图4为本申请防碰撞安全距离示意图；

图5为本申请最终路径优化前后对比示意图；

图6为本申请融合算法的流程示意图；

图7为本申请的系统结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

为使本申请的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本申请作进一步详细的说明。

实施例一 本实施例中，首先利用A-star算法进行初步路径规划，得到基础路线。

A-star算法为在Dijkstra算法的基础上添加了启发式函数，不仅考虑节点的实际消耗，也计算节点的估计代价消耗，利用评价函数来指导节点的搜索与扩展，因此评价函数影响搜索空间的大小和算法的速度。A-star算法的代价函数定义为公式(1)：

其中f(n)表示从起始节点S到目标节点G总的代价消耗。g(n)表示从起始节点S到当前节点n的实际消耗，h(n)为当前节点n到目标节点G的估计代价消耗值。

由式(1)可知，A-star算法的评价函数f(n)由代价函数g(n)和启发函数h(n)组成。其中，启发函数h(n)主导了A-star算法的搜索性能。在不同的地图环境中，随着搜索深度即当前节点到起始点的距离depth与启发函数h(n)之间的变化，启发函数的取值有如下情况：

当depth小于等于h(n)时，意味着目前位置离起始点近离目标点远，自适应增大权重，可减小搜索空间，加快搜索速度，提高目标点的方向性，在一定程度上减少遍历栅格数；

当depth大于h(n)时，不能一味增大加权系数ω(n)，只向目标点方向搜索，会陷入局部最优，出现多曲折转弯的路线，所以ω(n)取1，增大搜索空间，获得全局最优的路径。

栅格地图中每个栅格的中心点称为栅格节点，如图1所示，可移动的邻居栅格节点有8个，左右上下方向移动距离为1个单元格，斜对角方向的移动距离为

个单元格。

传统A-star算法规划出的路径存在斜穿障碍物顶点的现象，容易导致移动机器人与障碍物发生碰撞，风险路径如图2。

为避免斜穿障碍物的不安全行为，要对A-star算法进行优化；优化的方法包括：增加子节点选择规则和删除冗余节点。

增加子节点选择规则，生成子节点时判断子节点与障碍物的位置关系，以图1为例，假设某个子节点为障碍物，则子节点选择规则定义为：

(1)当障碍物在(0,+1)或(0,-1)位置时，则删除障碍节点的左右两个可选子节点(-1,+1)(+1,+1)或(-1,-1)(+1,-1);

(2)当障碍物在(-1,0)或(+1,0)位置时，则删除障碍节点的上下两个可选子节点(-1,+1)(-1，-1)或(+1,+1)(+1,-1);

(3)当障碍物在斜对角方向上，则不做处理。

经过上述选择节点的处理，有效避免了斜穿障碍物顶点的不安全行为，提高搜索效率，运行更平稳，图2所示的风险路径经过子节点优化后的路径如图3所示。

如图4所示，已知A(x_a,y_a)、B(x_b,y_b)、C(x_c,y_c)坐标，可求出障碍物中心C到路径AB的垂直距离d:

设安全距离D，若

，则该路径不可选，往后退一个路径点重新判断；若D≥d，则该路径可选。

删除冗余节点，增加防碰撞距离。如图5(a)所示为优化前路径点，路径点从起始点到目标点排序集合closelist=[S,N₁,…N_n,G]，定义N₁(x₁，y₁)，N₂(x₂，y₂)…N_n(x_n，y_n)。连接SN₂，若SN₂不经过障碍物，且线段SN₂到障碍物的距离d小于预设阈值D，则继续连接SN₃，直到SN_m(m=3,4,…,n)到障碍物的距离d大于预设阈值D，则向后退一个单元格，把S与N_m-1连接起来，同时删除中间冗余点，更新路径；从节点N₂重复上述操作，直到路径中没有冗余转折点，如图5(b)所示为优化后路径点keylist=[S,P₁,…P_n,G]。若不考虑防碰撞安全距离，则得到图5(c)的路径点，有发生碰撞的可能，不具备安全性。

最后，在改进后的A-star算法中融合动态窗口法（DWA）。动态窗口法的原理是根据机器人运动模型在速度空间(

)中进行多组速度的采样，确定由所有可行速度组成的动态窗口，预测时间T内每组不同速度(/>

)下机器人可能的运动轨迹，同时使用特定的评价函数评估集合的轨迹并选择具有最高评估的集合执行直到下一个执行时间。/>

假设机器人在时间间隔Δt内作匀速直线运动，故运动模型为

（5）

其中，

为机器人下一时刻的位姿信息，/>

为间隔时间。速度空间中有若干组采样速度，需要通过评价函数选取最优轨迹，DWA算法的评估函数表示为：

（6）

其中，α、β、γ为3项评价函数的加权系数，设置不同的比重。

用于衡量机器人对目标点的方向性，dist(v，ω) 表示预轨迹中距离障碍物的最小距离，vel（v，ω）表示当前速度大小评价函数，移动机器人速度越快，则评价得分越高。

传统的动态窗口法参照点只有一个目标，遇到U型障碍物容易陷入局部最优。改进的A-star算法遇到未知障碍物无法避免，实时性差。针对以上两大问题，提出将改进A-star算法与动态窗口法融合，步骤包括：

设定keylist=[S,P₁,…P_n,G]中的每一个路径点作为局部路径规划的子目标点；

当机器人从起始点S移动时，以P₁为子目标G_min，即设置G_min=P₁；

当机器人与子目标Gmin = P_i的距离小于阈值时，子目标将切换到下一个关键路径点，即设置G_min= P_{i + 1}；

当Gmin= P_i且关键路径点P_{i + j}，j>0，出现在规划窗口的路径中时，子目标切换到Gmin=P_{i + j}。融合算法的流程如图6所示。

实施例二

如图7所示，为本申请实施例二的系统结构示意图，包括：初始规划模块、优化模块和融合模块；初始规划模块基于A-star算法进行初步路径规划，得到基础路线；优化模块用于对基础路线进行优化，得到改进后算法；融合模块用于在改进后算法中融合动态窗口法，得到最终算法；并利用最终算法来对机器人路径进行规划。

首先利用初始规划模块进行初步路径规划，得到基础路线。

在本实施例中，初始规划模块利用A-star算法进行初步路径规划。

其为在Dijkstra算法的基础上添加了启发式函数，不仅考虑节点的实际消耗，也计算节点的估计代价消耗，利用评价函数来指导节点的搜索与扩展，因此评价函数影响搜索空间的大小和算法的速度。A-star算法的代价函数定义为公式(7)：

(7)

其中，f(n)表示从起始节点S到目标节点G总的代价消耗。g(n)表示从起始节点S到当前节点n的实际消耗，h(n)为当前节点n到目标节点G的估计代价消耗值。

由式(7)可知，A-star算法的评价函数f(n)由代价函数g(n)和启发函数h(n)组成。其中，启发函数h(n)主导了A-star算法的搜索性能。在不同的地图环境中，随着搜索深度即当前节点到起始点的距离depth与启发函数h(n)之间的变化，启发函数的取值有如下情况：

当depth小于等于h(n)时，意味着目前位置离起始点近离目标点远，自适应增大权重，可减小搜索空间，加快搜索速度，提高目标点的方向性，在一定程度上减少遍历栅格数；

当depth大于h(n)时，不能一味增大加权系数ω(n)，只向目标点方向搜索，会陷入局部最优，出现多曲折转弯的路线，所以ω(n)取1，增大搜索空间，获得全局最优的路径。

(8)

栅格地图中每个栅格的中心点称为栅格节点，如图1所示，可移动的邻居栅格节点有8个，左右上下方向移动距离为1个单元格，斜对角方向的移动距离为

个单元格。

传统A-star算法规划出的路径存在斜穿障碍物顶点的现象，容易导致移动机器人与障碍物发生碰撞，风险路径如图2。

为避免斜穿障碍物的不安全行为，利用优化模块对A-star算法进行优化；优化的方法包括：增加子节点选择规则和删除冗余节点。

增加子节点选择规则，生成子节点时判断子节点与障碍物的位置关系，以图1为例，假设某个子节点为障碍物，则子节点选择规则定义为：

(1)当障碍物在(0,+1)或(0,-1)位置时，则删除障碍节点的左右两个可选子节点(-1,+1)(+1,+1)或(-1,-1)(+1,-1);

(2)当障碍物在(-1,0)或(+1,0)位置时，则删除障碍节点的上下两个可选子节点(-1,+1)(-1，-1)或(+1,+1)(+1,-1);

(3)当障碍物在斜对角方向上，则不做处理。

经过上述选择节点的处理，有效避免了斜穿障碍物顶点的不安全行为，提高搜索效率，运行更平稳，图2所示的风险路径经过子节点优化后的路径如图3所示。

如图4所示，已知A(x_a,y_a)、B(x_b,y_b)、C(x_c,y_c)坐标，可求出障碍物中心C到路径AB的垂直距离d:

(9)

(10)

设安全距离D，若

，则该路径不可选，往后退一个路径点重新判断；若D≥d，则该路径可选。

删除冗余节点，增加防碰撞距离。如图5(a)所示为优化前路径点，路径点从起始点到目标点排序集合closelist=[S,N₁,…N_n,G]，定义N₁(x₁，y₁)，N₂(x₂，y₂)…N_n(x_n，y_n)。连接SN₂，若SN₂不经过障碍物，且线段SN₂到障碍物的距离d小于预设阈值D，则继续连接SN₃，直到SN_m(m=3,4,…,n)到障碍物的距离d大于预设阈值D，则向后退一个单元格，把S与N_m-1连接起来，同时删除中间冗余点，更新路径；从节点N₂重复上述操作，直到路径中没有冗余转折点，如图5(b)所示为优化后路径点keylist=[S,P₁,…P_n,G]。若不考虑防碰撞安全距离，则得到图5(c)的路径点，有发生碰撞的可能，不具备安全性。

最后，利用融合模块在改进后的A-star算法中融合动态窗口法（DWA）。动态窗口法的原理是根据机器人运动模型在速度空间(

)中进行多组速度的采样，确定由所有可行速度组成的动态窗口，预测时间T内每组不同速度(/>

)下机器人可能的运动轨迹，同时使用特定的评价函数评估集合的轨迹并选择具有最高评估的集合执行直到下一个执行时间。

假设机器人在时间间隔Δt内作匀速直线运动，故运动模型为

（11）

其中，

为机器人下一时刻的位姿信息，/>

为间隔时间。速度空间中有若干组采样速度，需要通过评价函数选取最优轨迹，DWA算法的评估函数表示为：

（12）

其中，α、β、γ为3项评价函数的加权系数，设置不同的比重。

用于衡量机器人对目标点的方向性，dist(v，ω) 表示预轨迹中距离障碍物的最小距离，vel（v，ω）表示当前速度大小评价函数，移动机器人速度越快，则评价得分越高。

传统的动态窗口法参照点只有一个目标，遇到U型障碍物容易陷入局部最优。改进的A-star算法遇到未知障碍物无法避免，实时性差。针对以上两大问题，提出将改进A-star算法与动态窗口法融合，步骤包括：

设定keylist=[S,P₁,…P_n,G]中的每一个路径点作为局部路径规划的子目标点；

当机器人从起始点S移动时，以P₁为子目标G_min，即设置G_min=P₁；

当机器人与子目标Gmin = P_i的距离小于阈值时，子目标将切换到下一个关键路径点，即设置G_min= P_{i + 1}；

当Gmin= P_i且关键路径点P_{i + j}，j>0，出现在规划窗口的路径中时，子目标切换到Gmin=P_{i + j}。融合算法的流程如图6所示。

以上所述的实施例仅是对本申请优选方式进行的描述，并非对本申请的范围进行限定，在不脱离本申请设计精神的前提下，本领域普通技术人员对本申请的技术方案做出的各种变形和改进，均应落入本申请权利要求书确定的保护范围内。

Claims (6)

1.一种融合全局算法和局部算法的路径规划方法，其特征在于，步骤包括：

基于A-star算法进行初步路径规划，得到基础路线；

对所述基础路线进行优化，得到改进后算法；

在所述改进后算法中融合动态窗口法，得到最终算法；利用所述最终算法来对机器人路径进行规划；对所述基础路线进行优化的方法包括：增加子节点选择规则和删除冗余节点；

子节点选择规则定义为：

(1)当障碍物在(0,+1)或(0,-1)位置时，则删除障碍节点的左右两个可选子节点(-1,+1)(+1,+1)或(-1,-1)(+1,-1);

(2)当障碍物在(-1,0)或(+1,0)位置时，则删除障碍节点的上下两个可选子节点(-1,+1)(-1，-1)或(+1,+1)(+1,-1);

(3)当障碍物在斜对角方向上，则不做处理；

所述删除冗余节点的方法包括：设置路径点从起始点到目标点排序集合closelist=[S,N₁,…N_n,G]，定义N₁，N₂，…，N_n；其中，S表示节点；G表示目标节点；连接SN₂，若SN₂不经过障碍物，且线段SN₂到障碍物的距离小于预设阈值，则继续连接SN₃，直到SN_m(m=3,4,…,n)到障碍物的距离大于预设阈值，向后退一个单元格，将S与N_m-1连接起来，同时删除中间冗余点，更新路径；重复步骤直至路径中没有冗余转折点；

所述A-star算法的评价函数定义为：数f(n)由代价函数g(n)和启发函数h(n)组成；随着搜索深度即当前节点到起始点的距离depth与启发函数h(n)之间的变化，启发函数的取值有如下情况：

当depth小于等于h(n)时，意味着目前位置离起始点近离目标点远，自适应增大权重，可减小搜索空间，加快搜索速度，提高目标点的方向性，在一定程度上减少遍历栅格数；当depth大于h(n)时，不能一味增大加权系数ω(n)，只向目标点方向搜索，会陷入局部最优，出现多曲折转弯的路线，所以ω(n)取1，增大搜索空间，获得全局最优的路径；

。

2.根据权利要求1所述的融合全局算法和局部算法的路径规划方法，其特征在于，进行所述初步路径规划的方法包括：

在Dijkstra算法的基础上添加启发式函数；

评价当前节点到起始点的距离与所述启发式函数之间的大小关系，来进行所述初步路径规划。

3.根据权利要求1所述的融合全局算法和局部算法的路径规划方法，其特征在于，所述增加子节点选择规则的方法包括：生成子节点时，判断所述子节点与障碍物的位置关系，来避免斜穿障碍物顶点的不安全行为，提高搜索效率，使运行更平稳。

4.根据权利要求1所述的融合全局算法和局部算法的路径规划方法，其特征在于，所述动态窗口法包括：根据机器人运动模型在速度空间中进行多组速度的采样，确定由所有可行速度组成的动态窗口，预测时间内每组不同速度下机器人可能的运动轨迹，同时使用预设的评价函数评估集合的轨迹并选择具有最高评估的集合执行直到下一个执行时间。

5.根据权利要求4所述的融合全局算法和局部算法的路径规划方法，其特征在于，融合所述动态窗口法的方法包括：

设定keylist=[S,P₁,…P_n,G]中的每一个路径点作为局部路径规划的子目标点；

当机器人从起始点S移动时，以P₁为子目标G_min，即设置G_min =P₁；

当机器人与子目标Gmin = P_i的距离小于阈值时，子目标将切换到下一个关键路径点，即设置G_min= P_{i + 1}；

当Gmin= P_i且关键路径点P_{i + j}，j>0，出现在规划窗口的路径中时，子目标切换到Gmin=P_{i + j}。

6.一种融合全局算法和局部算法的路径规划系统，其特征在于，包括：初始规划模块、优化模块和融合模块；

所述初始规划模块用于基于A-star算法进行初步路径规划，得到基础路线；

所述优化模块用于对所述基础路线进行优化，得到改进后算法；

所述融合模块用于在所述改进后算法中融合动态窗口法，得到最终算法；利用所述最终算法来对机器人路径进行规划；

优化的方法包括：增加子节点选择规则和删除冗余节点；

增加子节点选择规则，生成子节点时判断子节点与障碍物的位置关系，子节点选择规则定义为：

(1)当障碍物在(0,+1)或(0,-1)位置时，则删除障碍节点的左右两个可选子节点(-1,+1)(+1,+1)或(-1,-1)(+1,-1);

(2)当障碍物在(-1,0)或(+1,0)位置时，则删除障碍节点的上下两个可选子节点(-1,+1)(-1，-1)或(+1,+1)(+1,-1);

(3)当障碍物在斜对角方向上，则不做处理；

所述A-star算法的评价函数定义为：数f(n)由代价函数g(n)和启发函数h(n)组成；随着搜索深度即当前节点到起始点的距离depth与启发函数h(n)之间的变化，启发函数的取值有如下情况：

当depth小于等于h(n)时，意味着目前位置离起始点近离目标点远，自适应增大权重，可减小搜索空间，加快搜索速度，提高目标点的方向性，在一定程度上减少遍历栅格数；当depth大于h(n)时，不能一味增大加权系数ω(n)，只向目标点方向搜索，会陷入局部最优，出现多曲折转弯的路线，所以ω(n)取1，增大搜索空间，获得全局最优的路径；

。/>

Images