2017年上海事业单位考试公共基础知识：运动规划

● 根据交通规则分配本地驾驶任务。

● 选择连续路径并在环境中进行本地导航。

● 修正运动规划的执行错误。

1.1 A. 路线规划

实现自动驾驶首先需要通过路网确定当前位置，并选择通往目的地的路线。道路网络表示为有向图，边表示为通过路段的成本。通过选择最小成本总和来选择行驶路线。

这样的表示可能有数万条边，通过 Dijkstra 或 A* 找到最短路径并不是很实用。高效路径规划算法——如何在大陆尺度路网中预处理后返回毫秒级耗时的最优路径是研究热点。

1.2 B. 行为决策

行车路线确定后，行为层的作用是感知其他交通参与者的行为、基础设施信号，并随时根据交通规则选择行车行为。

驾驶手册明确规定了驾驶环境中的行为。因为上下文和可操作的行为可以建模为有限集。每个行为都可以自然地建模为有限状态机。

一些团队（例如 DARPA Urban 挑战赛中的许多团队）使用有限状态机和各种场景的启发式规则作为行为决策的基础。在真实驾驶环境中，其他交通参与者的行为和意图存在很多不确定性。

应对这些不确定性是行为决策层需要考虑的问题。大量的研究都集中在交通参与者的意图预测和轨迹估计上，例如高斯混合模型、高斯过程回归，以及直接从传感器数据估计意图的基于模型的方法。

这种不确定性通常被建模为马尔可夫决策过程 (MDP)。

1.3 C. 运动规划

行为决策导致驾驶操作，如：沿线巡航、变道、右转等。行为需要进一步转化为路径或轨迹输入下级反馈控制器，这些路径或轨迹必须满足：1）车辆可行，2）乘客舒适，3）车载传感器监测无碰撞。运动规划负责选择这样的路径或轨迹。

精确的运动规划解决方案难以计算，通常采用数值近似。最常用的是非线性优化，将问题建模为函数空间，图搜索——将车辆状态空间表示为图。

graph-search 计算最短路径，基于基于树的方法增量创建可达状态树，并选择最优分支。

1.4 D. 车辆控制

控制车稳定地执行运动规划选择的轨迹，轨迹误差反映了控制的准确性，是反馈控制的依据。

02 规划模型与控制模型

高保真模型 – 准确反映车辆响应子集和问题 近似算法，更详细地使规划和控制问题复杂化。本节仅关注一般模型，旨在说明与模型规划和控制相关的一般概念。

车辆的位姿下面会多次提到： 1. 车辆某点的位置和车辆前方的方位。车辆运动通过位姿表示为二维欧几里得空间刚体运动。

2.1 A. 单轨运动学模型

（单轨模型）也是实际应用中最基本的模型，想象一辆自行车。车辆有前后轮，它们刚性连接在一起。轮子需要与地面保持接触，每个轮子都可以围绕自己的轴线自由旋转。前轮具有绕垂直于运动平面的纵轴旋转的自由度（前轮转向）。

图1.2：单轨运动学模型

该模型除了两轮同步向前运动外，不能进行单向整体平移。这种可操作性约束被称为非完整约束——一个你不能去的方向的约束，并且是一个速度约束——一个微分约束。

如图(1.2)车辆静止时，惯性坐标系基向量(^ex,e^y,e^z)，前后轮位置pf,pr，车辆方向为等于 e^x 和 pf, – pr 角度表示。

前后轮必须满足共线和接地约束：

1.后轮与地面接触约束定义：

2. 前轮接地约束定义：

轮地接触约束使用惯性坐标系基向量的参数化表达式：

或者就前轮的运动而言：

差分约束由前轮的运动表示

模型规划与前后轮转速控制的主要内容包括舵角（或方向盘角或转向角等）的选择，以及舵角的可行范围：

简化解决方案：使用转向角速度 ω 代替舵角 δ

单轨模型适用于低速路径规划。与接触约束相比，惯性效应很小且可以忽略不计。该模型的主要缺点是允许瞬时舵角变化会导致运动规划中的一些问题。

舵角的连续性要求可以通过数据增强（1.5).

这样做的缺点是模型尺寸变大，使运动规划和控制变得复杂。局部坐标系可以选择车轮或质心等。

2.2 B. 惯性效应

当车辆充分加速时，可能会违反车轮与地面接触的约束。此时，车辆被视为满足动量定律的整个刚体。加速度与地面对轮胎的反作用力成正比。

那么前、后轮的速度与质心之间的动态关系如下：

法向力可用于计算后轮胎的牵引力

牵引到前轮

牵引力抵消了摩擦力，摩擦力与接触面法线方向的压力成正比。公式 (1.11)- (1.16) 给出了净力的表达式

从旋转对称性(1.16)和(1.17)车辆可以施加的法向力的最大值，这个峰值也称为摩擦锥。

上述模型在自动驾驶中经常使用。较低级别的电子可靠控制、底盘和悬架支撑需要更精细和复杂的模型。

03 运动规划

在行为层计算车辆的目标位姿，如：将当前车道的中心点向前行驶几米；或下一个路口停车线的中心点；或理想的停车场。

运动规划层的主要功能是计算出一条安全、舒适、可执行的到目标点的轨迹。同时接收车辆附近静止或移动障碍物的信息子集和问题 近似算法，确保路径或轨迹符合动力学和运动学约束，安全、平稳、无碰撞。

很多时候，运动规划过程本质上是最小化给定的目标函数。除了交通时间，目标函数还惩罚危险行为或使乘客不舒服的行为。

此外，运动计划的输出被反馈到控制层以校准车辆运动以符合运动计划。车辆运动规划的结果可以是路径或轨迹。

返回路径规划中的函数

3.1 路径规划

算法复杂度

在过去的几十年里，可行解和最优解都得到了很好的研究，算法的复杂性备受关注。由于该算法是 NP 难的，人们转向寻求近似解，或在某些子集中求解。

普遍的看法

在某些没有障碍物的环境中，存在曲率约束的路径，计算效率很高。在大多数情况下，没有实用的精确算法，只能求助于数值解。算法的性能取决于问题的定义和对收敛速度的要求。

数值解大致可分为3类：

变化

变分法将路径视为有限维向量的函数，通过非线性连续优化可以快速收敛到局部最优解。需要好的初始值才能收敛到全局最优解。

图表搜索 图表搜索

通过将位姿空间表示为离散图，找到所有路径中成本最低的。不容易陷入局部极小解，但只适用于有限路径搜索。

增量搜索 增量搜索方法

对可行位姿空间进行采样，逐步构建可达图（通常是树形结构），保持一组离散的可达位姿，一旦图足够大，目标区域就会有一个节点，可以追踪到理想路径通过跟踪从当前位姿节点到目标节点的边。

这些方法可以组合使用：可以通过粗略的图搜索来猜测初始值，然后使用变分法求解。

3.2 B. 轨迹规划

具有动态约束的动态环境或运动规划问题适合使用轨迹规划框架来解决。

复杂性

动态环境下的轨迹规划是静态环境下路径规划的推广，也是一个PSPACE-hard问题。与路径规划相比，轨迹规划中的轨迹规划更难解决。

无法获得非平凡轨迹规划的精确解。人们也寻求数值解。加入时间维度后，轨迹规划问题可以转化为路径规划问题。

同时，轨迹与路径的位姿之间存在映射关系：

根据可以处理差分约束的路径规划求解方法，可以将结果转化为轨迹形式。

3.3 C. 变分方法

如上所述，路径规划问题可以看成是单位时间间隔的轨迹规划问题，所以这里一般称为轨迹规划。

轨迹优化是一个非线性的连续优化问题。首先将无限维函数空间投影到有限维向量空间，问题（1.22)应该写成：

其中完整的（位姿约束）和微分约束（速度约束等）表示为等于或小于约束。在某些应用中，使用惩罚函数来放松约束优化问题以规划无约束优化问题。

成本函数变为

在不等式约束中，成本函数变为

代价函数满足

这些成本函数背后的直觉是，如果 ε 足够小，则成本函数的最小值将接近原始成本函数最小值。这类算法的共同点是容易陷入局部极小值，需要好的初始值。

但总会有解决办法的。其中，车轮与地面接触的约束可以通过坐标变换转化为线性约束。给定车辆模型和路段，可以有效地计算出耗时最少的轨迹。

此类方法有两个子类：

直接法

直接法将近似解限制为 Π(X , T) 的有限维子空间，假设

间接法

就是直接求解常微分方程。可以使用转换自由初始条件，直到可以集成所需的最终状态。这种方法叫拍法，非常形象。

另外，变分的方法还有很多，以上只是一二的简单说明。

3.4 D. 图形搜索

1)。车道线图

（车道图）足够好的车道图由可以遵循并相互连接的路径组成。车道线图可以是高层街道网络图加上一些人工标注。

图1.6：手绘与行车路径

大多数情况下，车辆只需要沿车道线行驶，但对于路网设计中未考虑的障碍物或路网地图未覆盖的区域，车辆需要具备导航功能。

例如，如果车辆在路中间抛锚，则需要避开障碍物以确保车辆安全通行。

解决方案（取决于障碍物的表示方式）分为两大类：

1）几何（组合法），障碍物表示为多边形或多面体。

2) 如何避开障碍物转而判断给定路径是否与障碍物发生碰撞称为采样。

2)。几何方法

如前所述，可达姿势在图上表示为节点，节点之间的边对应于连接两个姿势的路径。

具有最大曲率约束的路径规划问题是 NP-hard，并且已经有多项式复杂度算法可以产生近似解。

3)。基于抽样的方法

变分法难以避免陷入局部最优，但计算成本较高。路径规划问题的复杂性远不能用曲率约束来概括，基于图的方法不足以处理复杂的情况。

人们转向基于采样的方法，这些方法简单地探索姿势是否可达以及路径中是否有障碍物。

图1.7：Meshed/Meshless Primitive Graph

(a) 90 度左圆弧，90 度右圆弧，直线

(b) 89 度左圆弧，89 度右圆弧，直线

由于分支在原点形成一个环，因此它最终是一棵树而不是一个网格，覆盖面积较小。

图1.8：递归生成路线图

从 X 中生成一组离散的样本，覆盖 X，将它们与可行路径连接起来。

图1.9：基于抽样的路线图构建

这种方法基本遵循上述流程，区别在于采样点数（X，n）和邻域（x，V）的选择。新算法总是不断涌现。

4).Graph 搜索策略

前三个部分 (1.3.4),(1.3.4),(1.3.< @4) 是关于如何构建图，本节是关于如何搜索路径。

最著名的路径搜索算法 Dijkstra 通过采用从一个点到所有其他点的最短路径来构建树结构。

如果只有一条路径可用，则使用启发式搜索、最著名的 A* 和加权 A*。而根据传感器部分更新路径的D*、Focused D*、D* Lite算法等，新的算法层出不穷。

3.5 E.增量搜索策略

对固定图的搜索可能无法返回可行路径或产生次优解决方案。增量搜索为 X 构建了一个越来越精细的图，同时在每个级别确定是否存在可行路径。从本质上讲，它提供了一系列路径，这些路径随着质量的提高而逐渐接近最优路径。

缺点是路径规划是在不同分辨率的图上独立进行的，缺乏信息重用；以及增加分辨率的速度以及如何增加节点都需要考虑。为了解决这些问题，分散度的增加和路径规划通常集成在一个过程中。

增量路径规划从起始姿势向外生成一棵树（探索 X 空间）。探索行为是指从树中随机选择一个点，扩展点是指从该点执行驱动函数（1.3.4)，当树足够大时，它将找到从初始姿势到目标姿势的路径。

代表性算法，例如扩展空间树 (EST)、快速探索随机树 (RRT) 和改进 RRT 的 RRT*。

图1.10：基于增量树的算法

3.6 F. 实际部署

上面列出的运动方案都有实际的应用案例，每个团队采用的方案也大相径庭。

● CMU的BOSS车辆：变分法+格图+anytime D*

● 亚军车辆：搜索策略+ Hybrid A* + 运动原语

● VictorTango：图形离散化可能的操作 + A* 搜索

● 麻省理工学院：RRT

04 车辆控制

经过路径规划（1.3.1)和轨迹规划（1.22)）后，车辆需要沿着给定的路径和轨迹稳定行驶。