基于改进A*算法和lattice算法的路径规划方法

吴 正 赵怀林

(上海应用技术大学电气与电子工程学院 上海 201499)

0 引 言

随着无人驾驶领域和范围的不断扩展，路径规划算法的研究越来越受到重视，路径规划主要分为基于环境信息已知的全局路径规划和依靠传感器感知的局部路径规划。全局路径规划可以迅速规划出障碍物已知情况下的路径，但如果在突然有障碍物闯入的情况下容易发生不必要的碰撞，局部路径规划根据传感器反馈的信息进行处理，形成闭环控制，从而可以绕过环境中的动态障碍物，但缺乏全局掌控能力。

基于栅格地图的路径规划算法中，常用的有Dijkstra[1]、最佳优先算法和A*算法等。其中Dijkstra算法以初始点开始迭代搜索初始点周围的所有节点，将周围的节点全部纳入未搜索节点集，一圈圈向外扩散，直到到达目标节点，这样可以保证得到最优路径，但效率极低。最佳优先算法会选择离目标最近的节点，它基于贪心策略[2]向目标点移动即使它不是正确的路径，所以不能保证路径最短，但速度比Dijkstra快得多。A*算法将Dijkstra靠近初始点的节点与最佳优先算法靠近目标点的节点的信息块结合起来。利用启发函数信息来引导搜索方向，提高搜索效率[3]。

A*算法是路径规划中比较常用的全局路径规划方法，它是一种基于栅格地图的启发式搜索算法[4]，具有最优性和完备性的特点[5]，但规划出来的路径不够平滑，且不满足车辆非完整性约束[6]，而且当环境中突然出现动态障碍物时[7]，就需要回溯重新计算，生成新的路径，增加了计算量。但是A*对于不同的环境具有很强的扩展性和适应性，对项目工程来说十分实用。针对A*算法，研究者对其进行了大量的研究，如文献[8]中的双重A*算法同时完成全局和局部路径规划，解决了A*算法在动态环境中规划效果不佳的问题，但是每次遇到障碍物都需要重新进行一次A*搜索，效率不高；文献[9]提出一种增加搜索节点的A*算法，减小了路径长度和转折角度，使路径更加平滑，但不满足车辆非完整性约束；为了解决单个启发式函数不能兼顾效率和最优性的问题，MHA*算法设置多个启发式函数[10]，在不同情况下使用不同的启发式函数，从而找到最优解；为了满足车辆模型需要，文献[11]提出一种惩戒式混合A*算法，能够得到安全的适合车辆行驶的路径，但是不够平滑，且对动态障碍物没有做特定处理。

本文针对A*算法规划出来的轨迹不够平滑、不满足车辆非完整性约束、在动态环境中规划效果不佳等缺陷对A*算法做出一定改进，省略了一些劣质节点，使得A*算法规划出来的轨迹转折少且更加平滑，在运动中需要考虑物体的方向属性和实际运动约束，并配合lattice算法，将A*规划出来的全局路径作为lattice的参考线，进行局部候选路径的生成。然后设计一系列cost代价函数筛选出最优局部路径，解决A*算法对于动态障碍物处理不灵活的问题[12]。

1 全局路径生成单元

1.1 传统A*算法

A*算法是从静态网络中求解最短路径，是一种直接搜索方法，算法通过比较当前路径栅格的8个邻居的启发式函数值F来逐步确定下一个路径栅格。公式为：

f(n)=g(n)+h(n)

(1)

式中：f(n)是从初始点经由节点n到目标点的估价函数;g(n)是在状态空间中从初始节点到节点n的实际代价值;h(n)是从n到目标点最佳路径的预计花费估计代价值。当从初始点向目标点移动时，A*权衡这两者。

1.2 实际运动约束

在传统A*算法中，一般不会考虑运动物体的方向和实际运动约束，只假设物体可以直接转移到邻近栅格中。但对于实际的车辆路径规划而言，必须要考虑物体运动的实际约束，所以它们不一定出现在格点的中心。从初始点位置出发，物体在下一步搜索中只能到达它能够到达的位置，比如，在车辆模型中，受制于汽车转向角的限制，如图1所示。

图1 实际运动约束轨迹图

如图2所示：(a)为传统A*算法节点扩展方式，它将成本与单元格中心相关联，并且只访问与单元格中心相对应的状态；(b)为D*算法节点扩展方式，它将成本与单元格角联系起来，允许单元格间任意线性路径；(c)为改进A*算法节点扩展方式，它将连续状态与每个单元格关联，单元格的得分是其关联连续状态的成本。

图2 节点扩展图

1.3 省略冗余点

1.3.1无必要遍历点

在搜索过程中，会有很多劣质的点，相比于其他节点，代价明显更高，对于这些点，可以直接删除。如图3所示，在a-b-c、a-d-c、a-e-c几条路径中，明显a-b-c具有最优解，那么其他两条轨迹没有太大的估价意义，所以在传统遍历节点的基础上直接删除这些代价值大的点，既提高了效率也节省了计算量。

图3 无必要遍历点

1.3.2夹 点

所谓夹点，就是在两个点连线之间的点，如图4所示，b为a和c的夹点，可以直接删除，最终获得只包含起始点、转折点、目标点的节点，减小计算量。

图4 夹点示意图

1.4 实际代价值优化

考虑到物体实际运动约束，对实际代价值G进行优化。

① 如果前进，则有:

G=G+S

(2)

② 如果后退，则有:

G=G+P1S

(3)

③ 如果有转向，则有:

G=G+P2|δ(c)|

(4)

式中：S表示每次搜索走过的距离;P1、P2表示系数;δ代表车辆的转向角;c表示轮距。

1.5 优化启发式函数

启发式函数可以控制A*的行为。传统的A*算法都使用曼哈顿距离和欧氏距离定义启发式函数，本文使用Reeds-Shepp曲线、Dubins曲线和曼哈顿距离三种cost解算出来的最大值来作为A*的预期花费估计代价值。其中Reeds-Shepp曲线由几段半径固定的圆弧和一段直线段拼接组成，而且圆弧的半径就是车辆模型的最小转向半径，它是车辆模型行驶的最短路径；Dubins曲线和Reeds-Shepp曲线相比，多了一个约束条件：车辆模型只能朝前开，不能后退；曼哈顿距离即为普通A*算法的预估代价值。

1.6 改进后的A*算法流程

A*算法需要创建两个列表：open list和close list，其中:open list用来存放当前位置周围可以被考虑的路径;close list用来存放不被考虑的路径。改进后A*算法具体流程如图5所示。

图5 改进后A*算法流程

2 局部运动轨迹生成单元

局部运动轨迹规划总的来说是在全局路径规划模块下，结合改进A*算法生成的全局路径生成局部路径的模块。局部路径规划算法有好多种，例如人工势场法、动态窗口法等，但是动态窗口法只适合能够自由转向的差速机器人运动，不满足车辆非完整性约束，针对动态窗口法的局限性[12]，使用基于采样的局部轨迹生成算法，将改进A*算法生成的全局路径作为参考线，将各时刻的纵向偏移量和横向偏移量还原成二维平面内的轨迹点，从而形成一系列轨迹，然后通过cost评价函数选取最优轨迹。

2.1 lattice采样生成轨迹

2.1.1 Frenet坐标系

通常在二维平面中采用X-Y坐标系来表示坐标，但是为了更好地规划运动轨迹，本文坐标基于路径，lattice采样生成轨迹基于Frenet坐标系，需要用Frenet坐标系来表示车辆的状态。作一条光滑的参考线如图6所示，将汽车的坐标点投影到参考线上，得到一个参考线上的投影点。从参考线起始点到投影点的路径长度就是汽车在Frenet坐标系下的纵向偏移量，用S表示；投影点到汽车位置的距离则是汽车在Frenet坐标系下的横向偏移量，用L表示。因为参考线是足够光滑的，也可通过汽车的朝向、速度、加速度来计算出Frenet坐标系下，横向和纵向偏移量的一阶导和二阶导。

图6 Frenet坐标投影

2.1.2 lattice轨迹生成

(2) 将t0起始状态和t1末状态做多项式拟合，分别形成纵向多项式轨迹和横向多项式轨迹,如图7所示。

图7 多项式轨迹

纵向轨迹的五次多项式s(t)，表示为:

(5)

横向轨迹的五次多项式l(s)，表示为:

(6)

2.2 lattice评价函数

本文设计了5个cost评价函数：

(1) 横向偏移cost：离中心轨迹越远的轨迹cost越高。计算各候选局部路径和改进A*算法生成的全局路径的距离A作为横向偏移cost值。

(2) 障碍物水平距离cost：计算车辆离障碍物沿着中心线轨迹的水平距离B作为障碍物水平距离cost值。

(3) 障碍物垂直距离cost：计算障碍物的各个轮廓点距离各候选路径的垂直距离，然后进行加权得到C作为障碍物垂直距离cost值。

(4) 纵向加速度cost：加速度是速度对时间的导数，表示加速度的变化率。本文用加速度的最大值D来表示纵向加速度cost值。

(5) 横向加速度cost：为了平稳换道，打方向盘力度越大，横向加速度cost越大。将方向盘转角E作为横向加速度cost值。

最终的cost值如下：

cost=(a×A+b×B+c×C+d×D+e×E)/5

(7)

式中：a为横向偏移cost的权重系数;b为障碍物水平距离cost的权重系数;c为障碍物垂直距离cost的权重系数;d为纵向加速度cost的权重系数;e为横向加速度cost的权重系数。结合这五个cost评价函数，筛选出代价最低的轨迹作为最优轨迹。

lattice总体算法流程如图8所示。

图8 lattice算法流程

局部轨迹生成效果如图9所示，图中黑色为生成的候选轨迹，白色为cost最小的最优轨迹，可以看出，此算法可以有效避开障碍物且轨迹最优。

3 实验与结果分析

本文将改进后的A*算法和lattice算法融合，将改进后的A*算法规划出来的全局路径作为参考线，作为lattice算法的输入，lattice算法将根据参考线在二维平面内还原一系列轨迹点，从而得到轨迹。这样的结合方法全局与局部兼顾，且轨迹平滑，利于车辆行驶。系统整体框架如图10所示。

图10 融合算法系统框架

根据该路径规划方法，本路径规划方法仿真建立在机器人操作系统ROS下，使用Ubuntu16.04+kinetic版本进行实验。

3.1 实际运动约束实验

不考虑物体实际运动约束的A*算法与考虑物体实际运动约束的A*算法效果对比如图11和图12所示。

图11 不考虑物体实际运动约束的A*算法路径规划图

图12 考虑物体实际运动约束的A*算法路径规划图

可以看出，在增加了车辆实际运动约束，即最小转弯半径之后，规划出来的路径明显更利于车辆行驶，不会发生规划出来的路径车辆跟踪不了的问题。本文最小转弯半径为4 m。

3.2 计算速度优化实验

针对A*算法计算量大、计算速度不够的问题，对A*算法的冗余点进行了删除，减少了计算量，增加了路径规划的速度。本文对图13的环境，设定相同起始点和目标点，进行了20次规划然后取平均值，得到了平均每次规划的时间如表1所示。

图13 普通A*算法效果图

表1 路径规划计算速度对比测试

可以看出，虽然普通A*算法的速度是最快的，但是它没有考虑实际运动约束，规划出来的路径对于车辆来说可能无法行驶，改进后的A*算法速度要快于普通混合A*算法，所以本文删除冗余点的方法有效果。

3.3 路径平滑优化实验

下面介绍改进后的A*算法与结合实际运动约束的A*算法效果对比实验情况。图13-图15为同一幅栅格地图，相同的起始点和目标点进行的路径规划效果图，为了测试算法的实用性，该环境设置了大量的隔板作为障碍物，增加了路径规划的难度。图13为普通A*算法效果图，可以看出，虽然路径比较平滑，但是没有考虑到实际运动约束，不利于车辆行驶。图14为结合实际运动约束的A*算法规划的路径，该路径考虑了车辆最小转弯半径为4 m，存在明显的转折痕迹，不利于车辆行驶。图15为改进后的A*规划的路径，考虑了车辆最小转弯半径，且路径较为平滑，利于车辆行驶。

图14 结合实际运动约束的A*算法效果图

图15 改进后的A*算法效果图

3.4 多代价函数轨迹优化实验

本文为lattice算法设定5个cost函数去约束轨迹，最后筛选出最优轨迹，对于每个cost函数设定不同的权重，通过实验测试出最佳权重比例分配。

表2中避障即为躲避障碍物的性能；稳定性表示规划出来的路径是否稳定，是否会一直跳变；舒适度代表规划出来的轨迹与当前行驶轨迹相差是否过大，过大则需要猛打方向盘，影响乘客舒适度。该路径规划方法经过调参，可以规划出兼备避障性能、稳定性、舒适度的轨迹供车辆行驶。

表2 代价值权重比例分配测试

3.5 实时避障效果实验

实验结果如图16-图18所示，图中的坐标轴代表车辆，黑线为全局路径，白线为最优局部轨迹。从效果上来看，该路径规划方法可以在不同的环境下很好地躲避障碍物，解决了A*算法在动态环境下需要重新规划的问题，减小了计算量，提高了A*算法的灵活性。

图16 实时避障实验1

图17 实时避障实验2

图18 实时避障实验3

4 结 语

针对A*算法缺乏动态性、规划出来的轨迹不够平滑、不满足车辆非完整约束等问题，本文提出一种改进A*算法和lattice算法相结合的路径规划方法，消除传统A*算法中的无必要遍历点和夹点，同时考虑物体的方向属性和实际运动约束，优化启发式函数最终生成全局路径，然后把全局路径作为lattice的参考线，采样生成一系列候选路径，并结合障碍物信息和其他因素计算候选路径的代价值，能够选出平滑且无障碍的局

部轨迹。仿真实验表明了本文的路径规划方法效果显著，能快速规划出一条平滑、安全且满足车辆非完整性约束的运动路径。