CN108549378B - 一种基于栅格地图的混合路径规划方法和系统 - Google Patents

Abstract

本发明公开了一种基于栅格地图的混合路径规划方法，包括：根据原始栅格地图模型并使用A*搜索算法中的可搜索邻域来获得空闲栅格节点的局部障碍物势场函数，根据得到的局部障碍物势场函数获得代价函数，使用A*搜索算法对原始栅格地图模型进行处理，以得到所有目标栅格节点的通行代价，将权重因子μ依次加0.1，并重复上述步骤处理过程，直到μ的值为10为止，从得到的这些通行代价中找到最小值所对应的权重因子μ，将其对应的所有目标栅格节点相连，构成了全局通行代价最小路径，提取全局通行代价最小路径中的所有转折点作为局部目标点，使用DWA算法对这些局部目标点进行处理，将处理后的结果相连后构成一条满足全局最优的局部轨迹。

Description

一种基于栅格地图的混合路径规划方法和系统

技术领域

本发明属于机器人导航技术领域，更具体地，涉及一种基于栅格地图的混合路径规划方法和系统。

背景技术

移动机器人在工业生产、生活服务、灾害救援、环境探测等领域有着广泛的需求与应用，其中导航问题是其技术领域的研究热点之一。现有针对移动机器人的导航路径规划方法主要是将全局路径规划法和局部路径规划法进行组合，其中最有代表性的，是将Hart等于1968年提出的A*搜索算法与动态窗口法(Dynamic window approach，简称DWA)进行组合，构成所谓的A*-DWA方法。

然而，上述A*-DWA方法在实际应用中存在一些不可忽略的缺陷：首先，由于A*搜索算法并没有考虑空闲节点的通行代价，规划的路径可能会在局部距离障碍物非常近，从而移动机器人与障碍物发生碰撞的概率较大；其次，由于A*-DWA方法中，多个局部目标节点会在同一朝向角评价函数中使用同一参数，如果参数的取值过小，则会导致规划的路径偏离原始轨迹，如果参数的取值过大，则会导致速度采样评价函数的权值变低，移动机器人的移动速度低，算法的实现速度慢。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于栅格地图的混合路径规划方法，其目的在于，解决现有A*-DWA方法中A*搜索算法没有考虑空闲节点的通行代价，导致移动机器人与障碍物发生碰撞的概率较大，以及由于多个局部目标节点会在同一朝向角评价函数中使用同一参数，导致规划的路径偏离原始轨迹或算法的实现速度慢的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种基于栅格地图的混合路径规划方法，包括以下步骤：

(1)根据原始栅格地图模型并使用A*搜索算法中的可搜索邻域来获得空闲栅格节点的局部障碍物势场函数；

(2)根据步骤(1)得到的局部障碍物势场函数获得代价函数，具体采用以下公式：

f(n)＝g(n)+μS(n)+h(n)

其中，g(n)表示使用A*搜索算法得到的从移动机器人的起始点到待搜索节点n的实际代价值，h(n)表示从待搜索栅格节点n到移动机器人的最终目标点的估计代价值，μ表示权重因子，其初始值为0；

(3)使用A*搜索算法对原始栅格地图模型进行处理，以得到所有目标栅格节点的通行代价，其中A*搜索算法中的代价函数使用的是步骤(2)中得到的代价函数；

(4)将步骤(2)中的权重因子μ依次加0.1，并重复上述步骤(2)和(3)的处理过程，直到μ的值为10为止，从而得到多次处理后所有目标栅格节点的通行代价，从得到的这些通行代价中找到最小值所对应的权重因子μ，将其对应的所有目标栅格节点相连，构成了全局通行代价最小路径；

(5)提取全局通行代价最小路径中的所有转折点作为局部目标点，使用DWA算法对这些局部目标点进行处理，将处理后的结果相连后构成一条满足全局最优的局部轨迹。

优选地，局部障碍物势场函数的具体表达式如下：

S(n)＝k 0≤k≤m

其中，n表示栅格节点，m代表A*搜索算法中待搜索邻域的范围，即m邻域路径搜索，k表示可搜索邻域内存在被障碍物占据的栅格节点的个数。

优选地，m的取值等于4的正整数倍，优选地等于8。

优选地，步骤(5)中DWA算法中的评价函数表达式为：

G(v,ω)＝α·localhead(v,ω)+β·dist(v,ω)+γ·vel(v,ω)

α＝a·kphead(v,ω)+b

其中v表示移动机器人的移动速度，ω表示移动机器人的旋转角速度，localhead表示DWA算法中预测轨迹终点与不同局部目标点之间的方向夹角，dist表示预测轨迹上的点与障碍物之间的最小距离，vel表示预测轨迹对应的移动机器人的速度大小，α、β、γ为加权系数，β＝0.9，γ＝0.2，kphead表示到达局部目标点时的轨迹终点与下一个局部目标点之间的方向夹角，a，b为缩小α范围的固定参数，且有a＝0.04，b＝1。

按照本发明的另一方面，提供了一种基于栅格地图的混合路径规划系统，包括：

第一模块，用于根据原始栅格地图模型并使用A*搜索算法中的可搜索邻域来获得空闲栅格节点的局部障碍物势场函数；

第二模块，用于根据第一模块得到的局部障碍物势场函数获得代价函数，具体采用以下公式：

f(n)＝g(n)+μS(n)+h(n)

其中，g(n)表示使用A*搜索算法得到的从移动机器人的起始点到待搜索节点n的实际代价值，h(n)表示从待搜索栅格节点n到移动机器人的最终目标点的估计代价值，μ表示权重因子，其初始值为0；

第三模块，用于使用A*搜索算法对原始栅格地图模型进行处理，以得到所有目标栅格节点的通行代价，其中A*搜索算法中的代价函数使用的是步骤(2)中得到的代价函数；

第四模块，用于将第二模块中的权重因子μ依次加0.1，并重复上述第二模块和第三模块的处理过程，直到μ的值为10为止，从而得到多次处理后所有目标栅格节点的通行代价，从得到的这些通行代价中找到最小值所对应的权重因子μ，将其对应的所有目标栅格节点相连，构成了全局通行代价最小路径；

第五模块，用于提取全局通行代价最小路径中的所有转折点作为局部目标点，使用DWA算法对这些局部目标点进行处理，将处理后的结果相连后构成一条满足全局最优的局部轨迹。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

(1)由于本发明方法通过获得局部障碍物势场函数，充分考虑到空闲节点的通行代价，因此在所构建的满足全局最优的局部轨迹中，移动机器人与障碍物均保持一定的距离，因此二者发生碰撞的概率较小。

(2)由于本发明步骤(5)中使用了函数表达式来表现加权系数α，能够根据到达局部目标点时的轨迹终点与下一个局部目标点之间的方向夹角动态调整评价函数，因此在方向夹角偏大或偏小时，满足全局最优的局部轨迹与全局通行代价最小路径均比较接近，且算法处理时间较短。

附图说明

图1是本发明基于栅格地图的混合路径规划方法的流程图；

图2是本发明原始栅格地图模型的示意图；

图3是本发明局部势场栅格地图的示意图；

图4是μ值为0.1时的全局通行代价最小路径；

图5是μ值为1时的全局通行代价最小路径；

图6是μ值为6时的全局通行代价最小路径；

图7是本发明方法的步骤(5)中得到的满足全局最优的局部轨迹的示意图；

图8是使用传统A*算法得到的全局通行代价最小路径；

图9示出了现有A*-DWA算法得到的满足全局最优的局部轨迹。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明提出了一种基于栅格地图的混合路径规划方法，其利用局部势场法的思想对A*算法中的路径节点进行局部障碍物势场计算，得到移动机器人安全等级较高的全局路径，然后检测局部障碍物的碰撞，修正局部路径，最终得到满足移动机器人导航的混合路径规划方法。

如图1所示，本发明基于栅格地图的混合路径规划方法包括以下步骤：

(1)根据原始栅格地图模型(如图2所示)并使用A*搜索算法中的可搜索邻域来获得空闲栅格节点(即原始栅格地图中未被黑色填充所示的障碍物占据的栅格节点)的局部障碍物势场函数；

在获得局部障碍物势场函数后，在原始栅格地图上添加局部障碍物势场函数对应的栅格地图模型，从而形成局部势场栅格地图(如图3所示，颜色越深的栅格节点表示对应的局部障碍物势场函数越大)

该局部障碍物势场函数的具体表达式如下：

S(n)＝k 0≤k≤m

其中，n表示栅格节点，m代表A*搜索算法中待搜索邻域的范围，即m邻域路径搜索(此处设为m＝8，其取值为4的任意正整数倍，m越大，则搜索时间越长，反之则搜索时间越短)，k表示可搜索邻域内存在被障碍物占据的栅格节点的个数。

(2)根据步骤(1)得到的局部障碍物势场函数获得代价函数，具体采用以下公式：

f(n)＝g(n)+μS(n)+h(n)

其中，g(n)表示使用A*搜索算法得到的从移动机器人的起始点到待搜索节点n的实际代价值，h(n)表示从待搜索栅格节点n到移动机器人的最终目标点的估计代价值，μ表示权重因子，其初始值为0；

(3)使用A*搜索算法对原始栅格地图模型进行处理，以得到所有目标栅格节点的通行代价，其中A*搜索算法中的代价函数使用的是步骤(2)中得到的代价函数；

(4)将步骤(2)中的权重因子μ依次加0.1，并重复上述步骤(2)和(3)的处理过程，直到μ的值为10为止，从而得到多次处理后所有目标栅格节点的通行代价，从得到的这些通行代价中找到最小值所对应的权重因子μ，将其对应的所有目标栅格节点相连，构成了全局通行代价最小路径，如图4、5和6所示，其分别为μ值为0.1、1、6时的全局通行代价最小路径；

(5)提取全局通行代价最小路径中的所有转折点作为局部目标点，使用DWA算法对这些局部目标点进行处理，将处理后的结果相连后构成一条满足全局最优的局部轨迹(如图7所示)，其中DWA算法中的评价函数表达式为：

G(v,ω)＝α·localhead(v,ω)+β·dist(v,ω)+γ·vel(v,ω)

α＝a·kphead(v,ω)+b

其中v表示移动机器人的移动速度，ω表示移动机器人的旋转角速度，localhead表示DWA算法中预测轨迹终点与不同局部目标点之间的方向夹角，dist表示预测轨迹上的点与障碍物之间的最小距离，vel表示预测轨迹对应的移动机器人的速度大小，α、β、γ为加权系数，β＝0.9，γ＝0.2，kphead表示到达局部目标点时的轨迹终点与下一个局部目标点之间的方向夹角，a，b为缩小α范围的固定参数，且有a＝0.04，b＝1。

实验结果

一、全局通行代价最小路径对比

本发明所选取的实验环境是大小为20*20的栅格地图，其中黑色栅格表示障碍物区域，白色栅格表示可自由通行区域。

图8是使用传统A*算法得到的全局通行代价最小路径，从图4-6以及图8的结果可以看出，随着μ的逐渐增大，通行代价所占的权重也会增大，使得最终得到的轨迹会经过通行代价更低的节点，但是有可能增加全局路径的长度。

下表1示出了传统A*算法和本发明方法得到的全局通行代价最小路径的仿真结果。

表1全局通行代价最小路径的仿真结果对比

表1中定量分析了不同参数条件下本发明算法与传统A*算法的各项指标，从其结果可以看出，μ＝6时算法整体的通行代价最小，但是总体路径长度会显著增加，这是因为在遇到多障碍物的情况下，此时算法选择了绕行的策略替代穿行，在μ＝0.1的条件下算法在不改变整体路径长度的情况下，能够显著减少栅格节点的通行代价。此外，由于本发明算法是在原始A*算法的基础上引入了局部势场函数的计算，因此算法在搜索时长以及总体搜索栅格节点数都有略微增加。

二、满足全局最优的局部轨迹对比

图9示出了现有A*-DWA算法得到的满足全局最优的局部轨迹(其中α＝1)，其中直线表示全局通行代价最小路径，曲线表示满足全局最优的局部轨迹对比。

下表2示出本发明算法和传统A*-DWA算法在静态障碍物环境下的仿真结果对比。

表2静态障碍物环境下的仿真结果对比

从表2的结果可以看出，本发明中所提出的算法，相对于A*-DWA算法，通行代价减少了63.9％，同时本发明算法得到的路径规划结果能够在保证最大偏移距离不变的情况下，减少了27.1％的搜索时间。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于栅格地图的混合路径规划方法，其特征在于，包括以下步骤：

(1)根据原始栅格地图模型并使用A*搜索算法中的可搜索邻域来获得空闲栅格节点的局部障碍物势场函数；

(2)根据步骤(1)得到的局部障碍物势场函数获得代价函数，具体采用以下公式：

f(n)＝g(n)+μS(n)+h(n)

其中，g(n)表示使用A*搜索算法得到的从移动机器人的起始点到待搜索节点n的实际代价值，h(n)表示从待搜索栅格节点n到移动机器人的最终目标点的估计代价值，μ表示权重因子，其初始值为0；

(3)使用A*搜索算法对原始栅格地图模型进行处理，以得到所有目标栅格节点的通行代价，其中A*搜索算法中的代价函数使用的是步骤(2)中得到的代价函数；

(4)将步骤(2)中的权重因子μ依次加0.1，并重复上述步骤(2)和(3)的处理过程，直到μ的值为10为止，从而得到多次处理后所有目标栅格节点的通行代价，从得到的这些通行代价中找到最小值所对应的权重因子μ，将其对应的所有目标栅格节点相连，构成了全局通行代价最小路径；

(5)提取全局通行代价最小路径中的所有转折点作为局部目标点，使用DWA算法对这些局部目标点进行处理，将处理后的结果相连后构成一条满足全局最优的局部轨迹；步骤(5)中DWA算法中的评价函数表达式为：

G(v,ω)＝α·localhead(v,ω)+β·dist(v,ω)+γ·vel(v,ω)

α＝a·kphead(v,ω)+b

其中v表示移动机器人的移动速度，ω表示移动机器人的旋转角速度，localhead表示DWA算法中预测轨迹终点与不同局部目标点之间的方向夹角，dist表示预测轨迹上的点与障碍物之间的最小距离，vel表示预测轨迹对应的移动机器人的速度大小，α、β、γ为加权系数，β＝0.9，γ＝0.2，kphead表示到达局部目标点时的轨迹终点与下一个局部目标点之间的方向夹角，a，b为缩小α范围的固定参数，且有a＝0.04，b＝1。

2.根据权利要求1所述的混合路径规划方法，其特征在于，局部障碍物势场函数的具体表达式如下：

S(n)＝k，0≤k≤m

其中，n表示栅格节点，m代表A*搜索算法中待搜索邻域的范围，即m邻域路径搜索，k表示可搜索邻域内存在被障碍物占据的栅格节点的个数。

3.根据权利要求2所述的混合路径规划方法，其特征在于，m的取值等于4的正整数倍。

4.一种基于栅格地图的混合路径规划系统，其特征在于，包括：

第一模块，用于根据原始栅格地图模型并使用A*搜索算法中的可搜索邻域来获得空闲栅格节点的局部障碍物势场函数；

第二模块，用于根据第一模块得到的局部障碍物势场函数获得代价函数，具体采用以下公式：

f(n)＝g(n)+μS(n)+h(n)

其中，g(n)表示使用A*搜索算法得到的从移动机器人的起始点到待搜索节点n的实际代价值，h(n)表示从待搜索栅格节点n到移动机器人的最终目标点的估计代价值，μ表示权重因子，其初始值为0；

第三模块，用于使用A*搜索算法对原始栅格地图模型进行处理，以得到所有目标栅格节点的通行代价，其中A*搜索算法中的代价函数使用的是步骤(2)中得到的代价函数；

第四模块，用于将第二模块中的权重因子μ依次加0.1，并重复上述第二模块和第三模块的处理过程，直到μ的值为10为止，从而得到多次处理后所有目标栅格节点的通行代价，从得到的这些通行代价中找到最小值所对应的权重因子μ，将其对应的所有目标栅格节点相连，构成了全局通行代价最小路径；

第五模块，用于提取全局通行代价最小路径中的所有转折点作为局部目标点，使用DWA算法对这些局部目标点进行处理，将处理后的结果相连后构成一条满足全局最优的局部轨迹；第五模块中DWA算法中的评价函数表达式为：

G(v,ω)＝α·localhead(v,ω)+β·dist(v,ω)+γ·vel(v,ω)

α＝a·kphead(v,ω)+b

其中v表示移动机器人的移动速度，ω表示移动机器人的旋转角速度，localhead表示DWA算法中预测轨迹终点与不同局部目标点之间的方向夹角，dist表示预测轨迹上的点与障碍物之间的最小距离，vel表示预测轨迹对应的移动机器人的速度大小，α、β、γ为加权系数，β＝0.9，γ＝0.2，kphead表示到达局部目标点时的轨迹终点与下一个局部目标点之间的方向夹角，a，b为缩小α范围的固定参数，且有a＝0.04，b＝1。

Images