一种基于A星优化算法的寻路方法与流程

本发明涉及无人驾驶汽车技术领域，尤其涉及一种基于a星优化算法的寻路方法。

背景技术：

无人驾驶汽车是通过车载传感系统感知道路环境，自动规划行车路线并控制车辆到达预定目标的智能汽车。无人驾驶汽车集自动控制、体系结构、人工智能和视觉计算等众多技术于一体，是计算机科学、模式识别和智能控制技术高度发展的产物，也是衡量一个国家科研实力和工业水平的一个重要标志，在国防和国民经济领域具有广阔的应用前景。

其中寻路作为无人驾驶车辆基本的问题之一，即车辆按照程序指定的合适的路径从地图的a点抵达b点，根据车辆对周围环境的了解程度的不同，分为全局路径规划和局部路径规划两种方法。随着网络的快速发展，寻路技术已经成为无人驾驶汽车的核心组成部分，物体按照某种指定方式移动，就要求程序必须能够找到一条从起点到目标点的最佳路径，这条路径应该是绕过障碍物并且到达目的地的最短的路径。

目前应用最为广泛的寻路方法为启发式a星搜索算法，就是有启发地寻找目标结束点，并且在基于最小成本的情况下，尽可能的找到通向目标点的最合适最短的路径。但是传统启发式a星搜索算法在面对障碍时会进行许多无用节点的搜索，国内外的学者也对此进行了大量的研究，例如：王善坤等根据改进的人工势场法让绕过障碍物的曲线更加平滑，但是依然需要在障碍物周围进行搜索；蔡方方等根据双层a星算法进行二次搜索来绕过障碍物，虽然可以避开障碍物，但是增加了a星算法的搜索时间；高庆吉等引入了“人工搜索标记”起到预先判断或者逃离障碍物的作用，但是需要对障碍物周围进行无用节点的预搜索处理。

综上所述，虽然目前学者们做出了许多研究，但是依然存在搜索过程中无用节点的搜索，导致增加了搜索的时间和计算的内存。

技术实现要素：

本发明提供的一种基于a星优化算法的寻路方法，解决了传统启发式a星搜索算法在面对障碍时会进行许多无用节点搜索，导致增加搜索时间和计算内存的问题，使无人驾驶汽车能顺利的绕开障碍，减少了搜索所用的时间和计算内存。

为达到上述目的，本发明的技术方案具体是这样实现的：

本发明公开一种基于a星优化算法的寻路方法，包括以下步骤：

给出地图的网格图，并确定相应障碍点的位置坐标，确定起始点和目标点坐标；

构建起始点和目标点之间的直线函数，根据所连直线的倾角，判断采用横向遍历还是纵向遍历，求出直线与网格相交的关键点，再求出相关毗邻的节点，判断这些节点中是否有与障碍点重合的节点；

若节点与障碍节点不重合，则寻找的最佳路径就是汽车起始点和目标点之间的一条直线；

若遇到障碍点，就内部调用a星算法：

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

其中f(n)是估价函数，g(n)是起始点到当前点之间的代价值，h(n)是从当前点到目标点的最短路径的启发值。将当前节点放入close列表中，周围节点放入到open列表中，所述open列表中存储当前节点周围的节点，不包括障碍节点和已经存在于close列表中的节点；所述close列表用来存储路径确定的相关节点。计算出当前节点的邻近点的f值，选择f值最小的节点作为下一个父节点，将其放在close列表中；

判断open列表是否为空，如果不为空，说明在达到结束点前已经找到所有可能路径点，寻路失败，算法结束，否则继续寻路；

从open列表中拿出一个f值最小的点，作为寻找路径的下一步；

判断该点是否是目标点，如果是，则寻路成功，算法结束；否则继续寻路，将该点设为当前点。

进一步地，所述相关相毗邻节点，通常有四种形式：

第一种：关键点的x,y值都不是整数，则这个关键点只有一个节点；

第二种：关键点的x值为整数，y值不是整数，则这个关键点的节点在网格横向相邻的邻边上；

第三种：关键点的y值为整数，x值不是整数，则这个关键点的节点在网格纵向相邻的邻边上；

第四种：x,y同时为整数，则这个关键点同时拥有四个节点。

进一步地，所述f值最小的点是由g值和h值相加确定的，其中g值的大小是从进入a星算法的起始点到当前节点的代价值，h值采用对角线距离，其计算公式如下：

有益技术效果：

本发明公开一种基于a星优化算法的寻路方法，包括以下步骤，给出地图的网格图，并确定相应障碍点的位置坐标，确定起始点和目标点坐标；构建起始点和目标点之间的直线函数，根据所连直线的倾角，判断采用横向遍历还是纵向遍历，求出直线与网格相交的关键点，再求出相关毗邻的节点，判断这些节点中是否有与障碍点重合的节点；若节点与障碍节点不重合，则寻找的最佳路径就是汽车起始点和目标点之间的一条直线；若遇到障碍点，就内部调用a星算法，将该点周围节点放入open列表中，所述open列表中存储当前节点周围的节点，不包括障碍节点和已经存在于close列表中的节点，所述close列表用来存储路径。计算出当前节点的邻近点的f值，选择f值最小的节点作为下一个父节点，将其放在close列表中

判断open列表是否为空，如果不为空，说明在达到结束点前已经找到所有可能路径点，寻路失败，算法结束，否则继续寻路；从open列表中拿出一个f值最小的点，作为寻找路径的下一步；判断该点是否是目标点，如果是，则寻路成功，算法结束；否则继续寻路，将该点设为当前点；解决了传统启发式a星搜索算法在面对障碍时会进行许多无用节点搜索，导致增加搜索时间和计算内存的问题，使无人驾驶汽车能顺利的绕开障碍，减少了搜索所用的时间和计算内存。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍。

图1为本发明一种a星优化算法的基本原理示意图；

图2为本发明一种a星优化算法的预处理流程图；

图3为本发明一种a星优化算法的具体流程图；

图4为本发明一种a星优化算法的网格效果图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

下面结合附图对本发明的实施方式进行详细说明。

无人驾驶车辆在行驶时需要规划出一条合理的路径，路径规划的方法是在有障碍物的环境下，按照一定的评价标准规划出一条从起始状态到目标位置的无碰撞路径，主要考虑局部移动主体和障碍物之间的几何关系，找到一条不发生碰撞的路径。

a星算法作为一种常用的寻路算法，还存在着对无用节点进行搜索，导致增加搜索时间及计算内存，本发明通过一个对障碍物的预处理，进行算法的优化，减少节点的搜索时间和计算内存。

a星算法的函数为

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

其中f(n)是估价函数，g(n)是起始点到当前点之间的代价值，h(n)是从当前点到目标点的最短路径的启发值。通过计算f值来确定下一个节点的位置。为了让物体移动的方向可以除了正方向外，还可以对角运动，h(n)值的计算采用对角线距离作为新的估价函数。

设置走单格直线的代价值为d，走单格对角线的代价值为d2，基于网格的关系，d2与d之间有如下关系：

d2＝sqrt(2)d

先计算当前节点n和目标节点goal之间沿斜线可以移动的步数，建立函数h_diagonal(n)表示沿斜线方向可以移动的步数，其公式如下：

h_diagonal(n)＝min(abs(n.x-goal.x),abs(n.y-goal.y))

再计算当前节点n和目标节点goal之间沿直线可以移动的步数，根据曼哈顿距离，建立函数h_straight(n)表示沿直线可以移动的步数，其公式如下：

h_straight(n)＝(abs(n.x-goal.x)+abs(n.y-goal.y))

其中，n.x和goal.x分别表示节点n和节点goal的横坐标；n.y和goal.y分别表示节点n和节点goal的纵坐标。

合并这两项，确定最终h(n)的函数，其函数如下：

h(n)＝d2*h_diagonal(n)+d*(h_straight(n)-2*h_diagonal(n))

其中，d是代价值，h_diagonal(c)是沿斜线可以移动的步数，h_strainght(c)是直线移动的步数。

同时采用open和close两个列表来存储节点，其中open中存储当前节点周围的节点，不包括障碍节点和已经存在于close列表中的节点；close列表用来存储路径确定的相关节点。

具体地，本发明的一种基于a星优化算法的寻路方法，包括以下步骤：

给出地图的网格图，并确定相应障碍点的位置坐标，确定起始点和目标点坐标；

作为本发明的一个实施例，具体地，将地图划分为多个正方形网格，同时表明其中的起始点、目标点以及搜索域中无法通过的障碍节点的位置，同时将起始点放入到close列表中。

构建起始点和目标点之间的直线函数，根据所连直线的倾角，判断采用横向遍历还是纵向遍历，求出直线与网格相交的关键点，再求出相关毗邻的节点，判断这些节点中是否有与障碍点重合的节点；

作为本发明的一个实施例，具体地，构建无人驾驶汽车起始点和目标点之间的数学函数y＝kx+b，根据起始点和目标点的位置求出函数中的参数，在根据直线的倾斜角判断是采用横向遍历还是纵向遍历的方式。根据网格的单位确定直线和网格相交的关键点，根据直线的斜率确定倾角的大小，从而确定遍历方向，当倾角大于45°时，则采用纵向遍历，即根据y的值求出x的值，当倾角小于45°时，则采用横向遍历，即根据x的值求出y的值。从而确定直线与网格之间的相交的关键点，再根据这些关键点求出相关节点所毗邻的所有节点。

作为本发明的一个实施例，起始点和目标点之间存在障碍物，根据经过的首个障碍节点，计算出周围的比邻节点，从这些比邻节点中选择代价值f最小的节点作为下一个搜索的父节点。此时再计算当前节点和目标节点之间是否有障碍物存在。没有障碍物，就生成一条从当前节点直接到目标节点的路径则寻路成功；若还有障碍物存在，则从当前节点开始，从障碍物周围的非障碍物节点中选择到目标节点代价值最小的节点，作为寻路的下个起始节点。

作为本发明的一个实施例，得到关键点后，求出它相毗邻的所有节点，通常有四种形式：

第一种：关键点的x,y值都不是整数，则这个关键点只有一个节点；

第二种：关键点的x值为整数，y值不是整数，则这个关键点的节点在网格横向相邻的邻边上；

第三种：关键点的y值为整数，x值不是整数，则这个关键点的节点在网格纵向相邻的邻边上；

第四种：x,y同时为整数，则这个关键点同时拥有四个节点；

根据一个关键点求出它毗邻的节点有几个，是哪些，同时判断这些节点是否和障碍点是否发生重合。

若节点与障碍节点不重合，则寻找的最佳路径就是汽车起始点和目标点之间的一条直线。

若遇到障碍点，就内部调用a星算法：

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

其中f(n)是估价函数，g(n)是起始点到当前点之间的代价值，h(n)是从当前点到目标点的最短路径的启发值，将该点放入open列表中，搜寻该点的邻近点，相邻的网格为起点所在网格的上下左右以及左上、左下、右上、右下八个方向的网格，起点通过网格进行上下左右以及斜线的网格移动，当移动为直线的时候，代价值d取1；当移动为斜线的时候，代价值d2取1.2；根据起点的移动路径计算起点相邻的网格点和目标点的距离，假如邻近点既没有在open列表中，也没有在close列表中，则计算出该点的邻近点的f值，并作为父节点，将其放在open列表中；

判断open列表是否为空，如果没有，说明在达到结束点前已经找到所有可能路径点，寻路失败，算法结束，否则继续寻路；

从open列表中拿出一个f值最小的点，作为寻找路径的下一步；

作为本发明的一个实施例，具体地，最小f值的确定是由g值和h值相加确定的，其中g值的大小是从进入a星算法的起始点到当前节点的代价值，h值采用对角线距离，其计算公式如下：

在采用a星算法之前，先进行一个预处理，判断起始点和目标点之间是否可以直接走直线，合理的利用a星算法，提高整个搜索效率。在搜索过程中，当遇到相同的最小f值时，安排续后进的节点作为下一个继续搜索的节点，解决搜索过程中的不确定性。

判断该点是否是目标点，如果是，则寻路成功，算法结束；否则继续寻路，将该点设为当前点。

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