基于改进A*和Lattice 算法的自动驾驶汽车路径规划研究*

任书宇 吴钦木 周还籍

（贵州大学电气工程学院 贵阳 550025）

1 引言

自动驾驶汽车是一种集环境感知、路径规划、决策控制于一身的综合性系统，已经被世界各国所重视，并且将之视为汽车行业发展中新的动力与发展方向。路径规划模块是自动驾驶汽车研究的关键模块，此模块的性能直接影响到汽车的运行路线和运行的平顺性，其直接关系到自动驾驶汽车的安全性和舒适性。

根据目标范围划分，可以将路径规划分为全局路径规划和局部路径规划。根据已有的环境地图所提供的所有精确信息，以此为依据，从给定的起始点到目的地，利用全局路径规划规划算法，寻求一条最优或近似最优的路径进行全局路径规划。最优路径的定义标准包括距离最近、规划时间最短、路径转折曲率最小等。但当有动态障碍物突然出现时会造成不必要的冲突，或者车辆想要进行变道、超车等行为时，这就要求在全局路径规划的基础上，利用传感器对车辆周边环境信息进行实时的采集，并根据其在地图中的具体位置和周边车辆及行人等障碍物的分布，实现由当前节点到任一子目标节点的局部最优安全路径。

A*算法是最常用的全局路径规划算法之一，其在Dijstra算法基础上，引入启发式函数提高节点搜索速度，但其启发式函数仅考虑距离信息，使规划所得路径因存在大量不必要冗余拐点而造成路径不平滑。使用Floyd 算法对A*算法的结果路径进行迭代优化可以保证得到最优路径［1］，但其规划路径不一定满足车辆本身的动力学要求；文献［2］提出了一种二次规划方法使规划所得路径在运动学上可行，且可以在复杂环境中避免碰撞。人工势场法和动态窗口法是常用的局部路径规划算法，文献［3］提出了一种结合人工势场法构造同时包含距离信息和障碍物信息的启发函数改进A*算法，使得拐点数量明显减少，但其规划路径仍不满足车辆动力学约束；由于A*算法不具备动态避障的特性，文献［4］将其与动态窗口法相结合，使A*算法能够实现动态避障，且所得规划路径的平滑性也有所改善，但其算法效率仍需提高。对此，本文提出了一种改进A*算法和Lattice 算法相融合的路径规划算法，可以快速规划得到一条舒适安全的运动轨迹。最后通过在CARLA 模拟器上进行仿真实验，验证了该算法的有效性和可靠性。

2 A*算法及其改进

2.1 基本A*算法

传统的A*算法由Nilsson 提出，其为一种经典的启发式全局路径规划算法，也是寻找最短路径最有效的直接路径规划方法，由于其完备性、最优性和最优效率的优点，它在全局路径规划算法中应用广泛［5］。

公式为

式中：f(n)是经过指定节点n，从给定的初始节点到目标节点，最小的预估代价值函数；g(n)是从给定的初始节点到指定节点n，最小的当前实际代价值函数；h(n)是从指定节点n到给定的目标节点，最小的当前预估代价值函数。对于路径搜索问题，代价就是距离。按f(n)寻找代价最小的路径，确定一条从给定起始点到目的地的无碰撞最优路线。

2.2 改进A*算法

首先，在精度要求较高的复杂地图环境，如果想进行路径规划，需将地图网格划分较小，就会使A*算法运算过程中，消耗内存严重，并且加大计算量；其次，A*算法是利用栅格法进行路径规划，网格型的路径连接有可能造成规划路径过度转折的情形，由于汽车本身具有动力学约束，路径不平滑会使其不能正常行驶［4，6］。针对上述问题，对A*算法的搜索策略和权重系数进行优化，提高了算法效率。

2.2.1 搜索策略优化

A*算法的搜索策略是将当前节点周围8 邻域中，除障碍物以外的全部节点均加入Openlist中，利用评价函数选出代价最小的节点扩展为下一节点。若环境复杂、规划路径长就会导致Openlist 中的节点数量很多，使评价函数的计算量增大，导致路径规划效率不理想。为了提高搜索效率，设计了一种动态调整邻域搜索范围的方法，A*算法的邻域示意图如图1所示。

图1 A*算法邻域示意图

在真实世界坐标系下，设当前节点为C(Xc，Yc)，目标节点为G(XG，YG)，θ为向量CG与X轴单位向量的夹角，则使用动态邻域法的搜索范围如表1所示。

表1 动态邻域法搜索范围

从表1 中可以看出，使用动态邻域法进行搜索时，不再将当前节点周围8 邻域中，除障碍物外的所有栅格均放入Openlist 中，而是根据值实时调整需搜索的栅格点。动态邻域法在进行路径搜索时，更倾向于选择从初始节点到目标节点的连线方向的路径，从而减小了算法的搜算范围，在运行过程中可以显著降低内存占用，提高了算法的效率。

2.2.2 权重系数优化

作为启发式搜索算法的一种，其最核心的部分在于启发式函数h(n)的设计上，它决定了算法执行的准确率和效率，h(n) 传统的取值方法包括曼哈顿距离、对角线距离、欧拉距离等，经实验表明，在搜索路径长度和搜索时间长短方面，采用曼哈顿距离作为h(n) 函数的值效果都更好，因此本文采用曼哈顿距离［4］。其函数表达式如式（2）所示：

式中：(xn，yn)表示当前节点在栅格地图中的坐标值，(xgoal，ygoal)表示目标节点在栅格地图中的坐标值。

但当前节点存在多条启发式函数值相近的搜索结果时，很难搜索出最优扩展结点，因此对启发式函数赋予一个动态权重系数w(n)，公式为

式中：d(n)表示当前节点n，与指定的目标节点之间的距离，d(s) 表示指定的起始节点，与指定的目标节点之间的距离。使用动态加权提高A*算法快速性的原则为在规划末尾阶段，w(n) 减小，此时A*算法更倾向于搜索最优路径，其运算时间与改进之前几乎保持一致；在规划开始阶段，w(n) 较大，此时A*算法会快速向目标节点区域扩展，其运算时间较改进之前大大减少。

3 Lattice 算法及其与改进A*算法融合

无人驾驶汽车所处环境是一个动态环境，传统的全局路径规划算法只能规划出一条静态路径，并且规划出的路径不一定满足无人驾驶车辆的非完整性约束，同时目前常见的传统路径规划算法可能存在局部最优、模型计算量大甚至模型无解的情况，因此提出了基于Lattice算法的路径与速度同时规划的路径规划方法。采用改进的A*算法规划得到全局最优路径，并将其作为参考线，应用于Frenet坐标系下的Lattice算法，进行局部路径规划［7］。

3.1 Fenet坐标系

无人驾驶汽车的局部路径规划需要对周围环境中的车辆、行人等动态障碍物的未来轨迹进行预测，在规划过程中除了考虑车辆的位置信息，还需要考虑时间维度t，此时二维路径点(x，y)就变为了三维轨迹点(x，y，t)，因此在自动驾驶技术领域局部路径规划也可以称为轨迹规划，道路轨迹规划直接反映了自动驾驶汽车在城市道路上行驶时的智能水平［8］。如果使用笛卡尔坐标系进行轨迹规划，对当前车辆位置，与当前所处车道的关系，很难进行精确描述，并且与二维规划相比，三维规划更复杂。而在Frenet坐标系下，车辆与当前所处车道之间的横向及纵向距离关系，可以很轻松的得到，由此可以忽略非直线车道曲率的影响。同时还可以将车辆运动解耦为纵向运动和横向运动进行分别计算［9］，Frenet坐标系简化了，特别是在道路环境复杂的情况下，道路曲线拟合问题的求解［10］，因此引入Frenet坐标系。

无人驾驶车辆在Frenet 坐标系下的位置表示如图2所示。

图2 Frenet坐标系与笛卡尔坐标系转换关系图

在Frenet 坐标系中，使用改进后A*算法所得全局规划路径作为参考线，在参考线上找到距离车辆位置最近的点定为投影点，以车辆自身为原点，使用投影点在参考线上的切线方向和法线方向建立一个坐标轴互相垂直的坐标系，车辆位置在此下表示为(s，l)。其中s表示从起点到投影点的曲线距离，l表示车辆位置到参考点的法向距离。

3.2 Lattice算法

为了解决车辆行驶过程中纵向运动对横向运动产生的影响，设计了一种Frenet 坐标系下的lattice 算法［11］。Frenet 坐标系将车辆运动解耦为一种，不受约束的、互相独立的纵向移动和横向移动规划系统，利用Lattice算法，根据初始配置信息，通过更改采样时间间隔，随机采样生成一系列候选轨迹多项式，并利用可行性、安全性、舒适性等因素进行综合考量，对候选轨迹簇，使用评价函数对路径成本进行评估，选择代价值最小的候选轨迹作为局部最优轨迹［12］。由于Lattice 算法执行过程中有采样阶段，因此其可以使用离散化搜索空间，用于高效计算非结构化复杂环境中，最优路径规划问题的局部最优解［13］。使用Lattice 算法可以在规划时考虑运动的不确定性，获得安全和最佳的路径［14］。

3.2.1 Lattice轨迹生成

Lattice 轨迹是由横向轨迹簇和纵向轨迹簇进行二维运动合成得到的。横向规划主要是用于解决动态避障和换道等任务，与车辆的方向盘转角直接相关；纵向规划根据车辆在实际行驶场景中的要求分为定速巡航、避障、停车等，与车辆的油门和刹车直接相关。

首先获得车辆在Frenet坐标系下的初始位置、初始速度和初始加速度，记为，然后在Frenet 坐标系下，任意确定一采样时间间隔，采样得到t1时刻的车辆位置、车辆速度和车辆加速度，记为，通过五次多项式拟合，将将t0时刻和t1时刻的状态信息量，作为已知确定变量，带入五次多项式求其未知数，就可以获得横向多项式轨迹和纵向多项式轨迹，见图3。

图3 多项式轨迹

1）纵向轨迹五次多项式s*=s(t*)如式（4）所示：

2）横向轨迹五次多项式l*=l(s*)如式（5）所示：

更改采样时间间隔，得到一系列待选的横向和纵向路径点，采用改进A*算法生成的全局路径，作为Lattice局部路径规划算法的参考线，计算出各个采样时刻点，车辆相对于参考点的纵向偏移量和横向偏移量。再将纵向规划路径和横向规划路径，利用坐标变换进行二维运动合成，最后还原得到二维平面内的轨迹簇。

3.2.2 Lattice评价函数

为了满足无人驾驶车辆必须到达目的地、符合交通规则、避免障碍物碰撞以及行驶平稳舒适等要求，本文设计了5 个cost评价函数，分别为碰撞cost、到达cost、侧向偏移cost、横向加速度cost、纵向加速度cost。

1）碰撞cost：汽车在行驶时，计算其周围环境中的车辆、行人等障碍物中心点，与采样生成的轨迹簇上的点之间的最短距离，进行加权平均得到A，将其作为障碍物碰撞cost值。

2）到达cost：计算车辆距离最终目标点之间的距离B作为任务到达cost值。

3）侧向偏移cost：Lattice 算法更改采样间隔所得的一系列路径点，将其连接为候选轨迹簇，计算其上的点与改进A*算法生成的全局路径上的点之间的距离C，将其作为侧向偏移cost值。

4）横向加速度cost：计算方向盘的转角D 作为横向加速度cost值。

5）纵向加速度cost：计算沿轨迹线行驶方向的最大加速度E作为纵向加速度cost值。

将上述5 个cost评价函数进行加权得到最终的cost值如式（6）所示：

式中，a 为碰撞cost的权重系数；b 为到达cost的权重系数；c为侧向偏移cost的权重系数；d为横向加速度cost的权重系数；e为纵向加速度cost的权重系数。根据最终的cost值，从轨迹簇中筛选出具有最小代价值的轨迹，将其作为局部最优轨迹。

3.3 算法融合

采用改进A*算法生成的全局路径作为参考线，应用于Frenet 坐标系下的Lattice 算法实现，将纵向规划和横向规划进行解耦，更改采样时间间隔，生成一系列纵向和横向的候选路径点，然后通过二维运动合成将其转变为二维空间的一系列轨迹点，再结合动态障碍物信息及周围环境道路信息，根据评价函数选出候选轨迹簇中代价值最小的候选轨迹作为平滑无碰撞的局部最优轨迹。

4 仿真实验及结果分析

本文将改进的A*算法和基于Frenet 坐标系的Lattice算法融合，将经过搜索策略和权重系数优化改进后的A*算法，所得到的全局规划静态路径，作为局部规划Lattice 算法的参考线。在Frenet 坐标系下将纵向和横向规划进行解耦，实现路径与速度同时规划。根据参考线和坐标变换，得到二维平面内的轨迹簇，通过评价函数选出代价值最小的最优局部轨迹。这样的算法融合方法，将静态全局路径规划和动态局部路径规划综合考虑，可得到更好的汽车行驶轨迹，基于此汽车行驶更安全舒适。无人驾驶汽车基于融合算法的轨迹规划逻辑图如图4所示。

图4 基于融合算法的轨迹规划逻辑图

为验证本算法的有效性，在Matlab2018a 版本中进行仿真实验。在栅格地图场景（30m×30m，网格间距1m）下进行实验，阴影栅格为障碍物区域。实验中，起始点坐标为（1，1），目标点坐标为（27，27），A*算法、改进A*算法及融合算法的规划路径图如图5、图6所示。

图5 A*算法和改进A*算法路径规划图

图6 融合算法路径规划图

三种不同算法所得规划路径的曲率如图7 所示。

图7 规划路径曲率图

由图7可以看出，改进A*算法的路径曲率相较于基本的A*算法虽然在数值上并没有减小太多，但是其曲率变化速度明显改善，有利于无人驾驶汽车方向盘的稳定；融合算法的路径曲率数值明显减小，使得路径平滑性大大提高，有利于无人驾驶汽车行驶。

CARLA 作为一个开源的自动驾驶模拟器，可以提供多种开放的城市环境，包括城市空间的分布、建筑物以及车道和车辆行人等数字化模型；同时它还支持多种不同的传感器组合和不同的天气条件，对于研究自动驾驶具有重要意义［15］。将融合算法在CARLA中仿真如图8、图9所示。

图8 CARLA模拟环境下的路径规划图

图9 融合算法规划所得候选路径及最优路径

图9 中蓝色曲线为融合算法规划路径过程中生成的全部候选路径，红色曲线为根据Lattice代价函数选出的最优局部路径。

从图8 可以看出，本文提出的融合算法可以规划出，同时具有稳定性、舒适性以及避障性能的车辆行驶轨迹，使汽车在真实环境中正常行驶。

5 结语

针对A*算法规划出的路径平滑性较差、不满足无人驾驶车辆的动力学约束、不具有避免碰撞动态障碍物等问题，本文提出了一种改进A*算法和Lattice 算法相融合的路径规划算法，优化A*算法的搜索策略和权重系数，提高全局路径规划的效率，再将生成的全局静态最优路径，作为参考线，应用于Lattice 算法，并通过Frenet 坐标系进行解耦，同时进行速度规划和路径规划，采样生成一系列候选轨迹点，结合动态障碍物信息以及其他道路信息，根据候选轨迹代价值大小，选出平滑无碰撞的最优轨迹。