自动城市驾驶的实时轨迹规划：框架，算法和验证

参考

Real-time trajectory planning for autonomous urban driving: framework, algorithms and verifications

采用了分层运动规划框架。首先，使用来自高级行为规划器的命令从数字地图中提取粗略参考路径。共轭梯度非线性优化算法和三次Bspline曲线用于顺序地平滑和内插参考路径。为了遵循改进的参考路径以及处理静态和移动物体，轨迹规划任务在曲线坐标框架内解决了纵向规划问题。生成一组丰富的运动学可行路径候选者，以便在审议和反应中处理动态流量。同时，执行速度配置生成以提高驾驶安全性和舒适性。在此之后，通过目标函数仔细评估生成的轨迹，该目标函数通过对交通情况进行调整来组合行为决策。选择最佳的无碰撞，平滑和动态可行的轨迹，并将其转换为由低级横向和纵向控制器执行的命令。我们的测试自动驾驶汽车在现实的城市内部道路上进行了现场试验。实验结果证明了所提出的轨迹规划框架和算法的能力和有效性，以安全地处理各种典型的驾驶场景，例如静态和移动物体避让，车道保持和车辆跟随，同时遵守交通规则。
相关研究
图搜素法和随机抽样算法 。
结构化道路 路径生成
研究重点
采用分层框架。高级行为规划者负责推理复杂的动态交通情况并制定有意的离散机动决策，例如车道跟随，车道变换，车辆跟踪，超车缓慢行驶等等。使用行为决策，轨迹生成算法承担生成动态可行和无碰撞轨迹的责任，这些轨迹可由低级跟踪控制器轻松跟踪。本文重点研究实时轨迹生成算法。
通过基于LiDAR的定位方法，使用车道级精确定位信息从数字地图中提取粗略参考路径，这类似于[38]和[39]中提出的方法。为了提高驾驶舒适性并减少控制努力，采用共轭梯度非线性优化算法和三次B样条算法来顺序地平滑和插入参考路径。
基于曲线坐标框架，将轨迹规划任务解耦为空间路径规划和速度规划子任务。该轨迹不是使用优化方案来产生唯一的最佳轨迹，而是能够生成一组丰富的次优候选，这可以有效地克服感知和定位系统中的噪声。此外，它确保车辆能够在即将发生的情况下安全停车。为了保证驾驶安全性并提高驾驶舒适性，我们精心设计了具有一系列具有独特物理意义的成本条款的目标函数，以选择最佳的执行轨迹。
系统框架

图片.png

图中概述了AGV的软件系统架构。该系统由各种模块组成，如传感器，数字地图，任务文件，感知和定位系统，任务规划器，路线规划器， 行为规划器，轨迹规划器和跟踪控制器，低级执行器控制和人机界面（HMI）。 每个模块基于Inter Process Communication Toolkit通过特定的发布/订阅通信协议与其他模块进行通信。

诸如激光雷达，雷达，摄像机等传感器可实时提供周围环境的传感信息。此外，其他传感器，如GPS与惯性测量单元（IMU）和车轮编码器相结合，用于获取车辆的粗略定位信息。传感信息主要用于两个目的：一个用于感知系统，例如检测车道标记，交通灯，以及静态和动态物体（例如行人，骑自行车者和其他车辆）;另一个是实现准确和稳健的本地化。为了实现泳道级精确可靠的定位，许多研究人员利用GPS，IMU的地图和定位技术，结合激光扫描仪的在线3D点数据[38]，[40]，[41]或相机视觉数据[42]， [43]，[44]。
在本文中，来自HDL64E Velodyne LiDAR的在线感应3D点数据与通过手动驾驶记录的高分辨率3D地图数据相结合，已被应用于实时估计车辆的位置和姿势。基于这种定位方法，可以利用先前数字地图的丰富信息。在实践中，我们使用手动构造的详细数字地图，其提供了丰富的传输信息，信息（例如位置，数量和拓扑关系）和交通规则（例如速度限制）。本地化和地图构建方法的细节不是本文的主要焦点。

图片.png

感知和定位系统的结果在图2中示出，母图使用LiDAR的3D点数据来创建。具有品红色的细胞是在线检测到的障碍物，包括动态和静态障碍物。矩形块表示移动物体，箭头指示其移动方向。 白线表示基于数字地图的车道。在实践中，我们发现定位方法在保证我们的自动驾驶车辆在白天和夜晚，阳光或雨水中在动态城市环境中安全驾驶方面发挥着关键作用。
使用数字地图和准确的本地化信息，全球路线规划人员可以根据任务规划结果计算出最快的参考线。 行为规划者的目标是依靠在线感知信息和计划参考通道做出明智的决策。 本文重点研究轨迹规划模块，该模块在保证行车安全性和舒适性方面发挥着关键作用。

图片.png

图3中所示的轨迹规划算法的流程图，首先导出参考路径。我们采用非线性优化方法来平滑中心线，然后使用三次B样条曲线对其进行插值，而不是直接使用参考通道的中心线。以这种方式，可以获得平滑的参考路径（或基线）。如[13]所述，为了模仿城市环境中类似人的驾驶行为，车辆轨迹可以自然地分离为横向运动和纵向运动。鉴于此，基于曲线坐标而不是笛卡尔坐标将轨迹规划任务分解为空间路径规划和速度规划子任务。基于参考路径生成一组丰富的轨迹候选。之后，执行碰撞测试以修剪与障碍物碰撞的候选轨迹。然后，应用情境感知的轨迹评估方法来驱动最佳轨迹，由轨迹跟踪控制器将其转换为低级执行器命令。
参考线平滑/插值
在结构化环境中，可以使用所需车道的右边界和左边界直接计算参考路径。在实践中，我们发现计算中心线的曲率通常既不连续也不平滑。低级跟踪控制器无法轻松跟踪它。跟踪不连续的中心线很容易导致控制过冲和振荡。此外，当车辆正在通过弯曲的车道或谈判转弯时，遵循中心线需要更多的控制力并导致更大的横向加速度。为了克服这些问题并提高驾驶舒适性，采用共轭梯度优化方法使连续空间中的参考路径平滑。在[20]和[30]中提到了类似的方法。

图片.png

第一项最小化相邻矢量之间的矢量变化，第二项最小化沿路径的累积曲率。 执行它们都是为了改善参考路径的平滑度。 第三个成本项保证优化的参考路径满足道路边界条件施加的约束。 可以从数字地图获得道路边界条件。 成本项和迭代的权重对路径的平滑性有影响。 与其他非线性优化方法一样，共轭梯度算法仅返回局部最优解。 实际上，ω1设定为10，ω2设定为1，这是凭经验得到的。 此外，最大迭代次数限制为400，以满足实时要求。 将来，将采用机器学习技术来自适应地调整这些参数。

在平滑处理之前和之后，顶点的数量被设置为相同。 实际上，发现顶点之间的距离有时太大。 为了获得密集的航点，我们使用三次B样条进行插值。 优化参考路径的离散点可以用作控制点。 使用离散控制点Pi（i = 0,1，...，n），分段立方B样条曲线

图片.png

基于曲线坐标系的样条路径产生
为了在避开障碍物的同时遵循基准框架，[11]提出了一种模型预测运动规划器，它首先对从基准框架横向移动的一组终端状态进行采样。然后将路径生成问题表述为两点边值问题[45]。为了在遵守车辆运动约束的同时减小解空间，使用曲率多项式来参数化转向控制输入空间。虽然这种方法考虑了终端状态的约束，但是在接近过程中它是路径测量。因此，它无法生成长期轨迹以与弯曲的道路对齐，例如弯曲的直角转弯。正如我们之前提到的，生成与道路几何结构环境对齐的轨迹至关重要。解决问题，我们参考[25]，[26]，[46]中提到的路径生成策略。如图5所示，采用基于采样的运动规划策略来生成一组路径候选，其具有从基帧移位的不同横向偏移。以这种方式，车辆能够避开静态障碍物并且能够与基架的几何形状对齐。细节描述如下。
基于曲线坐标框架，采样终端状态可以使用两个参数来表示，一个是沿着基础框架的纵向距离sf，另一个是距基础框架的横向偏差lf。纵向距离sf确定车辆被调节到与基架对准的速度。在实践中，它可以根据车辆速度自适应地调整，而不会违反最大横向加速度。横向偏移l0是车辆当前位置与沿参考路径的对应点（最近点）之间的垂直距离。

根据[47]中提到的差分平稳控制理论，可以应用多项式样条来平滑从当前横向偏移l0到采样终端偏移lf的过渡。为了实现从初始侧向偏移设置偏移终端偏移的平滑过渡，三次多项式的湿润优势，这与[46]中提到的算法类似。

图片.png

四个参数 4个状态

图片.png

图片.png

速度规划
速度规划对驾驶安全性和舒适性有很大的影响，特别是对于在其他动态交通参与者的现实城市场景下驾驶的车辆。因此，需要在沿着生成的路径的每个点上仔细分配速度分布生成。此外，还需要明确考虑约束，例如纵向和横向加速度限制，交通限制的速度限制。考虑到这些约束可以大大减少速度规划的解决方案空间，并且允许速度规划器集中在最可能存在最优解的空间。因此，在空间路径生成之后执行速度生成。
1.道路最大速度
2.最大横向加速度限值

图片.png

3.纵向加/减速度限值

图片.png

为了舒适的目的，我们进一步平滑了梯形速度，确保了加速度的连续性。 受[30]中参数化速度曲线生成方法的启发，速度曲线使用三次多项式进行参数化。 与[30]和[26]中提出的方法相比，我们采用梯形速度分布，并分别平滑上升和下降过程。

图片.png

图片.png

终端速度vf使用行为决策来确定，这决定了交通情况。例如，当行为决策是车辆跟随时，终端速度将是我们当前车道中最近的前车的速度（如果其速度不超过最大允许速度）。换句话说，速度计划器使用类似于ACC（自适应巡航控制）的速度控制策略。当行为决定在停止线处停止时，终端速度将为零。

如图7所示，我们分别解决了加速度连续的S形斜坡上升速度曲线（红色实线）和斜坡下降速度曲线（蓝色虚线）。将空间路径与相应的速度分布相结合，产生轨迹。请注意，三次多项式速度曲线的加速度将超过某些点处的线性速度曲线的加速度。在实践中，梯形曲线的斜坡上升和斜坡下降曲线的加速度和减速度值被设置为非常保守的值以减轻负面影响。如[49]所述，可以通过学习人类示范来调整这些参数，以实现类似人类的驾驶。

轨迹评估及最优

在生成轨迹候选之后，执行碰撞测试以缩短或切断与静态和移动障碍物碰撞的轨迹。为此，使用在线感测数据将局部轨迹规划图表示为占用网格图。如图8所示，车辆的矩形形状由一组圆形近似[33]。使用这种方法，可以显着降低碰撞测试的计算复杂性。为了满足安全要求，障碍物与圆心之间的距离必须大于圆半径。考虑到定位，感知系统中的噪声，可以增强半径以增加车辆和障碍物之间的安全裕度。为了评估所产生轨迹的安全性，需要考虑静态障碍物和移动物体。为避免与移动物体发生碰撞，有必要预测其他移动物体的移动。由于不完美的感知和预测信息，不可能准确地预测大预测范围内的其他对象的移动。在实践中，可以利用车辆速度和周围驾驶环境来调整预测范围。此外，还可以考虑与其他交通参与者的互动，以及由于道路几何形状引起的驾驶操纵的限制，如[34]和[35]中所述。

图片.png

图片.png

成本值标准化为[0,1]。 成本项Jp反映了较长轨迹的偏好，这可以防止由于过于短视的轨迹导致的过度反应行为。

图片.png

为了增强横向运动的舒适度，平滑度标准被成本项Js考虑，成本项Js是沿着轨迹候选者的曲率的积分。

图片.png

为了确保车辆可以遵循基础框架的引导，成本项Jd惩罚偏离基础框架的轨迹。

图片.png

成本项Jc惩罚连续重放之间的路径不一致。 相邻重新计划之间的不一致很容易导致控制系统的振荡，过冲甚至不稳定。 为了确保重放之间的一致性，我们最小化连续重放之间所选路径的采样终端横向偏移。

图片.png

考虑到上述四个成本条件来评估生成的路径将不可避免地产生在安全性，平滑性，准确性和一致性之间做出折衷的问题。 在实践中，基于行为决策结果和数字地图的先验信息，经验地调整四个成本项的权重因子。 对于我们未来的研究，将采用机器学习技术来自适应地调整这些成本条款。
如图9所示，采用占用网格图来表示基于来自感知系统的感测信息的环境。 红色网格是静态障碍物。 紫色曲线表示参考路径（基础框架）。生成动态可行的轨迹，并与参考路径对齐生成。 如图所示的右下方颜色条，轨迹的颜色表示其成本值。 绿色曲线是通过上述目标函数评估后的最佳轨迹。 它将由轨迹跟踪控制器转换为执行器命令。

图片.png

验证测试
图11描绘了从我们的实验记录数据的一部分获得的规划算法的执行时间。生成的轨迹候选的最大数量被设置为500.总计划时间主要包括参考路径优化时间和轨迹规划时间。从图11中可以看出，每个计划周期内的轨迹规划都在20ms左右，而参考路径优化过程所花费的时间约为50ms。测试道路的交通限速为25km / h。我们的测试环境的道路宽度很窄（单车道宽度小于3米）。那里有迎面而来的车辆。骑自行车的人也与汽车共享道路。此外，还有沿着车道行走的行人。有时那里有婴儿车。出于安全考虑，最大允许速度设定为22km / h。

图片.png

图片.png

图片.png

图片.png