🚗 IV. Mothon Planning（运动规划）

The motion planning layer is responsible for computing a safe, comfortable, and dynamically feasible trajectory from the vehicle’s current configuration to the goal configuration provided by the behavioral layer of the decision making hierarchy. Depending on context, the goal configuration may differ. For example, the goal location may be the center point of the current lane a number of meters ahead in the direction of travel, the center of the stop line at the next intersection, or the next desired parking spot. The motion planning component accepts information about static and dynamic obstacles around the vehicle and generates a collision-free trajectory that satisfies dynamic and kinematic constraints on the motion of the vehicle. Oftentimes, the motion planner also minimizes a given objective function. In addition to travel time, the objective function may penalize hazardous motions or motions that cause passenger discomfort. In a typical setup, the output of the motion planner is then passed to the local feedback control layer. In turn, feedback controllers generate an input
signal to regulate the vehicle to follow this given motion plan.
根据决策层的行为层给出的目标构型，运动规划层负责计算出一条从车辆的当前构型到目标构型的安全、舒适且动力学可行的轨迹。在不同的情境中，目标构型可能有所不同。例如，目标位置可能是行驶方向上前方数米处的当前车道的中心点、下一个十字路口处的停车线的中心或者下一个期望的泊车点。运动规划组件接受关于车辆周围的静态和动态障碍物的信息，并生成满足车辆运动的动力学和运动学约束的无碰撞轨迹。通常，运动规划器最小化一个给定的目标函数。除了行程时间外，目标函数还可能包含有关危险运动或导致乘客不适的运动的惩罚项。在典型的设置中，运动规划器的输出传递给局部反馈控制层。反馈控制器随后产生一个输入信号以控制车辆跟随这个给定的运动计划。

A motion plan for the vehicle can take the form of a path or a trajectory. Within the path planning framework, the solution path is represented as a function $\sigma (\alpha ):[0,1]\to \mathcal{X}$, where $\mathcal{X}$ is the configuration space of the vehicle. Note that such a solution does not prescribe how this path should be followed and one can either choose a velocity profile for the path or delegate this task to lower layers of the decision hierarchy. Within the trajectory planning framework, the control execution time is explicitly considered. This consideration allows for direct modeling of vehicle dynamics and dynamic obstacles. In this case, the solution trajectory is represented as a timeparametrized function $\pi (t):[0,T]\to \mathcal{X}$, where $T$ is the planning horizon. Unlike a path, the trajectory prescribes how the configuration of the vehicle evolves over time.
车辆的运动计划可能是路径形式或轨迹形式。在路径规划的框架内，解路径表示为函数 $\sigma (\alpha ):[0,1]\to \mathcal{X}$，其中 $\mathcal{X}$ 是车辆的构型空间。注意这种解并未规定应该如何跟随该路径，因此跟随的方案可以是选择路径的速度分布，或者是将该任务移交决策层中的低级层。而轨迹规划的框架明确考虑到了控制执行时间，因此可以对车辆动力学和动态障碍物进行直接建模。在这种情况下，解轨迹表示为一个时间参数化的函数 $\pi (t):[0,T]\to \mathcal{X}$，其中 $T$ 是规划范围/区间。与路径不同，轨迹规定了车辆的构型如何随时间变化。

In the following two sections, we provide a formal problem definition of the path planning and trajectory planning problems and review the main complexity and algorithmic results for both formulations.
在接下来的两节中，我们将对路径规划和轨迹规划问题进行正式的定义，并综述这两种问题形式的主要复杂度和算法结果。

🔴 A. Path Planning（路径规划）

The path planning problem is to find a path $\sigma (\alpha ):[0,1]\to \mathcal{X}$ in the configuration space $\mathcal{X}$ of the vehicle (or more generally, a robot) that starts at the initial configuration and reaches the goal region while satisfying given global and local constraints. Depending on whether the quality of the solution path is considered, the terms feasible and optimal are used to describe this path. Feasible path planning refers to the problem of determining a path that satisfies some given problem constraints without focusing on the quality of the solution; whereas optimal path planning refers to the problem of finding a path that optimizes some quality criterion subject to given constraints.
路径规划问题是在车辆（或者更一般地说，机器人）的构型空间 $\mathcal{X}$ 中寻找一条从初始构型到目标区域的路径 $\sigma (\alpha ):[0,1]\to \mathcal{X}$，同时满足给定的全局和局部约束。根据是否考虑解路径的质量，分别用术语“可行”和“最优”来描述该路径。可行路径规划是指在不关注解的质量的情况下，确定一条满足某些给定问题约束的路径的问题；而最优路径规划是指在给定约束条件下找到一条优化某个质量标准的路径的问题。

The optimal path planning problem can be formally stated as follows. Let $\mathcal{X}$ be the configuration space of the vehicle and let $\sum (\mathcal{X})$ denote the set of all continuous functions $[0,1]\to \mathcal{X}$. The initial configuration of the vehicle is ${\mathbf{x}}_{\text{init }}\in \mathcal{X}$. The path is required to end in a goal region $X_{\text{goal }}\subseteq \mathcal{X}$. The set of all allowed configurations of the vehicle is called the free configuration space and denoted ${\mathcal{X}}_{\text{free }}$. Typically, the free configurations are those that do not result in collision with obstacles, but the free-configuration set can also represent other holonomic constraints on the path. The differential constraints on the path are represented by a predicate $D\left(\mathrm{x},{\mathrm{x}}^{\prime },{\mathrm{x}}^{\prime \prime },\dots \right)$ and can be used to enforce some degree of smoothness of the path for the vehicle, such as the bound on the path curvature and/or the rate of curvature. For example, in the case of $\mathcal{X}\subseteq {\mathbb{R}}^2$, the differential constraint may enforce the maximum curvature $\kappa$ of the path using Frenet-Serret formula as follows:
最优路径规划问题的一种正式表述如下。设 $\mathcal{X}$ 是车辆的构型空间，设 $\sum (\mathcal{X})$ 表示所有连续函数 $[0,1]\to \mathcal{X}$ 的集合；车辆的初始构型为 ${\mathbf{x}}_{\text{init }}\in \mathcal{X}$；路径需要在目标区域 $X_{\text{goal }}\subseteq \mathcal{X}$ 内结束。车辆的所有允许的构型的集合称为自由构型空间/自由配置空间，记为 ${\mathcal{X}}_{\text{free }}$ 。通常，自由构型是那些不会导致与障碍物发生碰撞的构型，但自由构型集也可以表示路径上的其他完整约束。路径上的微分约束由谓词$D\left(\mathrm{x},{\mathrm{x}}^{\prime },{\mathrm{x}}^{\prime \prime },\dots \right)$ 表示，并且可以用于强制车辆实现一定程度的路径平滑度，例如路径曲率的界限以及/或者曲率变化率。例如，在 $\mathcal{X}\subseteq {\mathbb{R}}^2$ 的情况下，微分约束可以使用如下的Frenet-Serret公式来限制路径的最大曲率 $\kappa$ :
$$D\left(\mathrm{x},{\mathrm{x}}^{\prime },{\mathrm{x}}^{\prime \prime },\dots \right)\iff \frac{\Vert {\mathrm{x}}^{\prime }\times {\mathrm{x}}^{\prime \prime }\Vert }{{\Vert {\mathrm{x}}^{\prime }\Vert }^3}\le \kappa$$

Further, let $J(\sigma ):\sum (\mathcal{X})\to \mathbb{R}$ be the cost functional. Then, the optimal version of the path planning problem can be generally stated as follows.
如果再进一步地设 $J(\sigma ):\sum (\mathcal{X})\to \mathbb{R}$ 为成本泛函，那么最优路径规划问题可以一般地表示如下：

IV.1 (Optimal path planning).
问题 IV.1 最优路径规划
Given a 5-tuple
给定一个五元组 $\left({\mathcal{X}}_{\text{free }},{\mathrm{x}}_{\text{init }},X_{\text{goal }},D,J\right)，find{\sigma }^{\ast }=\arg {\min }_{\sigma \in \sum (X)}J(\sigma )$
subj. to
约束条件：
$$\begin{array}{rl}& \sigma (0)={\mathrm{x}}_{\text{init }},\sigma (1)\in X_{\text{goal }}\\ & \sigma (\alpha )\in {\mathcal{X}}_{\text{free }},\forall \alpha \in [0,1]\\ & D\left(\sigma (\alpha ),{\sigma }^{\prime }(\alpha ),{\sigma }^{\prime \prime }(\alpha ),\dots \right),\forall \alpha \in [0,1]\end{array}$$

The problem of feasible and optimal path planning has been studied extensively in the past few decades. The complexity of this problem is well understood, and many practical algorithms have been developed.
可行和最优路径规划问题在过去几十年中得到了广泛的研究。此问题的复杂度已经被很好地理解，并且已经开发了许多实用的算法。

Complexity: A significant body of literature is devoted to studying the complexity of motion planning problems. The following is a brief survey of some of the major results regarding the computational complexity of these problems.
复杂度：大量文献致力于研究运动规划问题的复杂度。接下来本文将综述关于这些问题的计算复杂度的一些主要结果。

The problem of finding an optimal path subject to holonomic and differential constraints as formulated in Problem IV.1 is known to be PSPACE-hard [64]. This means that it is at least as hard as solving any NP-complete problem and thus, assuming $\mathrm{P}\ne \mathrm{NP}$, there is no efficient (polynomialtime) algorithm able to solve all instances of the problem. Research attention has since been directed toward studying approximate methods, or approaches to subsets of the general motion planning problem.
问题 IV.1陈述了在完整和微分约束下找到一条最优路径的问题，后者被认为是PSPACE困难的（PSPACE-hard）[64]。这意味着该问题至少与解决任何NP完全问题一样困难。因此，如果假设 $\mathrm{P}\ne \mathrm{NP}$，那么就不存在有效的（多项式时间的）算法能够解决此问题的所有实例。因此，研究者此后便将注意力集中在研究近似方法，或研究一般路径规划问题的子集的方法上。

Initial research focused primarily on feasible (i.e., nonoptimal) path planning for a holonomic vehicle model in polygonal/polyhedral environments. That is, the obstacles are assumed to be polygons/polyhedra and there are no differential constraints on the resulting path. In 1970, Reif [64] found that an obstacle-free path for a holonomic vehicle, whose footprint can be described as a single polyhedron, can be found in polynomial time in both 2-D and 3-D environments. Canny [65] has shown that the problem of feasible path planning in a free space represented using polynomials is in PSPACE, which rendered the decision version of feasible path planning without differential constraints as a PSPACEcomplete problem.
最初的研究主要集中于在多边形/多面体环境中的完整（holonomic）车辆模型的可行（非最优）路径规划。即，假设障碍物是多边形/多面体，且假设在生成的路径上没有微分约束。1970年，Reif发现，在二维和三维环境中，可以在多项式时间内找到完整车辆的无障碍路径，其足迹可以描述为单个多面体[64]。Canny已经证明，使用多项式表示的自由空间中的可行路径规划问题是PSPACE问题，因此，没有微分约束的可行路径规划的决策问题是PSPACE完全问题[65]。

For the optimal planning formulation, where the objective is to find the shortest obstacle-free path. It has been long known that a shortest path for a holonomic vehicle in a 2-D environment with polygonal obstacles can be found in polynomial time [66], [67]. More precisely, it can be computed in time $O\left(n^2\right)$, where n is the number of vertices of the polygonal obstacles [68]. This can be solved by constructing and searching the so-called visibility graph [69]. In contrast, Lazard, Reif and Wang [70] established that the problem of finding a shortest curvature-bounded path in a 2-D plane amidst polygonal obstacles (i.e., a path for a car-like robot) is NP-hard, which suggests that there is no known polynomial time algorithm for finding a shortest path for a car-like robot among polygonal obstacles. A related result is that, that the existence of a curvature constrained path in polygonal environment can be decided in EXPTIME [71].
最优路径规划问题的目标是找到一条最短的无障碍路径。学界很早就意识到，可以在多项式时间内找到完整车辆在具有多边形障碍物的二维环境中的最短路径[66][67]。更准确地说，可以在 $O\left(n^2\right)$ 时间内计算得到，其中 n 是多边形障碍物的顶点数[68]。求解的方法可以是构造和搜索所谓的“可见图”[69]。相对地，Lazard、Reif和Wang发现，在多边形障碍物中找到二维平面上的曲率有界的最短路径（即类车机器人的路径）的问题是NP难题[70]，这表明没有已知的多项式时间算法可以在多边形障碍中找到类车机器人的最短路径。相关的研究成果是，可以在指数时间内确定一条在多边形环境中的存在曲率约束的路径[71]。

A special case where a solution can be efficiently computed is the shortest curvature bounded path in an obstacle free environment. Dubins [57] has shown that the shortest path having curvature bounded by $k$ between given two points $p_1$, $p_2$ and with prescribed tangents ${\theta }_1$, ${\theta }_2$ is a curve consisting of at most three segments, each one being either a circular arc segment or a straight line. Reeds and Shepp [58] extended the method for a car that can move both forwards and backwards. Another notable case due to Agraval et al. [72] is an $O\left(n^2\log n\right)$ algorithm for finding a shortest path with bounded curvature inside a convex polygon. Similarly, Boissonnat and Lazard [73] proposed a polynomial time algorithm for finding an exact curvature-bounded path in environments where obstacles have bounded-curvature boundary.
一种能够有效计算解的特殊情况是无障碍环境中的曲率有界的最短路径。Dubins已经证明[57]，给定两个点 $p_1$, $p_2$ ，并且给定两个点处的切线 ${\theta }_1$, ${\theta }_2$ ，则两点之间的、限制最大曲率 $k$ 的最短路径是一条由最多三个段组成的曲线，每个段是圆弧段或直线。Reeds和Shepp将该方法扩展到能够前后移动的汽车上[58]。Agraval等人[72]提出的另一种值得注意的情况是 $O\left(n^2\log n\right)$ 算法，其用于在凸多边形内找到一条曲率有界的最短路径。类似地，Boissonnat和Lazard提出了一种多项式时间算法[73]，用于在一种特定的环境中找到一条精确的曲率有界的路径，这种环境中的障碍物的边界都具有有界曲率。

Since for most problems of interest in autonomous driving, exact algorithms with practical computational complexity are unavailable [70], one has to resort to more general, numerical solution methods. These methods generally do not find an exact solution, but attempt to find a satisfactory solution or a sequence of feasible solutions that converge to the optimal solution. The utility and performance of these approaches are typically quantified by the class of problems for which they are applicable as well as their guarantees for converging to
an optimal solution. The numerical methods for path planning can be broadly divided in three main categories:
由于自动驾驶中受到关注的大多数问题都不存在具有实用的计算复杂度的精确算法[70]，因此必须转向更一般的数值求解方法。这些方法通常不会找到一个精确的解，而是试图找到一个满意的解或一系列收敛到最优解的可行解。这些方法的效用和性能通常通过它们适用的问题类别以及它们是否能保证收敛到最优解来量化。路径规划的数值方法可以大致分为三大类：

Variational methods represent the path as a function parametrized by a finite-dimensional vector and the optimal path is sought by optimizing over the vector parameter using non-linear continuous optimization techniques. These methods are attractive for their rapid convergence to locally optimal solutions; however, they typically lack the ability to find globally optimal solutions unless an appropriate initial guess in provided. For a detailed discussion on variational methods, see Section IV-C.
🥇变分法：其将路径表示为一个由有限维向量参数化的函数，并通过非线性连续优化技术对向量参数进行优化，以寻找最优路径。由于能快速收敛到局部最优解，因此备受关注；然而，除非提供一个适当的初始猜测，否则变分法通常难以找到全局最优解。有关变分法的详细讨论，请见第4-C节。

Graph-search methods discretize the configuration space of the vehicle as a graph, where the vertices represent a finite collection of vehicle configurations and the edges represent transitions between vertices. The desired path is found by performing a search for a minimum-cost path in such a graph. Graph search methods are not prone to getting stuc in local minima, however, they are limited to optimize only over a finite set of paths, namely those that can be constructed from the atomic motion primitives in the graph. For a detailed
discussion about graph search methods, see Section IV-D.
🥈图搜索方法：其将车辆的构型空间离散化为一个图。图的顶点表示车辆构型的有限集合，图的边表示顶点之间的转移。该方法通过在这样的图中搜索一条最小成本路径，来找到期望的路径。图搜索方法不容易陷入局部极小值，但是它们仅限于在有限的路径集上进行优化，这种路径集包含的路径都可以由图中的运动基元构建得到。有关图搜索方法的详细讨论，请见第IV-D节。

Incremental search methods sample the configuration space and incrementally build a reachability graph (oftentimes a tree) that maintains a discrete set of reachable configurations and feasible transitions between them. Once the graph is large enough so that at least one node is in the goal region, the desired path is obtained by tracing the edges that lead to that node from the start configuration. In contrast to more basic graph search methods, sampling-based methods incrementally increase the size of the graph until a satisfactory solution is found within the graph. For a detailed discussion about incremental search methods, see Section IV-E.
🥉增量搜索方法：其对构型空间进行采样，并逐步构造一个可达图（通常是树）。可达图包含可达构型的一个离散集合以及可达构型之间的可行转移。一旦图已经构造得足够大，使得至少存在一个节点位于目标区域中，则算法通过跟踪从起始构型出发的、最终指向该节点的边，来获得期望路径。相比更基本的图搜索方法，基于采样的方法逐渐增加图的大小，直到在图中找到满意的解。有关增量搜索方法的详细讨论，请见第IV-E节。

Clearly, it is possible to exploit the advantages of each of these methods by combining them. For example, one can use a coarse graph search to obtain an initial guess for the variational method as reported in [74] and [75]. A comparison of key properties of select path planning methods is given in Table I. In the remainder of this section, we will discuss the path planning algorithms and their properties in detail.
显然，通过将这些方法结合起来，我们有望充分利用每种方法的优点。例如，可以使用粗略的图搜索来获得[74]和[75]中报告的变分法的初始猜测。表I中给出了路径规划方法的关键性质的比较。本节的剩余部分将详细讨论路径规划算法以及它们的性质。

🟠 B. Trajectory Planning（轨迹规划）

The motion planning problems in dynamic environments or with dynamic constraints may be more suitably formulated in the trajectory planning framework, in which the solution of the problem is a trajectory, i.e. a time-parametrized function $\pi (t):[0,T]\to \mathcal{X}$ prescribing the evolution of the configuration of the vehicle in time.
动态环境的或具有动力学约束的运动规划问题可能更适合表述于轨迹规划的框架中，其中问题的解是一条轨迹，即时间参数化函数 $\pi (t):[0,T]\to \mathcal{X}$ ，其规定了车辆构型随时间的变化。

Let $\Pi (\mathcal{X},T)$ denote the set of all continuous functions $[0,T]\to \mathcal{X}$. be the initial configuration of the vehicle. The goal region is $X_{\text{goal}}\subseteq \mathcal{X}$. The set of all allowed configurations at time $t\in [0,T]$ is denoted as ${\mathcal{X}}_{\text{free}}(t)$ and used to encode holonomic constraints such as the requirement on the path to avoid collisions with static and, possibly, dynamic obstacles. The differential constraints on the trajectory are represented by a predicate $D\left(\mathrm{x},{\mathrm{x}}^{\prime },{\mathrm{x}}^{\prime \prime },\dots \right)$ and can be used to enforce dynamic constraints on the trajectory. Further, let $J(\pi ):\Pi (\mathcal{X},T)\to \mathbb{R}$ R be the cost functional. Under these assumptions, the optimal version of the trajectory planning problem can be very generally stated as:
记 $\Pi (\mathcal{X},T)$ 为所有连续函数 $[0,T]\to \mathcal{X}$ 的集合，同时记 $x_{\text{init }}\in \mathcal{X}$ 为车辆的初始构型。目标区域是 $X_{\text{goal}}\subseteq \mathcal{X}$ 。在时间 $t\in [0,T]$ 内的所有允许构型的集合记为 ${\mathcal{X}}_{\text{free}}(t)$，并被用于编码完整约束，例如路径上的避免与静态以及可能的动态障碍物发生碰撞的要求。轨迹上的微分约束表示为谓词 $D\left(\mathrm{x},{\mathrm{x}}^{\prime },{\mathrm{x}}^{\prime \prime },\dots \right)$ ，并可用于对轨迹施加动力学约束。此外，再设 $J(\pi ):\Pi (\mathcal{X},T)\to \mathbb{R}$ 为成本泛函。在这些假设下，最优轨迹规划问题可以非常一般地表述为：

🟡 C. Variational Methods（变分法）

We will first address the trajectory planning problem in the framework of non-linear continuous optimization. In this context, the problem is often referred to as trajectory optimization. Within this subsection we will adopt the trajectory planning formulation with the understanding that doing so does not affect generality since path planning can be formulated as trajectory optimization over the unit time interval. To leverage existing nonlinear optimization methods, it is necessary to project the infinite-dimensional function space of trajectories to a finite-dimensional vector space. In addition, most nonlinear programming techniques require the trajectory optimization problem, as formulated in Problem IV.2, to be converted into the following form
我们将首先在非线性连续优化的框架下解决轨迹规划问题。在这种情况下，该问题通常被称为轨迹优化。在本小节中，我们将采用轨迹规划的形式——注意这种做法并不会影响一般性，因为路径规划可以表示为单位时间间隔内的轨迹优化。为了利用现有的非线性优化方法，有必要将轨迹的无限维函数空间投影到一个有限维向量空间。此外，大多数非线性规划技术要求将问题IV.2形式的轨迹优化问题转化为如下形式：
$$\arg \underset{\pi \in \Pi (x,T)}{\min }J(\pi )$$
subj. to
约束条件：
$$\begin{array}{rl}& \pi (0)={\mathrm{x}}_{\text{init }},\pi (T)\in X_{goal}\\ & f\left(\pi (t),{\pi }^{\prime }(t),\dots \right)=0,\forall t\in [0,T]\\ & g\left(\pi (t),{\pi }^{\prime }(t),\dots \right)\le 0,\forall t\in [0,T]\end{array}$$

where the holonomic and differential constraints are represented as a system of equality and inequality constraints.
其中完整约束和微分约束被表示为系统的等式约束和不等式约束。
In some applications the constrained optimization problem is relaxed to an unconstrained one using penalty or barrier functions. In both cases, the constraints are replaced by an augmented cost functional. With the penalty method, the cost functional takes the form
一些应用场景通过使用惩罚函数或障碍函数，将带约束的优化问题放松为无约束的优化问题。无论是使用惩罚函数还是障碍函数，都是利用一个增强的成本泛函代替约束。使用惩罚方法时，成本泛函的形式如下

$$\tilde{J}(\pi )=J(\pi )+\frac{1}{ϵ}{\int }_0^T\left[{\Vert f\left(\pi ,{\pi }^{\prime },\dots \right)\Vert }^2+{\Vert \max \left(0,g\left(\pi ,{\pi }^{\prime },\dots \right)\right)\Vert }^2\right]dt$$

Similarly, barrier functions can be used in place of inequality constraints. The augmented cost functional in this case takes the form
类似地，障碍函数可以用于替代不等式约束。此时的增强成本泛函的形式为

$$\tilde{J}(\pi )=J(\pi )+ϵ{\int }_0^{T}h(\pi (t))dt$$

where the barrier function satisfies $g(\pi )<0⟹h(\pi )<\infty$, $g(\pi )\ge 0⟹h(\pi )=\infty$, and lim ⁡ g ( π ) → 0 { h ( π ) } = ∞ \lim _{g(\pi) \rightarrow 0}\{h(\pi)\}=\infty limg(π)→0​{h(π)}=∞. The intuition behind both of the augmented cost functionals is that, by making " small, minima in cost will be close to minima of the original cost functional. An advantage of barrier functions is that local minima remain feasible, but must be initialized with a feasible solution to have finite augmented cost. Penalty methods on the other hand can be initialized with any trajectory and optimized to a local minima. However, local minima may violate the problem constraints. A variational formulation using barrier functions is proposed in [60] where a change of coordinates is used to convert the constraint that the vehicle remain on the road into a linear constraint. A logarithmic barrier is used with a Newton-like method in a similar fashion to interior point methods. The approach effectively computes minimum time trajectories for a detailed vehicle model over a segment of roadway.
其中障碍函数满足 $g(\pi )<0⟹h(\pi )<\infty$ ，$g(\pi )\ge 0⟹h(\pi )=\infty$ ，以及 lim ⁡ g ( π ) → 0 { h ( π ) } = ∞ \lim _{g(\pi) \rightarrow 0}\{h(\pi)\}=\infty limg(π)→0​{h(π)}=∞ 。构造这两个增强成本泛函的直觉动机是，通过使 \epsilon 变小，成本的最小值将接近原始成本函数的最小值。障碍函数的一个优点是，局部最小值仍然是可行解，但必须用可行解初始化，以保证增强成本为有限值。相比之下，惩罚方法可以用任何轨迹初始化，并且能优化到局部最小值，然而这个局部最小值可能不满足问题的约束条件。文章[60]中提出了一种使用障碍函数的变分公式，其中使用坐标的变化将车辆保持在道路上的约束转换为一个线性约束。对数障碍与牛顿类的方法一起使用，其形式与内点法相似。这种方法有效地计算了一段道路上的一个详细车辆模型的最短时间轨迹。

Next, two subclasses of variational methods are discussed: Direct and indirect methods.
接下来将讨论变分法的两个子类：直接法和间接法。

Direct Methods: A general principle behind direct variational methods is to restrict the approximate solution to a finite-dimensional subspace of $\Pi (x,T)$. To this end, it is usually assumed that
🍤直接法： 直接变分法背后的一般原理是将近似解限制在 $\Pi (x,T)$ 的有限维子空间。为此，通常假设
$$\pi (t)\approx \tilde{\pi }(t)=\sum _{i=1}^N{\pi }_i{\varphi }_i(t)$$

where πi is a coefficient from R, and φi(t) are basis functions of the chosen subspace. A number of numerical approximation schemes have proven useful for representing the trajectory optimization problem as a nonlinear program. We mention here the two most common schemes: Numerical integrators with collocation and pseudospectral methods.
其中 ${\pi }_i\in \mathbb{R}$ 的系数， ${\varphi }_i(t)$ 是所选子空间的基函数。许多数值近似方法已被证明可用于将轨迹优化问题表示为非线性规划问题。这里我们将提及两种最常见的方案：利用配点法/配置法的数值积分器和伪谱法。

🍷1) Numerical Integrators with Collocation (利用配点法的数值积分器):
With collocation, it is required that the approximate trajectory satisfies the constraints in a set of discrete points { t j } j = 1 M \left\{t_j\right\}_{j=1}^M {tj​}j=1M​. This requirement results in two systems of discrete constraints: A system of nonlinear equations which approximates the system dynamics
使用配点法，要求近似轨迹在一组离散点 { t j } j = 1 M \left\{t_j\right\}_{j=1}^M {tj​}j=1M​ 上满足约束条件。这个要求导致了两个带离散约束的系统：一个近似于系统动力学的非线性方程组系统
$$f\left(\tilde{\pi }\left(t_j\right),{\tilde{\pi }}^{\prime }\left(t_j\right)\right)=0,\forall j=1,\dots ,M$$

and a system of nonlinear inequalities which approximates the state constraints placed on the trajectory
以及一个近似于轨迹上的状态约束的非线性不等式组系统
$$g\left(\tilde{\pi }\left(t_j\right),{\tilde{\pi }}^{\prime }\left(t_j\right)\right)\le 0,\forall j=1,\dots ,M$$

Numerical integration techniques are used to approximate the trajectory between the collocation points. For example, a piecewise linear basis
数值积分方法用于近似配置点之间的轨迹。例如，一个分段线性基函数
$${\varphi }_i(t)=\{\begin{array}{cl}\frac{\left(t-t_{i-1}\right)}{\left(t_i-t_{i-1}\right)},& \text{ if }t\in \left[t_{i-1},t_i\right]\\ \frac{\left(t_{i+1}-t\right)}{\left(t_{i+1}-t_i\right)},& \text{ if }t\in \left[t_i,t_{i+1}\right]\\ 0,& \text{ otherwise }\end{array}$$

together with collocation gives rise to the Euler integration method. Higher order polynomials result in the Runge-Kutta family of integration methods. Formulating the nonlinear program with collocation and Euler’s method or one of the RungeKutta methods is more traightforward than some other methods making it a popular choice. An experimental system which successfully uses Euler’s method for numerical approximation of the trajectory is presented in [14].
与配点法结合，将一同导致欧拉积分法。高阶多项式将导致龙格-库塔族的积分法。与其他一些方法相比，用配点法和欧拉法或某种龙格-库塔法来表述非线性规划问题更为直接，这使其成为一种流行的选择。文章[14]中介绍了一个成功使用欧拉法来数值近似轨迹的实验系统。

In contrast to Euler’s method, the Adams approximation, is investigated in [87] for optimizing the trajectory for a detailed vehicle model and is shown to provide improved numerical accuracy and convergence rates
与欧拉法相比，文章[87]研究了亚当斯法/亚当斯近似，用于优化详细车辆模型的轨迹，并证明了亚当斯法可以提高数值准确度和收敛速度。

🍹***2) Pseudospectral Methods (伪谱法): ***
Numerical integration techniques utilize a discretization of the time interval with an interpolating function between collocation points. Pseudospectral approximation schemes build on this technique by additionally representing the interpolating function with a basis. Typical basis functions interpolating between collocation points are finite subsets of the Legendre or Chebyshev polynomials. These methods typically have improved convergence rates over basic collocation methods, which is especially true when
adaptive methods for selecting collocation points and basis functions are used as in [88].
数值积分技术利用时间间隔的离散化以及配置点之间的插值函数。伪谱近似法基于该技术，同时还利用基来表示该插值函数。配置点之间插值的典型的基函数是勒让德多项式或切比雪夫多项式的有限子集。相比于基础的配点法，这些方法通常具有更好的收敛速度，当使用自适应方法选择配置点和基函数（如[88]）时尤其如此。

Indirect Methods: Pontryagin’s minimum principle [89], is a celebrated result from optimal control which provides optimality conditions of a solution to Problem IV.2. Indirect methods, as the name suggests, solve the problem by finding solutions satisfying these optimality conditions. These optimality conditions are described as an augmented system of ordinary differential equations (ODEs) governing the states and a set of co-states. However, this system of ODEs results in a two point boundary value problem and can be difficult to solve numerically. One technique is to vary the free initial conditions of the problem and integrate the system forward in search of the initial conditions which leads to the desired terminal states. This method is known as the shooting method, and a version of this approach has been applied to planning parking maneuvers in [90]. The advantage of indirect methods, as in the case of the shooting method, is the reduction in dimensionality of the optimization problem to the dimension of the state space.
🦐间接法： 庞特里亚金最小值原理[89]是最优控制的一个著名结论，它提供了问题IV.2的解的最优性条件。顾名思义，间接法通过找到满足这些最优性条件的解来解决问题。这些最优性条件被描述为一个增强/扩充系统，后者由控制状态和一组共状态的常微分方程所定义。然而，这种常微分方程组描述的系统会导致两点边值问题，并且很难用数值方法求解。一种技术是改变问题的自由初始条件，并向前对系统进行积分，以搜索能到达期望终止状态的初始条件。这种方法被称为打靶法。该方法的一个版本已在[90]中应用于规划泊车策略。以打靶法的情况而言，间接法的优点是将优化问题的维数降低到了状态空间的维度。

The topic of variational approaches is very extensive and hence, the above is only a brief description of select approaches. See [91], [92] for dedicated surveys on this topic.
变分法的主题非常广泛，因此，上文仅仅是对所选方法的简要描述。有关该主题的专门综述，请见[91][92]。