CN115268441A - 基于改进a*算法的动态路径规划方法及系统 - Google Patents

Abstract

本发明公开了基于改进A*算法的动态路径规划方法及系统，通过在传统A*算法的评价函数引入环境障碍影响项，构建改进A*算法；所述环境障碍影响项由当前节点周围的障碍物数量决定；基于改进A*算法对规划对象进行动态路径规划，能有效绕开了障碍密集区域，减少规划对象与障碍发生碰撞的风险。

Description

基于改进A*算法的动态路径规划方法及系统

技术领域

本发明涉及路径规划领域，尤其涉及基于改进A*算法的动态路径规划方法及系统。

背景技术

A*算法是一种由Dijstra算法优化得来的算法，具有简单，高效的特点，广泛应用于路径规划的任务中，是二维平面环境中使用最多的路径规划算法。与传统算法相比，A*算法融入了启发函数的概念，将路径损耗作为选择节点的评判机制，不再是无目的全局进行搜索。在搜索过程中，通过考虑当前节点到起点，当前节点到目标点两个方面的评价，来确定下一个节点的选择，以此类推，直到搜索到目标节点。

A*算法的具体步骤为：在一个二维网格地图中，设置起点为A点，目标点为B点。路径规划任务开始时，将A点设置成为开始节点，同时遍历A附近8个点中的非障碍点，分别计算出每个节点的评价函数，最后将当前节点从开始节点A点改为评价函数最低的点，并对邻近节点的信息进行更新，以此类推循环，不断选择评价函数最低的点作为当前节点，直到找到B点。最后，从目标点不断向前寻找当前节点的父节点，直到开始节点，以此来获取最佳路径。

由于现有的A*算法搜索过程中没有考虑周围障碍物多少这一条件，导致搜索出的路径会出现距离障碍物过近，甚至贴着障碍物的现象。这样的路径对于运动中的物体，特别是快速移动的物体，可能会发生碰撞。例如机器人的身体，手臂等，行驶过程中如果离障碍物过近，稍有偏离就有碰撞的风险。

因此，现有的A*算法搜索路径搜索出的路径容易导致规划对象与障碍物发生碰撞，已成为本领域技术人员亟待解决的技术问题。

发明内容

本发明提供了基于改进A*算法的动态路径规划方法及系统，用于解决现有的A*算法搜索路径搜索出的路径容易导致规划对象与障碍物发生碰撞的技术问题。

为解决上述技术问题，本发明提出的技术方案为：

一种基于改进A*算法的动态路径规划方法，包括以下步骤：

将传统A*算法的评价函数加入环境障碍影响项，构建改进A*算法；所述环境障碍影响项由当前节点周围的障碍密度和全局的障碍密度共同决定；基于改进A*算法对规划对象进行动态路径规划。

优选的，所述改进A*算法的评价函数为：

f(n)＝g(n)+h(n)+d_sg×ω×o(n)；

其中，n表示节点，g(n)是真实代价，表示从起点到当前节点的距离；h(n)为启发函数，表示从当前节点到目标节点的剩余距离；d_sg表示当前节点与目标节点的直线距离，o(n)为环境障碍函数，其数值表示当前点位周围障碍密集程度；ω是障碍权重因子，其决定规划路径对障碍物的敏感度。

优选的，所述环境障碍函数o(n)通过以下公式获取得到：

o(n)＝sum(O×D)，其中，矩阵O表示以当前节点为中心，附近多个节点的障碍物信息；矩阵D表示所述附近多个节点对当前节点的距离权重矩阵，距离中心点越近的节点，其距离权重越大。

优选的，在12％低障碍密度地图中，ω的取值范围为[0.2,0.4]；在24％高障碍密度地图中，ω的取值范围[0.8,1.0]。

优选的，所述改进A*算法在规划全局路径时，执行以下关键点搜索策略：

设已知路径节点集P_k＝{P₁,P₂......P_N}及其坐标信息；其中P₁为起点，P_N为目标点，对于所述已知路径节点集P_k中任意节点P_i，i＝1,2,3,...,N，均执行以下步骤：

计算由所述节点P_i构成的向量

和向量

之间的夹角，判断所述夹角是否为0：

若夹角为0，则说明

与

共线，三点在一条直线上，则P_i为共线的冗余节点，需去除；

若夹角不为0，则说明不共线，P_i是转折点，求解

所在直线方程，并判断是否与障碍物相交：若线段内不存在障碍物，则P_i是冗余的转折节点，去除；若存在障碍物，则P_i为关键转折节点，保留。

优选的，基于改进A*算法对规划对象进行动态路径规划，包括以下步骤：

使用所述改进A*算法进行全局路径规划，并提取全局路径中的关键点；

并按规划对象经过每个关键点的时序，依次将每个关键点作为局部目标点，采用DWA算法规划规划对象从每个关键点移动至下个关键点的局部路径，直至规划对象抵达全局路径的目标点。

优选的，采用DWA算法规划规划对象从每个关键点移动至下个关键点的局部路径，具体包括以下步骤：

对于任意第i个关键点P_i及其下一个关键点P_i+1；当规划对象移动至关键点P_i时，将关键点P_i+1作为局部关键点；当规划对象遇到未知障碍或动态障碍时，开始进行局部路径规划，采样规划对象当前的速度，根据当前采样的速度，结合预先构建的规划对象的运动模型计算规划对象当前运动的位姿，同时将运动的位姿与当前区域全局路径规划的位姿进行对比，并根据比对结果计算方向权重N，基于所述方向权值N当前区域全局路径规划的位姿规划规划对象从每个关键点移动至下个关键点的局部路径。

优选的，所述DWA算法的评价函数为：

G(v,w)＝σ(N×heading(v,w)+β×dist(v,w)+γ×velocity(v,w))

其中，heading(v,w)是方向因子，表示运动到当前轨迹终点时规划对象的方位角与目标点之间的夹角大小，夹角越大评分越低；dist(v,w)表示该运动轨迹上距离障碍物的最小距离，若轨迹上无障碍物则为较大的定值；velocity(v,w)表示运动线速度的评分；N为融合了运动位姿比重后的方向角评分权重，β，γ分别表示障碍物距离，运动线速度的评分权重；σ表示归一化操作。

一种计算机系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。

本发明具有以下有益效果：

1、本发明针对传统A*路径规划算法在进行全局路径规划时仅考虑距离因素，导致路径可能经过密集障碍区域的问题，提出了将路径周围障碍数引入评价函数中的改进A*算法，通过该改进A*算法规划出的全局路径，能有效绕开了障碍密集区域，减少规划对象与障碍发生碰撞的风险。

2、在优选方案中，本发明分别在不同障碍密度的地图中进行实验，探究障碍项系数ω对于路径周围障碍数、路径长度以及搜索节点的影响，确定不同障碍密度的优先障碍项系数，进一步减少规划路径周围障碍数。

3、在优选方案中，本发明中的针对传统A*算法路径拐点多，容易给机器人运动带来速度突变的问题，研究了去除冗余节点及连接关键节点的方法。在6％的障碍密度环境下，拐点数量相比原算法降低约50％，路径长度可降低约3％；在12％的障碍密度环境下，拐点数量相比原算法减少约28％，路径长度降低约3％；在24％的较高障碍密度环境下，拐点数量相比原算法降低约25％，路径长度可降低约1％；

4、在优选方案中，本发明探基于DWA算法与A*算法融合的动态路径规划，通过改进A*算法进行全局路径规划，DWA算法进行局部路径规划。同时提出对全局路径进行分段处理和改进方位评价函数的方法，改善传统DWA算法避障时可能偏离全局路径的问题。动态路径规划仿真实验表明，融合算法可有效避免避障时偏离全局路径的问题。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照附图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例提供的低障碍密度地图中ω取[0，30]时的测试图，其中，(a)为搜索节点数示意图，(b)为路径长度示意图，(c)为周围障碍数的变化情况图；(d)为ω取15时测试典型图；

图2为本发明实施例提供的低障碍密度地图中ω取[0，5]时的测试图，其中，(a)为搜索节点数示意图，(b)为路径长度示意图，(c)为周围障碍数的变化情况图；(d)为ω取2.5时测试典型图；

图3为本发明实施例提供的低障碍密度地图中ω取[0，1.5]时的测试图，其中，(a)为搜索节点数示意图，(b)为路径长度示意图，(c)为周围障碍数的变化情况图；(d)为ω取0.2时测试典型图；

图4为本发明实施例提供的不同ω分别在同一张低障碍密度随机地图中对路径规划的影响示意图，其中，(a)、(b)分别表示两张不同的低障碍密度随机地图；

图5为本发明实施例提供的高障碍密度地图中ω取[0，30]时的测试图，其中，(a)为搜索节点数示意图，(b)为路径长度示意图，(c)为周围障碍数的变化情况图；(d)为ω取15时测试典型图；

图6为本发明实施例提供的高障碍密度地图中ω取[0，5]时的测试图，其中，(a)为搜索节点数示意图，(b)为路径长度示意图，(c)为周围障碍数的变化情况图；(d)为ω取2.5时测试典型图；

图7为本发明实施例提供的高障碍密度地图中ω取[0，1.5]时的测试图，其中，(a)为搜索节点数示意图，(b)为路径长度示意图，(c)为周围障碍数的变化情况图；(d)为ω取0.2时测试典型图；

图8为本发明实施例提供的不同ω分别在同一张高障碍密度随机地图中对路径规划的影响示意图，其中，(a)、(b)分别表示两张不同的高障碍密度随机地图；

图9为本发明实施例提供的传统A*搜索邻域示意图；

图10为本发明实施例提供的障碍密度6％时，本发明与传统A*算法规划路径对比图；

图11为本发明实施例提供的障碍密度12％时，本发明与传统A*算法规划路径对比图；

图12为本发明实施例提供的障碍密度24％时，本发明与传统A*算法规划路径对比图；

图13为本发明实施例提供的障碍密度为6％时，本发明与传统A*算法拐点数和路径长度对比图；

图14为本发明实施例提供的障碍密度为12％时，本发明与传统A*算法拐点数和路径长度对比图；

图15为本发明实施例提供的障碍密度为24％时，本发明与传统A*算法拐点数和路径长度对比图；

图16为本发明实施例提供的改进A*融合DWA算法流程图；

图17为本发明实施例提供的实验初始化地图以及路径规划示意图，其中，图(a)表示初始化地图，图(b)表示路径规划示意图；

图18为本发明实施例提供的机器人避开静态未知障碍的两种不同的典型案例示意图，(a)表示典型案例1，(b)表示典型案例2；

图19为本发明实施例提供的机器人避开动态未知障碍的两种不同的典型案例示意图，(a)表示典型案例1，(b)表示典型案例2；

图20为本发明实施例提供的融合算法路径偏离全局路径的情况示意图；

图21为本发明实施例提供的传统DWA与改进DWA遇到动态障碍的避障结果示意图。

具体实施方式

以下结合附图对本发明的实施例进行详细说明，但是本发明可以由权利要求限定和覆盖的多种不同方式实施。

实施例一：

本实施中公开了一种基于改进A*算法的动态路径规划方法，包括以下步骤：

将传统A*算法的评价函数加入环境障碍影响项，构建改进A*算法；所述环境障碍影响项由当前节点周围的障碍密度和全局的障碍密度共同决定；基于改进A*算法对规划对象进行动态路径规划。

此外，在本实施例中，还公开了一种计算机系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。

本发明针对传统A*路径规划算法在进行全局路径规划时仅考虑距离因素，导致路径可能经过密集障碍区域的问题，提出了将路径周围障碍数引入评价函数中的改进A*算法，通过该改进A*算法规划出的全局路径，能有效绕开了障碍密集区域，减少规划对象与障碍发生碰撞的风险。

实施例二：

实施例二是实施例一的优选实施例，其与实施例一的不同之处，对基于改进A*算法的动态路径规划方法的具体步骤进行了详细介绍：

本实施例中公开了一种基于改进A*算法的动态路径规划方法，包括以下内容：

1、构建改进A*算法

11)评价函数的模型构建：

针对传统A*算法规划路径以距离为评价，没有考虑绕开障碍物密集区域，在障碍密集区域运动稍有偏差就会造成碰撞危险。针对这一问题，本章在启发函数中增加了物体附近障碍物的影响项，在搜索过程中引入四周环境中的障碍对评价函数f(n)的影响。

传统A*算法的评价函数由实际代价和启发代价组成，其定义如式(1)所示：

f(n)＝g(n)+h(n) (1)

其中，n表示节点，g(n)是真实代价，表示从起点到当前节点的距离；h(n)为启发函数，表示从当前节点到目标节点的剩余距离；由于启发函数h(n)仅仅包含了当前节点到目标节点的距离信息，并未考虑周围环境以及方向性的问题。为此，在评价函数中加入环境障碍影响项改进后的启发函数由式(2)所示：

f(n)＝g(n)+h(n)+d_sg×ω×o(n) (2)

g(n)以及h(n)定义不变，d_sg表示当前节点与目标节点的直线距离

o(n)为环境障碍函数，其定义如式(3)所示，w是障碍权重因子。

o(n)＝sum(O×D) (3)

其中，O和D是3×3的矩阵，矩阵O如式(4)所示，其代表当前节点为中心，附近8个节点的障碍物信息，矩阵中1表示有障碍，0表示无障碍。

矩阵D如式(5)所示，其代表距离权重，距离中心点越近的点为距离权重越大。

sum(O×D)代表将O，D两矩阵各对应位置元素相乘再全部求和的运算操作。当前点位周围障碍信息越多，则O矩阵中的非0元素越多，sum(O×D)的值就会增大，评价函数f(n)的值也会增大，以此来让选择这些环境障碍多的点位作为路径的可能性减小。反之，当前点位周围障碍较稀疏时，O矩阵中的非0元素很少，sum(O×D)的值就会减小，评价函数f(n)的值也随之减小，以此来让选择这些环境障碍少的点位作为路径的可能性增大。

12)静态权值ω的确定

引入障碍影响项以及方向影响项后的A*评价函数如式(6)所示，静态权值ω控制障碍物数量对路径选择的影响。ω越大，障碍项对评价函数的贡献增大，系统在选择节点时会对障碍物数量敏感。

f(n)＝g(n)+h(n)+d_sg×ω×o(n) (6)

为了静态权值ω的具体赋值对路径选择的影响，将搜索节点数，路径长度，周围障碍数作为评价指标，分别在10％障碍密度及20％障碍密度的环境下进行测试。搜索节点数小，路径长度小，周围障碍数少的路径是最理想的选择。但为了减少路径周围的障碍数量，难免增加搜索的范围，搜索节点数就会有所增加，而且为了绕开了障碍密集区域，路径长度难免也会有所增加。

121)低障碍密度环境下静态权值ω的确定

对于改进后的启发函数，其中的ω需要具体确定最佳取值。对于ω进行三个区间的测试，ω取值分别取大区间[0，30]，细化区间[0，5]以及小区间[0，1.5]，每个区间分为10个数据取值来进行实验。

因为ω控制的是路径周围障碍物密集程度的参数，所以实验时选取搜索节点数来反映计算时间，路线节点周围8个节点存在的障碍物数量来反映避障效果，路径长度反映路径规划的效果。为此，选择10个随机生成的50×50的栅格地图，并随机将其中的300个网格设置成为障碍点，以此来实验寻找低障碍密度下的最佳静态权值ω。第一组实验结果如图1所示。测试典型图中实线为未加入障碍影响项的传统A*算法，点线为加入了障碍影响项的本文算法。

首先在大的范围选取不同ω进行测试，在[0，30]内每间隔5选择一个测试值进行测试，并取十次测试的平均值。通过测试典型图5(d)可以看出，当ω取15时，传统A*算法规划的路径(实线)距离比本文算法规划路径长度更短，但传统算法经过了地图中心障碍最密集的区域，其路径有7处是紧临障碍物的；而本文算法绕过了障碍物最密集的区域，路径中仅1处是紧临障碍物的，可以看出在障碍项系数较大时，规划路径长度会有大幅增加，但更加安全。由结果图可以看出，随着ω的增加，搜索的节点数以及路径长度刚开始都有明显增加，随后节点数以及路径长度的变化也减少，趋于稳定，这是由于增加了障碍项的影响，不在以距离为最优标准，搜索范围增大，同时绕过障碍密集区域意味着路径的长度的增加。随着ω的增加，路径周围障碍数开始呈现大幅下降，随后小幅上下波动，趋于稳定，这是由于障碍项评价抑制了选择障碍密集区域的概率，转之选择障碍不那么密集的路径，但由于障碍项作用有限，并不是唯一评判路径的标准，所以路径周围依然存在少量障碍物。但障碍项系数并不是越大越好，从实验数据可以看出随着障碍项的系数增加大，搜索的节点数以及路径长度都有大幅度的增加，但路径周围障碍数并没有继续减少，而是小幅波动中趋于稳定。如果仅将周围障碍数作为路径规划评价标准，将周围障碍数降到最低，需要增加更多的搜索节点和路径长度。

因此，将最佳ω设置在[0，5]的区间内是可大幅降低路径周围障碍物的数量，同时没有过多增加搜索节点以及路径长度，为了进一步确定最佳取值，进一步将ω的测试范围缩小到[0，5]内，并等距取10个采样数据进行测试，测试结果如图2所示。测试典型图中实线路径为未加入障碍影响项的传统A*算法，点线路径为加入了障碍影响项的本文算法。

在区间[0，5]上每隔0.5个单位取一个测试值进行同样的十轮测试，取平均值.通过测试典型图6(d)可以看出，当ω取2.5时，传统A*算法规划的路径(实线)距离比本文算法规划路径长度更短，但传统算法经过了地图中心障碍最密集的区域，其路径有9处是紧临障碍物的；而本文算法绕过了障碍物最密集的区域，路径中仅3处是紧临障碍物的，可以看出在障碍项系数降至2.5时，规划路径也能明显绕开障碍物最密集的区域，相比障碍系数为15时，其绕开路径并不算长，虽然规划路径长度也有增加，但两种路径长度的差距已经在减小。通过结果图依然可以发现当ω在靠近0时变化，搜索节点以及路径长度变化很大，有大幅增加，其原因是引入障碍项评价抑制了选择障碍密集区域的概率，绕过障碍物密集区域所导致；同时当ω在靠近0时变化时周围障碍数下降的斜率也很大，说明障碍项系数不需要取值很大即可对路径的选择产生影响。通过结果图可以看出在[0，1.5]的区间内，周围障碍数下降的最快，当ω在大于1.5以后的范围内增加时，周围障碍数下降效果并不明显，但搜索节点数以及路径长度依然在增加，如果要进一步减少周围障碍数，将增加更多的搜索节点以及增大路径长度，路径规划结果也会因此效率变低。

因此，将最佳ω设置在[0，1.5]的区间内可大幅降低路径周围障碍物的数量，同时没有过多增加搜索节点以及路径长度，为了进一步确定最佳取值，进一步将ω的测试范围缩小到[0，1.5]内，并等距取10个采样数据进行测试，实验结果如图3所示。测试典型图中红色路径为未加入障碍影响项的传统A*算法，绿色路径为加入了障碍影响项的本文算法。

在[0，1.5]区间内隔0.2取一个值进行测试，并取平均。通过测试典型图3(d)可以看出，当ω取0.8时，传统A*算法规划的路径(实线)距离比本文算法规划路径长度更短，但传统算法经过了地图中心障碍最密集的区域，其路径有7处是紧临障碍物的；而本文算法绕过了障碍物最密集的区域，路径中仅4处是紧临障碍物的，可以看出在障碍项系数降至0.8时，规划路径也能明显绕开障碍物最密集的区域，相比障碍系数取2.5时，其路径增长并不多，只是在最密集的区域有所偏离，但两种路径长度的差距进一步减小。通过结果可以看出ω在[0.2，0.4]区间时，随着ω的增长障碍数下降最快，在[0.4，1.5]区间时，障碍数下降变缓，但节点数和路径长度的增长依然很快，减少周围障碍数，将增加更多的搜索节点以及增大路径长度。因此障碍项系数ω选择在[0.2，0.4]区间内可以取得最好的效果，在ω取0.2时将路径周围障碍数减少了约58％。

如图4展示了在同一张低障碍密度的地图中，不同的ω取值对路径选择带来的影响。随着障碍项系数ω的增大，路径选择绕开障碍物的趋势越发明显，路径周围障碍物数量明显减少，但路径长度也有所增加。其原因是障碍项系数的增大增加了路径规划对障碍物的敏感度，当障碍项系数较小时路径规划评价依然是距离因素占主导，但当障碍项系数增大后，距离因素不再是主导，甚至变为以路径周围障碍数量为主导，此时路径会发生绕行以避免遇到障碍物，安全性虽然有了增加，但也大幅增加了路径长度通过实验对比，障碍项系数ω选择在[0.2,0.4]区间内可以取得障碍减少的同时，路径也没有大幅增长的效果。

122)高障碍密度环境下静态权值ω的确定

上述分析建立在12％障碍密度(300个障碍)的地图中，为了进一步探究障碍密度更高的地图对静态权值的影响，选择10个随机生成的50×50的栅格地图，并随机将其中的300个网格设置成为障碍点，以此来实验寻找高障碍密度环境下的最佳的静态权值ω。第一组实验结果如图5所示。测试典型图中实线路径为未加入障碍影响项的传统A*算法，点线路径为加入了障碍影响项的本文算法。

由图5所示，在[0，30]内每间隔5选择一个测试值进行测试，并取十次测试的平均值。通过测试典型图5(d)可以看出，当ω取15时，传统A*算法规划的路径(实线)距离比本文算法规划路径长度相比有较大差距，其原因是高障碍密度环境下，有更多的障碍需要绕开，较大的ω使得路径对障碍的敏感度很大，绕开的距离消耗也更大。由结果图可以看出，随着ω的增加，搜索的节点数以及路径长度刚开始都有明显增加，随后节点数以及路径长度的变化也减少，趋于稳定。随着ω的增加，路径周围障碍数开始呈现大幅下降，随后降幅减缓，趋于稳定。较大的ω使得路径有大范围的绕行，牺牲了大量路径长度，为了进一步确定更优的取值，将ω的测试范围缩小到[0，5]内，并等距取10个采样数据进行测试，测试结果如图6所示。测试典型图中红色路径为未加入障碍影响项的传统A*算法，点线为加入了障碍影响项的本文算法。

由图6所示，在区间[0，5]上每隔0.5个单位取一个测试值进行同样的十轮测试，取平均值.通过测试典型图6(d)可以看出，当ω取2.5时，传统A*算法规划的路径(实线)路径较短，但路径附近有较多障碍物，而本文算法绕过了密集障碍物区域，可以看出在障碍项系数降至2.5时，规划路径也能明显绕开障碍物最密集的区域，相比障碍系数为15时，其规划路径依然很长，但两种路径长度的差距已经在减小。通过结果图依然可以发现当ω在靠近0时变化，搜索节点以及路径长度变化很大，有大幅增加，其原因是引入障碍项评价抑制了选择障碍密集区域的概率，绕过障碍物密集区域所导致；同时当ω在靠近0时变化时周围障碍数下降的斜率也很大，说明障碍项系数不需要取值很大即可对路径的选择产生影响。通过结果图可以看出在[0，1.5]的区间内，周围障碍数下降的最快，当ω在大于2.5以后的范围内增加时，周围障碍数下降效果并不明显，但搜索节点数以及路径长度依然在增加，如果要进一步减少周围障碍数，将增加更多的搜索节点以及增大路径长度，路径规划结果也会因此效率变低。因此为了测试最佳取值，将ω的测试范围缩小到[0，1.5]内，并等距取10个采样数据进行测试，测试结果如图7所示。测试典型图中实线路径为未加入障碍影响项的传统A*算法，点线路径为加入了障碍影响项的本文算法。

由图7所示，在区间[0，1.5]上每隔0.2个单位取一个测试值进行同样的十轮测试，取平均值。通过测试典型图8(d)可以看出，当ω取0.6时，传统A*算法规划的路径(实线)路径与改进后的路径长度差别已经不大，但本文算法绕过了密集障碍物区域。通过结果图依然可以发现当ω在靠近0时变化，周围障碍数下降最为明显，但搜索节点以及路径长度变化很大，有大幅增加。当ω大于0.6以后，搜索节点数的增长变缓，但路径长度依然保持增长；当ω大于1.0以后，周围障碍数的下降趋势减缓，但搜索节点数和路径长度依然有所增加，所以将ω设置在区间[0.8，1.0]上可以取得比较好的全局避障效果，在ω取0.8时将路径周围障碍数减少了约18％。

图8展示了在同一张高障碍密度的地图中，不同的ω取值对路径选择带来的影响。

由图8所示，随着障碍项系数ω的增大，路径选择绕开障碍物的趋势越发明显，因为处在高障碍密度环境中，障碍多，为了绕开障碍物所需要的路径长度也有所增加，障碍项系数的增大增加了路径规划对障碍物的敏感度，此时路径会发生绕行以避免遇到障碍物，安全性虽然有了增加，但也大幅增加了路径长度，甚至变得不可实现。通过实验对比，障碍项系数ω选择在[0.8，1.0]区间内可以取得障碍减少的同时，路径也没有大幅增长的效果。

13)关键点搜索策略的确定

针对传统A*算法规划出的路径存在拐点多，影响机器人正常运动这一问题，本文提出冗余点剔除的方法，连接拐点产生新路径的思想来减少拐点，平滑路路径。传统A*算法中，栅格地图中每个栅格的中心是搜索路径的节点，搜索结束后将各个节点连接即得到完整的路径结果。如图9所示，A*算法搜索过程只沿中心点周围8个点来进行，因此路径只存在水平，垂直，45度以及135度4个方向，难免会对路径长度有所限制，增加了不必要的路径。

其次，这样的连接方法难免会存在一些多余的转折点(拐点)，以此路径行驶机器人每次遇到拐点都要改变航向角以及速度，严重影响了机器人的安全运行。因此，为了减少拐点，最大程度的增加机器人运行的安全稳定度，本节利用关键节点的策略来进行优化，将冗余节点去除，只留下关键的必要节点，并进行连接，以此减少拐点以及减少路径长度。

131)关键点搜索策略内容：

已知路径节点P_k＝{P₁,P₂......P_N}及其坐标信息。其中起点为P₁，目标点为P_N。为了判断是否为共线还是转折，通过求向量夹角的方法来计算。计算向量

和向量

之间的夹角。

若夹角为0，则说明

与

共线，三点在一条直线上，则P₂为共线的冗余节点，需去除。

若夹角不为0，则说明不共线，P₂是转折点。此时再求解

所在直线方程，并判断是否与障碍物相交。若线段内不存在障碍物，则P₂是冗余的转折节点，去除；若存在障碍物，则P₂为关键转折节点，保留。

132)验证实验

为了验证改进去除冗余节点对实际路径规划的帮助，分别对障碍物密度由稀疏到密集的三种情况进行测试。测试中使用的地图是50×50的网格地图，障碍密度分别设置为6％、12％以及24％三种情况，每组实验的10个地图均由随机矩阵生成。如图10、11、12分别展示了障碍密度为6％、12％以及24％时，本文算法路径规划以及传统A*算法路径规划的结果。

由图10可以看出在障碍密度较低时，本文算法相比传统A*算法，拐点优化很明显。图中矩形内可以看出，通过连接关键节点，以直线代替了折线，大幅减少了拐点数量。

由图11可以看出在障碍密度增加到12％时，由于障碍增加带来了更多的必要转折，但相比传统A*算法，拐点数量依然有明显减少。

由图12可以看出障碍密度继续增加到24％时，在障碍密集区域时算法对路径拐点的改进逐渐不明显，其原因是直线连接关键点时路径会经过障碍点。但在地图中的空旷区域，依然可以起到优化效果。

将6％障碍密度实验组中拐点以及路径长度等数据统计整理，可得图13可以看出本文算法在低障碍密度环境下，相比传统算法，拐点减少了55.07％，路径长度减少了3.4％。

将12％障碍密度实验组中拐点以及路径长度等数据统计整理，可得图14，可以看出本文算法在中等障碍密度环境下，相比传统算法，拐点减少了28.40％，路径长度减少了2.95％。

将24％障碍密度实验组中拐点以及路径长度等数据统计整理，可得图15，可以看出本文算法在高障碍密度环境下，相比传统算法，拐点减少了17.02％，使路径长度减少了2.60％。

测试结果表明本文算法相较传统A*算法，在减少搜索节点，减少路径中拐点，减少路径长度以及避开障碍密集区域有了明显的改进。

2、将所述改进A*算法与所述DWA算法融合进行规划对象的路径规划

DWA算法可以在运动中对速度进行采样，并将每组速度对应的轨迹进行定量评价，以输出最优的局部避障路径。改进后的A*算法可以快速输出全局路径，并避开障碍密集区域，但其只能处理障碍已知的情况。针对A*算法只能应用在全局障碍已知，但对未知的局部障碍无法处理的缺点，将DWA算法融入其中，由局部路径规划负责解决未知局部障碍的躲避。

21)融合算法内容

首先进行A*路径规划，再沿着全局路径运行的过程中，如果遇到障碍物，则实用DWA算法进行局部避障。首先将A*路径中的关键点提取，这些关键点将路径分解为若干个局部，关键点作为DWA算法的局部目标点，然后进行速度采样，轨迹模拟以避开局部障碍物。当机器人运动到关键点时，将局部目标点设置为下一个关键点，以此循环，直到机器人抵达全局路径的目标点。其中，A*融合DWA算法的流程如图16所示。

22)验证实验

为了验证A*算法融入DWA算法后对实际路径规划的帮助，本文采用Matlab2021进行仿真实验。如图17(a)所示，分别设置起点和终点；如图17(b)分别设置未知静态障碍；未知动态障碍的轨迹。其中黑色是已知静态障碍，灰色为静态未知障碍，蓝色虚线为A*全局规划路径，红色虚线为位置动态障碍运动轨迹，蓝色实线为A*结合DWA算法的运动轨迹。

221)未知静态障碍仿真实验

实验时，为了让运动目标一定遇到未知静态障碍，如图18(a,b)所示，将若干未知静态障碍(灰色)设置在A*规划的全局路径上(蓝色虚线)，以测试融入DWA算法后对未知静态障碍的响应效果。典型测试结果如图18所示；

由测试结果可知，融合了DWA算法后，在机器人运动过程中，可以有效地检测周围的未知静态障碍情况，并有效地通过局部路径规划来避开障碍，如图18(a,b)所示，物体改变路径绕开了静态障碍，同时依然沿着全局路径行动，其原因是引入DWA算法后，系统有了局部路径规划避障的能力，通过对速度的采样模拟，在运动的过程中感知到位置静态障碍的存在，并选择可以绕行开障碍的速度组；相比传统A*算法只能规划已知障碍的问题，本文在A*算法中融入DWA算法后解决了其对未知静态障碍无法处理的问题。

221)动态未知障碍仿真实验

实验时，为了让运动目标一定遇到动态障碍，如图19(a,b)所示，将未知动态障碍(黄色)的运动轨迹设置与A*规划的全局路径相交，确保其会与机器人相撞，以测试融入DWA算法后系统对未知动态障碍物的响应效果。典型测试结果如下图：

由实验结果可知，融合了DWA算法后，在机器人运动过程中，可以有效地检测周围的未知动态障碍情况，在有可能与动态障碍发生碰撞的情况下，会改变原先轨迹，绕开动态障碍，同时依然沿着全局路径行动。其原因是融入DWA算法后，系统有了局部路径规划避障的能力。在运动过程中对速度采样并对轨迹的模拟可以感知到动态障碍的存在，并提前选择可以避开障碍的速度。相比传统A*算法只能解决静态已知障碍，本文在A*算法中融入DWA算法解决了其对未知动态障碍无法处理的问题。

将A*算法与DWA算法融合，提高了机器人避开动态障碍以及未知静态障碍的能力，但同时在避障过程也存在避障后路径偏离全局规划路径的可能，如图20所示，在遇到局部位置静态障碍后系统进行了避障处理，但避障后遇到了新的障碍物，再次进行躲避，导致路径(图中点线)大幅偏离了全局规划路径(图中虚线)，路径长度大大增加。

造成这个现象的原因是DWA算法评价函数中方向因子的权重不够大，仅仅考虑选择安全的路径避开障碍物，而避开障碍物的路径中可能也会再次遇到障碍物，导致路径进一步偏离。因此，在DWA算法进行速度评价的过程中，应该增加方向项的比重，以此来保证避障过程中不偏离全局路径。为了解决这个问题，对DWA算法进行如下改进：

S1将全局路径规划分为多段，取全局路径中的关键节点。每一个关键节点分别是当前局部路径规划区域的目标点以及下个局部路径规划区域的起点。分段设置关键点的方法可以增加系统的稳定性，避免在复杂环境中陷入局部循环的问题；

S2当遇到未知障碍或动态障碍时，开始进行局部路径规划，根据当前采样的速度可以根据机器人运动模型计算出机器人当前运动的位姿，同时将运动的位姿与当前区域全局路径规划的位姿进行对比，得出新的方向权重N。以此来促进机器人在避障的同时，避免过多偏离全局路径规划的运动轨迹。

改进后的DWA算法评价函数如所示：

G(v,w)＝σ(N×heading(v,w)+β×dist(v,w)+γ×velocity(v,w)) (4-12)

其中heading(v,w)是方向因子，表示运动到当前轨迹终点时机器人的方位角与目标点之间的夹角大小，夹角越大评分越低；dist(v,w)表示该运动轨迹上距离障碍物的最小距离，若轨迹上无障碍物则为较大的定值；velocity(v,w)表示运动线速度的评分；N为融合了运动位姿比重后的方向角评分权重，β，γ分别表示障碍物距离，运动线速度的评分权重；σ表示将归一化的操作。

为了验证融入位姿比重后的DWA算法，分别对其遇到动态以及位置静态障碍的情况进行仿真，并测量路径长度进行对比。如图21展示了遇到动态障碍物时的情况。其中虚线是全局路径，线段上的点为关键分段点；点线为传统DWA遇到动态障碍的避障结果；实线是改进后的DWA遇到动态障碍的避障结果。

由仿真结果可以看出，两种算法都避开了动态障碍物，但传统DWA算法(点线)在避障是发生了路径偏移，并没有将全局路径作为导航，虽然没有发生碰撞但大大增加了路径长度；而本文算法(实线)在避障的同时路径没有发生大范围的偏移，解决了DWA算法规划偏离原路径的问题。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种基于改进A*算法的动态路径规划方法，其特征在于，包括以下步骤：

在传统A*算法的评价函数加入环境障碍影响项，构建改进A*算法；所述环境障碍影响项由当前节点周围的障碍数量决定；基于改进A*算法对规划对象进行动态路径规划。

2.根据权利要求1所述的基于改进A*算法的动态路径规划方法，其特征在于，所述改进A*算法的评价函数为：

f(n)＝g(n)+h(n)+d_sg×ω×o(n)；

其中，n表示节点，g(n)是真实代价，表示从起点到当前节点的距离；h(n)为启发函数，表示从当前节点到目标节点的剩余距离；d_sg表示当前节点与目标节点的直线距离，o(n)为环境障碍函数，其数值表示当前点位周围障碍密集程度；ω是障碍权重因子，其决定规划路径对障碍物的敏感度。

3.根据权利要求2所述的基于改进A*算法的动态路径规划方法，其特征在于，所述环境障碍函数o(n)通过以下公式获取得到：

o(n)＝sum(O×D)，其中，矩阵O表示以当前节点为中心，附近多个节点的障碍物信息；

矩阵D表示所述附近多个节点对当前节点的距离权重矩阵，距离中心点越近的节点，其距离权重越大。

4.根据权利要求2所述的基于改进A*算法的动态路径规划方法，其特征在于，在12％低障碍密度地图中，ω的取值范围为[0.2,0.4]；在24％高障碍密度地图中，ω的取值范围[0.8,1.0]。

5.根据权利要求1-4中任意一项所述的基于改进A*算法的动态路径规划方法，其特征在于，所述改进A*算法在规划全局路径时，执行以下关键点搜索策略：

设已知路径节点集P_k＝{P₁,P₂......P_N}及其坐标信息；其中P₁为起点，P_N为目标点，对于所述已知路径节点集P_k中任意节点P_i，i＝1,2,3,...,N，均执行以下步骤：

计算由所述节点P_i构成的向量

和向量

之间的夹角，判断所述夹角是否为0：

若夹角为0，则说明

与

共线，三点在一条直线上，则P_i为共线的冗余节点，需去除；

若夹角不为0，则说明不共线，P_i是转折点，求解

所在直线方程，并判断是否与障碍物相交：若线段内不存在障碍物，则P_i是冗余的转折节点，去除；若存在障碍物，则P_i为关键转折节点，保留。

6.根据权利要求5所述的基于改进A*算法的动态路径规划方法，其特征在于，基于改进A*算法对规划对象进行动态路径规划，包括以下步骤：

使用所述改进A*算法进行全局路径规划，并提取全局路径中的关键点；

并按规划对象经过每个关键点的时序，依次将每个关键点作为局部目标点，采用DWA算法规划规划对象从每个关键点移动至下个关键点的局部路径，直至规划对象抵达全局路径的目标点。

7.根据权利要求6所述的基于改进A*算法的动态路径规划方法，其特征在于，采用DWA算法规划规划对象从每个关键点移动至下个关键点的局部路径，具体包括以下步骤：

对于任意第i个关键点P_i及其下一个关键点P_i+1；当规划对象移动至关键点P_i时，将关键点P_i+1作为局部关键点；当规划对象遇到未知障碍或动态障碍时，开始进行局部路径规划，采样规划对象当前的速度，根据当前采样的速度，结合预先构建的规划对象的运动模型计算规划对象当前运动的位姿，同时将运动的位姿与当前区域全局路径规划的位姿进行对比，并根据比对结果计算方向权重N，基于所述方向权值N当前区域全局路径规划的位姿规划规划对象从每个关键点移动至下个关键点的局部路径。

8.根据权利要求7所述的基于改进A*算法的动态路径规划方法，其特征在于，所述DWA算法的评价函数为：

G(v,w)＝σ(N×heading(v,w)+β×dist(v,w)+γ×velocity(v,w))

其中，heading(v,w)是方向因子，表示运动到当前轨迹终点时规划对象的方位角与目标点之间的夹角大小，夹角越大评分越低；dist(v,w)表示该运动轨迹上距离障碍物的最小距离，若轨迹上无障碍物则为较大的定值；velocity(v,w)表示运动线速度的评分；N为融合了运动位姿比重后的方向角评分权重，β，γ分别表示障碍物距离，运动线速度的评分权重；σ表示归一化操作。

9.一种计算机系统，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述权利要求1至8任一所述方法的步骤。

Images