CN108444488A - 基于等步采样a*算法的无人驾驶局部路径规划方法 - Google Patents

Abstract

本发明属于无人驾驶路径规划领域，为满足汽车的运动学约束与实际交通限制，本发明，基于等步采样A*算法的无人驾驶局部路径规划方法，具体步骤如下：步骤1：定义搜索步长和搜索安全域；步骤2：确定局部栅格图中路径搜索的起始点与目标点；步骤3：建立Open列表和Closed列表；步骤4：求解Open列表中栅格点的代价函数；步骤5：从Open列表中选择代价函数值最小的栅格点；步骤6：分别考察当前节点的所有安全相邻节点；步骤7：搜索过程中当前节点无子节点的处理；步骤8：重复步骤5‑7的过程，直到满足条件，返回可行路径，或返回搜索失败。本发明主要应用于无人驾驶路场合。

Description

基于等步采样A*算法的无人驾驶局部路径规划方法

技术领域

本发明属于无人驾驶路径规划领域，具体讲，涉及一种利用等步采样和A*算法原理的路径搜索算法进行无人驾驶车辆的局部路径规划。

背景技术

随着社会的发展，人们对生活品质的要求越来越高，汽车成为人类生活不可或缺的交通工具，然而汽车数量的增加以及驾驶员自身安全意识薄弱导致全球交通事故发生率急剧增长。面对日益严峻的交通安全与交通拥堵问题，构建智能交通***的任务变得更加急切。无人驾驶车辆作为构建智能交通***的关键部分，近年来受到各方的关注。无人驾驶车辆融合了环境感知与定位、决策规划与运动控制等多项功能，从而代替驾驶员的眼、脑和手，具有反应迅速、行驶安全可靠等优点。目前美国、英国和德国等一些国家的无人驾驶技术已发展较为成熟，我国的无人驾驶技术起步较晚，在一些关键技术的发展距离世界先进水平还有一定的差距。

无人驾驶车辆的路径规划算法主要继承了机器人领域的算法，例如A*算法、RRT算法、人工势场法等。A*(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，其鲁棒性好，对环境信息反应速度快，被广泛应用于各类机器人中。A*算法是Dijkstra算法的改进，比起原始的Dijkstra算法具有收敛速度快、指向性明显、搜索空间小的优点。但是传统的基础A*算法由于依赖于方形栅格中点进行搜索，且其代价函数仅是路径长度的函数(代价只与路径长短有关)，所以存在路径方向固定、路径平滑度不足、转折角度大等不足，而且在考虑车道线限制的实际道路仿真和大弯道道路仿真中，效果不理想。早在2007年美国DARPA(美国高级研究计划局)举办的城市挑战赛中，斯坦福大学研制的Junior无人车即利用一种改进的A*算法取得了比赛的亚军。我国一些科研单位也将A*算法用于无人驾驶车辆的路径规划中，例如中国科学技术大学即提出一种基于可搜索连续邻域A*算法的无人驾驶车辆路径规划方法。另外，RRT算法是一种随机采样的规划方法。自1998年提出，RRT算法由于其增量式生长特性被广泛应用于动态环境、高维状态环境以及存在运动动力学约束的环境中。人工势场法是由Khatib提出的一种虚拟力法，将机器人的运动设计成一种在人造引力场中的运动，规划出安全且较平滑的路径。由于以上两种方法在本发明中没有应用，后面不再进行说明。

虽然前人已提出多种无人驾驶路径规划算法，可是大多算法不满足车辆运动学约束，不能直接用于车辆控制，需要进行大量的处理工作。

发明内容

为了满足汽车的运动学约束与实际交通限制，针对无人驾驶车辆的局部路径规划做出以下几方面研究：

(1)加入车道线约束下的代价函数的选择；

(2)带有避障要求的代价函数的选择；

(3)考虑车辆实际运动学模型，带有转向约束的A*算法；

(4)基于栅格内任意点(非中心点)的A*路径搜索及安全域选择；

(5)十字路口车辆转弯的路径规划。

本发明将等步采样的思想引入到局部路径规划策略中，基于等步采样A*算法的无人驾驶局部路径规划方法，具体步骤如下：

步骤1：定义搜索步长和搜索安全域；

搜索步长计算公式为：

其中Step表示搜索步长，l表示栅格图精度，v表示当前车辆的行驶速度，T表示局部路径更新时间；

采用圆形安全域判断，圆形域的半径r_safe定义为：

这里L_vehicle为车身长度，max(·)为取最大值函数，在已知当前搜索节点的父节点的情况下，需要判断当前搜索节点的圆形域和当前节点与其父节点中心点的圆形域，两个圆形域覆盖的区域均无障碍物则为安全，若存在障碍物则为危险区域，该节点的代价函数值为无穷大；

步骤2：确定局部栅格图中路径搜索的起始点与目标点；

路径搜索的起始点即为车辆的当前位置，在车载坐标系下为(0,0)点，车头指向x轴正方向，故车辆在每帧栅格图中的位置和方向均相同，依据栅格图中的车道线信息可以判断出当前道路形状，通过二次拟合可以得到车道线曲线，进而得到当前车道中心线曲线，由于在车载坐标系下车辆严格按照x坐标增大的方向行驶，故路径搜索的目标点定义为栅格图范围内当前车道中心线曲线x坐标值最大位置，若该位置安全域范围内存在障碍物，则在该位置附近选择距离障碍物满足安全距离的点作为目标点；

步骤3：建立Open列表和Closed列表，Open列表存放所有已生成而未考察的节点，Closed列表存放已考察过并添加到路径中的节点，Closed列表中最后一个元素即为当前路径搜索节点，

步骤4：求解Open列表中栅格点的代价函数；

f(n)＝K₁g(n)+K₂h(n)+K₃p(n) (4a)

g(n)＝g₁L_acc(n)+g₂D_acc(n) (4b)

h(n)＝h₁L_est(n)+h₂D_est(n) (4c)

其中，f(n)表示栅格点的总代价，K₁、K₂和K₃为三个正的权重系数，g(n)表示起点到该节点的累积代价，g₁和g₂为正的权重系数，L_acc(n)为累积的步长代价,D_acc(n)为累积的转向代价，

h(n)表示从该节点到目标点的预估代价，h₁和h₂为正的权重系数，L_est(n)为预估的步长代价，D_est(n)为预估的转向代价，

p(n)为惩罚项，定义为从父节点到该节点的固定代价，由于距离代价在这一项上没有区别，故该部分仅由转向角度确定，α₁为正的权重系数。θ(n)表示当前节点的运动方向。

对于起点的运动方向θ(0)＝0，累积代价g(0)＝0，惩罚项为p(0)＝0，故起点的代价函数为：

f(0)＝K₂(h₁L_est(0)+h₂D_est(0)) (5)

步骤5：从Open列表中选择代价函数值最小的栅格点，标记为当前结点，并将其从Open列表移到Closed列表中；

步骤6：分别考察当前节点的所有安全相邻节点，若该点既不在Open列表，也不在Closed列表中，将该点添加到Open列表中，求解其代价函数值，当前节点即为该点的父节点；

步骤7：若在搜索过程中当前节点无子节点，则由当前节点返回其父节点，并将该点从Closed列表中删除，在Open列表中再次选择代价函数值最小的节点；

步骤8：重复步骤5-7的过程，直到满足条件：

则视为到达目标点，结束搜索，返回可行路径。若在搜索过程中出现Open列表和Closed列表均为空，则不存在可行路径，返回搜索失败。

步骤3具体操作流程如下：

①将起点添加到Open列表和Closed列表中；

②将与起点相邻的安全栅格点添加到Open列表中，则起点即为相邻栅格点的父节点，考

虑到车的转向约束，相邻栅格点坐标(x_cur,y_cur)为：

其中，(x_father,y_father)表示父节点的坐标，θ_fatehr为父节点的运动方向。N决定选取的节点个数，即以固定的角度差选择2N+1个相邻栅格点，φ_max表示车辆最大前轮偏角，i表示节点计数，其正负决定车辆的转向，i为正数时，车辆表现为右转，i为负数时，车辆表现为左转。相邻栅格点的运动方向也即为

如步骤4中的代价函数所示，K₁、K₂、K₃、g₁、g₂、h₁、h₂以及α₁均为正实数，需要通过参数整定确定，在K₁＝0.8,K₂＝1.52,K₃＝0.25,g₁＝1,g₂＝1.2,h₁＝1,h₂＝0.6,α₁＝1.2时效果符合实际要求。

与已有技术相比，本发明的技术特点与效果：

本发明提出的等步采样选取搜索节点，在确定相邻节点时即限制了节点的运动方向，既满足了车辆的转向约束，搜索到的路径也较为平滑，在匀速的情况下，车辆控制无需对路径进行拟合。传统A*算法以及前人提出的一些改进A*算法普遍存在的问题是路径中节点之间的距离不等，并且搜索方向无限制，造成搜索到的路径需要大量的处理才能用于车辆控制，例如去节点、曲线拟合等。

与已有技术相比，本发明所提出的代价函数中的累积代价和预估代价部分不仅包括距离代价，还包括角度代价，并且代价函数带有惩罚项，定义为当前节点到下一节点的固定代价，由于距离代价在这一项上没有区别，故该项仅由转向角度确定。以上因素可以尽可能的避免大转弯，使路径更加平滑，减少车辆机械损耗。在避障结束时，预估角度代价的设置使得路径尽可能快地回到车辆应行驶的车道。为降低计算复杂度，代价函数中距离代价部分均采用曼哈顿距离，实验证明采用曼哈顿距离所用的搜索时间要明显短于采用欧氏距离所用的搜索时间。

由于本发明的搜索点不在栅格中心，故安全域采用以往的方形邻域并不合适，且方形邻域对带有方向的安全检查并不准确，而圆形邻域由于其旋转对称性，与方向无关，故作为安全邻域更为合适。在本发明中所采用的安全邻域为以当前搜索节点和当前搜索节点与其父节点的中心点为圆心，直径为一个搜索步长与车身长度中最大值的两个圆所覆盖的所有栅格区域，保证了路径不会存在穿越障碍物的情况，车辆无论往哪个方向行驶均无碰撞危险。

该发明经过测试在十字路口转弯、避障以及S型弯等路况均能较快地得到可行路径，所得路径均满足车辆控制限制，在Visual Studio中所得的搜索时间均小于30ms。(Visual Studio是Windows平台应用程序的集成开发环境)

附图说明

图1为本发明算法整体流程图。

图2为车载坐标系定义。

图3为Visual Studio2013中实现的十字路口左转结果图。

图4为Visual Studio2013中实现的十字路口右转结果图。

图5为Visual Studio2013中实现的避障结果图。

图6为Visual Studio2013中实现的S型弯道路结果图。

具体实施方式

本发明将等步采样的思想加入到局部路径规划策略中，提出一种新的代价函数，具体实施步骤如下：

步骤1：定义搜索步长和搜索安全域；

定义搜索步长为一个控制周期内车辆前进的距离，故搜索步长计算公式为：

其中Step表示搜索步长，l表示栅格图精度，v表示当前车辆的行驶速度，T表示局部路径更新时间。

由于车辆前进具有方向变化的可能性，为确保路径中每一个节点都为安全，采用圆形安全域判断，圆形域的半径为：

这里L_vehicle为车身长度，max(·)为取最大值函数，在已知当前搜索节点的父节点的情况下，需要判断当前搜索节点的圆形域和当前节点与其父节点中心点的圆形域，两个圆形域覆盖的区域均无障碍物则为安全，若存在障碍物则为危险区域，该节点的代价函数值为无穷大。与方形安全搜索域相比，采用圆形安全搜索域的好处是判断范围减小，降低了路径搜索失败的可能性。

步骤2：确定局部栅格图中路径搜索的起始点与目标点；

路径搜索的起始点即为车辆的当前位置，在车载坐标系下为(0,0)点，车头指向x轴正方向，如图2所示故车辆在每帧栅格图中的位置和方向均相同。依据栅格图中的车道线信息可以判断出当前道路形状，通过二次拟合可以得到车道线曲线，进而得到当前车道中心线曲线。由于在车载坐标系下车辆严格按照x坐标增大的方向行驶，故路径搜索的目标点定义为栅格图范围内当前车道中心线曲线x坐标值最大位置，若该位置安全域范围内存在障碍物，则在该位置附近选择距离障碍物满足安全距离的点作为目标点。

步骤3：建立Open列表和Closed列表，Open列表存放所有已生成而未考察的节点，Closed列表存放已考察过并添加到路径中的节点，Closed列表中最后一个元素即为当前路径搜索节点，具体操作流程如下：

①将起点添加到Open列表和Closed列表中。

②将与起点相邻的安全栅格点添加到Open列表中，则起点即为相邻栅格点的父节点，考虑到车的转向约束，相邻栅格点坐标(x_cur,y_cur)为：

其中，(x_father,y_father)表示父节点的坐标，θ_fatehr为父节点的运动方向。N决定选取的节点个数，即以固定的角度差选择2N+1个相邻栅格点，φ_max表示车辆最大前轮偏角，i表示节点计数，其正负决定车辆的转向，i为正数时，车辆表现为右转，i为负数时，车辆表现为左转。相邻栅格点的运动方向也即为

步骤4：求解Open列表中栅格点的代价函数；

f(n)＝K₁g(n)+K₂h(n)+K₃p(n) (4a)

g(n)＝g₁L_acc(n)+g₂D_acc(n) (4b)

h(n)＝h₁L_est(n)+h₂D_est(n) (4c)

其中，f(n)表示栅格点的总代价，K₁、K₂和K₃为三个正的权重系数。g(n)表示起点到该节点的累积代价，g₁和g₂为正的权重系数，累积的步长代价为：

L_acc(n)＝1+L_acc(n-1)

累积的转向代价为：

h(n)表示从该节点到目标点的预估代价，h₁和h₂为正的权重系数，预估的步长代价为：

其中，(x_goal,y_goal)表示目标点的坐标。

预估的转向代价为：

其中，arctan(·)为取最大值函数。

p(n)为惩罚项，定义为从父节点到该节点的固定代价，由于距离代价在这一项上没有区别，故该部分仅由转向角度确定，α₁为正的权重系数。θ(n)表示当前节点的运动方向。

对于起点的运动方向θ(0)＝0，累积代价g(0)＝0，惩罚项为p(0)＝0，预估代价为：

其中，(x_start,y_start)表示起点的坐标。

故起点的代价函数为：

f(0)＝K₂(h₁L_est(0)+h₂D_est(0)) (5)

步骤5：从Open列表中选择代价函数值最小的栅格点，标记为当前结点，并将其从Open列表移到Closed列表中；

步骤6：分别考察当前节点的所有安全相邻节点，若该点既不在Open列表，也不在Closed列表中，将该点添加到Open列表中，求解其代价函数值，当前节点即为该点的父节点；

步骤7：若在搜索过程中当前节点无子节点，则由当前节点返回其父节点，并将该点从Closed列表中删除，在Open列表中再次选择代价函数值最小的节点；

步骤8：重复步骤5-7的过程，直到满足条件：

则视为到达目标点，结束搜索，返回可行路径。若在搜索过程中出现Open列表和Closed列表均为空，则不存在可行路径，返回搜索失败。

本发明实施步骤流程图如图1所示，实验结果表明该算法可以满足无障碍物时保持车道行驶，如图2、3所示，十字路口转弯时也可得到较为平滑的路径并在无障碍物时做到车道保持，如图4、5所示，可以在满足车辆转向限制的基础上做到静态障碍物的避障。如步骤4中的代价函数所示，K₁、K₂、K₃、g₁、g₂、h₁、h₂以及α₁均为正实数，需要通过参数整定确定。参数的不同会导致对于同种路况搜索到的路径和搜索时间具有很大的差距，实验数据显示在K₁＝0.8,K₂＝1.52,K₃＝0.25,g₁＝1,g₂＝1.2,h₁＝1,h₂＝0.6,α₁＝1.2时效果基本符合实际要求。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

A*算法的提出见参考文献如下：

P.E.Hart,N.J.Nilsson,and B.Raphael.A formal basis for the heuristicdetermination of minimum cost paths in graphs.IEEE Trans.Syst.Sci.andCybernetics,SSC-4(2):100-107,1968”。

Claims (3)

1.一种基于等步采样A*算法的无人驾驶局部路径规划方法，其特征是，具体步骤如下：

步骤1：定义搜索步长和搜索安全域；

搜索步长计算公式为：

其中Step表示搜索步长，l表示栅格图精度，v表示当前车辆的行驶速度，T表示局部路径更新时间；

采用圆形安全域判断，圆形域的半径r_safe定义为：

这里L_vehicle为车身长度，max(·)为取最大值函数，在已知当前搜索节点的父节点的情况下，需要判断当前搜索节点的圆形域和当前节点与其父节点中心点的圆形域，两个圆形域覆盖的区域均无障碍物则为安全，若存在障碍物则为危险区域，该节点的代价函数值为无穷大；

步骤2：确定局部栅格图中路径搜索的起始点与目标点；

路径搜索的起始点即为车辆的当前位置，在车载坐标系下为(0,0)点，车头指向x轴正方向，故车辆在每帧栅格图中的位置和方向均相同，依据栅格图中的车道线信息可以判断出当前道路形状，通过二次拟合可以得到车道线曲线，进而得到当前车道中心线曲线，由于在车载坐标系下车辆严格按照x坐标增大的方向行驶，故路径搜索的目标点定义为栅格图范围内当前车道中心线曲线x坐标值最大位置，若该位置安全域范围内存在障碍物，则在该位置附近选择距离障碍物满足安全距离的点作为目标点；

步骤3：建立Open列表和Closed列表，Open列表存放所有已生成而未考察的节点，Closed列表存放已考察过并添加到路径中的节点，Closed列表中最后一个元素即为当前路径搜索节点，

步骤4：求解Open列表中栅格点的代价函数；

f(n)＝K₁g(n)+K₂h(n)+K₃p(n) (4a)

g(n)＝g₁L_acc(n)+g₂D_acc(n) (4b)

h(n)＝h₁L_est(n)+h₂D_est(n) (4c)

其中，f(n)表示栅格点的总代价，K₁、K₂和K₃为三个正的权重系数，g(n)表示起点到该节点的累积代价，g₁和g₂为正的权重系数，L_acc(n)为累积的步长代价,D_acc(n)为累积的转向代价，

h(n)表示从该节点到目标点的预估代价，h₁和h₂为正的权重系数，L_est(n)为预估的步长代价，D_est(n)为预估的转向代价，

p(n)为惩罚项，定义为从父节点到该节点的固定代价，由于距离代价在这一项上没有区别，故该部分仅由转向角度确定，α₁为正的权重系数。θ(n)表示当前节点的运动方向。

对于起点的运动方向θ(0)＝0，累积代价g(0)＝0，惩罚项为p(0)＝0，故起点的代价函数为：

f(0)＝K₂(h₁L_est(0)+h₂D_est(0)) (5)

步骤5：从Open列表中选择代价函数值最小的栅格点，标记为当前结点，并将其从Open列表移到Closed列表中；

步骤6：分别考察当前节点的所有安全相邻节点，若该点既不在Open列表，也不在Closed列表中，将该点添加到Open列表中，求解其代价函数值，当前节点即为该点的父节点；

步骤7：若在搜索过程中当前节点无子节点，则由当前节点返回其父节点，并将该点从Closed列表中删除，在Open列表中再次选择代价函数值最小的节点；

步骤8：重复步骤5-7的过程，直到满足条件：

则视为到达目标点，结束搜索，返回可行路径了若在搜索过程中出现Open列表和Closed列表均为空，则不存在可行路径，返回搜索失败。

2.如权利要求1所述的基于等步采样A*算法的无人驾驶局部路径规划方法，其特征是，步骤3具体操作流程如下：

①将起点添加到Open列表和Closed列表中；

②将与起点相邻的安全栅格点添加到Open列表中，则起点即为相邻栅格点的父节点，考虑到车的转向约束，相邻栅格点坐标(x_cur,y_cur)为：

其中，(x_father,y_father)表示父节点的坐标，θ_fatehr为父节点的运动方向。N决定选取的节点个数，即以固定的角度差选择2N+1个相邻栅格点，φ_max表示车辆最大前轮偏角，i表示节点计数，其正负决定车辆的转向，i为正数时，车辆表现为右转，i为负数时，车辆表现为左转。相邻栅格点的运动方向也即为

3.如权利要求1所述的基于等步采样A*算法的无人驾驶局部路径规划方法，其特征是，如步骤4中的代价函数所示，K₁、K₂、K₃、g₁、g₂、h₁、h₂以及α₁均为正实数，需要通过参数整定确定，在K₁＝0.8,K₂＝1.52,K₃＝0.25,g₁＝1,g₂＝1.2,h₁＝1,h₂＝0.6,α₁＝1.2时效果符合实际要求。