一种基于A星算法的自动驾驶无人车双向动态路径规划方法[发明专利]

(12)发明专利申请

(10)申请公布号 CN 110220528 A(43)申请公布日 2019.09.10

(21)申请号 201910494421.7(22)申请日 2019.06.10

(71)申请人 福州大学

地址 350108 福建省福州市闽侯县上街镇

福州大学城学院路2号福州大学新区(72)发明人 张卫波　肖继亮　陈泉泉　王浩　

刘朋　王冬招　(74)专利代理机构 福州元创专利商标代理有限

公司 35100

代理人 钱莉　蔡学俊(51)Int.Cl.

G01C 21/34(2006.01)

权利要求书1页 说明书3页 附图3页

CN 110220528 A()发明名称

一种基于A星算法的自动驾驶无人车双向动态路径规划方法(57)摘要

本发明涉及一种基于A星算法的自动驾驶无人车双向动态路径规划方法，包括如下步骤：初始化，载入栅格地图，设置好起点和目标点位置及无人车的移动步长；创建两个open列表和两个close列表，分别用于从起点向目标点方向使用A星算法搜索和从目标点向起点方向使用A星算法搜索；当两个open列表中出现相同节点时，表示找到了在当前地图环境下的最佳路径，之后再通过移动无人车位置，将新位置设置为新的起点，通过判断新位置是否为目标点位置结束无人车移动。当无人车未到达目标位置时，更新地图信息，重复寻路及移动过程直到无人车到达目标位置。本发明通过对起点和目标点分别使用A星算法，加快寻路速度，还可以适应于变化的环境中。

CN 110220528 A

权　利　要　求　书

1/1页

1.一种基于A星算法的自动驾驶无人车双向动态路径规划方法，其特征在于：包括以下步骤：

步骤S1：初始化：将地图导入无人车的电脑中，并将地图栅格化；所述栅格地图为M*M,M表示每行或每列栅格的个数；

步骤S2：在所述栅格地图上设置起始点、目标点和无人车移动步长；所述起始点、目标点分别占据一个方格；所述无人车移动的步长设置为一个栅格数；

步骤S3：分别使用A星算法对从起点到目标点和从目标点到起点两个方向进行逐步搜索；

步骤S4：创建open1列表和close1列表，用以存放步骤S3中从起始点到目标点位置使用A星搜索算法进行逐步搜索生成的节点；创建open2列表和close2列表，用于存放步骤S3中从目标点到起始点位置使用A星搜索算法进行逐步搜索生成的节点；

步骤S5：在步骤S4每次逐步搜索后，通过遍历open1列表和open2列表中的节点信息判断两个列表中是否存在坐标值相同的节点，若坐标值相同，表明是相同节点；如果存在相同节点，表示已经找到最佳路径，进行步骤S6；若不存在相同节点，则表明还未找到最佳路径，则再次返回步骤S3进行下一次逐步搜索；

步骤S6：无人车移动到新起点，判断新起点是否为目标点；若是，则结束；否则，更新地图信息，返回步骤S3。

2.根据权利要求1所述的一种基于A星算法的自动驾驶无人车双向动态路径规划方法，其特征在于：步骤S1中所述栅格地图为M*M,M表示每行或每列栅格的个数。

3.根据权利要求1所述的一种基于A星算法的自动驾驶无人车双向动态路径规划方法，其特征在于：步骤S2中所述无人车移动的步长设置为一个栅格数。

4.根据权利要求1所述的一种基于A星算法的自动驾驶无人车双向动态路径规划方法，其特征在于：所述步骤S3的具体内容为：

A星算法计算从初始状态即起点到目标状态即目标点的代价估计的计算公式为 f=g+h；

其中，f表示从初始状态到目标状态的代价估计；g表示从初始状态到下一状态的代价；h表示下一状态到目标状态的最短路径的代价；

所述从起点向目标点方向使用A星算法时，将此时栅格地图中的起点设置为A星算法中的起点，此时地图中的目标点设置为A星算法的目标点；从目标点向起点方向使用A星算法时，将此时地图中的目标点设置为A星算法的起点，此时地图中的起点设置为A星算法的目标点，然后进行逐步搜索。

5.根据权利要求1所述的一种基于A星算法的自动驾驶无人车双向动态路径规划方法，其特征在于：所述步骤S6的具体内容为：找到最佳路径后，无人车根据设置的移动步长开始沿着最佳路径移动，移动完成后将无人车当前位置设置为新起点，再通过对比当前点坐标和目标点坐标值是否相同来判断当前位置是否为目标位置，若为目标位置则表示无人车已经成功移动到目标位置；否则，更新地图信息，返回到步骤4中，重复此循环，直到无人车移动到目标位置。

2

CN 110220528 A

说　明　书

1/3页

一种基于A星算法的自动驾驶无人车双向动态路径规划方法

技术领域

[0001]本发明涉及寻路算法技术领域，特别是一种基于A星算法的自动驾驶无人车双向动态路径规划方法。

背景技术

[0002]路径规划技术是自动驾驶无人车研究的一个重要领域，是实现无人车自主定位与导航的关键技术之一，其主要的任务是让无人车能够快速平稳的通过有障碍物的环境，同时以最佳的路径到达设定的目标点。根据无人车周围环境是否发生变化可将路径规划分为静态路径规划和动态路径规划。

[0003]在用于无人车的路径规划算法中，A星算法是一个经典的路径规划算法，通过在开始时给定地图信息，遍历周围节点信息来找寻最短路径的，其算法具有很好的稳定性，在自动驾驶无人车实际的行驶环境中，路径规划需要考虑的地图范围大，同时，在自动驾驶无人车的环境下，会有障碍物经常出现的情况。但由于A星算法只有开始给定的地图信息及寻路时需遍历周围所有节点，这使得其在大地图中需要遍历大量的节点，导致计算规划的时间较长，实时性不好且不能应对新出现障碍物等问题。发明内容

[0004]有鉴于此，本发明的目的是提出一种基于A星算法的自动驾驶无人车双向动态路径规划方法，以克服传统A星算法在大尺寸地图中不能快速规划出路径以及只能用于静态路径规划中的不足。

[0005]本发明采用以下方案实现：一种基于A星算法的自动驾驶无人车双向动态路径规划方法，包括以下步骤：

步骤S1：初始化：将地图导入无人车的电脑中，并将地图栅格化；所述栅格地图为M*M,M表示每行或每列栅格的个数；

步骤S2：在所述栅格地图上设置起始点、目标点和无人车移动步长；所述起始点、目标点分别占据一个方格；所述无人车移动的步长设置为一个栅格数；

步骤S3：分别使用A星算法对从起点到目标点和从目标点到起点两个方向进行逐步搜索；

步骤S4：创建open1列表和close1列表，用以存放步骤S3中从起始点到目标点位置使用A星搜索算法进行逐步搜索生成的节点；创建open2列表和close2列表，用于存放步骤S3中从目标点到起始点位置使用A星搜索算法进行逐步搜索生成的节点；

步骤S5：在步骤S4每次逐步搜索后，通过遍历open1列表和open2列表中的节点信息判断两个列表中是否存在坐标值相同的节点，若坐标值相同，表明是相同节点。如果存在相同节点，表示已经找到最佳路径，进行步骤S6；若不存在相同节点，则表明还未找到最佳路径，则再次返回步骤S3进行下一次逐步搜索；

步骤S6：无人车移动到新起点，判断新起点是否为目标点；若是，则结束；否则，更新地

3

CN 110220528 A

说　明　书

2/3页

图信息，返回步骤S3。[0006]进一步地，所述步骤S3的具体内容为：

A星算法计算从初始状态即起点到目标状态即目标点的代价估计的计算公式为 f=g+h；

其中，f表示从初始状态到目标状态的代价估计；g表示从初始状态到下一状态的代价；h表示下一状态到目标状态的最短路径的代价；

所述从起点向目标点方向使用A星算法时，将此时栅格地图中的起点设置为A星算法中的起点，此时地图中的目标点设置为A星算法的目标点；从目标点向起点方向使用A星算法时，将此时地图中的目标点设置为A星算法的起点，此时地图中的起点设置为A星算法的目标点，然后进行逐步搜索。[0007]进一步地，所述步骤S6的具体内容为：找到最佳路径后，无人车根据设置的移动步长开始沿着最佳路径移动，移动完成后将无人车当前位置设置为新起点，再通过对比当前点坐标和目标点坐标值是否相同来判断当前位置是否为目标位置，若为目标位置则表示无人车已经成功移动到目标位置；否则，更新地图信息，返回到步骤4中，重复此循环，直到无人车移动到目标位置。[0008]与现有技术相比，本发明有以下有益效果：

本发明通过在起点和目标点分别采用A星算法，从起点开始向目标点方向搜索，从目标点开始向起点方向搜索，当两个open列表中出现相同节点时表示找到最短路径。这样可以在更短的时间内找到最佳路径。此外，在找到最佳路径后，通过移动无人车的位置，更新起点和地图再次使用上述方法，可以使得此方法在变化的环境中进行动态规划，而且比一般的A星算法寻找最佳路径速度更快。

附图说明

[0009]图1为本发明实施例的流程图。

[0010]图2为本发明实施例的双向搜索示意图。[0011]图3为本发明实施例的最佳路径图。

具体实施方式

[0012]下面结合附图及实施例对本发明做进一步说明。[0013]如图1所示，本实施例提供了一种基于A星算法的自动驾驶无人车双向动态路径规划方法，包括以下步骤：

步骤S1：初始化：将地图导入无人车的电脑中，并将地图栅格化；所述栅格地图为M*M,M表示每行或每列栅格的个数；

步骤S2：在所述栅格地图上设置起始点、目标点和无人车移动步长；所述起始点、目标点分别占据一个方格；所述无人车移动的步长设置为一个栅格数；

步骤S3：分别使用A星算法对从起点到目标点和从目标点到起点两个方向进行逐步搜索；

步骤S4：创建open1列表和close1列表，用以存放步骤S3中从起始点到目标点位置使用A星搜索算法进行逐步搜索生成的节点；创建open2列表和close2列表，用于存放步骤S3中

4

CN 110220528 A

说　明　书

3/3页

从目标点到起始点位置使用A星搜索算法进行逐步搜索生成的节点；所述创建列表是指在程序运行过程中创建的数组列表。[0014]步骤S5：在步骤S4每次逐步搜索后，通过遍历open1列表和open2列表中的节点信息判断两个列表中是否存在坐标值相同的节点，若坐标值相同，表明是相同节点。如果存在相同节点，表示已经找到最佳路径，进行步骤S6；若不存在相同节点，则表明还未找到最佳路径，则再次返回步骤S3进行下一次逐步搜索；

步骤S6：无人车移动到新起点，判断新起点是否为目标点；若是，则结束；否则，更新地图信息，返回步骤S3。[0015]在本实施例中，所述步骤S3的具体内容为：

A星算法计算从初始状态即起点到目标状态即目标点的代价估计的计算公式为 f=g+h；

其中，f表示从初始状态到目标状态的代价估计；g表示从初始状态到下一状态的代价；h表示下一状态到目标状态的最短路径的代价；

所述从起点向目标点方向使用A星算法时，将此时栅格地图中的起点设置为A星算法中的起点，此时地图中的目标点设置为A星算法的目标点；从目标点向起点方向使用A星算法时，将此时地图中的目标点设置为A星算法的起点，此时地图中的起点设置为A星算法的目标点，然后进行逐步搜索。[0016]在本实施例中，所述步骤S6的具体内容为：找到最佳路径后，无人车根据设置的移动步长开始沿着最佳路径移动，移动完成后将无人车当前位置设置为新起点，再通过对比当前点坐标和目标点坐标值是否相同来判断当前位置是否为目标位置，若为目标位置则表示无人车已经成功移动到目标位置；否则，更新地图信息，返回到步骤4中，重复此循环，直到无人车移动到目标位置。[0017]较佳的，在本实施例中，在步骤S5中，当在open1和open2列表中发现相同节点时，表示已经成功找到最佳路径，可以进行下一步；若在open1和open2列表中未发现相同节点，则表明还未找到最佳路径，此时应该返回步骤S3进行下一步搜索。[0018]如图2所示，为本实施例中的双向搜索示例图。本实施例将搜索区域均匀划分为8*8的方格区域，每个障碍物占据一个方格，起点和目标点也分别占据一个方格。从起点开始向目标点方向使用A星算法，如图中箭头所示，在逐步搜索的第一步搜索中，算法找到s1为最佳点，与此同时，从目标点向起点方向使用A星算法，如图中箭头所示，在逐步搜索的第一步搜索中算法找到g1为最佳点。重复搜索步骤后找到如图中箭头所示的路径，最后找到图示中的open1和open2相同节点，将s1、s2、open1与open2相同节点、g2、g1连接起来表示最后找到的最佳路径。[0019]如图3所示，为图2起点和目标点及当前地图情况下找到的最佳路径图。[0020]以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

5

CN 110220528 A

说　明　书　附　图

1/3页

图1

6

CN 110220528 A

说　明　书　附　图

2/3页

图2

7

CN 110220528 A

说　明　书　附　图

3/3页

图3

8