CN110487290B - 基于变步长a星搜索的无人驾驶汽车局部路径规划方法 - Google Patents

Abstract

本发明公开了一种基于变步长A星搜索的无人驾驶汽车局部路径规划方法，它利用事先设置好的搜索模板，根据拓展点与障碍物之间的距离关系，自适应的从搜索模板中得到不同的拓展步长，从而降低A星算法的拓展节点个数，提高A星算法的计算效率，满足应用场景的实时性要求。本发明的有益效果在于：利用变步长策略，使得A星搜索能够降低拓展节点个数，降低计算复杂度；利用预设的计算模板，提高计算效率，从而确保该发明能够直接用于对实时性要求较高的无人驾驶车辆局部路径规划中，能够提升无人驾驶技术，进一步推广无人驾驶产品的应用，产生较好的经济效益。

Description

基于变步长A星搜索的无人驾驶汽车局部路径规划方法

技术领域

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

背景技术

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

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

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

目前主要的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星算法计算复杂度与搜索空间、搜索步长密切相关，当搜索空间固定时，其搜索效率由拓展的搜索点个数决定，而拓展点个数跟搜索步长密切相关。以搜索空间为250×250的栅格地图空间为例，假设初始点S所在位置为坐标原点，经过初始点向右为X坐标，初始点S所指方向为Y坐标。目标点T在(0,240)，目标点判别准则为2个栅格距离以内，A星算法分别采用步长为step＝1,3,6,9(单位为栅格距离)的搜索效果如图1——图4所示，其中每幅图中的(a)是无障碍物时的搜索效果，(b)是有障碍物时的搜索效果。各个不同步长下搜索过程中A星算法总共拓展的节点个数如下表所示：

表1不同步长下A星算法的拓展节点个数

从附图的效果和搜索步长可以看出两个问题：第一，搜索步长变长，可以降低搜索复杂度，但是搜索步长与计算复杂度并不是成反比，即并不是步长越长，搜索节点就越少。通过分析可以发现，搜索步长变长能够减少拓展节点，但是当搜索步长过长时，导致无法快速满足判断目标点的条件(目标点判别准则为拓展节点距离目标点在2个栅格距离以内)，从而使算法进行大量的回溯拓展，增加了拓展节点的个数；第二，搜索步长过长会导致搜索错误结果的出现，如附图4(b)所示，因为搜索步长过长，较小的障碍物的大小小于步长时，搜索过程中就会跨过该类障碍物，从而使该类障碍物被淹没，算法给出错误的搜索结果。

正是因为这些问题的存在，经典的A星搜索和后续改进方法都是默认一个固定的搜索步长(往往取最小单位栅格距离)，搜索方向也是采用栅格地图的四邻域或者八邻域进行搜索。

发明内容

本发明的目的在于提供一种基于变步长A星搜索的无人驾驶汽车局部路径规划方法，利用事先设置好的搜索模板，根据拓展点与障碍物之间的距离关系，自适应的从搜索模板中获取不同的拓展步长，从而降低A星算法的拓展节点个数，提高A星算法的计算效率，满足应用场景的实时性要求。

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

步骤一：初始化搜索模板；

步骤二：构建搜索空间，输入带方向的初始点S、带方向的目标点T、引导线L和障碍物图costmap；

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

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

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

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

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

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

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

步骤九：根据当前拓展点K与障碍物图costmap的关系，得到拓展步长step；

步骤十：根据K的方向，在预设模板中寻找最接近的方向，根据拓展步长step，在预设模板中寻找最接近的预设搜索步长，然后向预设的搜索分支拓展节点K的相邻节点；

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

步骤十二：根据评估函数F(i)和拓展步长step，利用搜索模板计算拓展节点j(j∈{1，n})的评估值；

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

步骤十四：跳到步骤六。

所述的步骤一中初始化搜索模板，该模板包括N个方向、M个分支和T个步长。

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

所述的评估函数F(i)＝G(i)+H(i)；其中，i表示第i个节点，G(i)表示从初始点S到该i节点的已付出的代价值，H(i)表示从该i节点到目标点T的预测代价值。

所述的H(i)＝H1(i)*α1+H2(i)*α2，其中，H1(i)表示该节点到引导线L之间的最小距离值；H2(i)为i节点到引导线最小距离所对应的点到目标点之间的距离；α1、α2为权重系数。

所述的G(i)＝G1(i-1)*β1+G2(i)*β2，其中，G1(i-1)表示从初始点到i-1节点的已付出代价值；G2(i)表示从i-1节点拓展到i节点需要付出的代价值，该值与拓展步长的长度有关；β1、β2为权重系数。

所述的步骤九中，拓展步长step的计算方式如下：首先计算以当前节点K为中心，step_max*γ为半径的范围内是否存在障碍物，如否，则取step＝step_max；如是，则计算以节点K为中心，step_min*γ为半径的范围内是否存在障碍物，如是，则取step＝step_min；如否，则计算K与costmap中最近障碍物的距离dis(K)，使step＝dis(K)/γ，然后在预设步长中取最近的值；其中γ为系数，step_max为预设的最大步长，step_min为预设的最小步长。

本发明的有益效果在于：利用变步长策略，使得A星搜索能够降低拓展节点个数，降低计算复杂度；利用预设的计算模板，提高计算效率，从而确保该发明能够直接用于对实时性要求较高的无人驾驶车辆局部路径规划中，能够提升无人驾驶技术，进一步推广无人驾驶产品的应用，产生较好的经济效益。

附图说明

图1是传统A星算法在步长step＝1条件下的搜索效果图，其中，(a)是无障碍物时的搜索效果，(b)是有障碍物时的搜索效果；

图2是传统A星算法在步长step＝3条件下的搜索效果图，其中，(a)是无障碍物时的搜索效果，(b)是有障碍物时的搜索效果；

图3是传统A星算法在步长step＝6条件下的搜索效果图，其中，(a)是无障碍物时的搜索效果，(b)是有障碍物时的搜索效果；

图4是传统A星算法在步长step＝9条件下的搜索效果图，其中，(a)是无障碍物时的搜索效果，(b)是有障碍物时的搜索效果；

图5是本发明方法的流程图；

图6为计算step值的流程图；

图7为初始化搜索模板示意图；

图8为搜索分支和搜索步长示意图；

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

图10为本发明方法的规划效果示意图，其中，(a)是无障碍物时的搜索效果，(b)是有障碍物时的搜索效果。

具体实施方式

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

本发明提出一种基于变步长A星搜索的无人驾驶汽车局部路径规划方法，其流程如图5所示，具体包括如下步骤：

步骤一：初始化搜索模板；该模板包括N个方向、M个分支和T个步长。搜索模板如图7所示，本示例中把360度均匀的分成24等分，即设置N＝24个搜索方向，每个方向设置M＝3个搜索分支。如图8所示，搜索分支上的点表示不同的搜索步长，步长数T可以根据任务需求灵活设置。传统A星算法在拓展节点时频繁涉及到大量的三角符号运算，利用预设的搜索模板的优势在于对外拓展的所有涉及到的可能性(N＝24个角度方向，M＝3个搜索分支，T个步长数)都可以事先计算好，且这些三角符号运算只需要计算一次，在同样数量的拓展节点情况下，能够极大的降低运算量，节约运算时间。

步骤二：如图9所示，构建搜索空间，本示例为了更好的说明效果，设置大小为250*250的栅格地图作为搜索空间，然后输入带方向的初始点S、带方向的目标点T、引导线L和障碍物图costmap；

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

步骤四：设置评估函数F(i)＝G(i)+H(i)；其中，i表示第i个节点，G(i)表示从初始点S到该i节点的已付出的代价值，H(i)表示从该i节点到目标点T的预测代价值。H(i)＝H1(i)*α1+H2(i)*α2，G(i)＝G1(i-1)*β1+G2(i)*β2，其物理意义如图9所示：图中，H1(i)表示该节点到引导线L之间的最小距离值；H2(i)为i节点到引导线最小距离所对应的点到目标点之间的距离；G1(i-1)表示从初始点到i-1节点的已付出代价值；G2(i)表示从i-1节点拓展到i节点需要付出的代价值，该值与拓展步长的长度有关；α1、α2、β1、β2为权重系数。

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

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

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

步骤八：判断该节点K是否与目标点T足够靠近(距离在两个栅格以内)，如果是，则结束算法，输出节点K及其一系列父节点作为搜索结果；否则进行步骤九；

步骤九：根据当前拓展点K与障碍物图costmap的关系，得到拓展步长step；step的计算方式如图6所示：首先计算以当前节点K为中心，step_max*γ为半径的范围内是否存在障碍物，如否，则取step＝step_max；如是，则计算以节点K为中心，step_min*γ为半径的范围内是否存在障碍物，如是，则取step＝step_min；如否，则计算K与costmap中最近障碍物的距离dis(K)，使step＝dis(K)/γ，然后在预设模板的步长中取最近的值；其中γ为系数，step_max和step_min都是预设模板中事先设置好的步长；

在计算搜索步长时，还需要计算拓展点与目标点的距离dis(K,T),如果dis(K,T)小于上述计算得到的步长step，则step＝dis(K,T)，确保A星算法在拓展过程中以一个较小的搜索步长接近目标点。

步骤十：根据K的方向，在预设模板中寻找最接近的方向，根据拓展步长step，在预设模板中寻找最接近的预算搜索步长，然后向预设的三个搜索分支拓展节点K的相邻节点；

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

步骤十二：根据评估函数F(i)和拓展步长step，利用搜索模板计算拓展节点j(j∈{1，n})的评估值，其中n为拓展的节点个数，n≤M；

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

步骤十四：跳到步骤六。

本示例中，预先设置step_min＝1，单位为栅格距离，设置step_max＝9，中间设置了step＝3和step＝6两种预设步长，取γ＝2.0。

本发明方法克服了传统A星算法运算量大、无法在实时性要求较高的无人驾驶汽车规划局部路径这类场合应用的缺点，其搜索步长对比情况如下表所示，通过变步长的思路，使得A星算法的拓展过程在没有障碍物时可以以较大步长拓展，而在距离障碍物较近或者快到目标点时，以较小的步长进行拓展，从而更快的实现目标的规划。

表2 A星算法的拓展节点个数对比表

其搜索效果如图10所示，在靠近障碍物时其拓展的节点明显比无障碍物时更多。本发明既提升了A星算法搜索效率，降低了运算时间，又保留了A星算法的性能，使其能够在无人驾驶汽车、机器人等需要实时规划的应用中满足使用需求。

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

Claims (3)

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

步骤一：初始化搜索模板；

步骤二：构建搜索空间，输入带方向的初始点S、带方向的目标点T、引导线L和障碍物图costmap；

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

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

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

所述的评估函数F(i)＝G(i)+H(i)；其中，i表示第i个节点，G(i)表示从初始点S到该i节点的已付出的代价值，H(i)表示从该i节点到目标点T的预测代价值；

所述的H(i)＝H1(i)*α1+H2(i)*α2，其中，H1(i)表示该节点到引导线L之间的最小距离值；H2(i)为i节点到引导线最小距离所对应的点到目标点之间的距离；α1、α2为权重系数；

所述的G(i)＝G1(i-1)*β1+G2(i)*β2，其中，G1(i-1)表示从初始点到i-1节点的已付出代价值；G2(i)表示从i-1节点拓展到i节点需要付出的代价值，该值与拓展步长的长度有关；β1、β2为权重系数；

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

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

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

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

步骤九：根据当前拓展点K与障碍物图costmap的关系，得到拓展步长step；

所述的拓展步长step的计算方式如下：首先计算以当前节点K为中心，step_max*γ为半径的范围内是否存在障碍物，如否，则取step＝step_max；如是，则计算以节点K为中心，step_min*γ为半径的范围内是否存在障碍物，如是，则取step＝step_min；如否，则计算K与costmap中最近障碍物的距离dis(K)，使step＝dis(K)/γ，然后在预设步长中取最近的值；其中γ为系数，step_max为预设的最大步长，step_min为预设的最小步长；

步骤十：根据K的方向，在预设模板中寻找最接近的方向，根据拓展步长step，在预设模板中寻找最接近的预设搜索步长，然后向预设的搜索分支拓展节点K的相邻节点；

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

步骤十二：根据评估函数F(i)和拓展步长step，利用搜索模板计算拓展节点j(j∈{1，n})的评估值；

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

步骤十四：跳到步骤六。

2.如权利要求1所述的基于变步长A星搜索的无人驾驶汽车局部路径规划方法，其特征在于：所述的步骤一中初始化搜索模板，该模板包括N个方向、M个分支和T个步长。

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

Images