CN110333659B - 一种基于改进a星搜索的无人驾驶汽车局部路径规划方法 - Google Patents

Abstract

本发明公开了一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，给算法中所有拓展的节点带上方向属性，引入汽车转弯特有的机械约束。因此，算法只拓展无人驾驶车辆前进方向范围内的节点，更符合物理意义；引入引导线L，引导线L由一系列的引导点组成，是无人驾驶汽车领域用于引导车辆前进的一种信息表示；本发明中提出用L来修改A星算法的评估函数，使其更符合人类驾驶员的驾驶习惯；为了更符合人类驾驶员的避障策略，本发明提出引入关键点keyPoint的概念，用keyPoint来引导A星算法实现饶障的过程。本发明的有益效果在于：引入引导线对评估函数进行改进，能够得到期望的最优解；引入keyPoint使其避障策略更优。

Description

一种基于改进A星搜索的无人驾驶汽车局部路径规划方法

技术领域

本发明属于汽车领域的路径规划方法，具体涉及一种基于改进A星搜索的无人驾驶汽车局部路径规划方法。

背景技术

随着汽车工业的发展，汽车越来越成为社会生产与日常生活中的重要组成部分；与此同时，汽车安全问题也逐渐成为人们关注的焦点，如何能更有效地提高车辆行驶安全性已经成为各国政府和研究机构共同面对的问题。其中无人驾驶车辆被公认为大幅减小交通安全事故的最佳途径,因而成为世界交通运输领域研究的前沿和热点。

无人驾驶车辆是一个综合环境感知、路径规划、运动控制于一体的复合系统,其中路径规划是无人车辆环境感知和车辆控制的桥梁,是实现车辆主动避障、自动导航等重要功能的关键技术，是无人车辆自主驾驶的基础。

目前的汽车无人驾驶路径规划方法众多，主要有基于图搜索的方法、基于随机生成的方法、基于轨迹生成的方法、基于智能群落算法等几大类，但是用的最多的还是基于图搜索的方法。

目前主要的图搜索方法包括：

申请号为201710794646.5的专利申请提出了一种基于A星算法的最短寻路方法，该申请描述了经典A星算法的实现步骤：建立搜索网格，确定起点、目标点和障碍物位置，建立OPEN表和CLOSE表，利用点之间的距离作为代价，采用的评估函数F＝G+H的含义为：F为从初始状态到目标状态的代价估计，G为从初始状态到下一状态的代价,H为下一状态到目标状态的最佳路径的代价。

申请号为201711374451.1的专利申请提出了一种基于改进A星算法的移动机器人路径规划方法，在该申请中，利用经典的A星算法实现全局路径规划后，提出了对规划结果二次平滑处理,得到预规划路径；最后采用人工势场法与在静态障碍物环境中进行全局路径预规划的方法在遇到动态障碍物时进行局部路径规划。该方法在利用A星算法实现其所谓的全局路径规划方面与经典的A星算法一致。

经典A星搜索算法其步骤实现如下：

(a)输入初始点S、目标点T和障碍物图costmap；

(b)建立OPEN表和CLOSE表；

(c)设置评估函数F(i)＝G(i)+H(i)，i表示第i个节点，其中G表示从初始点到该i节点的代价值，设步长代价为1，H(i)表示从该i节点到目标点的预测的代价值，取两点之间的距离值，即H(i)＝distance(i,T)；

(d)把初始点放入OPEN表；

(e)OPEN表中按F的值从小到大排序；

(f)如果OPEN表不为空，则弹出OPEN表中最小的值对应的节点K，否则算法失败；

(g)判断该节点K是否为目标点，如果是则结束算法，否则继续；

(h)拓展K节点的相邻节点，按四邻域或者八邻域规则，得到四个或者八个拓展节点；

(i)判断该四个或者八个拓展节点是否已经在CLOSE表中，如是，则放弃该节点；

(j)根据评估函数F(i)，计算该四个或者八个拓展节点的评估值；

(k)把该四个或者八个带评估值的拓展节点放入OPEN表中，把节点K放入CLOSE表中；

(l)跳到步骤(e)；

A星算法属于寻优算法，但是其算法本身并不能保证得到最优解，而且在无人驾驶汽车这个具体应用中，A星算法规划出来的路径并不符合人类驾驶员的驾驶习惯和实际需求。在该类应用场景中，如何使路径规划算法更符合实际需求，以及如何来评价规划结果的优劣，都是需要重点研究的问题。

发明内容

本发明的目的是提供一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，它能够克服现有技术的不足，并用于无人驾驶车辆的自主导航。

本发明的技术方案如下：一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，它包括如下步骤：

步骤一：输入带方向的初始点S、带方向的目标点T、引导线L和障碍物图costmap；

步骤二：根据引导线L与障碍物图costmap，生成关键点keyPoint；

步骤三：建立OPEN表和CLOSE表；

步骤四：设置评估函数F(i)＝G(i)+H(i)；

其中，i表示第i个节点，G(i)表示从初始点到该i节点的代价值，H(i)表示从该i节点到目标点的预测的代价值；

步骤五：计算初始点S的评估函数值F，并放入OPEN表中；

步骤六：OPEN表中的点按F值从小到大排序；

步骤七：如果OPEN表不为空，则弹出OPEN表中排序的第一个节点K，否则算法失败；

步骤八：判断该节点K是否为目标点，如果是，则结束算法，输出节点K及其一系列父节点作为搜索结果；否则进行步骤九；

步骤九：拓展节点K的相邻节点，按车辆的方向和转弯特性进行拓展；

步骤十：判断待拓展的n个节点中是否有节点已经在CLOSE表中，如是，则放弃该节点；

步骤十一：根据评估函数F(i)，计算每一个拓展节点的评估值；

步骤十二：把带评估值的拓展节点放入OPEN表中，把节点K放入CLOSE表中；

步骤十三：跳到步骤六。

所述的步骤二中生成关键点keyPoint的规则首先判断引导线上是否存在障碍物，如果存在障碍物，则寻找生成绕障的关键点keyPoint。

所述的步骤二中关键点keyPoint的获取方式如下：找出所有落在引导线L上的障碍物，计算出其障碍物边缘点，标记为候选关键点keyPoint，然后按照以下原则从候选关键点keyPoint中选择一个点作为keyPoint：(1)用户倾向，从左绕障或者从右绕障；(2)引导线离障碍物距离较小一侧；(3)候选关键点keyPoint所在位置的可通行区域；(4)当有多个障碍物时，选择绕行侧与引导线L最大距离的候选点。

所述的步骤二中的四个原则可以根据需要任选其一或者若干进行组合使用。

所述的步骤三中OPEN表和CLOSE表，分别用于存放待拓展节点和已拓展节点。

所述的步骤四中H(i)包括四个部分，

H(i)＝H1(i)*α1+H2(i)*α2+H3(i)*α3+H4(keyPoint)*α4；

其中，H1(i)表示该节点到引导线L之间的最小距离值；H2(i)为i节点到引导线最小距离所对应的点到目标点之间的距离；H3(i)为节点i到关键点keyPoint之间的距离；H4(keyPoint)表示从keyPoint到目标点之间的距离；α1、α2、α3、α4为权重系数。

所述的步骤九中在[-θ,θ]区间内按角度均匀拓展n个节点，记为(xj,yj),j∈{1，n}，其中θ为车辆最小转弯半径对应的最小转弯角度，由车辆平台自身的性能决定。

所述的步骤十一中首先判断是否存在关键点keyPoint，即先判断是否需要绕障，如果否，则H(i)＝H1(i)*α1+H2(i)*α2，如果是，则判断该节点的Y坐标系是否小于关键点keyPoint的Y坐标系，如果否，则H(i)＝H1(i)*α1+H2(i)*α2，如果是，则H(i)＝H3(i)*α3+H4(keyPoint)*α4。

本发明的有益效果在于：(1)引入引导线对评估函数进行改进，能够得到期望的最优解；(2)更加符合人类驾驶员的驾驶行为；(3)引入keyPoint使其避障策略更优。该发明可以直接用于无人驾驶车辆局部路径规划中，能够提升无人驾驶技术，进一步推广无人驾驶产品的应用，产生较好的经济效益。

附图说明

图1是传统A星算法的搜索效果图；

图2是本发明所提供的一种基于改进A星搜索的无人驾驶汽车局部路径规划方法的流程图；

图3为生成关键点keyPoint的流程图；

图4为评估函数生成规则流程图；

图5为本发明中初始点、目标点、引导线、障碍物、预测函数的示意图；

图6为本发明方法的规划效果示意图；

图7为引入keyPoint前后算法的效果图；

图8为keyPoint的选择策略示意图。

具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

目前现有的A星算法属于寻优算法，但是其算法本身并不能保证得到最优解。如图1中(a)所示，假设目标为车辆正前方的点，但是传统A星算法搜索出来的结果并不能保证是最快通往目标点的直线。而且传统的A星算法并不是专门为无人驾驶汽车设计的，因此在无人驾驶汽车这个具体应用中，A星算法规划出来的路径并不符合人类驾驶员的驾驶习惯和实际需求，如图1(b)所示，在弯曲道路上，A星搜索方法会直接搜索出最近的直线，增加车辆开出路边等隐患，而不能像人类一样沿着道路内侧转弯。因此，为了使上述A星搜索算法在无人驾驶汽车局部路径规划中使用，本发明对传统的A星方法进行了以下改进：

第一，给算法中所有拓展的节点带上方向属性，引入汽车转弯特有的机械约束。因此，算法只拓展无人驾驶车辆前进方向范围内的节点，更符合物理意义；

第二，引入引导线L，引导线L由一系列的引导点组成，是无人驾驶汽车领域用于引导车辆前进的一种信息表示；本发明中提出用L来修改A星算法的评估函数，使其更符合人类驾驶员的驾驶习惯；

第三，为了更符合人类驾驶员的避障策略，本发明提出引入关键点keyPoint的概念，用keyPoint来引导A星算法实现饶障的过程。

根据上述三方面的改进思路，结合图2，本发明提出一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，具体步骤为：

步骤一：如图5所示，输入带方向的初始点S、带方向的目标点T、引导线L和障碍物图costmap；

步骤二：根据引导线L与障碍物图costmap，生成关键点keyPoint；

如图3所示，生成关键点keyPoint的规则首先判断引导线上是否存在障碍物，如果存在障碍物，则寻找生成绕障的关键点keyPoint，如图5所示。

关键点keyPoint的获取方式如下：找出所有落在引导线L上的障碍物，计算出其障碍物边缘点，标记为候选关键点keyPoint(如图8所示)，然后按照以下原则从候选关键点keyPoint中选择一个点作为keyPoint：(1)用户倾向，从左绕障或者从右绕障；(2)引导线离障碍物距离较小一侧；(3)候选关键点keyPoint所在位置的可通行区域；(4)当有多个障碍物时，选择绕行侧与引导线L最大距离的候选点。以上四个原则可以根据需要任选其一或者若干进行组合使用。

其中，关键点keyPoint的选择并不唯一，关键点keyPoint的选择只会体现期望路径的不同，而不影响算法规划结果。

步骤三：建立OPEN表和CLOSE表，分别用于存放待拓展节点和已拓展节点；

步骤四：设置评估函数F(i)＝G(i)+H(i)，i表示第i个节点，其中G(i)表示从初始点到该i节点的代价值；H(i)表示从该i节点到目标点的预测的代价值，本发明中H(i)包括四个部分，H(i)＝H1(i)*α1+H2(i)*α2+H3(i)*α3+H4(keyPoint)*α4；

其中，H1(i)、H2(i)、H3(i)、H4(keyPoint)的物理意义如图5所示，H1(i)表示该节点到引导线L之间的最小距离值；H2(i)为i节点到引导线最小距离所对应的点到目标点之间的距离；H3(i)为节点i到关键点keyPoint之间的距离；H4(keyPoint)表示从keyPoint到目标点T之间的距离；α1、α2、α3、α4为权重系数；

步骤五：计算初始点S的评估函数值F，并放入OPEN表中；

步骤六：OPEN表中的点按评估函数值F从小到大排序；

步骤七：如果OPEN表不为空，则弹出OPEN表中排序的第一个节点K，否则算法失败；

步骤八：判断该节点K是否为目标点，如果是，则结束算法，输出节点K及其一系列父节点作为搜索结果；否则进行步骤九；

步骤九：拓展节点K的相邻节点，按车辆的方向和转弯特性进行拓展，即在[-θ,θ]区间内按角度均匀拓展n个节点，记为(xj,yj),j∈{1，n}，其中θ为车辆最小转弯半径对应的最小转弯角度，由车辆平台自身的性能决定；

步骤十：判断待拓展的n个节点中是否有节点已经在CLOSE表中，如是，则放弃该节点；如果不是，继续下一步；

步骤十一：根据评估函数F(i)，计算每一个拓展节点的评估值。

评估准则流程图如图4所示，首先判断是否存在关键点keyPoint，即先判断是否需要绕障，如果否，则H(i)＝H1(i)*α1+H2(i)*α2，如果是，则判断该节点的Y坐标系是否小于关键点keyPoint的Y坐标系，如果否，则H(i)＝H1(i)*α1+H2(i)*α2，如果是，则H(i)＝H3(i)*α3+H4(keyPoint)*α4；

步骤十二：把带评估值的拓展节点放入OPEN表中，把节点K放入CLOSE表中；

步骤十三：跳到步骤六。

本发明方法克服了传统A星算法在无人驾驶汽车中规划局部路径的缺点，如图6所示，利用期望路线作为引导线，规划出来的结果比传统A星算法规划出来的结果更优，如图6(a)的结果显然比图1(a)的结果更优。在转弯的情况下，本发明方法的结果也更符合人类驾驶员的驾驶行为，如图6(b)的结果是沿着转弯轨迹内侧转弯，比图1(b)直接取直线行驶的结果更加合理。

本发明方法通过引入关键点keyPoint概念，使A星算法在避障规划中，能够更早的规避障碍物，其规划路线更加平顺。如图7所示，当车辆正前方出现障碍物时，图7(a)是没有关键点keyPoint情况下规划出来的路径，指引无人驾驶车辆继续向障碍物方向前进一定距离后才启动绕障动作，导致转弯角度变大，且离障碍物越近，绕障难度越大；图7(b)为引入关键点keyPoint之后的规划结果，由图可见其绕障动作更早，且更加平滑。

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

Claims (8)

1.一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：它包括如下步骤：

步骤一：输入带方向的初始点S、带方向的目标点T、引导线L和障碍物图costmap；

步骤二：根据引导线L与障碍物图costmap，生成关键点keyPoint；

步骤三：建立OPEN表和CLOSE表；

步骤四：设置评估函数F(i)＝G(i)+H(i)；

其中，i表示第i个节点，G(i)表示从初始点到该i节点的代价值，H(i)表示从该i节点到目标点的预测的代价值；

步骤五：计算初始点S的评估函数值F，并放入OPEN表中；

步骤六：OPEN表中的点按F值从小到大排序；

步骤七：如果OPEN表不为空，则弹出OPEN表中排序的第一个节点K，否则算法失败；

步骤八：判断该节点K是否为目标点，如果是，则结束算法，输出节点K及其一系列父节点作为搜索结果；否则进行步骤九；

步骤九：拓展节点K的相邻节点，按车辆的方向和转弯特性进行拓展；

步骤十：判断待拓展的n个节点中是否有节点已经在CLOSE表中，如是，则放弃该节点；否则进行步骤十一；

步骤十一：根据评估函数F(i)，计算每一个拓展节点的评估值；

步骤十二：把带评估值的拓展节点放入OPEN表中，把节点K放入CLOSE表中；

步骤十三：跳到步骤六。

2.如权利要求1所述的一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：所述的步骤二中生成关键点keyPoint的规则首先判断引导线上是否存在障碍物，如果存在障碍物，则寻找生成绕障的关键点keyPoint。

3.如权利要求2所述的一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：所述的步骤二中关键点keyPoint的获取方式如下：找出所有落在引导线L上的障碍物，计算出其障碍物边缘点，标记为候选关键点keyPoint，然后按照以下原则从候选关键点keyPoint中选择一个点作为keyPoint：(1)用户倾向，从左绕障或者从右绕障；(2)引导线离障碍物距离较小一侧；(3)候选关键点keyPoint所在位置的可通行区域；(4)当有多个障碍物时，选择绕行侧与引导线L最大距离的候选点。

4.如权利要求3所述的一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：所述的步骤二中的四个原则可以根据需要任选其一或者若干进行组合使用。

5.如权利要求1所述的一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：所述的步骤三中OPEN表和CLOSE表，分别用于存放待拓展节点和已拓展节点。

6.如权利要求1所述的一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：所述的步骤四中H(i)包括四个部分，

H(i)＝H1(i)*α1+H2(i)*α2+H3(i)*α3+H4(keyPoint)*α4；

其中，H1(i)表示该节点到引导线L之间的最小距离值；H2(i)为i节点到引导线最小距离所对应的点到目标点之间的距离；H3(i)为节点i到关键点keyPoint之间的距离；H4(keyPoint)表示从keyPoint到目标点之间的距离；α1、α2、α3、α4为权重系数。

7.如权利要求1所述的一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：所述的步骤九中在[-θ,θ]区间内按角度均匀拓展n个节点，记为(xj,yj),j∈{1，n}，其中θ为车辆最小转弯半径对应的最小转弯角度，由车辆平台自身的性能决定，(xj,yj)表示待拓展点的位置坐标，xj表示X轴的位置，yj表示Y轴的位置，j表示第j个点。

8.如权利要求1所述的一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：所述的步骤十一中首先判断是否存在关键点keyPoint，即先判断是否需要绕障，如果否，则H(i)＝H1(i)*α1+H2(i)*α2，如果是，则判断该节点的Y坐标系是否小于关键点keyPoint的Y坐标系，如果否，则H(i)＝H1(i)*α1+H2(i)*α2，如果是，则H(i)＝H3(i)*α3+H4(keyPoint)*α4，H1(i)表示该节点到引导线L之间的最小距离值；H2(i)为i节点到引导线最小距离所对应的点到目标点之间的距离；H3(i)为节点i到关键点keyPoint之间的距离；H4(keyPoint)表示从keyPoint到目标点T之间的距离；α1、α2、α3、α4为权重系数。

Images