Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimiz

Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization

通过凸优化实现更完整、更灵活、更安全的自动驾驶速度规划

Abstract: In this paper, we present a complete, flexible and safe convex-optimization-based method to solve speed planning problems over a fixed path for autonomous driving in both static and dynamic environments. Our contributions are five fold. First, we summarize the most common constraints raised in various autonomous driving scenarios as the requirements for speed planner developments and metrics to measure the capacity of existing speed planners roughly for autonomous driving. Second, we introduce a more general, flexible and complete speed planning mathematical model including all the summarized constraints compared to the state-of-the-art speed planners, which addresses limitations of existing methods and is able to provide smooth, safety-guaranteed, dynamic-feasible, and time-efficient speed profiles. Third, we emphasize comfort while guaranteeing fundamental motion safety without sacrificing the mobility of cars by treating the comfort box constraint as a semi-hard constraint in optimization via slack variables and penalty functions, which distinguishes our method from existing ones. Fourth, we demonstrate that our problem preserves convexity with the added constraints, thus global optimality of solutions is guaranteed. Fifth, we showcase how our formulation can be used in various autonomous driving scenarios by providing several challenging case studies in both static and dynamic environments. A range of numerical experiments and challenging realistic speed planning case studies have depicted that the proposed method outperforms existing speed planners for autonomous driving in terms of constraint type covered, optimality, safety, mobility and flexibility.
摘要: 本文提出了一种完整、灵活、安全的基于凸优化的自动驾驶静态和动态环境下固定路径速度规划方法。我们的贡献是五倍。首先，我们总结了各种自动驾驶场景中最常见的约束，作为速度规划器开发的需求和衡量现有速度规划器大致用于自动驾驶能力的指标。其次，我们引入了一个更通用、更灵活、更完整的速度规划数学模型，该模型包含了目前最先进的速度规划器所总结的所有约束，解决了现有方法的局限性，并能够提供平滑、安全保证、动态可行和时间高效的速度曲线。第三，在不牺牲汽车机动性的前提下，在保证基本运动安全的前提下，强调舒适性。在通过松弛变量和惩罚函数进行优化时，我们将舒适区约束视为半硬约束，这与现有的方法有所区别。第四，我们证明了我们的问题在附加约束条件下保持了凸性，从而保证了解的全局最优性。第五，我们通过在静态和动态环境中提供几个具有挑战性的案例研究，展示了我们的方法如何在各种自动驾驶场景中使用。一系列数值实验和具有挑战性的现实速度规划案例研究表明，所提出的方法在约束类型覆盖、最优性、安全性、移动性和灵活性方面优于现有的自动驾驶速度规划。

1. Introduction

Speed planning plays an important role in guaranteeing the ride comfort and safety in autonomous driving applications. All different kind of scenarios together raises distinct requirements and consequently different constraint types for speed planning problem formulations, which makes it challenging to solve.
在自动驾驶应用中，速度规划对保证乘坐舒适性和安全性起着重要作用。各种不同的场景对速度规划问题的表述提出了不同的要求，从而产生了不同的约束类型，这使得速度规划问题的求解具有挑战性。
In most of urban driving scenarios, autonomous driving systems prefer smooth speed profiles for the sake of ride comfort. These scenarios require the speed planner to consider the maximum lateral and longitudinal accelerations and decelerations (comfort box (CB) constraints), jerk (smoothness (S)) to manage smooth transitions between states of cars from time to time. Such a smooth speed profile with these bounds does not only exhibit energy-saving behaviors of autonomous cars but also presents a decent reference that is easy to track for a speed controller, which results in a pleased ride experience for passengers in the end.
在大多数城市驾驶场景中，自动驾驶系统为了乘坐舒适性更倾向于平稳的速度曲线。这些场景要求速度规划器考虑最大的横向和纵向加速和减速(comfort box(CB)约束)、jerk(平稳性(S))，以控制汽车在不同状态之间不时的平稳过渡。这样一个具有这些边界的平稳的速度曲线，不仅表现了自动驾驶汽车的节能行为，而且为速度控制器提供了一个易于跟踪的良好参考，最终为乘客带来了愉快的乘坐体验。
There are some scenarios that need the speed planner to exploit the full mobility capacity of cars such as driving on the limits to pursue high speeds or dealing with emergencies [1]. These applications raise a common hard constraint called friction circle (FC) constraint that is related to vehicle dynamics and road conditions and a soft constraint called time efficiency (TE). Both constraints are closely related since the time efficiency objective will push a car to the limits to achieve the minimum travelling time, which may frequently activate the friction circle hard constraint during planning. A typical example is that cars race in a prescribed curvy track for speed. In academia, a large body of research is carried out to address the minimum-time speed planning problem over a fixed path and the outstanding ones of them are [2,3], which inspire our work in this paper.
有一些场景需要速度规划器充分利用汽车的移动能力，如在极限行驶以追求高速或处理紧急情况[1]。这些应用提出了一个常见的硬约束，称为friction circle(FC)约束，这与车辆动力学和路况有关，还有一个软约束，称为time efficiency(TE)。这两个约束是密切相关的，因为time efficiency目标会将汽车推到极限，以达到最小的行驶时间，这可能会在规划过程中频繁激活friction circle硬约束。一个典型的例子是汽车在规定的曲线赛道上进行速度比赛。在学术界，针对固定路径上的最小时间速度规划问题进行了大量的研究，其中较为突出的有[2,3]，这也启发了我们本文的工作.
Although constantly pursuing high speeds is not the goal of autonomous driving, varying weather conditions may have an impact on the road quality in such a way as to dramatically reduce the friction coefficients and in consequence the maximum safe velocity limits for vehicles [4]. Thanks to the rapid development of mature sensing, perception and scene understanding system relying on computer vision and machine learning techniques for autonomous driving, high level information such as weather conditions, road surface categories, together with vehicle state information, can be delivered from on-board perception systems to road friction estimators [5,6]. The environment-dependent, varying friction coefficient becomes available to speed or motion planners online, which can be used to generate safety-guaranteed speed profiles. Above applications all require the speed planning to consider the friction circle constraints in the problem formulation explicitly. Unfortunately, most of existing speed planner [7–11] does not take it into consideration. They conservatively search for solutions in a subset of the friction circle region, which sacrifices the mobility for safety. In addition, speed planning is oftentimes the last action to guarantee safety by regulating the speed to stop the car in front of obstacles when there is no room to adjust the shape of the path to avoid collision in emergencies. This imposes a zero speed constraint at the end of the path, which is called boundary condition (BC) constraints in this paper. The lack of this kind of constraint in [3,11] produces flaws in safety in their planners.
尽管不断追求高速并不是自动驾驶的目标，但不同的天气条件可能会对道路质量产生影响，从而显著降低摩擦系数，从而降低车辆的最大安全速度限制[4]。随着成熟的基于计算机视觉和机器学习技术的传感、感知和场景理解系统的快速发展，车载感知系统可以将天气状况、路面类别等高级信息以及车辆状态信息传递给道路摩擦估计器[5,6]。与环境相关的不同摩擦系数可用于在线速度或运动规划器，可用于生成安全保证的速度曲线。上述应用均要求速度规划在问题表述中明确考虑friction cicle约束。遗憾的是，大多数现有的速度规划[7-11]并没有考虑到这一点。他们在friction cicle区域的一个子集中保守地搜索解，牺牲了机动性以获得安全。此外，速度规划往往是在紧急情况下，在没有空间调整路径形状以避免碰撞的情况下，通过调节速度将汽车停在障碍物前以保证安全的最后一个动作。这在路径的末端施加了一个零速度约束，本文称之为边界条件(BC)约束。在[3,11]中缺乏这种约束导致了他们的规划者在安全方面的缺陷。
In dynamic environments, speed planning also makes a difference in terms of dynamic obstacle avoidance. Regulating speed along the fixed path to avoid dynamic obstacles rather than swerving the path to deal with the dynamic obstacles (such as pedestrians, cyclists who are crossing lanes, changing lanes or turning in the intersection) may be thought of as a smart, energy-saving, and risk-free behavior in certain situations. It also should be noted that not all the cases with dynamic obstacles can be overcome using this method. It works under the the assumption that the path has been well predefined using some sampling techniques. Given the prescribed path, overtaking a slow front car using an opposite lane [11] imposes one or several time window (TW) constraints for speed planning in time domain at the conflict region with other road participants along the path. Merging from a freeway entrance ramp to a lane with an oncoming high-speed vehicle on expressway [12,13] does not only bring in a time window constraint but also boundary conditions such as a desired final speed and acceleration constraints to keep the pace with other traffic participants. These cases require that the car reaches a certain point on the path in the time window to avoid collision. However, most of the existing methods [3,4,7,8,14] ignore these constraints, which make their methods applicable only in static environments.
在动态环境中，速度规划对动态避障也有影响。在某些情况下，沿着固定路径调节速度以避开动态障碍物，而不是改变路径以应对动态障碍物(例如行人，骑自行车的人正在穿越车道，变道或在十字路口转弯)，可能被认为是一种智能，节能和无风险的行为。还应注意的是，并非所有具有动态障碍的情况都可以使用该方法克服。它是在假设路径已经使用一些采样技术很好地预定义的情况下工作的。给定指定的路径，在与路径沿线其他道路参与者的冲突区域，从对面车道超车慢速前车[11]会对时域速度规划施加一个或多个time window(TW)约束。从高速公路入口匝道并入高速公路上即将接近的高速车辆的车道[12,13]不仅会带来time window约束，还会带来边界条件，如所需的最终速度和加速度约束，以保持与其他交通参与者的步伐。这些情况要求汽车在time window内到达路径上的某一点，以避免碰撞。然而，现有的大多数方法[3,4,7,8,14]都忽略了这些约束，这使得它们的方法只适用于静态环境。
From the task perspective, the speed limit traffic sign along a road enforces a speed limit on a certain segment of a path, which is known as a path constraint (PC), that is, a hard constraint, in optimal control domain. In the case that desired speed profile is given by high level modules such as behavior planners or task planners, the integral of deviations (IoD) between planned speeds and desired speeds over the path is used as an objective to optimized to accomplish certain goals, which is a soft constraint.
从任务的角度来看，道路限速交通标志是对某一路段的限速，在最优控制域中称为path constraint(PC)，即硬约束。在期望速度轮廓由行为规划器或任务规划器等上层模块给出的情况下，将路径上的规划速度与期望速度之间的偏差的积分integral of deviations(IoD)作为优化目标，以实现某些目标，这是一种软约束。
All aforementioned constraints are summarized and categorized by us in Table 1, as requirements for the speed planning module to meet and metrics to identify the capacity of speed planners. Please note that we introduce a “semi-hard” constraint type to describe the unique characters of comfort box (CB) constraints. By “semi-hard”, we mean this kind of constraints should be satisfied in the first place to achieve high performance when possible and can be violated to meet fundamental motion safety requirements while penalizing violations. This is different from soft constraints that treat all the solution space equally by assigning the same coefficients to the soft constraints.
我们在表1中对上述所有约束进行了总结和分类，作为速度规划模块需要满足的要求和识别速度规划器能力的指标。请注意，我们引入了“semi-hard”约束类型来描述comfort box(CB)约束的独特特征。所谓“semi-hard”，是指在可能的情况下，首先要满足这种约束，以实现好的表现性能; 有可能违反约束的来满足基本的运动安全要求，并对违反约束的行为进行处罚。这与软约束不同，软约束通过给软约束分配相同的系数来平等地对待所有的解空间。

A safety-guaranteed speed planner should be able to generate a solution satisfying at least all the hard constraints (safety) in Table 1. A mature speed planner should cover all the constraints that include soft and hard ones.
保证安全的速度规划器应该能够生成至少满足表1中所有硬约束(安全性)的求解结果。一个成熟的速度器应该涵盖所有的约束，包括软约束和硬约束。
By taking some additional steps beyond the seminal work done by [2,3], we present a general speed planning framework specifically for autonomous driving that is able to handle a wide range of different scenarios using convex optimization subject to a large collection of relevant constraints. Our contributions are as follows:
在[2,3]所做的开创性工作之外，我们采取了一些额外的步骤，提出了一个专门针对自动驾驶的通用速度规划框架，该框架能够在大量相关约束条件下使用凸优化处理各种不同的场景。我们的贡献如下:

• List item We summarize the most common constraints raised in various autonomous driving scenarios as the requirements for speed planner design and metrics to measure the capacity of the existing speed planners roughly for autonomous driving. We clarify which constraints need to be addressed by speed planners to guarantee safety in general.
• 我们总结了各种自动驾驶场景中最常见的约束条件，作为对速度规划器设计的要求和衡量现有速度规划器对自动驾驶能力的指标。我们阐明了速度规划器需要解决哪些限制以保证总体安全。
• In light of these requirements and metrics, we present a more general, flexible and complete speed planning mathematical model including friction circle, dynamics, smoothness, time efficiency, time window, ride comfort, IoD, path and boundary conditions constraints compared to similar methods explained in [3,11]. We addressed the limitations of the method of Lipp et al. [3] by introducing a pseudo jerk objective in longitudinal dimension to improve smoothness, adding time window constraints at certain point of the path to avoid dynamics obstacles, capping a path constraint (most-likely non-smooth) on speed decision variables to deal with task constraints like speed limits, imposing a boundary condition at the end point of the path to guarantee safety for precise stop or merging scenarios. Compared to the approach of Liu et al. [11], our formulation optimizes the time efficiency directly while still staying inside of the friction circle, which ensures our method exploits the full acceleration capacity of the vehicle when necessary.
• 根据这些要求和指标，与文献[3,11]中解释的类似方法相比，我们提出了一个更通用、更灵活、更完整的速度规划数学模型，包括摩擦环、动力学、平滑度、时间效率、时间窗口、平顺性、IoD、路径和边界条件约束。我们解决了Lipp等人[3]方法的局限性，通过在纵向维度上引入pseudo jerk目标来提高平滑度，在路径的某些点添加时间窗口约束以避免动态障碍物，在速度决策变量上设置路径约束(很可能是非平滑的)以处理速度限制等任务约束，在路径的终点施加边界条件以保证精确停车或并车场景的安全性。与Liu等人[11]的方法相比，我们的公式在保持在摩擦环内的情况下直接优化了时间效率，保证了我们的方法在必要时充分利用了车辆的加速能力。
  We introduce a semi-hard constraint concept to describe unique characters of the comfort box constraints and implement this kind of constraints using slack variables and penalty functions, which emphasizes comfort while guaranteeing fundamental motion safety without sacrificing the mobility of cars. To the best of our knowledge, none of the existing methods handle these constraints like ours. In contrast, Refs. [7–11] regarded comfort box constraints as hard constraints, which dramatically reduces the solution space and in consequence limits the mobility of cars.
  引入半硬约束概念来描述舒适区约束的独特性，并利用松弛变量和罚函数实现这种约束，在保证基本运动安全的同时强调舒适性，而不牺牲汽车的机动性。据我们所知，没有任何现有的方法能像我们的方法那样处理这些约束。相比之下，参考文献[7-11]将舒适区约束作为硬约束，极大地缩小了求解空间，从而限制了汽车的移动性。
• We demonstrate that our problem still preserves convexity with the added constraints, and hence, that the global optimality is guaranteed. This means our problem can be solved using state-of-the-art convex optimization solvers efficiently as well. We also provide some evidence to prove that our solution is able to keep consistent when the boundary conditions encounter some disturbances, which means only the part of results needed to be adjusted will be regulated due to the global optimality. This may benefit the track performance of speed controllers by providing a relative stable reference. It is not the case for these methods that solve the speed planning problem using local optimization techniques like [11]. A small change of boundary conditions or initial guess may result in a totally different solution due to local minimas in their problem.
• 我们证明了我们的问题在附加约束条件下仍然保持凸性，因此，保证了全局最优性。这意味着我们的问题可以使用最先进的凸优化求解器有效地解决。我们还提供了一些证据来证明当边界条件遇到一些干扰时，我们的解能够保持一致，这意味着由于全局最优性，只有需要调整的部分结果会受到调节。这可以提供一个相对稳定的参考，从而有利于速度控制器的跟踪性能。这些方法不是使用像[11]这样的局部优化技术来解决速度规划问题的。由于问题的局部极小值，边界条件或初始猜测的微小变化都可能导致完全不同的解。
• We showcase how our formulation can be used in various autonomous driving scenarios by providing several challenging case studies solved in our framework, such as safe stop on a curvy road with different entry speeds, dealing with jaywalking in two different ways and merging from a freeway entrance ramp to expressways with safety guaranteed.
• 我们通过提供在我们的框架中解决的几个具有挑战性的案例研究来展示我们的公式如何用于各种自动驾驶场景，例如在不同进入速度的弯曲道路上安全停车，以两种不同的方式处理乱穿马路，以及从高速公路入口匝道合并到高速公路并保证安全。

This paper is organized as follows. Section 2 reviews the featured speed planning methods for autonomous driving. Section 3 formulates the problem for speed planning along a fixed path by considering different constraints. Section 4 describes the implementation details. Section 5 shows a rich set of numerical experiment results and Section 6 demonstrates three case studies with parameters from real platforms. Section 7 draws conclusions.
本文组织如下。第2节回顾了自动驾驶的主要速度规划方法。第3节通过考虑不同约束条件，阐述了沿固定路径的速度规划问题。第4节描述了实现细节。第5节展示了一组丰富的数值实验结果，第6节展示了来自真实平台参数的三个案例研究。第7节得出结论。

2. Related Work

A rich literature exists on speed planning as a single research topic or part of motion planning systems. Speed planning methods used in literature fall into two categories roughly: coupled speed planning and decoupled speed planning. The former family exists in motion planning frameworks that explore the spatial-temporal space simultaneously using optimization techniques [15–17] or search algorithms[18,19]. Most of the time-parameterized trajectory planning based on optimal control belong to this family. Due to the non-convexity of objectives, dynamics and other constraints, it is already very hard to find a feasible path, let alone a time optimal trajectory. Finding a time optimal path may take a lot of time. Thus it is impracticable to apply these methods to autonomous driving applications due to run-time requirements. The latter family frequently appears in hierarchical motion planning frameworks [9,20–22] that decouple motions by planning a path first then reconstructing a speed profile along the path, or shows up as a standalone research with the assumption that the path is known.
速度规划作为一个单独的研究课题或运动规划系统的一部分，存在着丰富的文献。文献中使用的速度规划方法大致分为两类:耦合速度规划和解耦速度规划。前者存在于运动规划框架中，通过优化技术[15-17]或搜索算法[18,19]同时探索空间-时间空间。大多数基于最优控制的时间参数轨迹规划都属于这一类。由于目标的非凸性、动力学等约束，找到可行的路径已经非常困难，更不用说找到时间最优的轨迹了。寻找时间最优路径可能需要花费大量时间。因此，由于运行时间的要求，将这些方法应用于自动驾驶应用是不切实际的。后者经常出现在分层运动规划框架中[9,20 - 22]，通过首先规划路径然后沿着路径重建速度曲线来解耦运动，或者在路径已知的假设下作为独立研究。
As we focus on the second class of the problem, we review these featured methods that are closely related to ours, which generate speed profiles along a fixed path subject to certain constraints. We first compare them with others in terms of constraints coverage, optimality, safety, flexibility, and capacity without revealing details, as seen in Table 2. Most of the existing methods just provided a workable speed profile rather than an optimal one for autonomous driving. None of them covered all the constraints we list in Table 1.
当我们专注于第二类问题，我们回顾了这些与我们的方法密切相关的特征方法，它们在特定约束下沿固定路径生成速度曲线。我们首先在约束覆盖、最优性、安全性、灵活性和能力方面比较它们，但不包含细节，如表2所示。大多数现有的方法只是提供了一个可行的速度分布图，而不是自动驾驶的最佳速度分布图。它们都没有涵盖我们在表1中列出的所有约束。
Li et al. [7] employed a trapezoidal speed profile with constant accelerations and decelerations along the fixed path in a hierarchical trajectory planning framework and smoothed the ramp-up and ramp-down part of trapezoidal speed profiles with 3rd-order polynomials, which is neither optimal nor flexible. Besides, the acceleration value may exceed the threshold after smoothing. Thus very conservative accelerations for the ramp-up and deceleration for the ramp-down were selected in their work. Most of the constraints in Table 1 were not covered in their speed planner.
Li等[7]在分层轨迹规划框架中，采用沿固定路径加减速恒定的梯形速度曲线，用三阶多项式平滑梯形速度曲线的上、下斜坡部分，既不最优，也不灵活。此外，平滑后的加速度值可能会超过阈值。因此，他们在工作中选择了非常保守的爬坡加速度和减速度。表1中的大多数约束都没有包含在它们的速度规划中。
Gu et al. [8] developed a constraint-based speed planner that trimmed the reference speed profile according to maximum velocity, lateral acceleration, longitudinal acceleration and deceleration. Since smoothness of the speed profile is not taken into account, the excessive longitudinal jerk may be observed [9]. They considered moving obstacles in speed planning in a reactive way based on the distance between obstacles and the ego car to affect speed profiles in the following work [10] and further enforced jerk limits on the speed profile in [9] for the sake of smoothness. As dynamics constraints are considered separately in the form of comfort box constraints that its upper boundaries need to be selected conservatively to prevent the total force from exceeding the friction force limits, the capacity of driving on the limits to deal with emergencies or pursue time efficiency is highly restricted. The difference of potential solution space of comfort box constraints and friction circle constraints is shown in Figure 1. In addition, the reduction of friction coefficient in extreme weather conditions will shrink the friction circle and the original fixed comfort constraints may create one or several dangerous zones in solution space, as shown in Figure 1, which will inevitably cause potential safety issues.
Gu等人[8]开发了一种基于约束的速度规划器，根据最大速度、横向加速度、纵向加减速对参考速度曲线进行修整。由于没有考虑速度曲线的平稳性，可能会观察到过大的纵向抖动[9]。在接下来的工作[10]中，他们考虑在速度规划中根据障碍物与自我车之间的距离，以一种反应性的方式移动障碍物来影响速度曲线，并在[9]中进一步对速度曲线实施了平稳性限制。由于动力学约束以舒适区间约束的形式单独考虑，需要保守地选择其上边界，以防止总力超过摩擦力极限，因此在极限上行驶以应对紧急情况或追求时间效率的能力受到很大限制。舒适区间约束与摩擦圈约束的势能解空间之差如图1所示。此外，极端天气条件下摩擦系数的减小会使摩擦圈缩小，原有的固定舒适约束可能会在解空间中形成一个或几个危险区域，如图1所示，这必然会产生安全隐患。
Dakibay et al. [4] exploited an aggressive speed planning method by numerically solving a nonlinear differential equation (NDE) about friction circle constraints and capping the speed profile with forward and reverse integration of accelerations results along the fixed path. Due to the approximation of solution of NDE, the full capacity of car is not explored. None of their results reaches exactly the friction circle. As the driving conditions are quite close to the limits, admissible room left for track errors is little. We argue that the smoothness of speed profiles still need to be considered to improve tracking performance of the controller for safety concerns (jerky speed profiles may result in overshooting and oscillation of controllers), even for aggressive driving scenarios, which did not appear in their solution.
Dakibay等人[4]利用一种激进的速度规划方法，通过数值求解关于摩擦圆约束的非线性微分方程(NDE)，并通过沿固定路径的加速度结果的正向和反向积分来限定速度曲线。由于NDE解的逼近性，没有考虑到汽车的最大能力。它们的结果都不能完全达到摩擦圈。由于驾驶条件非常接近极限，赛道误差允许的空间很小。我们认为，为了提高控制器的跟踪性能，仍然需要考虑速度曲线的平稳性(速度曲线的不稳定可能导致控制器的超调和振荡)，即使是在侵略性驾驶场景中，也没有出现在他们的解决方案中。
Lipp et al. [3] presented a convex-optimization-based general minimum time speed planning method over the fixed path based on the approach proposed by [2]. The friction circle constraint is well considered as a convex set constraint acting on the problem formulation, which leads to an elegant solution. Not only the capacity of mobility of cars are fully explored, but also the total time travelling along the path is explicitly and analytically represented as a soft constraint to achieve time efficiency. The problem is solved by a customized interior point method using log barrier functions efficiently. Thanks to the preserving convexity of the problem formulation, the global optimality of solutions is guaranteed. However, smoothness of the speed profile is not consider, which most likely results in the same issues that we mentioned about Dakibay’s work regarding tracking performance and safety concerns. In addition, the use of customized Newton-based solver requires that constraints and objective functions are all at least twice differentiable, which seems very restrictive on the type of constraints that users can impose in convex optimization. Convex problems with non-differentiable constraint terms cannot be solved by their framework.
Lipp等[3]在[2]的基础上提出了一种基于凸优化的固定路径上的一般最小时间速度规划方法。将摩擦圆约束作为作用于问题公式的凸集约束来考虑，从而得到一个优美的解。不仅充分挖掘了车辆的移动能力，而且将沿路径行驶的总时间明确解析地表示为软约束，以实现时间效率。利用对数势垒函数的自定义内点法有效地解决了这一问题。由于问题表述的保凸性，保证了解的全局最优性。但是，没有考虑速度配置文件的平顺性，这很可能导致与我们提到的Dakibay关于跟踪性能和安全问题的工作相同的问题。此外，使用定制的基于牛顿的求解器要求约束和目标函数都至少是两次可微的，这似乎对用户可以在凸优化中施加的约束类型非常限制。约束项不可微的凸问题不能用其框架来求解。
Liu et al. [11] recently introduced a temporal optimization approach, optimizing time stamps for all waypoints along a fixed path with respect to time window constraints at each point, and then using a slack convex feasible set algorithm to solve it iteratively. Smoothness of the speed profile and time efficiency are taken into account in the problem formulation. However, the time efficiency is considered in an indirect way that optimizes IoD with respect to a reference speed over the path. Their formulation leads to a highly nonlinear and non-convex problem and is solved by a local optimization method, thus only local optimality is guaranteed. They addressed some important constraints in Table 1 such as smoothness, time window and comfort box constraints in their formulation but left out the friction circle constraint, which does not fully exploit the acceleration capacity of the vehicle. In addition, since they optimized timestamps directly, we do not see a quick way to impose a path constraint or a point constraint as a hard one to manipulate speed profiles.
Liu等人[11]最近引入了一种时间优化方法，根据每个点的时间窗约束，对固定路径上所有路点的时间戳进行优化，然后使用松弛凸可行集算法迭代求解。在问题的表述中考虑了速度曲线的平滑性和时间效率。然而，时间效率是以间接的方式考虑的，即相对于路径上的参考速度优化IoD。它们的表述是一个高度非线性的非凸问题，用局部最优方法求解，因此只能保证局部最优。他们在公式中解决了表1中一些重要的约束，如平顺性、时间窗和舒适区间约束，但忽略了摩擦圈约束，因为摩擦圈约束没有充分利用车辆的加速能力。此外，由于它们直接优化了时间戳，我们没有看到一种快速的方法来施加路径约束或点约束，因此它很难操纵速度曲线。

3. Problem Formulation

Assuming a curvature continuous path has been generated by a hierarchical motion planning framework like [9,22], the speed planning is to find a time-efficient, safe, and smooth speed profile travelling along the fixed path with respect to both safety and performance constraints.
假设像[9,22]这样的分层运动规划框架已经生成了曲率的连续路径，那么速度规划就是在考虑安全和性能约束的情况下，在固定路径上找到一个时间高效、安全、平滑的速度曲线。
To solve the proposed problem, we optimize the performance criterions from three aspects, smoothness $J_S$ , time efficiency $J_T$ , and speed deviation $J_V$ from a desired speed, with others left as hard constraints or semi-hard constraints. We first introduce the path representation and explain the relationship of an arc-length parametrized path and a time parametrized path, then present mathematical expressions of all the constraints, and pose the optimization problem at the end.
为了解决所提出的问题，我们从平滑度$J_S$、时间效率$J_T$和速度与期望速度的偏差$J_V$三个方面对性能准则进行优化，其他方面保留为硬约束或半硬约束。首先介绍了路径表示，解释了弧长参数化路径与时间参数化路径的关系，然后给出了所有约束条件的数学表达式，最后给出了优化问题。

3.1. Path Representation

The goal of speed planning is to find a speed profile along a fixed path. Since the path is known, we need to reconstruct the mapping between the known path and the speed profile, then represent the speed profile with parameters determined by the prescribed path. A rich set of parameterized path representations has been proposed in the literature, including B-spline [23,24], Bezier curve [25,26], clothoid [27,28], polynomial curve [29] and polynomial spiral [30,31]. It is trivial to convert all the listed curve models to a simple waypoints representation, but not vice versa. To avoid the non-trivial converting between curve models, we use the general waypoints parametrization to represent a fixed path, with the orientation and curvature encoded implicitly by the path. Formally, we define a waypoints parametrized curve as a workspace path. A workspace path, r, of the body point, b, at the center of the rear axle with footprint, A , is defined as $r:[0,s_f]\to R^2$ . More specifically, we consider the following arc-length parametric form in Cartesian coordinate system,
速度规划的目标是找到沿固定路径的速度曲线。由于路径是已知的，我们需要重建已知路径与速度曲线之间的映射，然后用指定路径确定的参数表示速度曲线。文献中已经提出了一组丰富的参数化路径表示，包括B样条[23,24]、Bezier曲线[25,26]、clothoid[27,28]、多项式曲线[29]和多项式螺旋[30,31]。将所有列出的曲线模型转换为简单的路径点表示很简单，但反之则不然。为了避免曲线模型之间的繁琐转换，我们使用一般路径点参数化来表示固定的路径，方向和曲率由路径隐式编码。形式上，我们将路径点参数化曲线定义为工作空间路径。工作空间路径为r，车体点b位于后轴中心A，工作空间路径r定义为$r:[0,s_f]\to R^2$。更具体地说，我们考虑笛卡尔坐标系下的弧长参数形式:

where s is the arc-length parameter along the path, x(s) and y(s) are the scalars along two orthogonal base axes respectively. The relationship between the arclength s and the corresponding time t is formed as the function $s=f(t)$ , therefore the time parameterized workspace path $r(t)=(x(t)，y(t))，t\in [0,t_f]$ can be easily acquired by substituting in for s.
其中s为路径上的弧长参数， x(s)和y(s)分别为两个正交基轴上的标量。弧长s与对应的时间t之间形成函数$s=f(t)$的关系，因此将s代入，可以很容易地得到时间参数化工作空间路径$r(t)=(x(t)，y(t))，t\in [0,t_f]$。
Since the path, r(s) , is known, the speed vector $\vec{v}$ in Cartesian coordinates can be calculated as below (the prime 0 and the dot · denote derivatives with respect to the arc-length, s, and the time, t, respectively for a curve throughout the paper),
由于路径r(s)是已知的，所以笛卡尔坐标下的速度矢量 $\vec{v}$ 可以这样计算(文中的撇’和点·分别表示曲线对弧长s和时间t的导数):

where $r^{\prime }(s)$ is the unit tangent vector of the path $r(s)$ at s that represents the direction of the speed of a car by assuming no sliding,$\dot{f}$ is the corresponding longitudinal speed of the car in ego frame. Let θ(s) represent the heading of the car at s of the path r, we get
其中$r^{\prime }(s)$是路径$r(s)$在s处的单位切向量，表示假定没有滑动的情况下汽车速度的方向，$\dot{f}$是汽车在坐标系中相应的纵向速度。设θ(s)表示汽车在路径r在s处的航向，我们得到:

The acceleration vector $\vec{a}$ in Cartesian coordinates system is
笛卡尔坐标系下的加速度矢量 $\vec{a}$ 为：

where $\ddot{f}$ is the longitudinal acceleration and $r^{\prime \prime }(s)$ is the principal normal vector of the path, which is also called the curvature vector. The 2-norm of the $r^{\prime \prime }(s)$ is the scalar of the curvature
其中 $\ddot{f}$是纵向加速度，$r^{\prime \prime }(s)$ 是路径的主法向量，也称为曲率向量。$r^{\prime \prime }(s)$ 的2范数是曲率的标量：

3.2. Vehicle Model and Vehicle Dynamics Constraints

Due to the non-holonomic dynamics of the vehicle system, the lateral motion and longitudinal motion are intrinsically coupled in a way that the car cannot move laterally without longitudinal speeds. The lateral motion is explicitly expressed by the prescribed path. The longitudinal motion is the goal of this paper. To build the connection between them and describe the vehicle dynamics explicitly in the problem formulation, we employ the single track vehicle model [32] (see Figure 2) to represent the actual vehicle kinematics and dynamics, which is widely used in motion planning research [9,19,22,30] and performs satisfactorily in practice [33]. The control force is defined as u $=(u^{\tau }，u^{\eta })$ , where $u^{\eta }$ is the lateral force and $u^{\tau }$ is the longitudinal force in ego frame. The dynamics of the car are given by
由于车辆系统的非完整动力学，横向运动和纵向运动本质上是耦合的，因此如果没有纵向速度，汽车就无法横向移动。横向运动由规定的路径明确表示。纵向运动是本文研究的目标。为了在问题表述中建立两者之间的联系，明确地描述车辆动力学，我们采用单车模型 [32] (见图2)来表示实际的车辆运动学和动力学，该模型在运动规划研究中得到了广泛的应用[9,19,22,30]，在实践中表现良好[33]。控制力定义为u $=(u^{\tau }，u^{\eta })$，(u是个向量)其中$u^{\eta }$为横向力，$u^{\tau }$为纵向力。汽车的动力学由


is the rotation matrix that maps forces from the ego frame to the global Cartesian coordinate system, m is the mass of the car. We replace the $\ddot{f}$with a function $\alpha (s)$ ,${\dot{f}}^2$ with a function $\beta (s)$ according to [2],
是将力从自车坐标系映射到全局笛卡尔坐标系的旋转矩阵，m是汽车的质量。根据[2]，我们用函数$\alpha (s)$代替$\ddot{f}$，用函数$\beta (s)$代替${\dot{f}}^2$，

其中$\dot{\beta }(s)={\beta }^{\prime }\dot{f}$ 。
Therefore, Equations (4), (6) and (8) form the dynamics constraints of cars.
因此，式(4)、(6)、(8)构成了汽车的动力学约束。

3.3. Friction Circle Constraints

Given sufficient engine powers, it is well known that the traction power of the car produced by tires to drive the car is limited by frictions between tires and the road surface. The combination of lateral and longitudinal control forces that is able to be leveraged by cars should stay inside a friction circle to prevent slipping or car from running out of control, which is defined as below
在发动机功率足够的情况下，众所周知，轮胎对汽车产生的驱动汽车的牵引力受到轮胎与路面之间摩擦力的限制。能够被汽车利用的横向和纵向控制力的组合应该保持在一个摩擦圈内，以防止打滑或汽车失控，定义如下：

where µ is the coefficient of friction between the tires and the road surface. The longitudinal force upper boundary can be calculated according to the maximum longitudinal acceleration by $u^{\tau }\le m\cdot a_{max}^{\tau }$ . This is only a necessary condition but not a sufficient condition to limit decision variables within the physical limits such as the nominal power. Take a driving case along a straight line for example, the speed will constantly increases to infinity if a fixed longitudinal force acts on the car and the path is long enough. However, in reality, the max force that a plant system can provide is also limited by the nominal power of the engine. For most of the time, the actual power used by car systems is maintained below the nominal power P, shown as below,
式中µ为轮胎与路面的摩擦系数。根据最大纵向加速度$u^{\tau }\le m\cdot a_{max}^{\tau }$可计算出纵向力上界。这只是将决策变量限制在标称功率等物理范围内的必要条件，而不是充分条件。以直线行驶为例，如果有一个固定的纵向力作用在汽车上，并且路径足够长，速度就会不断增加到无穷大。然而，在现实中，一个机械系统所能提供的最大力也受到发动机标称功率的限制。在大多数情况下，汽车系统实际使用的功率保持在标称功率P以下，如下图所示:

which also means, if the nominal power is reached, the driving force that a car is able to provide will decrease when the speed increases. This constraint is obviously nonlinear and non-convex. This issue ignored by [3] was first pointed out by Zhu et al. [20], but they did not solve it and left it as future work. Here we provide our solution by adding an upper boundary constraint on speed profiles according to platform limits. It will prevent the speed from increasing without limits. Other constraints like path constraints, boundary condition constraints, and the smoothness objective will also restrict the upper boundary of speed profiles. By doing so, we partially address this issue without bringing in non-convexity to our problem formulation. Given these factors, the formal mathematical representation of friction circle constraints can be defined as below,
这也意味着，如果达到标称功率，汽车能够提供的驱动力将随着速度的增加而减少。这个约束显然是非线性和非凸的。这个被[3]忽略的问题是Zhu等人[20]首先指出的，但他们没有解决，将其作为以后的工作。在这里，我们通过根据水平限制在速度曲线上添加上边界约束来提供我们的解决方案。它将阻止速度无限制地增长。其他约束如路径约束、边界条件约束、平滑目标等也会限制速度曲线的上边界。通过这样做，我们在不引入非凸性的情况下部分地解决了这个问题。考虑到这些因素，摩擦圆约束的形式数学表示可以定义为:

3.4. Time Efficiency Objective

Different from the approach used in [11] that optimizes deviation between the planned speed and desired speed to ensuring time efficiency implicitly, we optimize the total traveling time along the fixed path from 0 to $s_f$directly like [2,3], which can be expressed as $J_T=T={\int }_0^{T}1dt$. Substitute the time variable t with arclength s and we get
与[11]通过优化规划速度与期望速度之间的偏差来隐式地保证时间效率的方法不同，我们直接像[2,3]那样对固定路径从0到$s_f$的总行驶时间进行优化，可以表示为$J_T=T={\int }_0^{T}1dt$。用弧长s代替时间变量t，($dt\ast \dot{f}=ds=>dt=\frac{1}{\dot{f}}ds$)我们得到

3.5. IoD Objective

In autonomous driving applications, users, a behavior planning module or a task planning module may assign a reference speed $v_r(s)$ profile for a car to track. It is not a strict constraint like max speed thresholds or speed limits on the road that cannot be exceeded. Thus we introduce the integral of deviations between the planned speed and desired speed over the path as a soft constraint to measure this kind of performance, expressed as follows,
在自动驾驶应用中，用户、行为规划模块或任务规划模块可以为汽车指定一个参考速度曲线$v_r(s)$来跟踪。它不是像最高速度阈值或道路上不能超过的速度限制那样严格的约束。因此，我们引入规划速度和期望速度在路径上的偏差积分作为衡量这种性能的软约束，表示如下:
Unlike Ref. [11] regarding it as the measurement of time efficiency, we call it the task soft constraint, which makes more sense according to the purpose it serves in the form of (13).
与文献[11]将其视为时间效率的度量不同，我们将其称为任务软约束，根据其服务目的(13)的形式更有意义。

3.6. Smoothness Objective

Direct tracking of a minimum-time speed profile will lead to joint vibrations and overshoot of the nominal torque or force limits of actuators [34,35]. When this happens in autonomous driving cars, it most likely results in bad ride experience and unstable driving behaviors. To ensure a smooth speed profile for better tracking performance, reducing wear of power train systems and guaranteeing the ride comfort at the same time, the smoothness of the trajectory needs to be considered. Since we assume a smooth and curvature-continuous path has been generated by a path planning module, we only consider the longitudinal jerk component of the trajectory. Formally speaking, jerk is the first derivative of acceleration in terms of time t, which also means the second derivative of velocity and the third derivative of position. According to (7) and (8), the jerk $J(s)$) of the speed profile can be calculated as follows,
直接跟踪最小时间速度曲线将导致连接振动和超过执行器的公称扭矩或力限制[34,35]。当这种情况发生在自动驾驶汽车上时，很可能会导致糟糕的驾驶体验和不稳定的驾驶行为。为了保证平稳的速度曲线以获得更好的跟踪性能，减少动力传动系统的磨损，同时保证乘坐舒适性，需要考虑轨迹的平滑性。由于我们假设路径规划模块已经生成了光滑且曲率连续的路径，因此我们只考虑轨迹的纵向jerk部分。形式上，jerk是加速度对时间t的一阶导数，即速度的二阶导数和位置的三阶导数。根据式(7)和式(8)，速度曲线的 jerk是关于s的函数： $J(s)$可计算为:

which is nonlinear and non-convex. In fact, various smoothness metrics, including jerk, have been proposed to quantify the motion smoothness in literature [36,37]. However, the jerk objective brings in non-linearity and non-convexity, which makes our problem hard to solve, a better measurement which covers all the aspects we care about and also with good mathematical properties should be selected for the sake of fast convergence rate and optimality. Therefore, we introduce a pseudo jerk ${\alpha }^{\prime }(s)$, which is the first derivative of acceleration with respect to the parameter arc-length s, to the problem to encourage smooth transitions between states. The smoothness objective is then defined as
$J(s)$它是非线性和非凸的。事实上，文献[36,37]中已经提出了各种平滑度指标，包括jerk来量化运动平滑度。但是，jerk目标带来了非线性和非凸性，使我们的问题很难解决，为了更快的收敛速度和最优性，需要选择一个更好的、涵盖我们所关心的所有方面的、具有良好数学性质的度量。因此，我们在问题中引入一个伪jerk： ${\alpha }^{\prime }(s)$，它是加速度对参数弧长$s$的一阶导数，以促进状态之间的平滑过渡。然后将平滑目标定义为

which is convex. By minimizing the variation of acceleration in terms of parameter s, a smooth acceleration profile is preferred. By integrating the smooth acceleration along s, the speed profile can be further smoothed.
它是凸的（为什么 $J(s)$是非凸的，而 ${\alpha }^{\prime }(s)$得$J_S$是凸的??? $J(s{)}^2$是凸的吗，怎么判断是凸的还是非凸的？？？）。通过最小化参数s中加速度的变化，一个平滑的加速度曲线是优选的。通过对平滑的加速度沿s积分，可以进一步平滑速度曲线。

[如何判断一个函数是否是凸函数：
1.一元函数f(x)，我们可以通过其二阶导数f′′(x) 的符号来判断。如果函数的二阶导数总是非负，即f′′(x)≥0 ，则f(x)是凸函数
2.对于多元函数f(X)，我们可以通过其Hessian矩阵（Hessian矩阵是由多元函数的二阶导数组成的方阵）的正定性来判断。如果Hessian矩阵是半正定矩阵，则是f(X)凸函数
————————————————
版权声明：本文为CSDN博主「三分奶茶七分糖丶」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。]
原文链接：https://blog.csdn.net/qq_40651017/article/details/105660299
[正定矩阵和半正定矩阵]

原文链接：https://zhuanlan.zhihu.com/p/81169491?ivk_sa=1024320u

3.7. Path Constraints

Path constraints can be defined as the following form,
路径约束可以定义为如下形式:

where s is arclength or time, x is the state of the system and u is the control variable. It restricts the range of values of states or controls, or the mixed one of both over the time or arc-length interval, or sub-interval of either for safety reasons or task requirements [38]. The rationales behind imposing these constraints in our problem are:
其中s为长度或时间，x为系统状态，u为控制变量。出于安全原因或任务要求，它限制状态或控制的取值范围，或在时间或弧长区间内两者的混合，或其中一种的子区间内[38]。在我们的问题中施加这些限制的基本原理是:

• Speed limits on certain segments of roads happen to be common driving scenarios in urban environments. The speed limits cannot be exceeded by autonomous driving systems, or the driving system will violate the traffic regulations and be fined. The restrictions may happen along the whole path or just segments of the path, which is a little different from an overall speed threshold constraint and the IoD objective.
  在城市环境中，某些路段的限速是常见的驾驶场景。自动驾驶系统不能超过限速，否则将违反交通规则并被罚款。这些限制可能发生在整个路径上，也可能只是路径的一部分，这与总体速度阈值约束和IoD目标略有不同。
• A high-level planning system (i.e., behavior planning system, task planning system) may provide the upper boundary or lower boundary of the speed profile to a speed planner to make it behave well or satisfy certain task requirements. A speed planner has to plan a speed profile that stays in the prescribed region or below the envelope.
  上层规划系统(即行为规划系统、任务规划系统)可以为速度规划器提供速度曲线的上边界或下边界，使其表现良好或满足一定的任务要求。速度规划器必须规划一个保持在规定区域或低于包络线的速度曲线。
  Both cases enforce hard constraints on speed profiles (state), which cannot be ensured by using soft constraints of speed deviation presented in [11] or the IoD constraint described by us. The residues in soft constraint form can be minimized by optimization, but how the state (velocity) approaches the reference is not determined. Overshooting or oscillation may occur around the reference during the optimization. However, a hard constraint like (16) is able to limit the “trace” of the system states strictly. More concisely, the specific constraints in our problem are expressed in the following form without involving control variables explicitly,
  这两种情况都对速度曲线(状态)实施了硬约束，而使用[11]中提出的速度偏差软约束或我们描述的IoD约束无法保证这一点。通过优化可以使软约束残差最小化，但不能确定状态(速度)如何接近参考点。在优化过程中，参考点周围可能出现超调或振荡。然而，像(16)这样的硬约束能够严格限制系统状态的“跟踪”。更简洁地说，我们问题中的具体约束在不显式地涉及控制变量的情况下以以下形式表示:
  
  where $\overline{\beta }$is the upper boundary of $\beta$ at s, $0\le s_m\le s_n\le s_f$ and $m<n$. Three typical path constraints shapes of $\overline{\beta }(s_i)$ are demonstrated in Figure 3.
  $\overline{\beta }$为$\beta$关于s的上界，$0\le s_m\le s_n\le s_f$ and $m<n$。图3展示了$\overline{\beta }(s_i)$ 的三种典型路径约束形状。
  

3.8. Boundary Condition Constraints

The boundary condition constraints specifically refer to the terminal constraints that can be generally represented by
关于终端约束的边界条件约束，一般可以表示为

where $x_f$ is terminal state variable and $u_f$ is the final control variable. More specifically, we impose the following constraint type,
其中$x_f$ 是终端状态变量，$u_f$ 是最终控制变量。更具体地说，我们施加以下约束类型，

With ${\underline{\alpha }}_{sf}\le {\overline{\alpha }}_{sf}$ and ${\underline{\beta }}_{sf}\le {\overline{\beta }}_{sf}$ , we can enforce either equality constraints (by “ = ”) or target set inequality constraints (by “ < ”) on the terminal state of the speed profile. These constraints involve two types of typical applications. One is the scenario that the car needs to fully stop in front of obstacle at a certain point on the path or at the end of the path. A zero speed and a zero acceleration at $s_f$ need to be guaranteed in this case. The other scenario occurs as a car tries to merge into an expressway from an entrance ramp, which needs to have the final speed fall in the speed limit range of the expressway. Other applications, such as keeping a fixed distance to the front car at the end of the path while matching the final speed with that of the front car can also be solved using this constraint in our framework. Such capacities are not present in [3,11]. If no strict boundary conditions on terminal states are required, the constraints can be deactivated by making ${\underline{\alpha }}_{sf}=-\mu g,{\underline{\beta }}_{sf}=0,{\overline{\alpha }}_{sf}=\mu g,{\overline{\beta }}_{sf}=v_{max}^2$.
当${\underline{\alpha }}_{sf}\le {\overline{\alpha }}_{sf}$ and ${\underline{\beta }}_{sf}\le {\overline{\beta }}_{sf}$ 时，我们可以在速度曲线的终端状态上执行等式约束(通过“=”)或目标集不等式约束(通过“<”)。这些约束涉及两种类型的典型应用程序。一种情况是，汽车需要在路径的某一点或路径的尽头完全停在障碍物前面。在这种情况下，需要保证在 $s_f$ 处的零速度和零加速度。另一种情况是，一辆汽车试图从入口匝道驶入高速公路，最终速度需要落在高速公路的限速范围内。其他应用，例如在路径末端与前车保持固定距离，同时将最终速度与前车的速度相匹配，也可以在我们的框架中使用该约束来解决。这种能力在[3,11]中不存在。如果终端态不需要严格的边界条件，则可以通过使${\underline{\alpha }}_{sf}=-\mu g,{\underline{\beta }}_{sf}=0,{\overline{\alpha }}_{sf}=\mu g,{\overline{\beta }}_{sf}=v_{max}^2$来消除约束。

3.9. Time Window Constraints

Time window constraints are represented as
时间窗约束表示为


The constraint ensures that if the car passes the station s i during the time window $W_t$, non-collision with other traffic participants is guaranteed. The time window,$W_t$ can be acquired efficiently from a collision detection algorithm such as [39] with predicted trajectories of traffic participants in the workspace-time space. This type of constraint is very useful for handling time-critical tasks such as dynamic obstacle avoidance at certain points, $s_i$, along the path, and for arriving at the destination within the given max time duration. If no time window information about dynamic obstacles is available, this constraint can be relaxed by setting $T_U=\infty$. In fact, there are three types of time windows when involving dynamic obstacles. Take the cross scenario without traffic lights in Figure 4a for example, the oncoming vehicles (C1 , C2 ) are approaching the cross with predicted or prescribed speed profiles. They will occupy the station O during the time interval $[t_1,t_2]$ and $[t_3,t_4]$ .
该约束保证了当车辆在时间窗口$W_t$内通过车站$s_i$时，保证不与其他交通参与者发生碰撞。时间窗口$W_t$可以通过碰撞检测算法(如[39])有效地获得，该算法可以预测交通参与者在工作时空空间中的轨迹。这种类型的约束对于处理时间关键型任务非常有用，例如在路径上的某些点(例如 $s_i$)上的动态避障，以及在给定的最大时长内到达目的地。如果没有动态障碍物的时间窗口信息，则可以通过设置$T_U=\infty$来放松该约束。实际上，当涉及到动态障碍物时，有三种类型的时间窗口。以图4a中没有红绿灯的十字路口场景为例，迎面而来的车辆(C1、C2)正以预测或规定的速度接近十字路口。他们将在$[t_1,t_2]和[t_3,t_4]$时间段内占用O点。
These infeasible time intervals divide the feasible time window to three different pieces that have distinct forms. As shown in Figure 4b, the $W_A^T$ only has an upper boundary (see the green bar), and the $W_B^T$ owns both lower and upper boundaries (see the pink bar), and the $W_C^T$ has a lower boundary and an unlimited upper bound (see the blue bar). The complete feasible time window is an union of $W_A^T$, $W_B^T$ and $W_C^T$ , which is non-convex since $W_A^T$ is convex and $W_A^T$ , $W_B^T$ are non-convex. Inposing the combined time window seems straight-forward to do but will lead to a non-convex optimization problem, which makes our optimization problem hard to solve. In practice, a decision making system can rank the feasible time windows according to risks, energy to consume, or physical limits of vehicles, then select the best one to pass to the speed planning.
这些不可行的时间间隔将可行的时间窗口划分为具有不同形式的三个不同部分。如图4b所示，$W_A^T$只有上边界(见绿色条形图)，$W_B^T$同时拥有下边界和上边界(见粉色条形图)，$W_C^T$有下边界和无限上界(见蓝色条形图)。完全可行时间窗是$W_A^T$、$W_B^T$和$W_C^T$ 的并集，它是非凸的，因为$W_A^T$是凸的，而$W_A^T$、$W_B^T$是非凸的。引入组合时间窗似乎很简单，但会导致一个非凸优化问题，这使得我们的优化问题难以解决。在实际应用中，决策系统可以根据车辆的风险、能量消耗或物理限制对可行的时间窗进行排序，然后选择最佳的时间窗通过速度规划。
For example, the black curve shows the previous solution without considering the oncoming vehicles. If the autonomous car does not regulate the speed, it will collide with the oncoming car C1 during [ t1 , t2 ] . By enforcing three different time windows constraints, three possible solution classes (green, red and blue curves in Figure 4b) are available. The time window size of the red curve class is very small. It means that it is very risky to go though this kind of time window. The blue curve class needs great control efforts to change the current state to satisfy the corresponding time window constraint. In the end, the green curve class becomes the best option since it needs minimum efforts to avoid the moving vehicles and has fairly low risks. By doing so, decision making can select a single time window constraint to enforce on the path with the help of other useful information. Hence, imposing a single time window that is convex becomes applicable while still keeping the problem in good structure. The type A time window expression $W_A^T$ is employed as the simplified and generalized convex time window constraint for the optimization shown as (20). For the time windows constraints like $W_B^T$ or $W_C^T$ , we can pick $T_U\in W_B^T$ or $T_U\in W_C^T$ as the upper boundary to form the (20). Then a big coefficient for the smoothness objective can be used to “stretch” the travel time, which pushes the arrival time $t_i$ at the station $s_i$ to the upper boundary $T_U$ . It is an indirect way to achieve the goal. The exact usage cases of this constraint can be found in Sections 5.5 and 6.2.
例如，黑色曲线表示没有考虑迎面而来的车辆的前一个解决方案。如果自动驾驶汽车不进行速度调节，在[t1, t2]期间，自动驾驶汽车将与迎面驶来的汽车C1发生碰撞。通过执行三个不同的时间窗口约束，可以使用三种可能的解决方案类(图4b中的绿色、红色和蓝色曲线)。红色曲线类的时间窗非常小。这意味着通过这种时间窗口是非常危险的。蓝色曲线类需要大量的控制工作来改变当前状态以满足相应的时间窗口约束。最后，绿色曲线类别成为最佳选择，因为它需要最小的努力来避开移动车辆，并且风险相当低。通过这样做，决策制定可以在其他有用信息的帮助下选择单个时间窗口约束来强制执行路径。因此，施加一个凸的单一时间窗口变得适用，同时仍然保持问题的良好结构。采用A型时间窗表达式$W_A^T$作为优化的简化广义凸时间窗约束，如公式(20)所示。对于$W_B^T$或$W_C^T$这样的时间窗约束，我们可以选择$T_U\in W_B^T$或$T_U\in W_C^T$作为上边界，根据(20)。然后利用平滑目标的大系数对行程时间进行“拉伸”，将到达位置$s_i$的时间$t_i$推至上边界$T_U$。这是实现目标的一种间接方式。这个约束的确切用例可以在第5.5节和第6.2节中找到。

3.10. Comfort Box Constraints

The comfort box constraint as another requirement of the ride comfort other than the smoothness, appears in a threshold form in the literature [7,9,11],
舒适区间约束作为平顺性之外的另一个平顺性要求，在文献[7,9,11]中以阈值形式出现。

which is a hard constraint. The a τ c is the threshold for the longitudinal accelerations and decelerations. η The a c is the threshold for lateral accelerations. This box form of constraints ensures comfort at the cost of mobility. The mobility may dramatically drop if the comfort acceleration thresholds are set too conservatively. The feasible region for optimization is limited within a rectangle inside the friction circle if (21) is present, as shown in Figure 1. However, when an emergency occurs, the planner may have to violate the comfort constraint to leverage more mobility of the car to generate a safe speed profile by ignoring the comfort constraint temporally instead of failing by satisfying it. With a hard constraint presented in the problem, there is no way to reach this goal. Thus we employ a penalty method with slack variables to soften the comfort box constraint [40,41], which makes it a “semi-hard” constraint. If the original optimization problem was
这是一个硬性约束。$a_c^{\tau }$是纵向加速和减速的阈值。$a_c^{\eta }$是横向加速度的阈值。这种区间形式的约束以牺牲机动性为代价确保了舒适性。如果舒适性加速度阈值设置过于保守，移动性可能会急剧下降。当(21)存在时，优化可行区域被限制在摩擦圆内的一个矩形内，如图1所示。然而，当紧急情况发生时，规划器可能不得不违反舒适性约束，通过暂时忽略舒适性约束而不是满足舒适性约束，来利用汽车的更多机动性来产生安全的速度曲线。由于问题中存在硬约束，因此无法达到此目标。因此，我们采用带有松弛变量的惩罚方法来使舒适区间变成软约束[40,41]，使其成为“半硬”约束。如果最初的优化问题是

an equivalent optimization problem using slack variables can be acquired as
利用松弛变量的等效优化问题可得到

where σ is the slack variable that represent the constraint violations, λ is the corresponding weight. When σ = 0, the constraint is satisfied as a hard one. By doing so, we conserve the freedoms to explore full mobility of cars and capacity of breaking the comfort box constraint to recover the feasibility when necessary. The exact expression of the semi-hard constraint is shown in (24).
其中σ是表示约束违反的松弛变量，λ是相应的权值。当σ = 0时，满足硬性约束。通过这样做，我们保留了探索汽车充分机动性的自由，以及在必要时打破舒适区间约束以恢复可行性的能力。半硬约束的精确表达式如(24)所示。

3.11. Overall Convex Optimization Problem Formulation

Finally, the complete speed planning optimization problem over the fixed path is posed, which incorporates the full set of constraints presented above as,
最后，提出了固定路径上的完整的速度规划优化问题，该问题包含了上述全部约束条件:

where ${\dot{r}}^2(s)=(r^{\prime }(s){)}^2\beta (s)$ and $\ddot{r}(s)=r^{\prime }\alpha (s)+r^{\prime \prime }\beta (s)$ . Please note that $\alpha (s),\beta (s),u(s),{\sigma }^{\tau }(s),{\sigma }^{\eta }(s)$ are the decision variables to optimize. The parameters ${\omega }_1,{\omega }_2,{\omega }_3,{\lambda }_1,{\lambda }_2\in R_{+}$ are fixed in advance to suit the particular application objectives. When parameters ${\lambda }_1,{\lambda }_2$ are both set to zeros, the ${\sigma }^{\tau }(s),{\sigma }^{\eta }(s)$ are degenerated to constants zeros and $a_c^{\tau }$、$a_c^{\eta }$ are set to infinity, which means the comfort box constraint is relaxed. The problem formulation we presented can be demonstrated to be convex as follows. For these readers who are not familiar with convex optimization, we refer them to [40,42] for details.
式中， ${\dot{r}}^2(s)=(r^{\prime }(s){)}^2\beta (s)$，$\ddot{r}(s)=r^{\prime }\alpha (s)+r^{\prime \prime }\beta (s)$。请注意，$\alpha (s),\beta (s),u(s),{\sigma }^{\tau }(s),{\sigma }^{\eta }(s)$是要优化的决策变量。参数 ${\omega }_1,{\omega }_2,{\omega }_3,{\lambda }_1,{\lambda }_2\in R_{+}$ 是预先设定的，以适应特定的应用目标。当参数 ${\lambda }_1,{\lambda }_2$ 均为0时，${\sigma }^{\tau }(s),{\sigma }^{\eta }(s)$退化为0常数，$a_c^{\tau }$、$a_c^{\eta }$为无穷大，即舒适区间约束是松弛。我们提出的问题公式可以证明是凸的，如下所示。对于那些不熟悉凸优化的读者，我们参考[40,42]了解详细信息。

• For the objectives, $J_T$ is an integral of a negative power function and is therefore convex. $J_S$ is an integral of a squared power of absolute value and is therefore convex. $J_V$ is an integral of an identity power of absolute value and is therefore convex. So are $||{\sigma }^{\tau }||$ and $||{\sigma }^{\eta }||$ . As ${\omega }_1,{\omega }_2,{\omega }_3,{\lambda }_1,{\lambda }_2$ are all nonnegative, $J$ as a nonnegative weighted sum of convex functions, is convex.
  对于目标，$J_T$是一个负幂函数的积分，因此是凸的。$J_S$是绝对值的平方次方的积分，因此是凸的。$J_V$是一个绝对值的单位幂的积分，因此是凸的。 $||{\sigma }^{\tau }||$和$||{\sigma }^{\eta }||$也是。由于 ${\omega }_1,{\omega }_2,{\omega }_3,{\lambda }_1,{\lambda }_2$都是非负的，$J$作为凸函数的非负加权和，是凸的。
• For (6), the dynamics equality constraint is affine in $\alpha ,\beta ,u$ and is therefore convex. For equality constraints about decision variables (8), since the derivative is a linear operator, the relation between α and β is convex. For the inequality path constraint (17), $\beta (s_i)$ is a sublevel set of convex set in the interval $[s_m,s_n]$ and is thus convex. The equality and inequality constraints about boundary conditions (19) are linear constraints, thus convex. As the $T_i$ is an integral of a negative power function, therefore convex and $T_U$ is a fixed upper boundary, the time window inequality constraint (20) is a convex constraint.
  对于(6)，动力学等式约束在 $\alpha ,\beta ,u$ 中是仿射的，因此是凸的。对于决策变量(8)的等式约束，由于导数是一个线性算子，因此α和β之间的关系是凸的。对于不等式路径约束(17)，$\beta (s_i)$是区间$[s_m,s_n]$内凸集的子水平集，因此是凸的。关于边界条件(19)的等式和不等式约束是线性约束，因此是凸的。由于$T_i$ 是一个负幂函数的积分，因此是凸的，$T_U$是一个固定的上界，所以时间窗不等式约束(20)是一个凸约束。
• For the convex set constraint about the friction circle (11), the norm of u is convex, upper bounds are fixed and $v_{max}^2$ is fixed, so the control set constraint is the intersection of three convex sets and is therefore convex.
  对于关于摩擦圆(11)的凸集约束，u的范数是凸的，上界是固定的，$v_{max}^2$ 是固定的，因此控制集约束是三个凸集的交集，因此是凸的。
• The comfort box constraints with slack variables ${\sigma }^{\tau }$ and ${\sigma }^{\eta }$ are second-order cone constraints and convex.
  具有松弛变量${\sigma }^{\tau }$和${\sigma }^{\eta }$的舒适区间约束为二阶锥约束和凸的。
  Since the objectives are convex, equality constraints are affine and inequality constraints are convex, this optimization problem is convex [40]. The speed planning problem as stated is therefore an infinite-dimensional convex optimization problem.
  由于目标是凸的，等式约束是仿射的，不等式约束是凸的，所以这个优化问题是凸的[40]。因此，所述的速度规划问题是一个无限维凸优化问题。

4. Implementation

To solve the speed planning problem, we discretize the objectives, constraints and decision variables to form a finite dimensional approximated version of the original problem, which is known as direct transcriptions in optimal control. We consider N = 200 segments along the path, thus N + 1 discretised points for all these numerical experiments in Section 5. For one segment of the path, we assume constant acceleration, which is also used in [2,3]. According to (8), β ( s ) can be expressed as,
为了解决速度规划问题，我们将目标、约束和决策变量离散化，形成原始问题的有限维近似版本，这被称为最优控制中的直接转换。我们考虑沿路径有N = 200个片段，因此第5节中所有这些数值实验有N + 1个离散点。对于路径的一段，我们假设加速度恒定，这在[2,3]中也使用过。根据式(8)，β(s)可表示为:
It should be noted that a zero speed constraint will result in an infeasible optimization problem. In practice, two methods can be employed to avoid the singularity. The first one is pruning the path after the station where the speed is zero since the zero speed point is the switch point of the system. The speed of the pruned part of the path is set to zero or the pruned part of the path can be another speed planning problem with a zero start speed. The second method is to use a small speed value to approximate the zero. In this way, we can still evaluate the objectives, perform the optimization and get a solution. When speeds fall below a certain value (i.e., 0.02 m/s) in the solution, we can treat them as the zero speeds.
需要注意的是，零速度约束将导致不可行的优化问题。在实际应用中，可以采用两种方法来避免奇异性。第一个是在车速为零的位置之后修剪路径，因为零车速点是系统的开关点（可以理解为只能前进不能后退）。路径的修剪部分的速度设置为零，或者路径的修剪部分可以是另一个速度规划问题，其启动速度为零。第二种方法是使用一个小的速度值来接近零。这样，我们仍然可以评估目标，进行优化并得到解决方案。当求解中的速度低于某一值(即0.02 m/s)时，我们可以将其视为零速度。

4.1. Discretization of $J_T,J_S$ , and $J_V$

Substituting $\beta (s{)}^{-\frac{1}{2}}$ into (25) yields,
将$\beta (s{)}^{-\frac{1}{2}}$代入(25)，

(26)的推导如下：

where $∆s=s_{i+1}-s_i$ is a fixed arclength increment.
式中，$∆s=s_{i+1}-s_i$为固定弧长增量。
This integral can be approximated in the following form,
这个积分可以近似地表示为，

For the smoothness term, we use finite differences to approximate ${\alpha }^{\prime }(s)$ , which yields
对于平滑项，我们使用有限差分来近似${\alpha }^{\prime }(s)$，得到

The $J_V$ can be directly represented by
$J_V$可以直接表示为

4.2. Discretization of $r^{\prime }(s)$ and $r^{\prime \prime }(s)$

The discrete form representations of constraints are straight-forward to define, with the exception of the dynamics constraint (6), which involves first and second order derivatives of $r(s)$ with respect to the arclength s. We use finite differences to approximate $r^{\prime }(s)$ ,
约束的离散形式表示是直接定义的，除了动力学约束(6)，它涉及 $r(s)$对弧长s的一阶和二阶导数。我们使用有限差分来近似$r^{\prime }(s)$。

and a fourth-order Range-Kutta formula to approximate $r^{\prime \prime }(s)$ ,
和一个四阶Range-Kutta公式来近似$r^{\prime \prime }(s)$，

We model our problem using Convex.jl [43] , a convex optimization modeling framework in Julia, and solve it using a second-order cone programming solver from Gurobi [44].
我们使用Convex.jl [43]对问题进行建模。 Julia中的凸优化建模框架，并使用来自Gurobi[44]的二阶锥规划求解器进行求解。

5. Numerical Results

To evaluate the performance and capabilities of the proposed speed planning model, we use a curvy example path from [3], as shown in Figure 5, to conduct various challenging speed planning numerical experiments. To be fair, we implemented both our problem formulation and MTSOS in [3] in Julia [45] running on a PC with an Intel Xeon E3 processor at 2.8 GHz and 8 GB RAM in a Linux system and then compared our results with theirs to show the improvements and new capacities.
为了评估所提出的速度规划模型的性能和能力，我们使用了[3]中的曲线路径示例，如图5所示，进行了各种具有挑战性的速度规划数值实验。公平地说，我们在Julia[3][45]中分别实现了我们的问题公式和MTSOS（Minimum-Timed Speed Optimization Subroutines）的，并在Linux系统中运行在具有2.8GHz的英特尔至强E3处理器和8GB RAM的PC上，然后将我们的结果与他们的结果进行比较，以显示改进和新的能力。

The used parameters are listed in Table 3. As they are a proof of concept experiment, these parameters do not match those of the real platforms. However, it does show the capacities of the speed planner from functional aspects. We will demonstrate the case studies using parameters from real platforms and dealing with real on-road driving scenarios in the next section.
使用的参数如表3所示。由于它们是一个概念验证实验，这些参数与实际平台的参数不匹配。但是，它确实从功能方面显示了速度规划器的能力。在下一节中，我们将使用真实平台的参数演示案例研究，并处理真实的道路驾驶场景。
As the friction circle constraint is the essence of the safety regarding vehicle dynamics, we enabled it for all the experiments below. We first run the MTSOS algorithm on the example path to generate the speed profile, accelerations and their distribution within the normalized friction circle as the baseline to compare with.
由于摩擦圈约束是车辆动力学安全的本质，所以我们在下面的所有实验中都启用了摩擦圈约束。我们首先在示例路径上运行MTSOS算法，生成速度曲线，加速度及其在归一化摩擦圆内的分布来作为基准进行比较。

5.1. Smoothness

In this case, we show how the smoothness constraint of the our formulation affects the results and improve the performance. The initial speed $\sqrt{\beta (0)}$ of the car is a fixed point and assigned according to the current vehicle state in the optimization. In this case, we set the initial speed $\sqrt{\beta (0)}$ to 0m/s and enable only friction circle constraint, time efficiency objective, smoothness objective by setting the parameters to
在这种情况下，我们展示了我们的公式的平滑约束如何影响结果并提高性能。小车的初始速度$\sqrt{\beta (0)}$ 是一个固定点，是根据车辆的当前状态赋值的。在这种情况下，我们将初始速度$\sqrt{\beta (0)}$设置为0m/s，并通过仅启用摩擦圈约束，时间效率目标，平滑目标将参数设置为:

The other constraints are all relaxed or ignored to remove side effects and highlight the effects of the smoothness objective term. The black curve presented in Figure 6 represents the speed profile generated by MTSOS [3] with only time efficiency objective and friction circles constraints. The colored curves depict our results using different coefficients for the smoothness objective. Multiple cusps are observed in the MTSOS’s result, which definitely increases the difficulty of tracking such a speed profile for controllers. Overshooting and oscillation may happen when tracking a non-smooth speed profile such as the black one. Instead, our method generates way more smooth speed profile without cusps while still keeping time efficiency in mind.
其他约束都被放宽或忽略，以减少其作用并突出平滑目标项的效果。图6所示的黑色曲线为仅以时间效率目标和摩擦圈约束的MTSOS[3]方法生成的速度曲线。彩色曲线描述了我们使用不同系数的平滑目标的结果。在MTSOS的结果中观察到多个顶点，这无疑增加了控制器跟踪这种速度曲线的难度。超调和振荡可能发生在跟踪一个非平滑的速度曲线，如黑色的。相反，我们的方法生成了更平滑的速度曲线，没有尖峰，同时仍然保持时间效率。
With small coefficients for smoothness, the resulting speed profiles tend to stay close to the most time-efficient speed profile (the black one) while still maintaining high order continuity. As coefficients of smoothness increase, flatter slopes of speed profiles are encouraged, thus smoother speed profiles are generated. With this structure in hand, our method offers a way to balance the time efficiency performance and smoothness performance according to specific application requirements when necessary. We also demonstrated control efforts distribution of MTSOS, ours with ω2 = 0.0002, ω2 = 0.002, and ω2 = 0.02 using a normalized friction circle (“g-g” diagram [46,47]), as seen in Figure 7. Since the MTSOS only considers the time efficiency, most of their acceleration points tend to stay close to the limits of accelerations. Ours, with the increase of smoothness coefficients, tend to lie around the center of the friction circle and reach the limits when necessary, which leads to a gentler control sequence. None of [3,4,8,10,48] show such high quality results as ours by taking both smoothness and time efficiency into consideration.
在平滑系数较小的情况下，得到的速度曲线趋向于接近最省时的速度曲线(黑色曲线)，同时仍然保持高阶连续性。随着平滑系数的增加，速度曲线的斜率会变得更平坦，从而产生更平滑的速度曲线。有了这种结构，我们的方法提供了一种在必要时根据特定应用需求平衡时间效率性能和平滑性能的方法。我们还展示了MTSOS的控制力度分布，我们的ω2 = 0.0002， ω2 = 0.002， ω2 = 0.02使用归一化的摩擦圆(“g-g”图[46,47])，如图7所示。由于MTSOS只考虑时间效率，它们的加速度点大多趋向于接近加速度极限。随着平滑系数的增加，趋于靠近摩擦圆的中心，并在必要时达到极限，从而导致更温和的控制序列。在同时考虑平滑性和时间效率的情况下，没有一个[3,4,8,10,48]的结果能像我们的那样高质量。

5.2. Boundary Condition Constraint

To demonstrate the capacity of boundary condition constraints, we carried out two set of experiments. In the first set of experiments, we compared the results with the following setting,
为了证明边界条件约束的能力，我们进行了两组实验。在第一组实验中，我们将结果与以下设置进行了比较，
The case A, B in Figure 8 showed that our method is able to satisfy the final speed boundary condition while optimizing time efficiency (A) with a sharp slow-down slope or optimizing time efficiency and smoothness at the same time (B) with a flatter slow-down slope at the end. We conducted the second set of experiments with both time efficiency and smoothness objectives considered using same coefficients but with different type of boundary conditions,
图8中的案例A、B表明，我们的方法能够满足最终速度边界条件，同时优化时间效率(A)，最终减速斜率较大，或者同时优化时间效率和平滑度(B)，最终减速斜率较平缓。我们进行了第二组实验，同时考虑了时间效率和平滑目标，使用相同的系数，但使用了不同类型的边界条件。
Without limiting the final speed, a speed profile such as E is generated, which is the optimal shape under the given objectives. By adding an equality constraint (D) and an inequality constraint © to the final speed, we observed notable differences of the last portion of the speed profile among these results. The last segments of the speed profile are adapted by the optimization to satisfy the given constraints. The other parts almost stay the same for case C, D, E due to global optimality. A similar phenomenon is observed between the results of MTSOS and case A in Figure 8. Only the part that needs to be adjusted is regulated. This is an appealing feature for speed tracking regarding temporal consistency of references and control stability. Since time efficiency is one of the objectives, it makes sense that the final speed of the case C reached the upper boundary at the end when given a feasible range.
在不限制最终速度的情况下，生成的速度曲线如E，是给定目标下的最优形状。通过在最终速度中加入等式约束(D)和不等式约束 (C )，我们观察到这些结果中最后部分的速度分布存在显著差异。最后对速度曲线的最后段进行优化，使其满足给定的约束条件。由于全局最优性，C D E的其他部分几乎保持不变。在图8中，MTSOS的结果与案例A之间也观察到类似的现象。只调整了需要调整的部分。对于速度跟踪来说，这是一个关于参考的时间一致性和控制稳定性的吸引人的特性。由于时间效率是目标之一，所以当给定可行范围时，情况C的最终速度达到上边界是可以理解的。
Neither MTSOS [3] nor [11] can deal with this case due to the lack of corresponding constraints. Adding a similar constraint to the MTSOS requires re-arrangement of the problem and non-trivial, error-prone changes to their customized solver. Regarding the final speed constraint as a soft one like [11] cannot guarantee where and when the constraint is satisfied. Instead, our formulation and framework overcome above flaws.
由于缺乏相应的约束，MTSOS[3]和[11]都无法处理这种情况。向MTSOS添加类似的约束需要重新整理问题，并对自定义求解器进行重要的、容易出错的更改。将最终速度约束视为软约束[11]，并不能保证在何时何地满足约束。相反，我们的表述和框架克服了上述缺陷。

5.3. Path Constraint

In this part, to show effects of path constraints, we conducted experiments with the friction circle constraint, time efficiency and smoothness objectives by ${\omega }_1=1,{\omega }_2=0.005,{\omega }_3=0$. For the sake of clarity, all the other hard constraints except path constraints are relaxed or ignored. For reference, a speed profile without any path constraint is generated using the given parameters (see the black curve in Figure 9), which can be thought of as the original speed profile before imposing the path constraints. Then we enforced three types of path constraints to show the capacity of our method,
在这一部分中，为了显示路径约束的影响，我们以摩擦圆约束、时间效率和平滑度为目标，分别以${\omega }_1=1、{\omega }_2=0.005、{\omega }_3=0$进行实验。为了清晰起见，除了路径约束外，所有其他硬约束都被放宽或忽略。作为参考，使用给定参数生成没有任何路径约束的速度曲线(见图9中的黑色曲线)，可以将其视为施加路径约束之前的原始速度曲线。然后，我们实施了三种类型的路径约束来展示我们的方法的能力，

• straight line shape (A in Figure 9)
  直线形状(图9中的A)
• rectangle shape (B in Figure 9)
  矩形(图9中的B)
• serrated shape (C in Figure 9)
  锯齿形(图9中的C)
  as seen in Figure 9. The corresponding speed planning result is tagged using the same color with that of the path constraint. As shown in Figure 9, the original speed profile was deformed by optimization according to path constraints and all the resulting speed profiles stayed below the corresponding path constraints strictly while still keeping smooth. This provides a powerful tool for users to customize the speed profiles according to their needs while guaranteeing high quality of solutions.
  如图9所示。相应的速度规划结果使用与路径约束相同的颜色进行标记。如图9所示，根据路径约束对原速度曲线进行优化变形，得到的所有速度曲线都严格低于相应的路径约束，同时仍保持平滑。这为用户提供了一个强大的工具，可以根据他们的需求定制速度配置文件，同时保证高质量的求解结果。
  

5.4. IoD Task Constraints

We evaluated effects of IoD task constraints using two different desired speed profiles (the dash-dot line A and the dash-dash line B in Figure 10) to show the behaviors of our planner. We first ran the MTSOS planner to generate the upper boundary of the speed profile for reference. For the desired speed profile A in Figure 10, we consider the time efficiency objective and IoD objective only by ${\omega }_1=1，{\omega }_2=0，{\omega }_3=10$ and relaxed all the other constraints to generate the speed profile, shown as the orange curve in Figure 10. The orange curve aligned well with the desired speed profile except for the part that the desired speed exceeds the limit of the friction circle. For the exceeding part, the orange curve stayed as close as possible to the desired speed but limited by the speed upper boundary constrained by the friction circle. This result uncovers the strong safety feature of our method. Moreover, taking the smoothness objective into consideration by making ${\omega }_2=0.1$, the quality of the speed profile is further improved (see the green curve in Figure 10). We also tested the IoD constraint against the totally feasible desired speed profile B using the same parameters setting with the previous experiment. The blue curve in Figure 10 depicted the planning result without considering smoothness. The resulting speed almost perfectly aligned with desired speed B. Similarly, the quality of the speed profile was significantly improved by add the smoothness objective (see light red curve in Figure 10).
我们使用两种不同的期望速度配置文件(图10中的虚线A和虚线B)来评估IoD任务约束的影响，以显示我们的规划器的行为。我们首先运行MTSOS规划器来生成速度曲线的上边界以供参考。对于图10中期望的速度曲线A，通过设置${\omega }_1=1，{\omega }_2=0，{\omega }_3=10$，我们仅考虑时间效率目标和IoD目标，并放宽所有其他约束，生成速度曲线，如图10中的橙色曲线所示。橙色曲线除了期望速度超出摩擦圈极限的部分外，与期望速度曲线对齐良好。对于超出部分，橙色曲线尽可能接近期望速度，但受到摩擦圆约束的速度上限的限制。这一结果揭示了我们的方法具有很强的安全性。此外，考虑到平滑目标，使${\omega }_2=0.1$，进一步提高了速度曲线的质量(见图10中的绿色曲线)。我们还使用与之前实验相同的参数设置，针对完全可行的期望速度曲线B测试了IoD约束。图10中的蓝色曲线描述了不考虑平滑性的规划结果。得到的速度几乎与期望的速度b完全一致。同样，通过添加平滑目标，速度曲线的质量得到了显著提高(见图10中的浅红色曲线)。

5.5. Time Window Constraint

To reveal how the time window constraint affects the speed planning in our method, we first generate a baseline speed profile by considering only time efficiency and smoothness objectives with the following parameter setting, ${\omega }_1=1，{\omega }_2=0.5，{\omega }_3=0，\beta (s_f)=0$. All the other hard constraints are relaxed. The result without time window constraints is shown as a blue curve in Figure 11. With a large coefficient for smoothness, the travel time at the end of the path reached 6.626 s. Please note that the time window constraint in (20) can be enforced on any point along the path. For simplicity, we picked the $s_f$ point as the place where imposing the constraint. We added the time window constraint by limiting the arriving time $T(s_f)$ at the end of the path to $(0,T_U]$ , where the $T_U=5$ for case 1 and $T_U=4$ for case 2 and solved them with respect to these constraints. The resulting speed profiles were shown as green and red curves for case 1 and case 2 in Figure 11, respectively. The travel time at $s_f$ are listed in Table 4 and both time constraints were satisfied according to the data. The original speed profile (blue one) were regulated to meet the time window requirements. The resulting speed profile was clearly above the original speed profile. This is a powerful tool that makes us able to control the time arriving at a certain point of the path by using a large coefficient for smoothness then enforcing the time window constraint to compress the travel time below the upper boundary of the given time window. In this way, we can easily “stretch” or “compress” the travel time for a fixed path. An example of “stretching” the travel time can be found in Section 6.2 case study.
为了揭示时间窗约束如何影响我们方法中的速度规划，我们首先通过仅考虑时间效率和平滑目标生成基准速度曲线，参数设置为${\omega }_1=1，{\omega }_2=0.5，{\omega }_3=0，\beta (s_f)=0$。所有其他的硬约束都被放宽了。没有时间窗口约束的结果如图11中的蓝色曲线所示。在光滑系数较大的情况下，到达路径末端的行驶时间为6.626 s。请注意，(20)中的时间窗口约束可以在路径上的任何点上执行。为了简单起见，我们选择$s_f$点作为施加约束的地方。我们通过限制到达路径末端的时间$T(s_f)$为$(0,T_U]$来增加时间窗约束，其中情况1的$T_U=5$秒，情况2的$T_U=4$秒，并根据这些约束进行求解。图11中案例1和案例2的结果速度曲线分别显示为绿色和红色曲线。到达$s_f$处的时间如表4所示，从数据上看，两个时间约束都满足。原来的速度曲线(蓝色的)被调整，以满足时间窗口的要求。由此产生的速度曲线明显高于原始速度曲线。这是一个强大的工具，使我们能够通过使用较大的平滑系数来控制到达路径某一点的时间，然后强制时间窗口约束将行驶时间压缩到给定时间窗口的上边界以下。这样，我们就可以很容易地“拉伸”或“压缩”固定路径的行驶时间。“延长”时间时间的例子可以在6.2节的案例研究中找到。

5.6. Semi-Hard Comfort Box Constraint

To show the capacity of the semi-hard comfort box constraint, we conducted experiments with the following four different configurations,
为了展示舒适区间半硬约束的能力，我们进行了以下四种不同配置的实验:
The comfort acceleration thresholds $a^{\tau }$ and $a^{\eta }$ are listed in Table 3. For case A, we only took the time efficiency objective into account and enable the comfort box constraints. The light blue curve in Figure 12 shows the resulting speed profile and the black dots in Figure 13 depict the resulting acceleration points distribution. Due to the presence of the time efficiency objective and limits of semi-hard comfort box constraints, most of the acceleration points tend to stay on the edge of box to achieve minimum travel time under such constraints.For case B, we add the smoothness objective in based on case A. The resulting speed profile is shown as the green curve in Figure 12, which is smoother than previous one. The rationale behind this is that the smoothness term encourages gentle control efforts to keep smooth transitions between states. Thus the acceleration points of case B more focused around the center of the friction circle while still staying inside of the box, shown as green dots in Figure 13.
舒适加速度阈值$a^{\tau }$和$a^{\eta }$列于表3中。对于案例A，我们只考虑了时间效率目标并启用了舒适区间约束。图12中的浅蓝色曲线显示了得到的速度曲线，图13中的黑点表示得到的加速度点分布。由于时间效率目标的存在和半硬舒适区间约束的限制，在此约束下，大多数加速度点倾向于停留在区间的边缘，以实现最小的行驶时间。对于案例B，我们在案例A的基础上添加了平滑目标，得到的速度曲线如图12中的绿色曲线所示，比之前的曲线更平滑。这背后的基本原理是平滑项鼓励温和的控制过程，以保持状态之间的平滑过渡。因此，情况B的加速度点更集中在摩擦圆中心周围，但仍留在框内，如图13中绿色点所示。
To demonstrate the “semi-hard” feature of our formulation, we imposes a time window constraint by making the final arriving time $t_{s_f}\le 3.5s$. With this constraint, the mobility constrained by the box region is no longer enough to achieve the required time efficiency. To get a solution that satisfies the time window constraint, the optimization has to exploit the region that is within the friction circle but outside of the box. The results of the acceleration points distribution of case 3 (see cyan pentagons in Figure 13) and case 4 (see pink pluses in Figure 13) proved our statements. The acceleration points were no longer limited within the box region. The corresponding speed curves were shown as the light red curve for case 3 and blue curve for case 4 in Figure 12. This nice feature distinguishes our method from existing speed planning methods such as [7,9,11] that regard comfort box constraints as hard ones like (21).
为了证明我们公式的“半硬”特征，我们施加了一个时间窗约束，使最终到达时间$t_{s_f}\le 3.5s$。在此约束下，受区间约束的机动性不再足以达到所需的时间效率。为了得到满足时间窗约束的解，优化必须利用摩擦圈框外的区域。案例3(见图13中的青色五边形)和案例4(见图13中的粉色加号)的加速度点分布结果证明了我们的说法。加速度点不再局限于区域内。相应的速度曲线如图12中案例3的浅红色曲线和案例4的蓝色曲线所示。这个很好的特征将我们的方法与现有的速度规划方法(如[7,9,11])区分开来，这些方法将舒适区间约束视为硬约束(如[21])。
Their methods guaranteed the ride comfort at the expense of losing potential mobility. Limiting accelerations to the comfort box region dramatically reduces the solution space of the speed planning problem, which may lead to no solution when one does exist in certain situation. Our method, instead, turns the comfort constraint to a semi-hard constraint by leveraging penalty functions and slack variables. More precisely, when the region limited by the box constraint is able to provide the needed mobility to satisfy other hard constraints, the slack variables are reduced to zero and the penalty functions have no effects on the optimization. The comfort box constraint is equivalent to a hard constraint.
他们的方法以失去潜在的机动性为代价保证了乘坐的舒适性。将加速度限制在舒适区域大大减小了速度规划问题的求解空间，在某些情况下，即使存在，也可能导致无解。相反，我们的方法通过利用惩罚函数和松弛变量，将舒适约束转变为半硬约束。更准确地说，当区间约束限制的范围能够满足其他硬约束所需的机动性时，松弛变量降为0，惩罚函数对优化没有影响。舒适区间约束等价于硬约束。
However, when the mobility provided by the box region is not enough to satisfy other hard constraints, slack variables increase and the penalty functions penalize the constraints violation. The comfort box constraint then is transferred to a soft constraint. By doing so, our method gives priority to the solution space in box region and leverages the outside region when necessary, which emphasizes comfort while keeping the solution space complete. To the best of our knowledge, none of the existing speed planning methods for autonomous driving has done this.
然而，当区间提供的机动性不足以满足其他硬约束时，松弛变量增加，惩罚函数对违反约束进行惩罚。然后将舒适区间约束转换为软约束。这样，我们的方法优先考虑区间内的解空间，必要时利用区间外的解空间，在保证解空间完整的同时注重了舒适性。据我们所知，现有的自动驾驶速度规划方法都没有做到这一点。

6. Case Study

In this section, we demonstrate three case studies to show how to combine constraints we present to solve distinct sets of speed planning problems raised in different real autonomous driving scenarios with parameters from the real platform like a Lincoln MKZ.
在本节中，我们将展示三个案例研究，以展示如何将我们提出的约束结合起来，以解决不同真实自动驾驶场景中提出的不同速度规划问题，并使用林肯MKZ等真实平台的参数。

6.1. Speed Planning for Safe Stop

First, we considered a cornering scenario (see Figure 14) with different entry speeds. At the end of the road, a static obstacle blocks the road and the car must stop safely in front of the obstacle. The comfort box constraints parameters used in this experiment are listed in Table 5. First, we perform speed planning that considers the time efficiency, smoothness objectives, friction circle and final speed constraints by making ${\omega }_1=1，{\omega }_2=5，\beta (s_f)=0$. The initial speed of the car is $v_{init}=6m/s$. The semi-hard comfort box constraints were not taken into consideration in this one. The corresponding results are shown in Figures 15 and 16 in black color. The second experiment was carried out using the same parameters. In addition, the semi-hard comfort box constraints were added by setting ${\lambda }_1=10$ and ${\lambda }_2=10$. The corresponding results are shown in green color.
首先，我们考虑了一个具有不同入口速度的转弯场景(参见图14)。在路的尽头，一个静态障碍物挡住了道路，汽车必须在障碍物前安全停车。本实验使用的舒适区间约束参数如表5所示。首先，我们通过使${\omega }_1=1，{\omega }_2=5，\beta (s_f)=0$进行速度规划，考虑时间效率，平滑目标，摩擦圈和最终速度约束。汽车的初始速度为$v_{init}=6m/s$。本文未考虑半硬舒适区间约束。相应的结果在图15和16中以黑色显示。第二次实验使用相同的参数进行。此外，通过设置${\lambda }_1=10$和${\lambda }_2=10$，增加了半硬舒适区间约束。相应的结果以绿色显示。
As depicted in Figure 15, when comfort box constraints were not presented, the optimization uses more control efforts when cornering and stopping for the sake of time efficiency. Once comfort box constraints were added, the control efforts were limited into the box region when mobility is enough to use. Next, we conducted the next two experiments using the same setting with that of the green one except two different initial speed $v_{init}$ = 8 m/s (cyan curves and dots) and $v_{init}$ = 12 m/s (pink curve and dots). As shown in Figure 15, when the initial speed increase to 8 m/s, the region constrained by comfort box was still able to provide enough mobility to stop at the end. Thus all the acceleration points stayed inside the box region. However, when the initial speed was increased dramatically to 12 m/s, the optimization had to use more control efforts to stop in the end. In consequence, the box constraints are “softened” and acceleration points went beyond the box region to guarantee a safe stop. With the comfort box constraint as a hard one, the method cannot get a solution in the last case.
如图15所示，在不考虑舒适区间约束的情况下，出于时间效率的考虑，优化在转弯和停车时使用了更多的控制力度。一旦增加了舒适区域约束，当机动性足够使用时，控制力度被限制在区域内。接下来，除了初始速度$v_{init}$ = 8 m/s(青色曲线和圆点)和$v_{init}$ = 12 m/s(粉色曲线和圆点)不同，我们使用与绿色实验相同的设置进行了接下来的两个实验。如图15所示，当初始速度增加到8 m/s时，受舒适区间约束的区域仍然能够提供足够的机动性，使车辆最终停止。因此所有的加速点都停留在区域内。然而，当初始速度急剧增加到12 m/s时，优化必须使用更多的控制力度才能最终停止。因此，区间约束被“软化”，加速点超出了区域，以保证安全停车。若该方法的舒适区间约束为硬约束，不能得到最后一种情况下的解。

6.2. Speed Planning Dealing with Jaywalking(横穿马路) on a Curvy Road

Second, we considered a jaywalking scenario on a curvy road. The time window $[t_1=7s,t_2=11s]$ that the pedestrian occupies the road at s=30m is given by a dynamic obstacle prediction subsystem. As shown in the previous experiments, our method is able to stop at a specified point along the path. Here, we consider two advanced use cases to avoid the pedestrian safely without stop by manipulating the arrival time. Non-stop dynamic obstacle avoidance strategies may result in energy saving driving behavior or greatly reduced operation time in certain cases.
其次，我们考虑了在弯曲道路上横穿马路的场景。行人在s=30m处占据道路的时间窗$[t_1=7s,t_2=11s]$ 由动态障碍物预测子系统给出。从前面的实验中可以看出，我们的方法可以在路径的指定点处停止。在这里，我们考虑了两个高级用例，通过控制到达时间来安全地避开行人。在某些情况下，不间断动态避障策略的驾驶行为可以实现节能或大大缩短运动时间。
As the pedestrian occupied the road between 7 s and 11 s at s = 30 m along the path, if our car reaches s = 30 m in the same time window, an accident may happen. Unfortunately, with the parameter setting ${\omega }_1=1,{\omega }_2=5,{\omega }_3=0,{\lambda }_1=10,{\lambda }_2=10$, our car will collide with the pedestrian, which is shown as the green curve in Figure 17. Two strategies can be employed to avoid this failure. The first involves passing the potential collision point before the pedestrian arrives point A, that is, $t_{s=30m}<=t_1$ , which is shown as the blue car situation in Figure 18. The second involves passing the potential collision point just after the pedestrian passes point B, that is, $t_{s=30m}>=t_2$, which is shown as a green car situation in Figure 18.
由于行人在7秒到11秒之间占据了沿路径s=30米的道路，如果我们的汽车在同一时间窗内达到s=30米，可能会发生事故。不幸的是，在参数设置为 ${\omega }_1=1,{\omega }_2=5,{\omega }_3=0,{\lambda }_1=10,{\lambda }_2=10$的情况下，我们的汽车会与行人发生碰撞，如图17中绿色曲线所示。可以采用两种策略来避免这种失败。第一种是在行人到达A点之前通过潜在碰撞点，即$t_{s=30m}<=t_1$，如图18所示的蓝车情况。第二种是等行人经过B点后再通过潜在碰撞点，即$t_{s=30m}>=t_2$，如图18所示为绿车情况。
We solved this problem using both strategies. By making ${\omega }_1=1,{\omega }_2=15,{\omega }_3=0,{\lambda }_1=10,{\lambda }_2=10,t_{s=30m}<=6.8s$, we solved the former case and the corresponding results are demonstrated in color cyan in Figures 17, 19 and 20. In practice, we may be not able to pass the barrier in time using the former strategy due to dynamics constraints of cars. The latter approach or a safe stop at a specified point along the path can be always employed to avoid collision. The latter approach is solved by setting ${\omega }_1=1,{\omega }_2=15,{\omega }_3=0,{\lambda }_1=10,{\lambda }_2=10,t_{s=30m}<=11.2s$. The results are presented in color pink in Figures 17, 19 and 20. It should be noted that the second approach is an indirect method for avoiding collision in this scenario. We first stretch the time by increasing the coefficient ${\omega }_2$ from 5 to 15, then compress the arrival time by making . The exact arrival time $t_{s=30m}<=11.2s$ at $s=30m$ for three different cases are 10.656 s (green), 6.799 s (cyan), and 11.199 s (pink).
我们用两种策略解决了这个问题。通过令${\omega }_1=1,{\omega }_2=15,{\omega }_3=0,{\lambda }_1=10,{\lambda }_2=10,t_{s=30m}<=6.8s$，我们解决了前一种情况，其结果如图17、19、20所示，用青色表示。在实践中，由于车辆的动力学约束，我们可能无法使用前一种策略及时通过障碍。后一种方法或在路径的指定点安全停车总是可以避免碰撞。后一种方法通过设置 ${\omega }_1=1,{\omega }_2=15,{\omega }_3=0,{\lambda }_1=10,{\lambda }_2=10,t_{s=30m}<=11.2s$来解决。结果在图17、19和20中以粉红色显示。应该注意的是，第二种方法是在这种情况下避免碰撞的间接方法。我们首先通过将${\omega }_2$系数从5增加到15来延长时间，然后通过使$t_{s=30m}<=11.2s$来压缩到达时间。三种不同案例到达$s=30m$的确切时间分别为10.656秒(绿色)、6.799秒(青色)和11.199秒(粉色)。

6.3. Speed Planning for Freeway Entrance Ramp Merging

Finally, we demonstrate a freeway entrance ramp merging scenario. The oncoming yellow car is driving in around 20 m/s. The arrival time $t_A=8.5s$ at merging point A in Figure 21 is given by the dynamic obstacle prediction or V2V communication module. The initial speed of the autonomous driving car is 4 m/s. With the parameter setting ${\omega }_1=1,{\omega }_2=5,{\omega }_3=0,{\lambda }_1=10,{\lambda }_2=10,20m/s\le v_f\le 22m/s$, the arrival time $t_{s_f}$ at position B of the autonomous car provided by the optimization is 10.123 s. The related speed profile is shown as the green curve in Figure 22. The corresponding S-T graph is depicted in Figure 23 in green. The trajectory of the on-coming car is shown as the black curve in Figure 23. The scenario is designed such that the autonomous car would collide with the oncoming vehicle in the conflict zone if the oncoming car does not yield. To avoid the risk, we enforce a time window constraint at the end of the path, based on the previous parameter setting by making $t_f$ ≤ 8.5s. In this way, the autonomous vehicle has already reached position B by the time the oncoming vehicle arrives position A, which also keeps a safe distance between the two vehicles. Further, the final speed of the autonomous car is constrained to be no less than that of the oncoming vehicle, which ensures that the safety is guaranteed. The corresponding solution is depicted by the cyan curve in Figures 22 and 23. The exact arrival time at the end is 8.5 s.
最后，我们演示了高速公路入口匝道合并场景。迎面驶来的黄色汽车以20m/s的速度行驶。在图21中汇合点A到达时间$t_A=8.5s$，由动态障碍物预测或V2V通信模块给出。自动驾驶汽车的初始速度为4米/秒。在${\omega }_1=1,{\omega }_2=5,{\omega }_3=0,{\lambda }_1=10,{\lambda }_2=10,20m/s\le v_f\le 22m/s$的参数设置下，优化提供的自动驾驶汽车到达B位置的时间$t_{s_f}$为10.123 s。相关的速度配置文件如图22中的绿色曲线所示。相应的S-T图在图23中以绿色表示。驶来车辆的轨迹如图23中的黑色曲线所示。该场景设计为，如果迎面车辆不让步，自动驾驶汽车将在冲突区域与迎面车辆发生碰撞。为了避免这种风险，我们在之前的参数设置的基础上，通过使$t_f$≤8.5s，在路径的末端施加时间窗口约束。这样，当迎面而来的车辆到达A位置时，自动驾驶车辆已经到达B位置，这也使两辆车之间保持了安全距离。此外，自动驾驶汽车的最终速度被限制在不低于迎面而来车辆的速度，从而保证了安全。图22和图23中的青色曲线描述了相应的解决方案。到达终点的确切时间是8.5秒。
In terms of the run-time performance, for 201 discretized points, the solving time range of our method is 0.05 s to 0.2 s with the Gurobi solver as the backend in Julia. For 100 discretized points, the solving time range is 0.03 s to 0.07 s. It is worth noting that the computation time may be greatly reduced if the algorithm is implemented in C++.
在运行时性能方面，对于201个离散点，我们的方法在Julia中以Gurobi求解器为后端，求解时间范围为0.05 s ~ 0.2 s。对于100个离散点，求解时间范围为0.03 ~ 0.07 s。值得注意的是，如果在c++中实现该算法，计算时间可能会大大减少。

7. Conclusions

In this paper, we summarize and categorize the constraints needed to solve various speed planning problems in different scenarios as the requirements for speed planners design and metrics to measure the capacity of the existing speed planners for autonomous driving. Keeping these requirements and metrics in mind, we present a more general, complete, flexible speed planning mathematical model including time efficiency, friction circle, vehicle dynamics, smoothness, comfort, time window, boundary condition, speed deviations from desired speeds and path constraints for speed planning along a fixed path. The proposed formulation is able to deal with many more speed planning problems raised in different scenarios in both static and dynamic environments while providing high-quality, time-efficient, safety-guaranteed, dynamic-feasible solutions in one framework compared to existing methods. By considering the comfort box constraints as a semi-hard constraint and implementing it with slack variables and penalty functions in optimization, we emphasize comfort performance while guaranteeing fundamental motion safety without sacrificing the mobility of cars. We demonstrate that our problem preserves convexity with all these constraints added, therefore the global optimality is guaranteed. We conduct a range of numerical experiments to show how every constraint affects the speed planning results and showcase how our method can be used to solve speed planning problems by providing several challenging case studies in both static and dynamic environments. These results have depicted that the proposed method outperforms existing speed planners for autonomous driving in terms of constraint type covered, optimality, safety, mobility and flexibility.
在本文中，我们将解决不同场景下各种速度规划问题所需的约束条件归纳为速度规划器设计的要求和衡量现有速度规划器自动驾驶能力的指标。考虑到这些要求和指标，我们提出了一个更通用、更完整、更灵活的速度规划数学模型，包括时间效率、摩擦圈、车辆动力学、平稳性、舒适性、时间窗口、边界条件、速度与期望速度的偏差以及沿着固定路径进行速度规划的路径约束。与现有方法相比，所提出的公式能够处理静态和动态环境中不同场景下提出的更多速度规划问题，同时在一个框架内提供高质量，时间效率，安全保证，动态可行的解决方案。将舒适区间约束作为一种半硬约束，并在优化中采用松弛变量和惩罚函数实现，在保证基本运动安全的同时强调舒适性，而不牺牲汽车的机动性。我们证明了在所有这些约束条件下，我们的问题保持了凸性，因此保证了全局最优性。我们进行了一系列数值实验，以显示每个约束如何影响速度规划结果，并通过提供静态和动态环境中的几个具有挑战性的案例研究，展示我们的方法如何用于解决速度规划问题。这些结果表明，所提出的方法在约束类型覆盖、最优性、安全性、移动性和灵活性方面优于现有的自动驾驶速度规划。
Although our method is able to handle the dynamic obstacle with the time window constraint, it does rely on other modules to provide a single time window instead of the union of several time windows. It cannot handle multiple dynamic obstacles in optimization directly due to the non-convexity of the obstacle avoidance problem. In the future, nonconvex version of our problem formulation will be explored to specifically deal with multiple dynamic obstacles using the union of several time windows constraint in optimization directly. Since our problem is a multi-objective optimization problem, how to tune these coefficients systematically according to different autonomous driving applications will also be explored.
虽然我们的方法能够处理具有时间窗约束的动态障碍物，但它依赖于其他模块提供单个时间窗，而不是多个时间窗的联合。由于避障问题的非凸性，在优化中不能直接处理多个动态障碍物。在未来，我们将探索非凸版本的问题公式，以专门处理多个动态障碍，直接使用优化中的几个时间窗约束的联合。由于我们的问题是一个多目标优化问题，如何根据不同的自动驾驶应用系统地调整这些系数也将被探索。
Author Contributions: The work presented here was carried out in collaboration among all authors. All authors have contributed to, seen and approved the manuscript. Y.Z. conceived of the work, realized the algorithms, performed the experiments, analyzed the data and wrote the manuscript. S.L.W. reviewed and revised the manuscript and give some important advice. H.C., S.L.W., G.X., T.Y., S.Z. and K.L. provided many useful suggestions for the work and made great efforts with respect to the paper revision. T.Y. and S.Z. designed the scenarios for case studies and provided corresponding figures.
作者贡献: 这里介绍的工作是在所有作者的合作下进行的。所有作者都对稿件做出了贡献，看过并批准了稿件。Y.Z.构思工作，实现算法，进行实验，分析数据并撰写手稿。S.L.W. 对稿件进行了审阅和修改，并提出了一些重要意见。H.C., S.L.W., G.X., T.Y., S.Z. 和K.L.对论文的修改提出了许多有益的建议和努力。T.Y.和S.Z.设计了案例研究的场景，并提供了相应的数据。
Acknowledgments: We would like to thank Assylbek Dakibay for proof reading the manuscript. The authors would also like to thank the anonymous reviewers for their comments and suggestions.
致谢: 我们要感谢Assylbek Dakibay校对稿件。作者还要感谢匿名审稿人的评论和建议。
Conflicts of Interest: The authors declare no conflict of interest.
利益冲突: 作者声明无利益冲突。

References