[自动驾驶技术]-4 决策规划系统

在自动驾驶系统中，决策规划是一个关键部分，它位于环境感知和控制执行之间，负责将感知模块提供的静态和动态环境信息转化为具体的驾驶策略和路径规划，生成期望的路径和相应的控制量，给到下一阶段控制执行使用。

决策规划模块的主要任务包括：全局路径规划、行为决策、运动轨迹规划（主要进行局部路径规划和速度规划）。决策规划的体系架构早期是基于全局路径规划→行为决策→运动轨迹规划分层递进式的，具备推理解释能力，但中间存在延迟，路径误差累计较大，可靠性不高；目前大部分决策规划结合了针对局部目标的多个并行回路处理，决策规划模块分层递进主干道由全局路径规划和局部轨迹规划构成，在局部轨迹规划内部结合了行为决策模块，面向目标的多个并行回来获得决策结果，共同实现最终可执行的轨迹路径。

1 全局路径规划

全局路径规划是自动驾驶系统中的关键任务，负责在宏观层面上为车辆从起点到终点生成最佳行驶路线。该任务主要基于静态环境信息，如高精度地图和道路网络结构，根据周边交通状况进行几何路径规划，以确保车辆能够安全、高效地到达目的地；一般分为两个步骤，建立环境地图和调用搜索算法在环境地图中搜索可行路径。

1）全局路径规划的要求

全局路径规划需要考虑以下几个方面的要求：

• 最短路径： 路径应尽可能短，以减少行驶时间和燃料消耗。
• 安全性： 避开危险区域和不可行驶区域，确保行驶安全。
• 平稳性： 路径应尽量平滑，减少急转弯和突然的速度变化。
• 遵守交通规则： 考虑交通信号、道路标志和限速等交通规则。
• 可行性： 路径应考虑车辆的动力学和运动学约束。

2）构建环境网络图

在全局路径规划中，将驾驶环境简化为有向网格图是关键的一步。这个过程涉及将连续的环境模型转换为离散的图模型，常见构图方法如下：

2.1）DGN（Directional Grid Network）

DGN是最常见的图构建方法，尤其适用于规则网格环境，DGN将环境划分为一个规则的网格，每个网格单元（节点）与其邻近的网格单元通过有向边连接。

特点：规则性强，网格结构规则，易于实现；计算简单，适合规则道路和较简单的环境。

具体步骤包括：

• 网格划分：将环境划分为大小相等的方形或矩形网格。
• 节点定义：每个网格的中心或某个特定位置作为一个节点。
• 边的定义：节点之间的连接为有向边，表示车辆可以从一个节点移动到另一个节点。常见的邻接关系有4连通（上下左右）和8连通（包括对角线方向）。
• 权重分配：边的权重通常根据节点之间的距离或行驶成本（如路况、坡度等）确定。

2.2）RNG（Random Network Graph）

RNG用于更复杂或不规则的环境中，与DGN相比RNG的节点和边的分布更加随机化，以适应实际道路的复杂性。

具体步骤包括：

• 节点选择：根据实际道路网络或重要位置（如交叉口、停车位等）选择节点，不是均匀分布。
• 边的定义：根据实际道路连接情况，定义节点之间的有向边。
• 权重分配：边的权重基于实际道路的长度、行驶时间、交通流量等。
• 特点：灵活性高，适合复杂的道路网络和不规则的环境；现实性强与实际道路情况匹配度高。

2.3）可视图法（Visibility Graph）

VG基于环境中的障碍物顶点构建节点，节点之间的边表示无障碍物直线可视，适用障碍物较多的环境，如室内场景。

特点：生成的图较稀疏，减少了计算量；但在复杂环境中，图的构建和维护复杂度高。

2.4）Voronoi图

基于环境中的障碍物边界构建节点，节点之间的边表示最远距离路径，适用复杂的户外环境。

特点：避免了障碍物，提供较安全的路径；但计算复杂，路径可能不最短。

2.5）栅格地图法（Occupancy Grid Map）

栅格地图法是一种将环境表示为离散网格的技术，每个网格单元（栅格）代表环境中的一个区域，并包含该区域被占用的概率，概率值通常在0（完全空闲）到1（完全占用）之间。这个方法在机器人导航和自动驾驶中广泛应用，通过处理传感器数据（如激光雷达、相机、超声波传感器等），实时构建和更新环境模型。

特点：能够实时处理传感器数据，快速更新环境模型；通过调整栅格大小，可以实现不同精细度的环境表示；适用于不同类型的传感器数据，能够应对动态环境的变化。

特斯拉的自动驾驶技术中使用了类似栅格地图的方法来进行环境感知和导航，通过融合传感器（包括摄像头、超声波传感器和雷达）的数据，构建车辆周围的环境模型，实时更新环境的占用情况，识别道路、障碍物、行人等，进行精确的路径规划和避障决策。

工作步骤

• 初始化地图：创建一个初始的栅格地图，每个栅格的占用概率设置为中性值（通常是0.5）。
• 传感器数据获取：从各种传感器（激光雷达、摄像头等）获取环境数据。
• 数据处理：对传感器数据进行预处理，包括噪声过滤、数据融合（多种传感器数据，如激光雷达、摄像头、雷达等）。
• 概率更新：根据传感器数据，使用贝叶斯更新规则或其他概率更新方法，计算每个栅格的占用概率。
• 地图构建和更新：将更新后的占用概率值赋予相应的栅格，构建和更新栅格地图。
• 路径规划和导航：基于更新的栅格地图，进行路径规划和导航，确保避开占用区域。

3）常用的全局路径规划算法

3.1）基于图搜索的全局路径规划算法

Dijkstra 算法

原理：Dijkstra算法由计算机科学家Edsger W. Dijkstra在1956年提出，旨在找到图中一个顶点到其他所有顶点的最短路径，且要求图的边权重为非负；它通过逐步扩展起点到其他节点的最短路径，最终找到到达终点的最短路径。

特点：能保证找到最短路径，适用于所有节点都需要路径信息的场景；但计算效率比较低，计算复杂度： 时间复杂度为O(V2)，对于稀疏图可以通过使用优先队列优化至 O(ElogV)，不适用于大规模图。

算法步骤：Dijkstra算法通过逐步扩展起点到其他节点的最短路径，来构建最短路径树。其基本思想是每次选择未处理节点中具有最短距离的节点，然后通过该节点更新其邻居节点的最短路径估计值。

具体步骤如下：

A)初始化

设定起点到自身的距离为0，即 dist[start] = 0。

设定起点到其他所有节点的距离为无穷大，即 dist[v] = ∞ （其中 v 为图中的节点）。

将所有节点标记为未访问。

B)选择当前节点

从未访问的节点中选择一个具有最短距离估计值的节点 u。

C)更新邻居节点

对于当前节点 u 的每一个邻居节点 v，计算通过 u 到达 v 的距离 alt = dist[u] + weight(u, v)，其中 weight(u, v) 为 u 到 v 的边权重。

如果 alt < dist[v]，则更新 dist[v] 和 predecessor[v]，其中 predecessor[v] 用于记录路径。

D)标记节点

将节点 u 标记为已访问。

E)重复

重复步骤B-D，直到所有节点都被访问或当前节点为目标节点（用于单目的地最短路径问题）。

A* 算法（A-Star Algorithm）

原理：A*算法结合了Dijkstra算法和贪心搜索算法的优点，通过启发式函数引导搜索过程，从而找到从起点到终点的最优路径。其核心是f(n) = g(n) + h(n)，其中g(n)是起点到当前节点的实际代价，h(n)是当前节点到终点的启发式估计。

特点：能够找到从起点到终点的最优路径，计算效率较高；但计算复杂度取决于启发式函数的选择，可能需要较大的计算资源和存储空间，对动态环境适应性差。

算法步骤：A*算法通过启发式函数引导搜索过程，以找到从起点到终点的最优路径。其核心思想是评估每个节点的综合代价，并选择代价最低的节点进行扩展。综合代价函数定义为： f(n)=g(n)+h(n) 其中：g(n) 表示从起点到当前节点 n 的实际代价。h(n) 表示从当前节点 n 到终点的启发式估计代价。

具体步骤如下：

A)初始化

将起点start 加入开放列表（Open List），其代价 f(start)=h(start)。

将起点的 g 值设为0，即g(start)=0。

B)选择当前节点

从开放列表中选择具有最小 f 值的节点current，将其移到闭合列表（Closed List）。

C)目标检测

如果current 为终点节点goal，则算法终止，返回路径。

D)邻居节点处理

对于当前节点current 的每一个邻居节点neighbor，计算邻居节点的临时 g 值：gtemp= g(current)+d(current,neighbor)。

如果 neighbor 在闭合列表中且gtemp​≥g(neighbor)，则跳过该邻居节点。

如果neighbor 不在开放列表中或gtemp​<g(neighbor)，则更新邻居节点的 g 值和父节点信息，并计算 f(neighbor)=g(neighbor)+h(neighbor)。如果邻居节点不在开放列表中，则将其加入开放列表。

E)重复

重复步骤B-D，直到找到终点节点或开放列表为空（表示无可行路径）。

3.2）基于随机采样的全局路径规划算法

RRT 算法（Rapidly-exploring Random Tree）

• 原理： RRT通过在状态空间中随机采样点并构建树状结构，逐步扩展路径，从起点延伸到终点或覆盖整个搜索空间。
• 特点：适用于高维空间和复杂环境，能快速生成路径；但生成路径可能不够光滑，需要后续优化，经常应用于机器人运动规划、无人机导航等。

PRM 算法（Probabilistic Roadmap）

• 原理： PRM通过在自由空间中随机采样生成节点，并通过连接可行的路径构建一个路网图，之后在该图上进行路径搜索。
• 特点：适用于高维空间，能处理复杂的自由空间；但依赖于采样密度，路径质量和计算效率受采样数量影响。

3.3）其它全局路径规划算法

蚁群算法（Ant Colony Optimization, ACO）

原理：蚁群算法模拟了真实蚂蚁在寻找食物时通过信息素进行路径选择的过程，蚂蚁在行走时会在路径上留下信息素，其他蚂蚁倾向于选择信息素浓度更高的路径。

特点：适应性强，能有效处理复杂的环境；但收敛速度慢，容易陷入局部最优解。

算法步骤

• 初始化：生成一定数量的蚂蚁，并随机分配到起点。
• 路径选择：每只蚂蚁根据路径上的信息素浓度和启发式信息选择下一步移动的方向。
• 信息素更新：蚂蚁完成路径后，在经过的路径上增加信息素，同时根据信息素挥发机制减弱所有路径上的信息素浓度。
• 迭代优化：重复上述过程，逐渐增加最佳路径上的信息素浓度，使得最终找到最优路径。

粒子群算法（Particle Swarm Optimization, PSO）

原理：粒子群算法模拟了鸟群或鱼群在寻找食物时的群体行为。每个粒子代表一个解，粒子通过个体和群体的经验不断调整自己的位置以找到最优解。

特点：实现简单，收敛速度较快；但容易早熟，陷入局部最优。

算法步骤

• 初始化：随机生成粒子群，每个粒子有初始位置和速度。
• 速度更新：根据粒子的自身经验和群体经验更新每个粒子的速度。
• 位置更新：根据新的速度更新每个粒子的位置。
• 评估和更新最优解：评估每个粒子的新位置，并更新个体最优解和全局最优解。
• 迭代优化：重复上述过程，直到满足终止条件。

遗传算法（Genetic Algorithm, GA）

原理:遗传算法模拟了自然选择和遗传学原理，通过选择、交叉和变异操作在解空间中搜索最优解。

特点:全局搜索能力强，不易陷入局部最优；但参数选择较复杂，计算量大。

算法步骤

• 初始化：随机生成初始种群，每个个体代表一个解。
• 评估适应度：计算每个个体的适应度值。
• 选择：根据适应度值选择个体进行繁殖，适应度高的个体有更高的概率被选中。
• 交叉：选中的个体配对，通过交叉操作生成新的个体。
• 变异：对新生成的个体进行变异操作，以引入新的基因多样性。
• 更新种群：用新生成的个体替换旧种群中的一些个体。
• 迭代优化：重复上述过程，直到满足终止条件。

总结下，在实际应用中，全局路径规划通常结合多种算法以达到最优效果。目前使用最多的是全局规划算法主要是A*、Dijkstra、遗传算法，生成从起点到终点的全局路径。

2 行为决策

行为决策是自动驾驶系统中一个关键的部分，负责在感知预测模块提供的环境信息等原始数据基础上，做出驾驶决策，包括确定车辆的行驶路线、避障、变道、停车等操作，确保车辆在不同的交通情境中做出合理、安全的决策。这些输入的原始数据包括车辆自身信息（位置、速度、加速度、形势方向）、周围障碍物信息、实时交通状态和地图信息、交通规则、历史信息、全局规划模块的输出等。

1）行为决策的工作步骤

• 环境理解： 收集和分析来自感知模块的数据，包括道路、车道线、障碍物、交通标志、行人和其他车辆的位置和动态信息。
• 情景评估： 根据环境理解结果，评估当前驾驶情境，例如前方有无障碍物、交通流情况、行人动态等。
• 目标选择： 根据全局路径规划的输出行驶任务（如导航路径）和当前情境，选择具体的驾驶目标，如前进、变道、停车等。
• 决策制定： 结合驾驶规则和策略，制定具体的操作决策，包括速度控制、转向控制等。

2）行为决策的方式

2.1）基于规则的行为决策方法

基于规则的行为决策方法使用预定义的规则和逻辑来做出驾驶决策，这些规则通常是通过专家知识和驾驶法规制定的。

有限状态机（FSM, Finite State Machine）

• 原理：使用一组有限状态和转换规则来表示不同的驾驶情境和对应的决策。
• 特点：简单、直观，易于理解和实现。

决策树（Decision Tree）

• 原理：使用树状结构表示决策过程，每个节点表示一个判断条件，每个叶节点表示一个具体决策。
• 特点：清晰的层次结构，便于解释和调试，适用于决策条件较为明确的情况。

规则库系统

• 原理：通过预先定义的一组规则来进行推理和决策。每条规则包含条件和相应的行动。
• 特点：灵活性高，易于扩展和维护，适用于复杂的驾驶场景。

2.2）基于强化学习的行为决策方法

基于强化学习的行为决策方法通过与环境的交互学习最佳决策策略，适用于复杂、动态和不确定的驾驶环境。

Q学习（Q-Learning）

• 原理：通过对状态-行动对的价值进行更新，学习一个策略，使得长期回报最大化。使用 Q 表示每个状态-行动对的价值。
• 特点：无模型的强化学习算法，适用于离散状态空间；学习过程较为缓慢。

深度Q网络（DQN, Deep Q-Network）

• 原理：DQN是一种结合了深度学习和强化学习的算法，由DeepMind公司的研究人员在2015年提出，利用神经网络来近似Q值函数，从而实现对复杂环境下的动作决策，支持处理高维和连续状态空间。
• 特点：能处理复杂的输入数据（如图像），适用于高维状态空间；需要大量训练数据和计算资源。

策略梯度方法（Policy Gradient Methods）

• 原理：直接优化策略函数，通过梯度上升（或下降）来最大化（或最小化）策略的预期回报。
• 特点：适用于连续动作空间和复杂策略。需要平衡探索和利用。

演员-评论家算法（Actor-Critic Methods）

• 原理：结合策略梯度和价值函数方法，使用演员（Actor）选择行动，评论家（Critic）评估行动。
• 特点：改进了策略梯度方法的效率，能更快地收敛到最优策略。

总结下，DQN是自动驾驶领域中比较主流的行为决策算法，在训练中通过神经网络来估计状态-动作值函数，从而实现对驾驶行为的决策。

3 运动轨迹规划

运动轨迹规划的目标通常是生成一条满足最短路程、无碰撞、可执行的轨迹。这种轨迹需要考虑驾驶安全性和乘坐舒适性，因此它不仅要求车辆能够顺利行驶，还需要确保乘客舒适，并且要避免与障碍物发生碰撞。

1）DWA（Dynamic Window Approach）

DWA是一种基于局部轨迹规划的方法，旨在在当前车辆状态和环境感知的基础上，生成一组动态窗口（Dynamic Window），并在这些窗口内搜索可行的速度和转向组合，以选择最优的行动，从而避免碰撞并实现到达目标位置。

特点：DWA算法通过限制搜索空间和使用简单的代价函数，能够在较短的时间内生成行动并做出决策，适用于实时控制应用；能够根据当前环境动态调整速度和转向，以应对不同的环境条件和车辆状态。

算法步骤

• 动态窗口生成：首先，根据车辆动力学特性和环境感知信息，生成一个动态窗口，该窗口表示了车辆在当前时刻内可能到达的速度和转向范围。
• 运动控制搜索：在动态窗口内搜索一组候选的速度和转向组合。这通常包括对每个速度和转向组合进行评估，计算其对应的代价函数值，如路径长度、安全性、舒适性等。
• 选择最优行动：根据代价函数的值，选择最优的速度和转向组合作为当前时刻的行动，执行该行动以更新车辆状态，并重复上述过程。

2）基于曲线拟合的算法

通过曲线拟合方法生成平滑的轨迹，来确保车辆在路径上的平稳运动，适用于各种复杂环境。很多轨迹规划算法都来源于机器人领域的路径规划算法的扩展，线段连接点存在不连续问题，导致车辆运动过程的不联系，因此目前多数厂商在做归控时都会对输出局部轨迹作进一步优化，使用基于曲线拟合的算法得到更加平滑的运动轨迹。

2.1）多项式轨迹生成

原理：多项式轨迹生成利用高次多项式函数来拟合路径，以生成平滑的轨迹，通常使用三次或更高次的多项式。

特点：高次多项式能够生成非常平滑的轨迹，而且可以通过调整多项式的系数来控制路径形状，相对较简单的数学形式，容易实现。

算法步骤：

• 确定多项式的阶数（通常是三次或五次）。
• 设定初始和终点的边界条件（位置、速度、加速度）。
• 使用线性方程组求解多项式的系数。
• 根据求得的多项式系数生成轨迹点。

2.2）贝塞尔曲线（Bezier Curve）

原理：贝塞尔曲线通过一组控制点定义曲线形状，常用于图形学和路径规划中，常见的是二次贝塞尔曲线和三次贝塞尔曲线。

特点：通过调整控制点位置可以轻松调整曲线形状，生成的曲线平滑且易于控制，增加控制点可以逐步精细地控制曲线形状。

算法步骤：

• 确定控制点。
• 使用贝塞尔曲线公式计算曲线上的点：二次贝塞尔曲线：B(t)=(1−t)2P0+2(1−t)tP1+t2P2，三次贝塞尔曲线：B(t)=(1−t)3P0​+3(1−t)2tP1​+3(1−t)t2P2​+t3P3。
• 调整控制点生成平滑路径。

2.3）三次样条曲线（Cubic Spline Curve）

原理：三次样条通过分段三次多项式连接一组数据点，确保在每个分段内曲线的连续性和光滑性。

特点：曲线及其一阶和二阶导数在分段之间是连续的；改变一个点的坐标只影响相邻的曲线段，适用于数据点较多的情况。

算法步骤：

• 确定样条的节点（数据点）。
• 构建并求解样条方程组，确保在每个节点处曲线及其导数连续。
• 使用求解得到的多项式生成曲线。

2.4）Dubbins曲线（Dubins Curve）

原理：Dubbins曲线用于描述具有最小转弯半径的车辆从一个点到另一个点的最短路径，由直线和圆弧段组合而成。

特点：仅由直线和圆弧组成，适合路径约束简单的场景；总是能找到最短路径，但路径可能不够光滑；适用于固定转弯半径的车辆，如无人机和固定翼飞机。

算法步骤：

• 确定起点和终点以及初始和最终方向。
• 根据几何关系计算可能的路径组合（如LSL、LSR等）。
• 选择最短路径作为最终路径。

2.5）Reeds-Shepp曲线（Reeds-Shepp Curve）

原理：Reeds-Shepp曲线是Dubbins曲线的扩展，允许车辆倒车，适用于非全向性车辆在平面上的路径规划。

特点：允许倒车，路径选择更多；路径可能包含多个直线和圆弧段，计算复杂；适用于需要高机动性的车辆，如自动驾驶汽车。

算法步骤：

• 确定起点和终点以及初始和最终方向。
• 根据几何关系计算可能的路径组合，包括前进和后退段。
• 选择最短路径作为最终路径。