基于采样的无人车路径规划算法仿真与验证

付 行，聂文福，王 凯，任文峰

（陕西重型汽车有限公司，陕西 西安 710200）

无人驾驶局部路径规划主要研究在结构化道路下依据参考线的规划和非结构化道路空间搜索路径规划。以参考线为基准的方法，通过横纵向平移产生多条候选轨迹，进而评估轨迹优劣性，选择最优轨迹［1-4］。A*及其变种算法通过启发式搜索可用于区域性路径规划［5］。本文对基于采样的算法研究，分别完成了路径规划、碰撞检测、路径评估和最优选择。同时，为了测试算法，在Prescan中搭建仿真场景对多种工况进行了仿真。还进行了实车测试，结果表明，该算法能够有效避障，并生成平滑、稳定的可行驶路径。

1 路径规划

对末状态进行采样，求解车辆当前位置到采样状态的解是局部路径规划的核心问题［3］，将路径生成表述为边界值问题，通过逐步优化迭代，得出最优的5次多项式系数，该方法最大的问题是需要建立庞大的查询系数表［4］。将采样状态进行横纵向分解，用高次多项式表达S纵向、L横向之间的关系，求解多项式参数，最后转换到笛卡尔坐标系。该方法在坐标变换过程中考虑了参考线的曲率，所以受参考线曲率影响较大。

本文应用frenet坐标系，将问题分解为S、L应用5次多项式进行求解，表达式为：

要求解这个方程组需要一些初始配置和目标配置，以横向路径规划为例，初始配置为，即t0时刻车辆的横向偏移、横向速度和横向加速度。

横向轨迹评估函数考虑了横向偏移、碰撞、横纵向加速度变化率等，计算每一条候选轨迹代价值。经过将横纵向轨迹合并，转换到笛卡尔坐标下进行碰撞检测，删除发生碰撞的轨迹，剩余轨迹中代价最小的即为最优路径。

2 算法仿真

搭建Prescan仿真场景测试环境，编译后生成对应的Simulink框图，如图1所示。Dynamics_Simple是车辆运动控制模块，SELF_Demux 是车辆自身信息的输出模块，Lidar_1_Demux是激光雷达感知模块。按照消息类型分别添加Subcribe、Publish模块用于接收和发送信息。图1中的MATLAB Function1是S函数，主要完成算法的主体部分，计算出转向盘转角信息。Subsystem是依据当前速度和接收到的期望速度V，计算油门开度和制动力度。

仿真验证如图2a所示，在没有障碍物的情况下，左侧的黑点为主车，右侧的黑点为障碍物，3条实线为车道隔离线，蓝色的小点是规划的轨迹，在轨迹规划没有碰到障碍物的时候，红色的点组成的连线为最优路径轨迹。从验证仿真图可以看出，最优的路径轨迹为红色的点连接成的轨迹，也就是车道的中心线是最优的轨迹路径。图2b是Prescan仿真验证的3D显示结果，从图2b中可以看出主车为蓝色车辆，前面的白车为障碍物，但是在本实验中蓝色车辆距离白色车辆距离较远，还没有形成障碍物，试验证明，主车一直沿着车道中心线运动，达到了预期的效果。

图1 Simulink框图

图2 无障碍物仿真验证

存在障碍物的情况下，如图3a所示，左侧的黑点为主车，右侧的黑点为障碍物，红色的点组成的连线为避过障碍物之后的最优路径轨迹。图3b是Prescan仿真3D显示效果图，后面的蓝色车为主车，前面的白色车为障碍物，在有障碍物时，主车明显的车头向左转动，在有意避开障碍物。试验证明，主车沿着红色的轨迹运行，成功避开了障碍物（白色车辆），然后继续沿着车道中心线运行，试验验证达到了预期效果。

图3 有障碍物仿真验证

3 实车验证

为了进一步研究该算法，借助我公司重型越野无人平台对其进行了实车验证。该平台搭载了多线激光雷达、GPS惯导设备。验证环境选择我公司车辆测试场，分别设置了起点、终点，用于对停止点规划，同时在道路内设置了假人障碍物，用于验证规划路径能剔除碰撞路径，选择最优路径行驶。测试开始之前，首先在道路中心采集了GPS轨迹，用于指引车辆前行。其次，对于道路的采样，横向轨迹的采样需要涵盖多种横向运动状态。最后，借助QT_GUI显示车辆行驶环境和规划路径，通过算法对最优路径的选择，调整设置参数。在本次验证中设计了7个末状态横向偏移量：-1.5m，-1.0m，-0.5m，0.0m，0.5m，1m和1.5m以及4个到达这些横向偏移量的纵向位移，分别为：10m，20m，40m，60m。

验证过程，激光雷达感知到障碍物信息，传递给规划节点，规划避障的最优路径和合理的行驶速度，然后将路点信息和速度信息发送给控制模块，控制车辆行进。

在避障过程中，GUI显示最优路径的选择先由远距离中心线转换为近距离中心线，进行了减速动作，再转换为近距离右偏1.5m，如图4所示，柱体是静态障碍物，选择向右侧偏1.5m，速度有所减缓的红色线为最优路径。接着车辆跟随轨迹向右转，避开障碍物，随即切换为近距离右偏1m，使转向盘稍有回正，车身避开障碍物后，最优路径选择为远距离中心线，加速行驶。图5为实车进行的避障情形。

经过实车验证，该算法能够在巡航、避障、停车等工况下给出合理可行的行驶路径，满足无人车控制约束条件。

图4 最优轨迹选择

4 结论

图5 实车避障

轨迹规划是自动驾驶决策模块的重要组成部分，高效生成平滑、合理的规划轨迹是实现自动驾驶的关键技术之一。本文针对自动驾驶车辆的轨迹规划建模问题展开研究，将Frenet 坐标系用于解决自动驾驶车辆轨迹规划问题，简化了建模过程，利用5次多项式分别建立了自动驾驶车辆横、纵向轨迹规划模型，然后通过损失函数的计算确定每条轨迹的代价值，碰撞检测去除与障碍物碰撞的轨迹，选择一条代价最小的轨迹，最终得到自动驾驶车辆在结构化道路上的最优运动轨迹。