一种基于改进A*算法的自动驾驶避障轨迹规划方法

本发明属于车辆全局规划与局部规划，尤其涉及一种基于改进a*算法的自动驾驶避障轨迹规划方法。

背景技术：

1、随着人工智能以及自动驾驶领域的飞速发展，多传感器融合技术、导航与定位技术、运动控制与决策技术、多目标协同技术等广泛应用到自动驾驶各个领域。其中为了满足无人车导航任务的需求，轨迹规划是非常关键的部分。在自动驾驶汽车行驶过程中，准确，安全，快速的避开障碍物不仅能提高其行驶安全性，同时出行效率也有一定的提高。其研究对提高车辆的智能化驾驶水平和增加道路通行能力有重要意义。

2、汽车已成为人们日常生活和生产中不可缺少的工具。在由人、车和环境组成的闭环驾驶系统中，驾驶员驾驶技能、生理状态和体力的差异是驾驶系统中最薄弱的环节，也是交通事故频发的主要原因。自驾汽车被认为能够解决交通安全、拥堵、车辆调度等问题，因为它们消除了部分或全部人为因素。自动驾驶技术已成为未来汽车的发展方向。自主驾驶的核心技术主要包括四个模块：感知、决策、规划和控制。路径规划是在获得车辆周围环境模型后，在满足避障和可行路径的前提下，确定最优行驶路径。当前路径规划的主要方法有：

3、(1)基于人工势场的路径规划算法，其基本思想是将目标和障碍物对机器人运动的影响具体化成人造势场。目标处势能低，障碍物处势能高。人工势场法计算方便，得到的路径安全平滑，但是复杂的势场环境可能在目标点之外产生局部极小点导致无法到达目标。

4、(2)基于图搜索的路径规划算法，图搜索法依靠已知的环境地图以及地图中的障碍物信息构造从起点到终点的可行路径。但是随着栅格地图分辨率的增加，将会导致该类算法的时间成本急剧增加，不适用于复杂环境下。

5、(3)基于随机采样的路径规划算法，其主要方法有快速扩展随机树(rrt)，rrt算法是一个相对高效率，同时可以较好的处理带有非完整约束的路径规划问题的算法，并且在很多方面有很大的优势，但是rrt算法并不能保证所得出的可行路径是相对优化的。

6、然而，现有的算法大多只适用于特定场合，对于复杂环境下的车辆的轨迹规划没有一套标准快速的规划方法。

技术实现思路

1、发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种基于改进a*算法的自动驾驶避障轨迹规划方法，该方法兼顾了全局最优性和实时性，提高了算法的效率和算法对环境的适应性，从而实现自动驾驶参考路径的在线规划和动态全局避障，完成高效的最优路径搜索任务。

2、本发明的一种基于改进a*算法的自动驾驶避障轨迹规划方法，具体包括如下步骤：

3、步骤1，通过多传感器融合技术实时获取车辆自身和周围障碍物的信息；

4、步骤2，建立周围环境与障碍物的栅格地图，简化车辆模型；

5、步骤3，基于步骤2实时生成的栅格地图与车辆简化模型，建立代价函数fij(k)并通过代价函数计算各处路径的代价大小。

6、步骤4，在步骤2中生成的m*n规格的栅格地图中确定起点、终点和障碍物位置。

7、步骤5，设置open、close和route集合，初始化为空集。

8、步骤6，将起点放在route集中，将所有障碍物点集放在close集中。

9、步骤7，以route中当前节点为基本点，搜索其所有可以展开的节点集放在open集中，同时在open中移除close中的所有集合；并将当前节点也放入close中。

10、步骤8，判断open集是否为空集，如果是，则返回到步骤7；如果不是，则在open中计算所有节点的代价函数fij(k)；并将fij(k)值最小节点置于route中，即作为下一个展开点。

11、步骤9，判断这个节点是否为终点，如果不是，则返回步骤7；

12、步骤10，将route中的所有节点的个数记为num；同时初始化递归起点route(i),i＝0，

13、且i<num-1；终点route(j),j＝1，且i<j<num；

14、步骤11，计算规划路线总长度disf(i,j)与原a*算法规划总长度disa(i,j)的大小；如果disf(i,j)≤disa(i,j)，则替换路径中的原序列；否则，从当前节点继续向终点搜索最短路径节点。直到遍历route中所有节点。

15、步骤12，输出最优路径节点序列。

16、进一步地，步骤1中，通过毫米波雷达、激光雷达、摄像头、惯性传感器imu、gps和车传感器，获取车辆自身和周围障碍物的信息。

17、步骤1中，所述车辆自身和周围障碍物的信息包括车辆自身状态和位置信息、障碍物的状态、类型和位置信息，以及道路的基本信息(如车道线、道路边界等)。

18、进一步地，步骤2中的车辆简化模型建立如下：

19、车辆左前转角点坐标：

20、

21、车辆右前转角点坐标：

22、

23、车辆左后转角点坐标：

24、

25、车辆右后转角点坐标：

26、

27、式中，(xr,yr)为汽车后轴中心点坐标；为汽车的中心线与x轴的夹角；s为汽车的宽度；l为汽车的总长；lr为汽车后轴中心到汽车尾部的距离；lf为汽车前轴中心到汽车前部的距离。

28、进一步地，步骤3包括：

29、通过计算任意点对之间的距离权重，实现已知图的路径搜索以求解任意两点间最短路径。状态转移公式如下：

30、map[i][j]＝min{map[i][j],map[i][k]+map[k][j]}               (5)

31、map[i][j]为点i到点j的最短路径值；map[i][k]为点i到点k的最短路径值；map[k][j]为点k到点j的最短路径值；n为所有节点数；1<k<n；

32、选择欧几里德距离作为启发式函数。为h(k,j)设置了一个偏差调整因子α，以提高基于高精度地图的路网诱导。改进的a*算法成本函数用于表示通过k点后从i到j的估计成本，代价函数fij(k)为：

33、fij(k)＝g(i,k)+αh(k,j)(7)

34、其中fij(k)为经过k点，从点i到点j的估计成本；g(i,k)为点i到点k的已付代价值；h(k,j)为k到j的预测代价值；α为偏差调整因子。

35、所述距离权重表示为：

36、

37、其中map[μ][ν]为点μ到点ν的距离权重，ω(μ,ν)为权重指数。

38、根据得到的距离权重，找到栅格地图中起点和终点的最短可行路径，符合以下要求：

39、

40、(i,j)∈p代表从点i到点j的所有可行路线的集合。

41、步骤3中，g(i,k)＝ξmap[i][k]，其中ξ表示步长代价。

42、步骤3中，h(k,j)为k到j的欧式距离。

43、步骤3中，disf(i,j)＝g(i,j)。

44、disa(i,j)＝α×sij(n)+β×dij(n)；其中设置α为走单格直线的代价值，设置β为走单格对角线的代价值，α与β的关系为sij(n)为从点i到点j沿直线移动的步数，dij(n)为从点i到点j沿对角线移动的步数。

45、进一步地，所述步骤5的open集合用于存储所有能够展开的节点集，close集合用于存储障碍点和已展开的节点集，route集合用于存储代价函数值最小节点。

46、有益效果：本发明的目的在于提供一种基于改进a星算法的自动驾驶轨迹规划方法，添加了偏差调整因子来降低a星算法的展开节点，从而提高了算法的计算效率；同时采用距离权重代替代价函数，提高效率的同时也兼并全局最优性。本发明提高了算法对复杂环境的适应性，满足应用场景的实时性要求。