自动驾驶之轨迹规划1——算法综述

1. 背景概述

本文立足于智能车领域的轨迹规划，根据自己的整理和理解输出，权当做一篇学习笔记。这篇只是综述，每种算法的详细过程会在别的篇幅整理出来。
首先解释一下一些基本概念：

规划（planning）承接环境感知，并下启车辆控制。其规划出来的轨迹是带速度信息的路径。广义上，规划（planning）可分为路由寻径（routing）、行为决策（behavioral decision）、运动规划（motion planning）。

路由寻径（routing）：是全局路径规划，可简单的理解为传统地图导航+高精地图（包含车道信息和交通规则等）；
行为决策（behavioral decision）:决策车辆是否跟车、在遇到交通灯和行人时的等待避让、以及路口和其他车辆的交互通过；
运动规划（motion planning）：是局部路径规划，是无人车未来一段时间内的期望行驶路径，需满足汽车运动学、动力学、舒适性和无碰撞等要求。

2. 轨迹规划综述

轨迹规划的任务是计算出一个无碰撞可执行的轨迹（包含路径和速度信息），保证车辆从起点安全的驾驶到目的地，并尽可能高效。其问题的本质是一个多目标的数学优化问题。

主要的优化目标包括：
安全性：避免与场景中的障碍物发生碰撞；针对动态障碍物，由于其未来运动的不确定性，降低其未来的碰撞风险；
稳定性：由于车辆的惯性较大，灵活性差，期望轨迹需要保证车辆的物理可行性和控制器的稳定性；
舒适性：考虑到乘员的舒适性，需要在满足安全性和稳定性的同时保证车辆的驾驶舒适度，包括加减速以及转向等过程；
驾驶效率：在满足安全性和稳定性的同时，保证车辆以更快的速度驾驶，从而更短的时间到达目的地。

在实际场景中，规划过程需要考虑各种物理约束，有且不限于：
加减速度约束：受到动力系统和制动系统的性能极限，及驾驶员的安全性和舒适性的制约；
非完整性约束：车辆具有三个运动自由度，但是只有两个控制自由度，其非完整性约束决定了轨迹的物理可行性；
动力学约束：考虑到车辆的动力学特性和车身稳定性，其驾驶过程中的曲率和横摆角速度具有一定的约束；

当然，在工程中，解决特定场景的规划问题，可以进行一定程度的简化。下面针对几种应用较为广泛算法，然后针对性的介绍实际场景（APA和换道）。
2.1 基于图搜索的算法：Dijkstra、A*、D*（全局路径规划）
2.2 基于曲线拟合的算法：圆弧与直线、多项式曲线、样条曲线、贝塞尔曲线、微分平坦（局部路径规划）
2.3 基于数值优化的算法：利用目标函数和约束对规划问题进行描述和求解（局部路径规划）
2.4 基于人工势场的算法：人工势场法（全局路径规划）
2.5 基于采样的算法：RRT（全局路径规划）
2.6 基于智能法的算法：模糊逻辑、神经网络、遗传算法（略）

2.1 基于图搜索的算法

将环境进行栅格化，一条路径可以利用图搜索的算法来访问栅格图中的节点，从而得到规划

2.1.1 Dijkstra

Dijkstra算法的主要特点是以起始点为中心向外层层扩展（广度优先搜索思想），直到扩展到终点为止。迪杰斯特拉算法的成功率是最高的，因为它每次必能搜索到最优路径。但迪杰斯特拉算法的搜索速度是最慢的：随着图维度的增大，其计算效率会明显变低。

基本步骤：

1. 指定起点s。
2. 引进两个集合S和U。S的作用是记录已求出最短路径的顶点（以及相应的最短路径长度），而U则是记录还未求出最短路径的顶点（以及该顶点到起点s的距离）。
3. 初始时，S中只有起点s；U中是除s之外的顶点，并且U中顶点的路径是“起点s到该顶点的路径”。然后，从U中找到路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。
4. 重复该操作，直到遍历完所有顶点。
   

2.1.2 A*

Dijkstra算法是广度优先算法，是一种发散式的搜索，搜索速度是很慢。这里引入一种启发式算法的深度优先算法：A*。其基本思想：

基本步骤：

1. 把起点加入 open list 。
2. 重复如下过程：
   a ) 遍历 open list ，查找 F 值最小的节点移到 close list ，把它作为当前要处理的节点。
   b ) 判断当前方格的 8 个相邻方格的每一个方格，若为unreachalbe或者已在 close list 中则忽略。否则做如下操作。
   c ) 如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。
   d ) 如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。
3. 直到openlist为空，从终点开始，每个方格沿着父节点移动直至起点，这就是最优路径。
   

Dijkstra与A star对比

A star算法的现实意义

A*他是导航的基础算法（如百度地图）：

1. 规划的第一步是路线导航。侧重于研究如何从地图上的A点前往B点：如手机导航系统。Apollo中通过路线规划模块处理该任务。
2. 规划的第二步是：在路线规划的基础上进行轨迹规划。该轨迹由一系列点定义，每个点都有一个关联速度和一个指示何时应抵达那个点的时间戳。Apollo通过规划模块处理该任务。
3. 路线规划的目标是，找到从地图上的A前往B的最佳路径。轨迹规划的目标是找到避免碰撞和保持舒适度的可执行轨迹。
4. 下图形由“节点”(node)和“边缘”(edge)组成。节点代表路段，边缘代表这些路段之间的连接。例如：在交叉路口，汽车可从节点1移动到节点2、节点3或节点4，反之亦然。
5. 我们可以对一个节点移动到另一个节点所需的成本进行建模。例如在现实生活中，拐过一个交叉路口比直行更费劲，所以从节点1到节点4的成本高于从节点1到节点3的成本.

备注：Apollo中的道路是车道线级别的，所以apollo中的node就是一条lane，而边则是车道和车道之间的连接，点对应具体的车道，而边则是一个虚拟的概念，表示车道之间的关系。下面我们可以先看下apollo中道路(road)和车道(lane)的概念。

假设我们到达了一个交叉路口，我们可以沿着公路直走、左转或右转。首先，我们将把这张地图转换为具有三个候选节点（left, straight, right）的图形。接下来，我们将对选项进行评估。在实践中，拐过交叉路口很费劲，所以我们为left节点分配了更高的g值（g值表示从起始点到候选节点的成本）。在查看公路选项之后，我们意识到必须走很长的路，才能离开公路并返回我们的目标，所以我们为straight选项分配了更高的h值（h值表示从候选节点到目的地的估计成本）。最后，我们通过将g值和h值相加来计算每个节点的f值。我们看到最低f值实际对应右边的候选节点。所以，这是我们接下来要前往的节点。

2.2 基于曲线拟合的算法

这块会在后面的APA和超车中整理出来。

基本方法：

1. 余弦曲线：在起始位置和终点位置二次导数取最大值（加速度为最大值），舒适性差；
2. 多项式曲线：可避免圆弧直线路径中曲率不连续的问题 ，达到曲率连续变化；
3. 圆弧及公切线：路径曲率不连续，车辆到达曲率间断点处时需停车转向，否则因方向盘转速和车速的影响，车辆将偏离目标路径；
4. 贝塞尔曲线：对路径进行平滑处理，达到曲率变化率连续性；
5. B样条曲线：对路径进行平滑处理，达到曲率变化率连续性；
6. 微分平坦：对路径进行平滑处理，达到曲率变化率连续性。

2.3 基于数学优化的算法

这一块儿是想基于Apollo整理，但是内容比较大，这里只概述一下，后续会用专门的一篇整理Apollo的rounting和motion planning（自动驾驶之轨迹规划2——Apollo规划与控制公开课）和自动驾驶之轨迹规划6——Apollo EM Motion Planner。Apollo的决策规划模块是“轻决策重规划”，且有好几种planner，RTK planner是基于录制的轨迹规划行车路线，EM planner是路径和车速分层规划，lattice planner是直接高维轨迹规划（路径和车速一起规划）。这里先只讲下EM planner。

在规划过程中解决决策问题。一般先是由rounting模块进行全局规划，得出reference line，然后motion planning在此基础上进行局部轨迹规划。motion planning将路径和车速分层规划，并在SL和ST坐标系下，使用动态规划进行路径和车速的决策和粗规划，然后使用二次规划进行平滑处理。

动态规划：使用动态规划的原因是Apollo把道路进行切片撒点，把轨迹问题变成分段最优问题，即动态规划中的最优子结构。
二次规划：使用二次规划的原因是Apollo把路径和车速平滑性，以平方项的方式进行量化，由此转化为二次规划问题。

2.4 基于人工势场的算法

APF假设车辆在一种虚拟力场下运动：车辆的初始点在一个较高的“山头”上，要到达的目标点在“山脚”下，这就形成了一种势场，车辆在这种势的引导下，避开障碍物，到达目标点。

当然，人工势场法（APF）也有缺点：可能被困在局部最优解。

2.5 基于采样的算法

RRT随机树：快速随机地扩张，一群像树一样的路径以探索（填充）空间的大部分区域，伺机找到可行的路径。

基本步骤：

1. 起点作为一颗种子，从它开始生长枝丫；
2. 在车辆所处的空间中，生成一个随机点 ；
3. 在树上找到距离 最近的那个点；
4. 朝着的方向生长，如果没有碰到障碍物就把生长后的树枝和端点添加到树上，返回 2；
   

但RRT缺点也很明显：

1. RRT 得到的路径一般质量都不是很好，例如可能包含棱角，不够光滑；
2. 通常也远离最优路径；
3. 难以在有狭窄通道的环境找到路径。因为狭窄通道面积小，被碰到的概率低，找到路径需要的时间要看运气。

3. 应用场景

3.1 低速场景：APA

大致分为3类：
基于轨迹规划：需额外考虑速度规划，APA为低速场景，该算法复杂一般不用。
基于路径规划：正弦曲线、多项式曲线、圆弧及公切线、贝塞尔曲线、B样条曲线。
基于经验：模糊逻辑等。
本篇着重说下以水平泊车为例的圆弧及公切线方法，样条曲线（可实现曲率连续）的解决方法与此类似，这里不再累述。

1. 最小车位分析：
如下图，在泊车过程中，若以最小转弯半径行驶，则车辆右前方B点不得与车库a点碰撞（决定了车库的最小长度），车辆右后方C点不得与车库内侧bc边碰撞（决定了车库的最小宽度）。由此得到两个不等式（如下图）

用matlab分析后发现：车辆泊车所需车位尺寸与车辆转弯半径相关，所需车位长度随转弯半径增大而增大，宽度随转弯半径增大而减小。

代码如下：

clc;
clear;

Lr=0.95;
Lk=1.645;
Lf=0.8;
L=2.405;

Lad=zeros(60,1);
Lab=zeros(60,1);
Rv=zeros(60,1);

for i=1:60
    R=4+i/10;
    ladmin=Lr+((R+Lk/2)^2+(L+Lf)^2-(R-Lk/2)^2)^0.5;
    labmin=Lk/2+((R+Lk/2)^2+Lr^2)^0.5-R;
    
    Rv(i,1)=R;
    Lad(i,1)=ladmin;
    Lab(i,1)=labmin;
end

% h=figure();
% plot(Rv,Lad,'linewidth',2);
% title('车位长度');
% xlabel('实际转弯半径 / m');
% ylabel('车位长度 / m');
% grid on;

h=figure();
plot(Rv,Lab,'linewidth',2);
title('车位宽度');
xlabel('实际转弯半径 / m');
ylabel('车位宽度 / m');
grid on;

frame = getframe(h); % 获取frame
img = frame2im(frame); % 将frame变换成imwrite函数可以识别的格式
imwrite(img,'车位宽度.jpg');