智能汽车泊车轨迹规划应用

高 涛

（1.江苏工程职业技术学院 航空与交通工程学院，江苏 南通 226007；2.江苏省智能网联汽车工程技术研究开发中心，江苏 南通 226007）

城市化发展过程中，交通拥堵、事故率高、环境污染和能源短缺等因素制约着城市的快速发展，而自动驾驶技术被普遍认为是破解这些难题的关键。自动驾驶技术四大核心技术分别是环境感知、精确定位、路径规划、线控执行。路径规划在智能汽车运动控制中占据核心地位，路径规划算法的效率将直接影响智能汽车的寻路效率及实施规划能力。其中，运动轨迹规划是对车辆运行轨迹进行预测规划。

现阶段大量研究主要集中在基于采样的方法、基于搜索的方法和基于生物启发的方法等方向。基于图搜索的A*算法能求解最优路径且实时性好，但传统图搜索算法得到的路径不能满足车辆非完整性约束。针对该问题，DOLGOV等提出一种满足车辆运动学约束的混合A*（Hybrid A*）路径搜索算法；HU等使用混合A*算法作为路径搜索方法、Pythagorean-Hodograph三次曲线作为路径后处理方法，但是路径不能与障碍保持足够的安全距离；文献[6]使用A*搜索结果与路线规划算法（Reeds-Shepp, RS）共同作为启发式数值，并使用Reeds-Shepp曲线进行节点扩展，加快了搜索的速度，但容易导致路径中频繁出现倒车、变换行驶方向等不合理的情况出现。

针对现有方法存在的问题，本文对Hybrid A*算法进行改进，优化启发函数解决规划路径中的频繁倒车与换向，考虑车辆安全，通过改进碰撞检测算法，使规划路径中车辆轮廓与障碍物保持一定安全距离。通过仿真实验，验证了算法的有效性，改进后算法能够实现智能汽车在局部泊车环境下的轨迹规划。

1 Hybrid A＊算法

Hybrid A*算法融合A*算法和RS曲线考虑车辆动力学约束，搜索路径满足车辆非完整性约束。

Hybrid A*算法搜索的维数由（,）变为（,,,），其中（,,）为车辆位姿状态，取±1，分别代表车辆正向行驶和倒车状态。A*算法与Hybrid A*算法节点扩展方式如图1所示，A*算法节点扩展为八邻域方式，都是直线相连，子节点必定是栅格中心，Hybrid A*算法节点扩展与车辆行驶一致，即左转前进、直行、右转前进、左转倒车、倒车、右转倒车，6个方向来生成路径，因此，路径是受车辆转弯半径约束的圆弧和直线。

Hybrid A*算法流程如图2所示，具体步骤如下：

（1）初始化地图和车辆信息：起始点、终点、障碍点、可行区域，车辆参数等；

（2）执行A*算法，计算栅格地图下Costmap：利用A*算法，在栅格地图中计算每个栅格到目标栅格的总代价，得到Costmap矩阵，用于指引Hybrid A*的扩展方向；

（3）执行Hybrid A*算法，搜索融合路径：利用Hybrid A*算法、RS曲线作为Hybrid A*的启发函数搜索泊车路径。泊车路径分两段，一部分是扩展路径，通过Costmap的指引搜出，一部分是RS曲线的路径；

（4）生成路径节点列表：将搜索出的节点信息记录生成路径节点列表信息（,,,）；

（5）路径节点平滑处理：对生成的路径节点进行平滑处理，得到满足车辆行驶需要的平滑路径。

2 启发函数优化

启发函数是影响Hybrid A*算法实时性与有效性的关键所在。为满足车辆运行路径最短且符合车辆动力学约束，在执行Hybrid A*算法搜索中，启发函数以Euclidean距离为基础，考虑实际行车环境增加相应的惩罚系数形成优化后的启发函数。

在车辆实际行车环境中，不同的场景对车辆倒车与转向行驶有不同的限制。例如，在城市快速路或高速公路禁止倒车，在停车场环境允许倒车，另外频繁的转向会增加车辆执行机构负担，增大行车安全风险和影响乘坐舒适性。在执行Hybrid A*算法搜索中，在子节点扩展的代价计算时引入惩罚系数，根据不同场景的决策需求设置惩罚系数大小来限制车辆是否出现倒车，使规划轨迹更符合实际场景。

从节点到扩展的代价计算式为

其中，norm（,）为节点到达扩展的Euclidean距离；为车辆行驶状态参数，=1为正向行驶，=-1为倒车状态；为汽车转向角；为倒车行驶的惩罚系数；为车辆行驶方向改变的惩罚系数；为车辆转向角惩罚系数；为转向角改变惩罚系数。

由式（1）可知，=-1为倒车状态时，倒车惩罚系数会引起代价增加，增大将减少倒车的发生，置为无穷大则禁止倒车；同理，车辆行驶方向改变的惩罚系数会调整车辆改变前进或倒车状态的代价成本，避免车辆频繁改变行驶方向；惩罚系数会因车辆转弯产生转向角调整代价；会调整车辆改变转向角大小的代价成本，避免频繁调整转向角大小的问题。以上四个惩罚系数可根据实车状态及栅格地图状态调试确定。

3 碰撞检测策略改进

基于MATLAB2020b仿真平台实施Hybrid A*算法，在进行路径节点搜索中，为避免车辆与障碍碰撞，节点扩展的过程中需进行碰撞检测，以确定车辆无碰撞安全行驶。碰撞检测的原理示意如图3所示，图中矩形为车辆边界的平面模型，多线段为障碍物边界，车辆与障碍的碰撞检测实际上转化为一个简单的数学问题，即判断矩形与多线段是否存在交点，若存在即判定碰撞发生，若不存在则认为无碰撞。

算法在进行规划的过程中，为了满足路径最短规划轨迹，往往靠近障碍物，汽车实际行驶时需与障碍物保持一定距离以确保车辆安全无刮蹭。算法需要改进碰撞检测策略，使车辆行驶过程中实时计算车辆轮廓与周围障碍的最小距离。如何在不影响算法复杂程度情况下简单实现车辆行驶中与障碍保持安全距离需要另辟蹊径。在车辆进行碰撞检测时，车辆平面模型外轮廓一周增加距离Safe dis，即安全距离。如图4所示，虚线为增加安全距离Safe dis后的车辆平面模型。

4 Hybrid A＊算法仿真应用与分析

通过建模与仿真，对启发函数优化及碰撞检测策略的有效性、可靠性进行验证。通过三个算例对典型的垂直倒车入库、侧方位停车和局部停车场环境进行停车轨迹规划仿真。仿真规划结果中粗实线包围的白色区域为可行驶区域，矩形为车辆模型轮廓轨迹，三角形尖角指向车头方向。生成车辆后轴中心轨迹线经拟合为粗实线显示。车辆模型设定为车辆尺寸4.7 m×1.8 m，轴距2.73 m，车辆参数如表1所示。根据国家相关标准，垂直车库尺寸6 m×2.6 m，平行车库尺寸6 m×2.5 m。

4.1 垂直倒车入库场景轨迹规划算例

图5为汽车完成倒车入库典型场景轨迹规划结果，表2为算例所使用的惩罚系数，仿真时安全距离Safe dis为0.1 m。

4.2 侧方位停车场景轨迹规划算例

如图6所示为汽车完成侧方位停车典型场景轨迹规划结果，表2仍为算例所使用的惩罚系数，仿真时安全距离Safe dis为0.1 m。

4.3 停车场寻径停车轨迹规划算例

图7为停车场典型场景下车辆寻径规划轨迹。表3为算例所使用的惩罚系数，为减少车辆倒车和频繁改变方向，对各惩罚系数均进行设置，规划路径较为合理。仿真结果显示，当安全距离Safe dis为0.15 m时车辆仍可有效安全地找到一条平滑的轨迹实现停车。

5 结论

停车场泊车环境下，智能汽车可以借助于Hybrid A*算法进行泊车轨迹规划。基于Matlab 2020b平台的Hybrid A*仿真算例分表明，在标准车位前或者局部停车场环境中，运用改进的启发函数和碰撞检测策略，智能汽车可实现安全停车。