Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constraint [翻译]

Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constraints for In-lane Driving Scenarios

向心加速度约束下车道内驾驶场景的自动驾驶汽车最优轨迹生成

Yajia Zhang*, Hongyi Sun, Jinyun Zhou, Jiangtao Hu, Jinghao Miao
Abstract— This paper presents a noval method that generates optimal trajectories for autonomous vehicles for in-lane driving scenarios. The method computes a trajectory using a two-phase optimization procedure. In the first phase, the optimization procedure generates a close-form driving guide line with differetiable curvatures. In the second phase, the procedure takes the driving guide line as input, and outputs dynamically feasible, jerk and time optimal trajectories for vehicles driving along the guide line. This method is especially useful for generating trajectories at curvy road where the vehicles need to apply frequent accelerations and decelerations to accommodate centripetal acceleration limits.
摘要—本文提出了一种新方法，该方法可为自动驾驶汽车在车道内驾驶场景生成最佳轨迹。 该方法使用两阶段优化程序计算轨迹。 在第一阶段，优化过程生成具有可微曲率的封闭式驾驶指导线。 在第二阶段，该程序以驾驶指导线为输入，输出车辆沿该指导线行驶的动态可行、加加速度和时间最优轨迹。 这种方法对于在弯曲道路上生成轨迹特别有用，在这种情况下，车辆需要频繁加速和减速以适应向心加速度限制。

一.INTRODUCTION

Trajectory planning is an important component in autonomous driving systems (ADS). It plays a critical role on safety and comfort. Safety is of top priority as any collision might lead to hazardous situations. Assuming predicted trajectories of surrounding obstacles are given from upper stream module of ADS, path-time obstacle graph is a commonly used tool for collision avoidance analysis if future path of the autonomous driving vehicle (ADV) is determined. This method projects the predicted trajectories of surrounding obstacles onto the spatio-time plane and forms path-time obstacles which specify at which time the further path of the ADV would be on collision. The free area forms the collision-free zone for trajectory planning. This method is particularly useful for autonomous vehicles in structured road scenarios. It fully utilizes the domain knowledge, as most vehicles are driving along lanes. The method we propose adopts path-time-obstacle graph in collision avoidance analysis and always plans a trajectory that lie within the collision-free zone
轨迹规划是自动驾驶系统（ADS）的重要组成部分。它对安全性和舒适性起着至关重要的作用。安全是重中之重，因为任何碰撞都可能导致危险情况。假设从 ADS 的上游模块给出周围障碍物的预测轨迹，如果确定了自动驾驶车辆 (ADV) 的未来路径，路径时间障碍图是一种常用的避碰分析工具。该方法将周围障碍物的预测轨迹投影到时空平面上，并形成路径时间障碍物，这些障碍物指定 ADV 的进一步路径将在何时发生碰撞。自由区域形成用于轨迹规划的无碰撞区域。这种方法对于结构化道路场景中的自动驾驶汽车特别有用。它充分利用了领域知识，因为大多数车辆都在车道上行驶。我们提出的方法在避碰分析中采用路径-时间-障碍图，并且总是规划一个位于无碰撞区域内的轨迹。
Comfort is another goal to achieve for ADS. Several factors affect and are used to measure the comfort of one trajectory. Acceleration and acceleration change rate (commonly known as jerk) are most commonly used metrics for vehicle trajectories. Furthermore, depending on the direction, human weight acceleration and jerk significant differently for longitudinal and lateral movement. Acceleration and jerk in lateral direction must be bounded and minimized. For driving along a curvy road, the longitudinal speed must be adjusted frequently according to the curve, i.e., the curvature of the road. A driving guide line is an abstraction of the road center line, which contains the geometrical information of the road. We assume the target of the autonomous vehicle in-lane driving is following the driving guide line. To achieve comfortable riding experience, the vehicle needs to accelerate and decelerate according to the curvature of the driving guide line. In our proposed method, the algorithm can directly consider the geometrical information of the driving guide line.
舒适是 ADS 实现的另一个目标。有几个因素会影响并用于衡量一条轨迹的舒适度。加速度和加速度变化率（通常称为 jerk）是车辆轨迹最常用的指标。此外，根据方向，纵向和横向运动的人体重量加速度和加加速度显着不同。横向方向的加速度和加加速度必须被限制和最小化。在弯道行驶时，纵向速度必须根据弯道经常调整，即道路的曲率。行车指引线是道路中心线的抽象，包含道路的几何信息。我们假设自动驾驶汽车车道内驾驶的目标是遵循驾驶指南。为实现舒适的骑行体验，车辆需要根据行驶引导线的曲率进行加速和减速。在我们提出的方法中，该算法可以直接考虑驾驶引导线的几何信息。
Optimization is a common approach in trajectory generation as it takes the objective or cost function and constraints directly into trajectory generation. For high degrees of freedom (DOFs) configuration space, optimization for trajectory generation is generally slow and prone to local minima, it is generally suitable for lower dimensional vehicle configuration space. In our method, we use a two-phase optimization procedure. Each one intends to solve a subset of trajectory generation problem. In this way, it greatly reduces the overall complexity of optimization. For the first phase, our method generates a smooth driving guide line for ADV to follow; in the second phase, the optimization procedure takes the collision-free zone resulted from path-time obstacle graph analysis and the close-formed driving guide line as input, and generates a collision-free and comfort trajectory that minimizes longitudinal acceleration, and centripetal acceleration and jerk.
优化是轨迹生成中的一种常用方法，因为它将目标或成本函数和约束直接用于轨迹生成。 对于高自由度（DOFs）配置空间，轨迹生成的优化通常较慢且容易出现局部极小值，一般适用于低维车辆配置空间。 在我们的方法中，我们使用两阶段优化程序。 每个人都打算解决轨迹生成问题的一个子集。 这样，大大降低了优化的整体复杂度。 对于第一阶段，我们的方法为 ADV 生成平滑的驾驶指导线； 在第二阶段，优化过程以路径时间障碍图分析得出的无碰撞区域和闭合形式的驾驶引导线为输入，生成一条纵向加速度、向心加速度和jerk最小的无碰撞舒适轨迹。

二.RELATED WORK

Trajectory planning is a critical component in autonomous driving systems. Recently, a number of algorithms [7], [8], [10] have been developed since DARPA Grand Challenge (2004, 2005) and Urban Challenge (2007).
轨迹规划是自动驾驶系统的关键组成部分。 最近，自 DARPA 大挑战（2004、2005）和城市挑战（2007）以来，已经开发了许多算法 [7]、[8]、[10]。
Randomized planners such as Rapidly Exploring Random Tree (RRT)[6] are intended to solve high-DOF robot motion planning with differential constraints. However, it is difficult for randomized planners to utilize the domain knowledge from the structured environment for quickly convergence. Nevertheless, the computed trajectory is generally low quality and thus cannot be used directly without a post-processing step. Recent research on optimal randomized planner, such as [3], can produce high-quality trajectories given enough planning time. But the convergence to optimal trajectory takes rather long time thus it cannot be used in the dynamically changing environment.
诸如快速探索随机树 (RRT)[6] 之类的随机规划器旨在解决具有差分约束的高自由度机器人运动规划。 然而，随机规划者很难利用结构化环境中的领域知识来快速收敛。 然而，计算出的轨迹通常质量较低，因此在没有后处理步骤的情况下不能直接使用。 最近对最优随机规划器的研究，如 [3]，可以在足够的规划时间下产生高质量的轨迹。 但收敛到最优轨迹需要相当长的时间，因此不能在动态变化的环境中使用。
Discrete search method [5] computes a trajectory by concatenating a sequence of pre-computed maneuvers. The contatenation is done by checking whether the ending state of a maneuver is sufficiently close to the starting state of the target maneuver. This method generally works well for simple environment such as highway scenarios. However, the number of required maneuvers needs to grow exponentially in order to solve complex urban driving cases.
离散搜索方法 [5] 通过连接一系列预先计算的机动来计算轨迹。 通过检查机动的结束状态是否足够接近目标机动的开始状态来完成连接。 这种方法一般适用于高速公路场景等简单环境。 然而，为了解决复杂的城市驾驶案例，所需的机动次数需要成倍增长。
The work in [13] runs an quadratic programming procedure in global/map frame. The trajectory is finely discretized in Cartesian space. The positional attributions of the trajectory are directly used as optimization variables, and outputs a trajectory that minimizes the objective function which combines the measurement of safety and comfort. The advantage of using optimization is it provides direct enforcement of optimality modeling. The dense discretization approach provides maximal control of trajectory to tackle complex scenarios.
[13] 中的工作在全局/地图框架中运行二次规划过程。 轨迹在笛卡尔空间中被精细离散。 轨迹的位置属性直接作为优化变量，输出一条结合了安全性和舒适性测量的目标函数最小化的轨迹。 使用优化的优点是它提供了优化建模的直接执行。 密集离散化方法提供了对轨迹的最大控制以应对复杂的场景。
In [12], trajectory planning is performed in Frenet frame. Given a smooth driving guide line, this method decouples the movement of vehicle in map frame into two orthogonal movements, one longitudinal movement that along the driving guide line and one lateral movement that perpendicular to the guide line. For both movements, the trajectories are generated using random samples, i.e., the end conditions and parameter are discretized into certain resolution. The sampled end conditions are directly connected with the initial condition using quintic or quartic polynomials. Then these longitudinal and lateral trajectories are combined and selected according to a predefined cost function.
在[12]中，轨迹规划是在 Frenet 框架中进行的。 给定一条平滑的驾驶指引线，该方法将车辆在地图框架中的运动解耦为两个正交运动，一个沿着驾驶指引线的纵向运动和一个垂直于指引线的横向运动。 对于这两种运动，轨迹都是使用随机样本生成的，即将结束条件和参数离散化为一定的分辨率。 采样的结束条件使用五次或四次多项式与初始条件直接相关。 然后根据预定义的成本函数组合和选择这些纵向和横向轨迹。
The major drawback with the method is it lacks control of the trajectory. For complex driving scenarios, it is difficult for this method to generate feasible trajectories. Nevertheless, some caveats of using polynomial include problems with stopping, unexpected acceleration and deceleration. To tackle complex problems in real world, we need to maximize our ability of controlling the trajectory. Thus, optimization is a promising direction as constraints in the task domain can be directly considered in trajectory generation. Existing optimization-based methods such as [13] [9] runs the optimization in map frame.
该方法的主要缺点是它缺乏对轨迹的控制。 对于复杂的驾驶场景，这种方法很难生成可行的轨迹。 然而，使用多项式的一些注意事项包括停止、意外加速和减速的问题。 为了解决现实世界中的复杂问题，我们需要最大限度地控制轨迹的能力。 因此，优化是一个很有前途的方向，因为可以在轨迹生成中直接考虑任务域中的约束。 现有的基于优化的方法，如 [13] [9] 在地图框中运行优化。
The overall algorithm framework we proposed is similar to the one in [12], however, we use optimization to solve the 1d planning problems, which greatly enhances the flexibility of the trajectory.
我们提出的整体算法框架与[12]中的类似，但是我们使用优化来解决一维规划问题，大大增强了轨迹的灵活性。
The method we present hybrids the Frenet frame trajectory planning framework and optimization-based trajectory generation. Also, the trajectory optimization is performed in twophases, where each phase is a lower dimensional problem. In this proposed method, the difficulty of optimization is greatly reduced.
我们提出的方法混合了 Frenet 框架轨迹规划框架和基于优化的轨迹生成。 此外，轨迹优化分两个阶段执行，其中每个阶段都是一个低维问题。 在这种提出的方法中，极大地降低了优化的难度。

三.PROBLEM DEFINITION

The configuration for a vehicle with differential constraints in Cartesian space can be represented using three variables, (x; y; θ), where x, y specify the coordinate of some reference point for the vehicle and θ specifies the vehicle’s heading angle in the Cartesian space. In our work, we incorporate one more dimension κ, which is the instant curvature resulted from vehicle’s steering, into the configuration space for more accurate configuration modeling, and hence the computed trajectory provides additional information that can be used for better designing the feedback controller. Trajectory planning for non-holonomic vehicles is essentially finding a function τ (t) that maps a time t to a specific configuration (x; y; θ; κ).
在笛卡尔空间中具有微分约束的车辆的配置可以使用三个变量 (x; y; θ) 来表示，其中 x, y 指定车辆的某个参考点的坐标，θ 指定车辆在笛卡尔空间中的航向角 空间。 在我们的工作中，我们将一个维度 κ（即车辆转向产生的瞬时曲率）合并到配置空间中以进行更准确的配置建模，因此计算出的轨迹提供了额外的信息，可用于更好地设计反馈控制器。 非完整车辆的轨迹规划本质上是找到一个函数 τ (t)，它将时间 t 映射到特定配置 (x; y; θ; κ)。

(Fig. 1. Illustration of vehicle trajectory planning in assist of Frenet frame. First, the vehicle dynamic state (x; y; θ; κ; v; a), which represents vehicles position, heading, steering angle, velocity and acceleration, respective, is projected on to a given driving guide line to obtain its decoupled states in Frenet frame. (s; s; _ s¨) represents the vehicle state, i.e., position, velocity and acceleration, along the guide line (i.e., longitudinal state) and (d; d; _ d¨) represents the vehicle state, i.e., position, velocity and acceleration, perpendicular to the guild line (i.e., lateral state). Then, plan longitudinal and lateral motions independently. Finally, longitudinal and lateral motions in Frenet frame are combined and transformed to a trajectory in Cartesian space.)
(图 1 Frenet 框架表示车辆轨迹规划示意图。 首先，将分别代表车辆位置、航向、转向角、速度和加速度的车辆动态状态（x；y；θ；κ；v；a）投影到给定的驾驶指导线上，以获得其解耦状态 在 Frenet 框架中。 ($s$;$\dot{s}$;$\ddot{s}$) 表示车辆状态，即位置、速度和加速度，沿引导线（即纵向状态），($d;\dot{d};\ddot{d}$ ) 表示车辆状态，即位置 ，速度和加速度，垂直于guide line（即横向状态）。 然后，独立规划纵向和横向运动。 最后，将 Frenet 坐标系中的纵向和横向运动结合起来并转换为笛卡尔空间中的轨迹。)
Trajectory Planning in Frenet frame
Frenet坐标系中的轨迹规划
Our method adopts a similar framework as in [12], which utilizes the concept of Frenet frame for trajectory planning (see Fig. 1). Given a smooth driving guide line, a Frenet frame decouples the vehicle motions in Cartesian space into two independent 1D movements, longitudinal movement that moves along the guide line and lateral movement that moves orthogonally to the guide line. Thus, a trajectory planning problem in Cartesian space is transformed to two lower dimensional and independent planning problems in Frenet frame. This framework exploits the task domain that most vehicles are moving along the lane, and it is particularly advantageous as it greatly simplifies problem by reducing the dimensionality of planning
我们的方法采用了与 [12] 中类似的框架，它利用 Frenet 框架的概念进行轨迹规划（见图 1）。 给定一条平滑的驾驶引导线，Frenet 框架将笛卡尔空间中的车辆运动解耦为两个独立的 1D 运动，即沿引导线移动的纵向移动和垂直于引导线移动的横向移动。 因此，笛卡尔空间中的轨迹规划问题转化为 Frenet 框架中的两个低维独立规划问题。 该框架利用了大多数车辆沿车道行驶的任务域，它特别有利，因为它通过降低规划的维数大大简化了问题.
We assume that the autonomous vehicle is roughly driving around the guild line. The main task for in lane driving is to generate a trajectory for the autonomous vehicle driving along the guide line. In this paper, we assume the vehicle will stay close to the guide line and possible lateral deviations to the guide line can be corrected by the controller. Thus, lateral state of the vehicle is always zero. The trajectory planning problem in our work is now reduced to finding a function s(t), where s is the longitudinal coordinate along the guide line. For any given time t, s(t) returns a vehicle longitudinal state (s; s; _ s¨) at time t. s(t) along with a zero function d(t) forms a trajectory in Cartesian space.
我们假设自动驾驶汽车大致围绕guide line行驶。 车道内驾驶的主要任务是生成自动驾驶汽车沿引导线行驶的轨迹。 在本文中，我们假设车辆将靠近引导线，并且控制器可以纠正与引导线的可能横向偏差。 因此，车辆的横向状态始终为零。 我们工作中的轨迹规划问题现在简化为寻找函数 s(t)，其中 s 是沿引导线的纵向坐标。 对于任何给定的时间 t，s(t) 在时间 t 返回车辆纵向状态 (s; s; s¨)。 s(t) 与零函数 d(t) 在笛卡尔空间中形成一条轨迹。

四.PHASE I: SMOOTH DRIVING GUIDE LINE GENERATION

A guide line is the prerequisite for planning in Frenet frame. The smoothness of a guide line is critical to generating high quality trajectories as the vehicle’s velocity, acceleration, heading and steering information are implicitly encoded in the guide line. According to Cartesian-Frenet frame conversions (see [12] for details), to obtain continuous acceleration and steering angles, the derivative of curvature of the guide line must be continuous as well.
一个指导线是在 Frenet 框架中进行规划的前提。 引导线的平滑度对于生成高质量的轨迹至关重要，因为车辆的速度、加速度、航向和转向信息都隐含在引导线中。 根据 Cartesian-Frenet 框架转换（详见[12]），为了获得连续的加速度和转向角，引导线的曲率导数也必须是连续的.
Generally, the guide line is obtained from map in the form of a sequence of coordinates in map frame, i.e, (x0; y0); : : : ; (xn−1; yn−1), without having the necessary geometrical information, such as curve tangent angle . Before the guide line can be used, a smoothing phase that assigns the necessary geometrical information is needed. In our implementation, we use a non-linear optimization procedure for guide line smoothing as optimization provides direct control of optimality and constraint satisfactions.
通常，引导线是从地图中获取的，在地图框中以坐标序列的形式，即(x0;y0)； : : : ; (xn−1; yn−1)，没有必要的几何信息，例如曲线切角 。 在可以使用引导线之前，需要分配必要的几何信息的平滑阶段。 在我们的实现中，我们使用非线性优化过程来平滑引导线，因为优化提供了对最优性和约束满足的直接控制.
To formulate an optimization for guide line smoothing, we consider the following aspects:
为了制定指导线平滑的优化，我们考虑以下方面：

1. Objective Minimal wiggling of geometrical properties. The wiggling of the geometrical properties will lead to unsmooth acceleration and wiggling of vehicle steering. The objective of the optimization is designed to minimize the length of the guide line s, curvature κ and curvature derivative $\dot{\kappa }$ (i.e., dκ/ds).
   目标几何特性的最小摆动。几何特性的摆动会导致加速不顺畅和车辆转向摆动。优化的目标旨在最小化引导线 s、曲率 κ 和曲率导数$\dot{\kappa }$ 的长度（即 dκ/ds）.
2. Line Representation The representation of driving guide line is flexible enough for complex road shapes. We choose a discrete-continuous hybridyzation for the guide line representation: the input points in Cartesian space work as ”knots” and close-form curves connect consecutive input points with continuous geometrical properties at the joint points. For a raw guide line with n input points, n − 1 pieces of curves are used.
   Line Representation 行驶引导线的表示对于复杂的道路形状足够灵活。 我们为引导线表示选择离散-连续杂交：笛卡尔空间中的输入点作为“结”工作，并且闭合曲线将连续输入点与关节点处的连续几何特性连接起来。 对于具有 n 个输入点的原始引导线，使用 n - 1 条曲线.
3. Constraints The resulted guide line do not necessarily need to pass through input points as the Cartesian coordinates might have errors from map generation; however, the deviations to input points must be limited within certain threshold to preserve the original shape. This requirement is formulated as constraints in optimization
   约束生成的引导线不一定需要通过输入点，因为笛卡尔坐标可能会因地图生成而出错； 然而，输入点的偏差必须限制在一定的阈值内，以保持原始形状。 该要求被表述为优化中的约束
   In our implementation, we are inspired by [2] to use polynomial spiral curve as piecewise baseis. Spiral curve, which represents its shape using a function of curve tangent angle θ w.r.t. accumulated curve length s, is the key element in guide line smoothing. The advantage of using spiral curves is the geometric properties, such as curve direction θ, curvature κ (dθ/ds) and curvature change rate$\dot{\kappa }$ (dθ2/ds2) can be easily derived by just taking the derivative of the function θ(s) w.r.t. s directly. Compared to other curve formulations, such as parametric curves in Cartesian space, the objective function can be greatly simplified
   在我们的实现中，我们受到 [2] 的启发，使用多项式螺旋曲线作为分段基。 螺旋曲线，使用曲线切角 θ w.r.t 的函数来表示其形状。 累积曲线长度 s，是引导线平滑的关键要素。 使用螺旋曲线的优点是曲线方向θ、曲率κ（dθ/ds）、曲率变化率$\dot{\kappa }$（dθ2/ds2）等几何性质，只需对函数θ(s)求导即可 w.r.t. s 直接。 与其他曲线公式相比，例如笛卡尔空间中的参数曲线，目标函数可以大大简化
   In the guide line smoothing problem, we use quintic polynomial spiral curves to connect consecutive input points. The following are the variables in the optimization where ${\theta }_i$, ${\dot{\theta }}_i$ ${\ddot{\theta }}_i$ represent curve tangent angle, curvature and curvature change rate for some input point i and ∆si is the length of the spiral curve that connects point i and i + 1:
   在引导线平滑问题中，我们使用五次多项式螺旋曲线连接连续的输入点。 以下是优化中的变量，其中${\theta }_i$, ${\dot{\theta }}_i$ ${\ddot{\theta }}_i$ 表示某个输入点 i 的曲线切线角、曲率和曲率变化率，Δsi 是连接点 i 和 i + 1 的螺旋曲线的长度：
   
   The geometrical property variables