分享来源：

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

路由寻径（Routing）：是全局路径规划，可简单的理解为传统地图导航+高精地图（包含车道信息和交通规则等）；

行为决策（Behavioral Decision）:决策车辆是否跟车、在遇到交通灯和行人时的等待避让、以及路口和其他车辆的交互通过；

运动规划（Motion Planning）：是局部路径规划，是无人车未来一段时间内的期望行驶路径，需满足汽车运动学、动力学、舒适性和无碰撞等要求。

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

主要的优化目标包括：

安全性：避免与场景中的障碍物发生碰撞；针对动态障碍物，由于其未来运动的不确定性，降低其未来的碰撞风险；

稳定性：由于车辆的惯性较大，灵活性差，期望轨迹需要保证车辆的物理可行性和控制器的稳定性；

舒适性：考虑到乘员的舒适性，需要在满足安全性和稳定性的同时保证车辆的驾驶舒适度，包括加减速以及转向等过程；

驾驶效率：在满足安全性和稳定性的同时，保证车辆以更快的速度驾驶，从而更短的时间到达目的地。

在实际场景中，规划过程需要考虑各种物理约束，有且不限于：

加减速度约束：受到动力系统和制动系统的性能极限，及驾驶员的安全性和舒适性的制约；

非完整性约束：车辆具有三个运动自由度，但是只有两个控制自由度，其非完整性约束决定了轨迹的物理可行性；

动力学约束：考虑到车辆的动力学特性和车身稳定性，其驾驶过程中的曲率和横摆角速度具有一定的约束；

当然，在工程中，解决特定场景的规划问题，可以进行一定程度的简化。下面针对几种应用较为广泛算法，然后针对性的介绍实际场景（APA和换道）。

1、基于图搜索的算法

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

1.1 Dijkstra

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

基本步骤：

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

1.2 A*

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

基本步骤：

• 把起点加入 Open list 。
• 重复如下过程：

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 值。

• 直到Openlist为空，从终点开始，每个方格沿着父节点移动直至起点，这就是最优路径。

Dijkstra与A star对比

A Star算法的现实意义

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

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

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

2、基于曲线拟合的算法

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

基本方法：

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

3、基于数学优化的算法

这一块儿是想基于Apollo整理，但是内容比较大，这里只概述一下，后续会用专门的一篇整理Apollo的Rounting和Motion Planning

Apollo规划与控制公开课：

【自动驾驶】运动规划丨论文分享丨Baidu Apollo EM Motion Planner37 赞同 · 0 评论文章

Apollo的决策规划模块是“轻决策重规划”，且有好几种Planner，RTK Planner是基于录制的轨迹规划行车路线，EM Planner是路径和车速分层规划，Lattice Planner是直接高维轨迹规划（路径和车速一起规划）。这里先只讲下EM Planner。

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

动态规划：使用动态规划的原因是Apollo把道路进行切片撒点，把轨迹问题变成分段最优问题，即动态规划中的最优子结构。

二次规划：使用二次规划的原因是Apollo把路径和车速平滑性，以平方项的方式进行量化，由此转化为二次规划问题。

4、基于人工势场的算法

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

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

5、基于采样的算法

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

基本步骤：

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

但RRT缺点也很明显：

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

强化学习/动态规划：贝尔曼最优求解Bellman Equation

clear all;clc
%贝尔曼最优原理V(x1)=min/max(L1+V(x2))
num_states=[1 2 3 3 1];%表示从A-J每列的元素个数
%A:=(1,1)

%B:=(2,1)
%C:=(2,2)

%D:=(3,1)
%E:=(3,2)
%F:=(3,3)

%G:=(4,1)
%H:=(4,2)
%I:=(4,3)

%J:=(5,1)
%%
distance=inf(num_states);%定义高维矩阵
%1
distance(1,1,2,1)=12;%A-B
distance(1,1,2,2)=7; %A-C
%2
distance(2,1,3,1)=5; %B-D
distance(2,1,3,2)=6; %B-E
distance(2,1,3,3)=9; %B-F

distance(2,2,3,1)=14; %C-D
distance(2,2,3,2)=10; %C-E
distance(2,2,3,3)=11; %C-F
%3
distance(3,1,4,1)=8;  %D-G
distance(3,1,4,2)=7;  %D-H
distance(3,1,4,3)=10; %D-I

distance(3,2,4,1)=9;  %E-G
distance(3,2,4,2)=7;  %E-H
distance(3,2,4,3)=9;  %E-I

distance(3,3,4,1)=10; %F-G
distance(3,3,4,2)=7;  %F-H
distance(3,3,4,3)=8;  %F-I
%4
distance(4,1,5,1)=5;  %G-J
distance(4,2,5,1)=9;  %H-J
distance(4,3,5,1)=8;  %I-J

%% 动态规划
V=inf(5);%定义5x5的矩阵
V(5,1)=0;%J处的COST
%动态规划倒推循环
for k=4:-1:1
    for i=1:num_states(k)
        path_len=zeros(num_states(k+1),1);
        for j=1:num_states(k+1)
         path_len(j)= distance(k,i,k+1,j)+V(k+1,j);
        end
        [V(k,i),ind]=max(path_len);
    end
end
%%
%显示A点最优成本函数
disp(['optimal cost at A:',num2str(V(1,1))])

%找到最优路径
optimal_path=zeros(5,1);
for k=1:5
    [~,optimal_path(k)]=max(V(k,:));
end
disp('optimal path')
disp(optimal_path)