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.
摘要—本文提出了一种新方法,该方法可为自动驾驶汽车在车道内驾驶场景生成最佳轨迹。 该方法使用两阶段优化程序计算轨迹。 在第一阶段,优化过程生成具有可微曲率的封闭式驾驶指导线。 在第二阶段,该程序以驾驶指导线为输入,输出车辆沿该指导线行驶的动态可行、加加速度和时间最优轨迹。 这种方法对于在弯曲道路上生成轨迹特别有用,在这种情况下,车辆需要频繁加速和减速以适应向心加速度限制。
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最小的无碰撞舒适轨迹。
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 框架轨迹规划框架和基于优化的轨迹生成。 此外,轨迹优化分两个阶段执行,其中每个阶段都是一个低维问题。 在这种提出的方法中,极大地降低了优化的难度。
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 s s; s ˙ \dot{s} s˙; s ¨ \ddot{s} s¨) 表示车辆状态,即位置、速度和加速度,沿引导线(即纵向状态),( d ; d ˙ ; d ¨ d; \dot{d}; \ddot{d} d;d˙;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) 在笛卡尔空间中形成一条轨迹。
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:
为了制定指导线平滑的优化,我们考虑以下方面:
A. Optimality Modeling 优化建模
The goal of trajectory generation for autonomous vehicles is to compute a trajectory that drives the passengers to the destination safely and comfortably. The following are key factors that are commonly used to measure the quality of a trajectory:
自动驾驶汽车轨迹生成的目标是计算出一条将乘客安全舒适地送到目的地的轨迹。 以下是通常用于衡量轨迹质量的关键因素:
B. Constraints 约束
As our algorithm is intended for vehicle driving in structured environment, we assume the vehicle is always moving forward monotonically. Based on vehicle’s dynamical properties, the following constraints must be satisfied at any time t of the trajectory with time range t τ t_τ tτ to achieve safety and comfort:
由于我们的算法适用于结构化环境中的车辆驾驶,我们假设车辆始终单调向前行驶。 基于车辆的动力学特性,必须在时间范围为 t τ t_τ tτ 的轨迹的任何时间 t 满足以下约束,以实现安全性和舒适性:
C. Trajectory Formulation and Discretization 轨迹公式化和离散化
To effectively compute an optimal trajectory while evaluating constraint satisfaction in practice, a common approach is discretizing the trajectory s(t) according to a certain parameter. The objective function is then approximated by evaluating these discretized points, and constraint satisfaction is checked on these points as well.
在实践中,为了在评估约束满足的同时有效地计算出最优轨迹,一种常用的方法是根据某个参数对轨迹 s(t) 进行离散化。 然后通过评估这些离散点来近似目标函数,并在这些点上检查约束满足。
Depending on the parameter we use for trajectory discretization, the formulation of optimization, including objective function, constraint formulation, can be quite differently. In this section, we discuss two choices of parameters for discretization and their pros and cons. Despite difference on choice of parameters for discretization, both methods discretize the trajectory to second-order derivative of s, i.e., s ¨ \ddot{s} s¨to achieve accurate controlling to the acceleration level, and assume a constant third-order term (i.e., jerk) connects consecutive discretized points
根据我们用于轨迹离散化的参数,优化的公式,包括目标函数、约束公式,可能会有很大的不同。 在本节中,我们将讨论离散化参数的两种选择及其优缺点。 尽管离散化参数的选择存在差异,但两种方法都将轨迹离散化为 s 的二阶导数,即 s ¨ \ddot{s} s¨以实现对加速度水平的精确控制,并假设一个恒定的三阶项 (即 jerk) 连接连续的离散点
In this formulation, between consecutive discretized point is an equal spatial distance ∆s. The optimization variables include the velocity s ˙ \dot{s} s˙, acceleration s ¨ \ddot{s} s¨ at each point and the time intervals ∆t between consecutive points. As we have discussed in previous section, the curvature of the guide line κ is a function of spatial parameter s.
在这个公式中,连续离散点之间是一个相等的空间距离Δs。 优化变量包括每个点的速度 s ˙ \dot{s} s˙、加速度 s ¨ \ddot{s} s¨ 和连续点之间的时间间隔 Δt。 正如我们在上一节中所讨论的,引导线 κ 的曲率是空间参数 s 的函数。
The major advantage of spatial parameter discretization is the spatial value s for each discretized point is fixed, thus the curvature. Therefore, the speed upper limit resulted from the centripetal acceleration constraint can be precomputed and remains fixed throughout the optimization. However, there are several drawbacks with this formulation:
空间参数离散化的主要优点是每个离散点的空间值 s 是固定的,因此曲率是固定的。 因此,由向心加速度约束产生的速度上限可以预先计算并在整个优化过程中保持不变。 然而,这个公式有几个缺点:
Because of these issues, despite the formulation of centripetal acceleration constraint is simplified, it is not a good choice to discretize the trajectory using spatial parameter.
由于这些问题,尽管简化了向心加速度约束的公式,但使用空间参数对轨迹进行离散化并不是一个好的选择。
where s ˙ \dot{s} s˙、 s ¨ \ddot{s} s¨ are the linear velocity and acceleration respectively. κ is a function that takes a spatial parameter s as input and outputs the curvature κ at the given point. Term 1-3 directly model the longitudinal acceleration, jerk and centripetal acceleration respectively. Term 4 implicitly models the minimization of time for accomplishing certain task. This term encourages the trajectory quickly reach a reference speed s_ref. Term 5-7 are used to model the task preference, which encourage the vehicle to reach certain predefined state by the end of trajectory
其中 s ˙ \dot{s} s˙、 s ¨ \ddot{s} s¨ 分别是线速度和加速度。 κ 是将空间参数 s 作为输入并输出给定点的曲率 κ 的函数。 第 1-3 项分别直接建模纵向加速度、加加速度和向心加速度。 第 4 项隐含地模拟了完成某些任务的时间最小化。 该项鼓励轨迹快速达到参考速度 s_ref。 5-7项 用于对任务偏好进行建模,它鼓励车辆在轨迹结束时达到特定的预定义状态(task代表什么呢?终点的状态还是什么???)
As we discretize the trajectory using temporal parameter, the objective function can be approximated by evaluating the n discretized points.
当我们使用时间参数对轨迹进行离散化时,可以通过评估 n 个离散点来近似目标函数。
As discussed in V-C, a constant jerk term … s is used to connect consecutive points. Its value can be computed by differencing the acceleration terms:
正如 V-C 中所讨论的,恒定的 jerk 项用于连接连续的点。 它的值可以通过对加速度项进行差分来计算:
Constraint checking is performed on these discretized points as well: ∀ ∈ Z , i ∈ [ 0 , n − 1 ] \forall \in \Zeta ,i \in[0,n-1] ∀∈Z,i∈[0,n−1]
约束检查也在这些离散点上执行: ∀ ∈ Z , i ∈ [ 0 , n − 1 ] \forall \in \Zeta ,i \in[0,n-1] ∀∈Z,i∈[0,n−1] --------( Z 怎么理解呢 ? ? ? \Zeta怎么理解呢??? Z怎么理解呢???)
Nevertheless, to maintain the continuity of the piecewise constant-jerk trajectory, the following equality equations must be satisfied between consecutive points:
然而,为了保持分段恒定加加速度轨迹的连续性,连续点之间必须满足以下等式:
The proposed two-step optimization are implemented using non-linear optimization package Interior Point OPTimizer (IPOPT) [11]. The source code is planned to be released as part of Baidu Apollo Open Source Platform [1].
所提出的两步优化是使用非线性优化包内点优化器 (IPOPT) [11] 实现的。(不是使用的OSQP优化器吗???) 源代码计划作为百度 Apollo 开源平台 [1] 的一部分发布。
All experiments are carried out on an Intel Xeon 2.6GHz PC with 32GB RAM. We use a synthetic 108°U-turn scenario with turning radius roughly 10 meters (m), which is common for urban driving, for testing. In guide line smoothing phase, the optimization for this 17-point 150 m guide line takes 948 millisecond ms. The input points are randomly perturbed within radius 0.05 m. The maximal allowed deviation is set to 0.1 m. Fig. 2 shows the geometrical properties of the smoothed guide line.
所有实验均在配备 32GB RAM 的 Intel Xeon 2.6GHz PC 上进行。 我们使用城市驾驶常见的转弯半径约为 10 米 (m) 的合成 108°U 转弯场景进行测试。 在引导线平滑阶段,这条 17 个点的 150 m 引导线的优化需要 948 毫秒 ms。 输入点在半径 0.05 m 内随机扰动。 最大允许偏差设置为 0.1 m。 图 2 显示了平滑引导线的几何特性。
The testings on trajectory optimization phase are carried out on this U-turn scenario. The vehicle starts with an initial speed of 15 m/s and the reference speed is set to 20 m=s. The following tables shows the vehicle’s dynamical limits we set in the experiments:
轨迹优化阶段的测试就是在这个掉头场景上进行的。 车辆以 15 m/s 的初始速度启动,参考速度设置为 20 m/s。 下表显示了我们在实验中设置的车辆动态限制:
The trajectory is discretized by 0.1 second (s). The computation time is strongly affected by the number of discretized point, the number of constraints (e.g., whether there is a preferred task state or there are obstacles exist). Our experiments show the optimization time for optimizing an 18-second trajectory ranging from 76 ms to 617 ms with an average of 259 ms. Fig. 4 shows the optimization results with different driving behaviors in cruise task, stop task and cruise task with a lead vehicle.
轨迹离散化 0.1 秒 (s)。 计算时间受离散点数量、约束数量(例如,是否存在首选任务状态或是否存在障碍)的强烈影响。 我们的实验表明,优化 18 秒的轨迹的优化时间范围从 76 毫秒到 617 毫秒,平均为 259 毫秒。 图 4 显示了巡航任务、停车任务和有前车情况下的车辆巡航任务中不同驾驶行为的优化结果。
图 4 掉头场景的轨迹优化结果。车辆从 s i n i t = 0 、 s ˙ i n i t = 15 m / s 、 s ¨ i n i t = 0 m / s 2 s_{init} = 0、\dot{s}_{init} = 15m/s、\ddot{s}_{init} = 0m/s2 sinit=0、s˙init=15m/s、s¨init=0m/s2 开始,目标巡航速度为 20 m/s。前两行:巡航任务的轨迹优化结果,使用两组参数,一组是舒适的,一组是运动型的驾驶行为。为了显示完整的掉头,使用了不同的轨迹总时间。第一列显示累积距离与时间的关系。舒适轨迹的时间长度为 18 s,空间长度为 121.83 m,运动轨迹的时间长度为 15 s 和 128.04 m。第二列显示优化的速度(蓝色)和速度边界(红色)。速度下限为 0 m/s,上限通过考虑速度限制 (30 m/s)、guide line的曲率和最大向心加速度 (2 m/s2) 来计算。第三列显示优化的加速度(蓝色)和预定义的加速度界限(红色,2 m/s2 用于加速,-4 m/s2 用于制动)。对于运动轨迹,优化的加速度达到加速度边界以最大化行驶距离,而舒适的轨迹提前制动以减少产生的向心加速度并优雅地加速以达到目标速度。第三行:停止任务的轨迹优化。车辆从 s = 0 开始,在 s = 130 处具有预定义的完全停止点。轨迹的结束状态是 0 速度和加速度。第四行:前方低速车辆(3 m/s)10 s巡航任务的轨迹优化结果。自我车辆减速并保持预定义的安全缓冲(5 m)。
We present a new non-linear optimization based trajectory generation method for in-lane driving scenarios. This method consists of two-step optimizations with one step computing a smooth driving guide line and the next step computing a safe and comfortable trajectory along the guide line. The experiments show the algorithm can generate jerk minimal and bounded trajectory under centripetal acceleration constraint.
我们为车道内驾驶场景提出了一种新的基于非线性优化的轨迹生成方法。 该方法由两步优化组成,一步计算平滑的驾驶指导线,下一步计算沿指导线的安全舒适轨迹。 实验表明,该算法可以在向心加速度约束下生成jerk极小和有界轨迹。
For future work, we intend to expand the algorithm to tackle multi-lane driving scenarios. There are two possible directions for the expansion, both use the strategy that path planning deals with static obstacles while speed planning deals with dynamic obstacles.
对于未来的工作,我们打算扩展算法以解决多车道驾驶场景。 扩展有两个可能的方向,都采用路径规划处理静态障碍物,速度规划处理动态障碍物的策略。
In the first direction, the guide line can be dynamically adjusted to take static obstacle avoidance into consideration. The guide line generation essentially becomes a path planning problem for static obstacle avoidance. The advantage of this direction is the rest of the algorithm can remain unchanged. However, the computation for smoothing the guide line is costly and adjustment of the guide line online may not be feasible for fast planning cycles.
在第一方向,可以动态调整引导线以考虑静态避障。 引导线生成本质上成为静态避障的路径规划问题。 这个方向的优点是算法的其余部分可以保持不变。 然而,用于平滑引导线的计算成本很高,并且在线调整引导线对于快速规划周期可能不可行。
Another direction is using optimization to generate the lateral trajectory/path d(s) that directly considers the projected static obstacles onto the Frenet frame. This direction is more consistent with the framework in Frenet frame planning. The benefit is the smooth guide line can be generated and stored offline and be loaded dynamically for online uses. Also, lane change actions will be handled more cleanly as lane changing can be planned by simply substituting the guide line.
另一个方向是使用优化来生成横向轨迹/路径 d(s),直接考虑投射到 Frenet 框架上的静态障碍物。 这个方向更符合 Frenet 框架规划中的框架。 好处是可以离线生成和存储流畅的guide line,并可以动态加载以供在线使用。 此外,由于可以通过简单地替换引导线来计划变道,因此可以更干净地处理变道动作。
本篇文章中的构造P,q,A矩阵的方法在apollo planning中的piecewise_jerk_speed_problem.cc中进行实现
// x(i)^2 * w_x_ref
for (int i = 0; i < n - 1; ++i) {
columns[i].emplace_back(
i, weight_x_ref_ / (scale_factor_[0] * scale_factor_[0]));
++value_index;
}
对应的公式描述:
w s f a c t o r 2 ∗ ∑ i = 0 n − 2 ( s i − s i r e f ) \frac{w_s}{factor^2}*\sum_{i=0}^{n-2}(s_i-s_{i_{ref}}) factor2ws∗∑i=0n−2(si−siref)
columns[n - 1].emplace_back(n - 1, (weight_x_ref_ + weight_end_state_[0]) /
(scale_factor_[0] * scale_factor_[0]));
++value_index;
对应的公式描述为:
其中有一个jerk项计算的推导:
j e r k i − > i + 1 = s ¨ i + 1 − s ¨ i Δ t jerk_{i->i+1} = \frac{\ddot{s}_{i+1}-\ddot{s}_i}{\Delta t} jerki−>i+1=Δts¨i+1−s¨i
从而
j e r k i − > i + 1 2 = ( s ¨ i + 1 − s ¨ i Δ t ) 2 = ( s ¨ i + 1 ) 2 ( Δ t ) 2 + ( s ¨ i ) 2 ( Δ t ) 2 − 2 s ¨ i + 1 ∗ s ¨ i ( Δ t ) 2 jerk_{i->i+1}^2 = (\frac{\ddot{s}_{i+1}-\ddot{s}_i}{\Delta t})^2=\frac{(\ddot{s}_{i+1})^2}{(\Delta t)^2}+\frac{(\ddot{s}_{i})^2}{(\Delta t)^2}-2\frac{\ddot{s}_{i+1}*\ddot{s}_{i}}{(\Delta t)^2} jerki−>i+12=(Δts¨i+1−s¨i)2=(Δt)2(s¨i+1)2+(Δt)2(s¨i)2−2(Δt)2s¨i+1∗s¨i
所以
∑ j e r k i − > i + 1 2 = ∑ ( s ¨ i + 1 ) 2 ( Δ t ) 2 + ∑ ( s ¨ i ) 2 ( Δ t ) 2 − 2 ∑ s ¨ i + 1 ∗ s ¨ i ( Δ t ) 2 \sum jerk_{i->i+1}^2 =\sum \frac{(\ddot{s}_{i+1})^2}{(\Delta t)^2}+\sum \frac{(\ddot{s}_{i})^2}{(\Delta t)^2}-2\sum \frac{\ddot{s}_{i+1}*\ddot{s}_{i}}{(\Delta t)^2} ∑jerki−>i+12=∑(Δt)2(s¨i+1)2+∑(Δt)2(s¨i)2−2∑(Δt)2s¨i+1∗s¨i