目录
Motion Planning的约束条件(constraints)
分级运动规划器(Hierarchical Motion Planning)
前言
自动驾驶中的路径规划是一种顶层导航问题的上层模块，主要通过一系列车道级引导规划，解决车辆从起点位置移动至终点位置。一般的路径规划与控制模块外围是交通预测模块，其输入是感知到的环境信息，包含如车道线、道路曲率、道路障碍物、交通标志牌等环境静态信息，亦或是道路移动车辆信息（如速度、加速度、距离等）；而路径预测得到的输出信息包含时间信息以及空间信息，预测可以优化感知模块输出，或者对决策/规划模块输入到物体进行预处理。
几个高频面试题目
无人车运动规划，路径规划，轨迹规划的区别和联系？
路径规划一般指对于行驶路线的规划，不涉及时间上的求解；轨迹规划要把时间考虑进来，即自车在什么时刻应该处在什么位置。
目前常用的轨迹求解方法是先路径规划与速度规划解藕进行，最后再和成为轨迹。
路径规划给出的是车辆位置的空间序列（X(s)和Y(s)，s是里程station），轨迹规划给出的是车辆位置的时空序列（X(t)和Y(t)）。对于同一条路径，以不同的速度通过，对于车辆来说其实是不同的轨迹。先规划路径再规划速度（称为path-velocity decomposition），是轨迹规划的一个常用手段，相当于将X(t)和Y(t)写成了X(s)、Y(s)以及s(t)的参数形式。当然也有一些轨迹规划方法是直接规划X(t)和Y(t)，比如基于MPC的方法。
路径规划是在忽略临时或移动障碍物的前提下做出的规划，解决的是走什么路线到达目的地的问题，和你打开地图软件，输入起点终点输出路线是一个道理。轨迹规划是在考虑临时或移动障碍物的前提下做出的规划，解决的是如何在避开障碍物的同时尽量沿着路径走的问题，和你看到地图上指示的路线后虽然是沿着路线走但是仍需要看路看车看人看红绿灯一样，重点在避障和遵守交规。所以说路径规划在前，轨迹规划在后，这两者是承前启后的关系。从尺寸上讲，路径规划是大尺寸，轨迹规划是小尺寸。路径规划使用的地图是静止的，而轨迹规划使用的地图是动态的。都是站在当前状态对未来的规划，都可以是三维的。
运动规划结果的作用域都在两个状态之间。因为无人车的运动规划问题通常是花多少时间到达什么地方，或者仅仅是到达什么地方，而不可能仅仅是花多少时间。所以状态可以没有时间但不能没有空间，路径和轨迹都可以不含时间信息，只包含空间信息，也就是说速度规划并不是必须的。但是在有移动障碍物的场景下，无人车在特定的状态下必须有特定的速度才能避开障碍物，因此才有了轨迹配速问题。
再回答为什么有的论文没有路径规划，可能是因为是室内机器人，园区机器人等小型封闭空间内的运动规划，或者是其他缺少移动障碍物的场景，所以轨迹约等于路径，没有两者之分。
无人车的自动控制流程如下，路径规划→轨迹规划→轨迹跟随其中路径规划主要依据地图信息，未来可能加入V2X，技术已经成熟。轨迹跟随属于传统的汽车自动控制领域，控制目标是车速和航向，由此可以分为纵向控制和横向控制。轨迹规划依据的信息来源多种多样，有传感器、高精度地图、V2V等等，目前以传感器为主，技术难点较多，是研究重点。有三个方向。
- 1.人工设计规则，靠工程师大量试验总结经验然后编成代码。现在应该没多少人是这么干了，但是这种方法仍然被大量采用作为打补丁的方案。特点是对于特定路况有奇效。
- 2.建模，工程师设计出各种数学模型来描述避障问题，例如人工势场法、栅格法等等。但是数学模型往往不能尽如人意，因为传感器往往是不准的，所以需要上面所提的人工设计规则来打补丁。
- 3.深度学习。如果没有精确的传感器信息来建立数学模型，那么让机器主动学习也是一种办法。这里又有两个方案。
一个是监督学习，让机器学习人类的驾驶经验，工程师采集大量人驾驶汽车的数据让机器学习。难点在于如何采集到足够的真实数据。对于特种车辆而言比如外卖无人车可能就没有办法拿到真实数据。
一个是强化学习，让机器在真实环境或仿真环境中自主学习，无需工程师参与。难点在于真实环境中学习可能不是把车撞坏了就是把其他东西撞坏了。而在仿真环境中，场景和传感器又不是那么真实，很影响强化学习结果。
算法原理
路径规划算法原理
1.建立轨迹规划坐标系
首先，是对自车建立可靠的坐标系，一般情况下有两种坐标系建立方式。1）比较简单且常用的坐标系是X-Y-Z三向，该坐标系采用美国机动车工程师学会(SAE)的设定，以自车车辆中心为原点，以车辆行驶方向的前后为 x 轴，且向前为正，以车辆行驶方向的两侧为 y 轴，且向右为正，以车辆行驶方向的上下方向为 z 轴，且向上为正。
2）针对规划控制比较好理解的坐标建立方式Frenet坐标系，该坐标系使用道路中心线作为参考线，使用参考线的切线向量和法线向量建立坐标系，Frenet–Serret公式用于描述粒子在三维欧氏空间R3内沿一条连续可微曲线的运动学特征，该坐标系在规划模块中广泛使用。
2、建立初始规划轨迹
路径规划的过程是包含了两个层面，其一是基于行车自动驾驶初始激活状态建立相应的初始运动轨迹规划，它包含如下几个步骤进行。
1） 将车辆位置及参考线参照如上定义并选择的坐标系进行车辆坐标系转化；
2） 按设定规则固定采样选取目标点；
3） 利用曲线插值或拟合方法生成备选轨迹；
4） 将备选轨迹进行膨胀计算剔除可能不合理的轨迹；
5） 基于控制量平滑或偏差最小等代价函数选择最优轨迹；
如下将对上面的轨迹规划过程进行一一介绍。
（1） 图搜索基础路径：
传感器识别的连续环境信息一般属于模拟信号，需要首先采用一定的图搜索路径规划算法将其转换为适应于所选路径规划算法的离散图，然后运用一定搜索算法得到基础路径。一般针对视频图的搜索算法包含如下：
Dijkstra算法：设置初始节点集，从目标车所在的初始点开始，迭代的检查结点集中的所有结点，并把和该结点最靠近的尚未检查的结点加入待检查结点集。该结点集从初始结点向外扩展，直到到达目标结点。
A*算法：设置代价函数g(n)表示为从初始结点到结点n的代价，h(n)表示从结点n到目标点的启发式评估代价。结合靠近初始点的结点和靠近目标点的结点的信息块。当从初始点向目标点移动时， A*权衡这两者得到最优路径。
（2）采样点生成路径
采样点生成实际是针对自车在某个设定构型空间生成样本点，寻找满足任务需求的样本点序列作为规划结果；采样方法分为随机采样和固定采样两种。随机采样是指在构型空间中生成随机性的样本点的方法，该方法在规划空间内随机选取N个节点，之后连接各节点，并去除与障碍物接触的连线，由此得到一个可行路径。固定采样则是按照明确给定的规则生成一系列待选样本点，并通过筛选选定其中质量最佳的样本。典型的应用是采用触须算法规划车辆局部轨迹，触须过程即是试探性的尝试多种方向控制方案，通过建立综合考虑运动可行性、平滑程度、安全性能的评价指标来选拔理想的行驶路径，摒除危险或不可用路径。
3. 轨迹曲线插值
一般情况下，针对规则型道路（即道路宽度或曲率等因素不存在突变），无论针对对中控制还是换道，车辆轨迹曲线往往与道路轨迹曲线是保持一致的运动规律，如下图表示了适合用轨迹曲线描述的车道类型。利用车道转弯半径所对应的圆弧确定行驶路径可以完全适配于车辆行驶路径，将行驶路径显式地表述为数学函数，通过确定函数的参数来决定行驶路径的具体形态。
ü 曲线元素组合方法：
（1） Dubins曲线；
（2） Reeds-Shepp曲线
（3） Clothoid 曲线 ( 即回旋曲线 ) 与圆弧段结合方式；
以Rees-Shepp曲线为例，该曲线由几段半径固定的圆弧表示汽车转弯的最小弯道半径和一段直线表示直行车道的线拼接组成；假设车辆以固定的半径转向，且车辆能够前进和后退，那么Reeds-Sheep曲线就是车辆在上述条件下从起点道终点的最短路径，该曲线不仅能够表示所能到达的终点，而且能够保证车辆的角度在终点所能到达的预期角度， Dubins曲线与Reeds-Sheep曲线设置方式相似，但是，Dubins多了一个约束条件从而导致汽车只能朝前开，不能后退（即不能挂倒挡）。
如果单就比较简单的几个驾驶场景而言，该曲线元素组合的方法完全可以满足拟合要求。但是针对存在障碍物的情况，如上曲线就不再满足要求。目前，针对路径规划障碍物检测的方法中，采用回旋曲线与圆弧段结合的方式是一种比较常规的方法。比如在自动换道中，该方法考虑的轨迹曲线预测中需要首先考虑驾驶场景下的道路曲率，然后再对限定状态下的道路进行对中曲线进行规划，其次，再利用自车道与目标车道的实际情况对换道曲线进行预测。最后将两者进行无缝衔接组合就形成了最终的轨迹曲线。当然实际过程中的圆弧段既可以采用最为简单的抛物线也可以采用比较简单的低阶回旋曲线。如下图分别清晰的表示回旋曲线与圆弧曲线的组合表示方式。除开驾驶路径规划以外，真正的自动驾驶更要考虑到对于车辆实际行驶路径的规划，也就是自动驾驶车辆如何沿着我们所规划的路径向前行进。这就要求我们具备一定的曲线插值拟合方法，如下列举了相应的曲线拟合方式。
（1） 三次B样条曲线
（2） 基于四阶贝塞尔曲线
（3） 采用五次多项式
对于自动驾驶开发而言，如上几种曲线插值拟合的方式，各自有自己的优缺点，但针对不同的场合，需要从适配度、算力等相关因素共同考虑。一般的，针对检测环境信息生成的离散航迹参考点可利用三次B样条曲线进行拟合，自动驾驶车辆的实际可行使轨迹规划可采用四阶贝塞尔曲线进行拟合，复杂路口的转弯行驶路径或正常变道的行驶路径可以采用5次多项式进行拟合。如下将以具体实例讲解完整的基于采样及曲线插值结合的方式规划驾驶路径，其中曲线拟合采用4阶贝塞尔曲线。对于给定离散点t，可分别得到x(y),y(t)，即该车所在位置的横纵坐标，那么利用四阶贝塞尔曲线表示轨迹的参数化表达式表示如下，其中P0表示曲线起始点，P1-P4表示曲线控制点（其中以控制点参数d1，d2，d3来表示），该四个曲线控制点影响形成曲线的扭曲程度（或运动趋势）：
以自动换道轨迹曲线为例，为了求解上述方程式中的所有参数预测过程中需要设置为初始状态和终止状态，作为以上方程的解集之一。其中，初始状态表示为车辆当前位置状态，终止状态则为按照当前状态和实际车道线位置预测的终止点信息。
如上图所示，对于已知点来说只有P0和P4两个点。由此，曲线经过起始点和终止点分别表示为；
为了求解P3点，则需要设置车辆目标位置约束P4=（xT, yT，ΨT，kT），d4=|P3P4|，同时为了满足目标状态的航向角约束，则第3个控制点的坐标为
以上推导过程已经满足无人车的初始状态及目标状态约束（即四阶贝塞尔曲线起始点、终止点、3个控制点坐标），计算得到四阶贝塞尔曲线，对于其中的自由变量控制参数p=[d1，d4，x2] 进行调整则可以更改生成的4阶贝塞尔曲线走向或称扭曲程度。
车辆路径规划跟随原理
当规划形成了车辆路径后，就需要对车辆如何遵循该路径移动达到最终点，这里我们称车辆行驶规划为运动规划，其中包含两个层面的运动规划：
1） 基于横向的路径规划，横向规划上一般是指轨迹形状规划；
当横向规划已经确定后，可以将横向规划转化为QP问题，即对当前给定范围内的约束条件可以输出相应的最优化结果曲线。对于QP问题实际是通过设置在之前已经检测到的已知条件，然后通过一定滤波算法（比如卡尔曼滤波），求解未来的相关参数，而其中可以设置约束条件来不断优化相应的曲线设置。已知条件：x[t-2],x[t-1],x[t]; 求解：x[1],x[2],…约束：x[i]表示的每个点都不能超越左右边界，且当检测到一定障碍物车辆目标时，x[i]与目标的距离应该大于安全距离Dsafe。由此，相应的QP公式及其优化目标设置如下：
2） 基于纵向的路径规划，纵向规划则是对轨迹规划中的速度进行分配；
运动规划
当虚拟人开始一次漫游时，首先全局规划器根据已有的长期信息进行全局静态规划，确定虚拟人应该经过的最优化路线。然后全局规划器控制执行系统按照该路径运动。在运动过程中，感知系统会持续对周围环境进行感知。当发现动态的物体或未知障碍时，局部规划器根据这些感知到的局部信息，确定短期內的运动。当避障行为的优先级高于沿原路径前进时，局部规划器就能够通过竞争获得执行系统的控制权，使得虚拟人按照局部规划结果运动。完成对当前感知障碍的规避行为后，全局规划器再次取得执行系统的控制权，使得虚拟人重新回到全局规划路径上，继续向目标点运动。
什么是Motion Planning
Motion Planning是在遵循道路交通规则的前提下，将自动驾驶车辆从当前位置导航到目的地的一种方法。
在实际开放道理场景下，自动驾驶要处理的场景非常繁杂：空旷的道路场景、与行人、障碍物共用道理的场景、空旷的十字路口、繁忙的十字路口、违反交通规则的行人/车辆、正常行驶的车辆/行人等等。场景虽然复杂，但都可以拆解为一系列简单行为(behavior)的组合:
将这些简单的行为(behavior)组合起来，就可以完成复杂的驾驶行为。
Motion Planning的约束条件(constraints)
Motion Planning是一个复杂的问题，它的执行过程需要满足很多约束条件：
车辆运动学约束
车辆运动受到运动学约束，比如它不能实现瞬时侧向移动，前驱的车辆必须依赖前轮的转向才能实现变道、转向等操作，在弯道上不能速度过快等等。通常我们采用单车模型(Bicycle Model)对车辆运动进行建模。
静态障碍物(Static Obstacle)约束
静态障碍物(Static Obstacle)是道路上静止的车辆、路面中间的石墩子等车辆不可行驶的区域。Motion Planning需要避开这些静态障碍物，避免与它们发生碰撞。解决碰撞的思路大概有两种：
1）将静态障碍物(Static Obstacle)在网格占位图中表示出来，然后检测规划路线是否与静态障碍物区域相交。
2）将车辆的轮廓扩大，比如扩展成一个圆形，然后检测障碍物是否与Circle发生碰撞。
动态障碍物约束
Motion Planning要实时处理行人、车辆等各种运动的障碍物，避免与障碍物发生碰撞事故。
道路交通规则约束
车辆在道路上行驶必须要遵守车道线约束规则(比如左转专用道只能左转、实线不能变道、路口必须遵守红绿灯的指示)和各种标志标牌的指示。
Motion Planning的优化目标
了解Motion Planning的约束条件之后，需要构造目标优化函数，然后最小化目标函数，从而获得在当前环境下的最优运动轨迹。目标函数的种类有很多，下面枚举一些常用的目标函数。
1）关注路径长度(Path Length)，寻求到达目的地的最短路径。
通过组合设计自己的目标优化函数，从而获得较好的Planning效果。
分级运动规划器(Hierarchical Motion Planning)
Motion Planning是一个异常复杂的问题，所以通常我们把它切分为一系列的子问题(Sub Problem)。比如Mission Planner、Behavior Planner、Local Planner、Vehicle Control等。
Mission Planner
Mission Planner关注High-Level的地图级别的规划；通过Graph Based的图搜索算法实现自动驾驶路径的规划。
Behavior Planner
Behavior Planner主要关注交通规则、其它道路交通参与者(自行车、行人、社会车辆)等等，决定在在当前场景下应该采取何种操作(如停车让行、加速通过、避让行人等等)。
Behavior Planner的实现方式比较常见的有几种：有限状态机(Finite State Machines)、规则匹配系统(Rule Based System)、强化学习系统(Reinforcement Learning)。
有限状态机中的State是各个行为决策，根据对外界环境的感知和交通规则的约束在各个状态之间转换。比如在路口红绿灯的场景，当路口交通灯为红色不可通行时，车辆会首先切换到Decelerate to Stop状态，然后在路口停止线完全停下来，进入Stop状态，并持续在Stop状态等待，直至交通灯变为绿色允许车辆通行，车辆进入Track Speed状态，继续前行。
Rule-Based System是通过一系列的分级的规则匹配来决定下一步的决策行为。比如交通灯绿色->通行；交通灯红色->停车等待。基于强化学习的Behavior Planner系统如下：
Local Planner
Local Planner关注如何生成舒适的、碰撞避免的行驶路径和舒适的运动速度，所以Local Planner又可以拆分为两个子问题：Path Planner和Velocity Profile Generation。Path Planner又分为Sampling-Based Planner、Variational Planner和Lattice Planner。
最经典的Sampling-Based Planner算法是Rapidly Exploring Random Tree，RRT算法。
Lattice Planner将空间搜索限制在对车辆可行的Action Space。Velocity Profile Generation要考虑到限速、速度的平滑性等。
Vehicle Control将Planner的规划结果转化为车辆的运动行为。
最佳优先搜索（BFS）算法
BFS按照类似的流程运行，不同的是它能够评估（称为启发式的）任意结点到目标点的代价。与选择离初始结点最近的结点不同的是，它选择离目标最近的结点。BFS不能保证找到一条最短路径。然而，它比Dijkstra算法快的多，因为它用了一个启发式函数（heuristic function）快速地导向目标结点。例如，如果目标位于出发点的南方，BFS将趋向于导向南方的路径。在下面的图中，越黄的结点代表越高的启发式值（移动到目标的代价高），而越黑的结点代表越低的启发式值（移动到目标的代价低）。这表明了与Dijkstra 算法相比，BFS运行得更快。
A*算法结合了Dijkstra和BFS的各自的优点，把Dijkstra算法（靠近初始点的结点）和BFS算法（靠近目标点的结点）的信息块结合起来。
随机路图法PRM
是基于图搜索的方法，随机路图（Probabilistic Road Maps，PRM）就是在规划空间内随机选取N个节点，之后连接各节点，并去除与障碍物接触的连线，由此得到一个随机路图。
显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所以PRM是概率完备且不最优的。
快速扩展随机树法RRT
是基于树状结构的搜索算法，RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确定的。与PRM类似，该方法是概率完备且不最优的。
快速扩展随机树法RRT
是基于树状结构的搜索算法，RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确定的。与PRM类似，该方法是概率完备且不最优的。
虽然基于采样的规划算法（如PRM和RRT）速度很快，但他们也有致命的缺点，那就是由随机采样引入的随机性。利用RRT和PRM算法进行运动规划，用户无法对规划结果进行预判，每次规划的结果都不一样，这就使得自动规划的机器人无法进入工业领域（极端追求稳定性）。
所以目前规划领域也主要集中在对PRM和RRT的改进上，大家都想要尽可能解决这类算法的不确定性，甚至能实现一些优化目标，如RRT，Informed-RRT，SBL等。
自主移动机器人中的路径规划
第一步将可能的连续的环境模型装换成适应于所选路径规划算法的离散图，有三种通用的策略：道路图、单位分解、势场。
道路图
-
可视性图
由连接彼此可见的全部顶点对的连线组成，连接这些无阻挡的顶点即是它们之间 的最短距离。
该方法仅适用于稀疏目标群，而且允许机器人尽可能的接近障碍物。 -
沃罗诺伊图
相对于可视化图，它倾向于使图中机器人与障碍物之间的距离最大化。
它也会使环境中的机器人与物体之间的距离最大化，使得机器人上的短距离传感器检测不到可能存在的危险。
单元分解路径规划
- 主要思想是区分几何区（也叫单元）之间的区别，即把单元区分为自由的和被物体占用的区间。
- 主要分为精确单元分解和
- 精确单元分解：基于以下的思想：在自由空间的各单元中内， 机器人的特殊位置不重要，重要的是机器人从各自由单元走向其相邻自由单元的能力。
在大的稀疏环境中，单元的数目较少，实施效果挺有效。但是一旦单元数目巨大，实现的难度就会剧增。
- 近似单元分解
单元的尺寸不依赖于环境中的特殊物体，路径规划的计算复杂性低。是基于栈格的环境表示的普遍性。
势场路径规划
主要思想：把机器人处理成人工势场影响下的一个点，像球滚下山一样，机器人跟随着场移动。机器人被吸引向目标，同时也被先前已知的障碍物所排斥。
如果障碍物新出现，应该及时更新势场。
基本势场包括从起点到目标的有一定梯度的势场和以障碍物为中心的排斥势场。
扩展势场法
在基本势场上，附加了两个场：转动势场和任务势场。
可以将三维问题分解成两个单独的二维问题，这是通过分离轨迹的纵向和横向分量来解决的。其中一个二维轨迹是具有时间戳的纵向轨迹称之为 ST 轨迹，另一个二维轨迹是相对于纵向轨迹的横向偏移称之为 SL 轨迹。
- 转动势场：当障碍物与机器人行走的方向平行时，减小斥力，因为这样的一个物体不会对机器人的轨迹造成及时的威胁。结果增强了沿墙跟踪能力。
- 任务势场：考虑了当前机器人速度，排除了根据近期势能对机器人速度无影响的障碍物。结果是穿过空间的轨迹更平滑。
规划
路径规划
路径和速度
轨迹规划分为两步：路径规划、速度规划。
首先在路径规划步骤中生成候选曲线，这是车辆可行驶的路径。使用成本函数对每条路径进行评估，该函数包含平滑度、安全性、与车道中心的偏离以及开发者想要考虑的其他任何因素。然后按成本对路径进行排名并选择成本最低的路径。
然后是确定沿这条路线行进的速度。我们可能希望改变在该路径上的速度，所以真正需要选择的是与路径点相关的一系列速度，而不是单个速度。我们将该序列称作“速度曲线”。我们可以使用优化功能为路径选择受到各种限制的良好速度曲线。通过将路径和速度曲线相结合可构建车辆行驶轨迹。
路径
为了生成候选路径，首先将路段分割成单元格。
然后对这些单元格中的点进行随机采样。通过从每个单元格中取一个点并将点连接，我们创建了候选路径。
通过重复此过程可以构建多个候选路径。使用成本函数对这些路径进行评估并选择成本最低的路径，成本函数可能考虑以下因素：与车道中心的偏离、与障碍物的距离、速度和曲率的变化、对车辆的压力、或希望列入的任何其他因素。
ST图
选择路径后的下一步是选择与该路径关联的速度曲线，一个被称为“ST 图”的工具可以帮助设计和选择速度曲线。
在ST图中，“s”表示车辆的纵向位移、“t”表示时间。
ST 图上的曲线是对车辆运动的描述，因为它说明了车辆在不同时间的位置。
由于速度是位置变化的速率，所以可以通过查看曲线的斜率从 ST 图上推断速度。斜坡越陡则表示在更短的时间段内有更大的移动，对应更快的速度。
速度规划
为构建最佳速度曲线需要将 ST 图离散为多个单元格。
单元格之间的速度有所变化，但在每个单元格内速度保持不变，该方法可简化速度曲线的构建并维持曲线的近似度。
ST 图中可以将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形。例如，假设预测模块预测车辆将在 t0 到 t1 的时间段内驶入的车道。由于该车将在此期间占据位置 s0 到 s1，因此在 ST 图上绘制了一个矩形，它将在时间段 t0 到 t1 期间阻挡位置 s0 到 s1。为避免碰撞，速度曲线不得与此矩形相交。既然有了一张各种单元格被阻挡的 ST 图，便可以使用优化引擎为该图选择最佳的速度曲线。优化算法通过复杂的数学运算来搜索受到各种限制的低成本解决方案。这些限制可能包括：法律限制，如速度限制；距离限制，如与障碍物的距离；汽车的物理限制，如加速度限制。
优化
路径-速度规划在很大程度上取决于离散化。
路径选择涉及将道路划分为单元格，速度曲线构建涉及将 ST 图划分为单元格。
尽管离散化使这些问题更容易解决，但该解决方案生成的轨迹并不平滑
为了将离散解决方案转换为平滑轨迹，可使用“二次规划”技术（Quadratic Programming)。
.
二次规划将平滑的非线性曲线与这些分段式线性段拟合。尽管二次规划背后的数学运算很复杂，但对于我们的目的而言，细节并不是必需的。
我们只需简单使用几种不同的优化包中的一种，包括一种由 Apollo 推出的运行方案来生成平滑的轨迹，一旦路径和速度曲线就绪，便可以用其构建三维轨迹。
路径-速度规划的轨迹生成
假设我们正在路上行驶，感知系统观察到一辆缓慢行驶的车辆离我们越来越近。
首先，在这辆车的周围生成多条候选路线，使用成本函数对这些候选路径进行评估并选择成本最低的路径。然后使用 ST 图来进行速度规划，根据其他车辆随时间变化的位置阻挡了 ST 图的部分区域。
优化引擎可帮助确定该图的最佳速度曲线，该曲线受制于约束和成本函数。
我们可以使用二次规划让路径和速度曲线变平滑。
最后，将路径和速度曲线合并构建轨迹。这里的轨迹在速度较快时为红色，在速度较慢时为蓝色。我们使用该轨迹来安全地绕开其他车辆并继续我们的旅程。
Lattice 规划
通过使用 Frenet 坐标可以将环境投射到纵轴和横轴上，目标是生成三维轨迹：
- 纵向维度
- 横向维度
- 时间维度。
Lattice 规划具有两个步骤即先分别建立 ST 和 SL 轨迹，然后将它们合并为生成纵向和横向二维轨迹。
先将初始车辆状态投射到 ST 坐标系和 SL 坐标系中，通过对预选模式中的多个候选最终状态进行采样。来选择最终车辆状态。对于每个候选最终状态构建了一组轨迹将车辆从其初始状态转换为最终状态，使用成本函数对这些轨迹进行评估并选择成本最低的轨迹。
ST轨迹的终止状态
根据情况可以将状态分成 3 组：
- 巡航
- 跟随
- 停止
巡航意味着车辆将在完成规划步骤后定速行驶，实际上在对图上的点进行采样，在图中横轴代表时间，纵轴代表速度。对于该图上的点，这意味着汽车将进入巡航状态，在时间 t 以 s 点的速度巡航，对于这种模式，所有最终状态的加速度均为零
跟随车辆，在这种情况下要对位置和时间状态进行采样，并尝试在时间t出现在某辆车后面，在跟随车辆时，需要与前方的车保持安全距离，这时速度和加速度将取决于要跟随的车辆，这意味着在这种模式下，速度和加速度都会进行修正。
最后一种模式是停止，对于这种模式只需对汽车何时何地停止进行抽样，这里速度和加速度会被修正为 0 。
SL轨迹的终止状态
无论车辆进入怎样的终止状态，车辆都应该稳定地与车道中心线对齐。
这意味着只需要在一个小区域内，对横向终止位置进行采样。
具体来说采样的是道路上相邻车道中心线周围的位置。为了确保稳定性，汽车驶向的终止状态应该与车道中心一致。
当用横向位置与纵向位置作图时 ，想要的候选轨迹应该以车辆与车道对齐并直线行驶而结束。为了达到这种终止状态，车的朝向和位置的一阶和二阶导数都应该为零。这意味着车辆既不是横向移动的，那是一阶导数；也不是横向加速，那是二阶导数。这意味着车辆正沿着车道直行。
Lattice规划的轨迹生成
一旦同时拥有了 ST 和 SL 轨迹，就需要将它们重新转换为笛卡尔坐标系。
然后可以将它们相结合构建由二维路径点和一维时间戳组成的三维轨迹。ST 轨迹是随时间变化的纵向位移，SL 轨迹是纵向轨迹上每个点的横向偏移。由于两个轨迹都有纵坐标 S，所以可以通过将其 S 值进行匹配来合并轨迹。
应用案例
自动驾驶路径规划-环境配置
1.更新镜像源
首先要更新镜像源吧，比如ubuntu20.04的清华源
2.安装gcc，g++
这里，我们要另外添加一个软件源，以安装gcc，g++。我这里讲的是ubuntu20的gcc，g++安装，18的好像没有9和10的版本，记得是7的，不过你们自己安装好这两个编译器就行，自行百度。
sudo gedit /etc/apt/sources.list
增加
deb http://cn.archive.ubuntu.com/ubuntu xenial main
注意这个源是ubuntu20的，18的可能不一样。
执行
sudo apt-get update
其次，安装gcc 与 g++:
sudo apt-get install gcc-9 g++-9
sudo apt-get install gcc-10 g++-10
3.安装ros和python3环境
网上教程太多啦，你们可以根据自己的系统安装对应的ros版本。我的是ubuntu20.04，安装的是ros-noetic。建议不要用ubuntu16，因为我没试过，用18或者20，也就是melodic和noetic。
4.安装osqp求解器
运行/src/planning/installers里面的install_osqp.sh
sudo ./install_osqp.sh
5.安装依赖库：ADOL-C
先按方法一（可能有版本不匹配问题）做，如果报错或者运行有问题再卸载重新用方法二
方法一：
先安装Colpack
git clone http://github.com/CSCsw/ColPack.git
cd ColPack/build/automake
autoreconf -vif
mkdir build
cd build
../configure --prefix=usr/local
make -j
sudo make install
再安装ADOL-c
git clone http://github.com/coin-or/ADOl-C.git
./configure prefix=usr/local
make -j
sudo make install
方法二：
运行/src/installers里面的install_adolc.sh
打开install_adolc.sh文件，先修改如下图所表标红色的路径为自己电脑ADOL-C-2.6.3下载的绝对路径。不然会找不到lib64。
然后执行：
sudo ./install_adolc.sh
6.安装依赖库：ipopt求解器
最近发现ipopt自带的库下载链接失效，所以此包安装麻烦一些
7.安装matplotcpp
运行/src/installers里面的install-matplotcpp.sh
sudo ./install-matplotcpp.sh
8.安装其他ros包依赖
运行/src/installers里面的install_base.sh
sudo ./install_base.sh
注意，里面有ros-noetic，是针对ros-noetic版本的，melodic版本要改成ros-melodic。
9.安装Qt5 （可选）
参考：https://blog.csdn.net/Joker__123/article/details/122438138
10.安装qpoases求解器
sudo ./install_qp_oases.sh
11.安装yaml读取
sudo ./install_yaml.sh
报错解决方案
问题1
CMake Error at /opt/ros/noetic/share/catkin/cmake/empy.cmake:30 (message):
Unable to find either executable 'empy' or Python module 'em'... try
installing the package 'python3-empy'
注意，因为我安装了conda环境，在base下编译的，所以才可能出现这个问题，解决方法：
pip install empy问题2
ImportError: "from catkin_pkg.package import parse_package" failed: No module named 'catkin_pkg'
Make sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.
CMake Error at /opt/ros/noetic/share/catkin/cmake/safe_execute_process.cmake:11 (message):
execute_process(/usr/local/anaconda3/bin/python3
"/opt/ros/noetic/share/catkin/cmake/parse_package_xml.py"
"/opt/ros/noetic/share/catkin/cmake/../package.xml"
"/home/cg/Automatic_driving/build/catkin_tools_prebuild/catkin/catkin_generated/version/package.cmake")
returned error code 1
注意，因为我安装了conda环境，在base下编译的，所以才可能出现这个问题，解决方法：
pip install catkin_pkg
再
catkin build 问题3
/usr/bin/ld: /lib/x86_64-linux-gnu/libapr-1.so.0: undefined reference to `uuid_generate@UUID_1.0'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/ui_console.dir/build.make:150：/home/cg/Automatic_driving/devel/.private/ui_console/lib/ui_console/ui_console] 错误 1
make[1]: *** [CMakeFiles/Makefile2:166：CMakeFiles/ui_console.dir/all] 错误 2
make: *** [Makefile:141：all] 错误 2
这个错误的原因，还是因为有了conda环境，QT引用的是anaconda里的qt，因为我在ui_console里面用了qt画图，当然，你可以把这个文件夹删除，也可以解决。解决方法：
到anaconda安装文件夹下的anaconda/lib下:
sudo mkdir libuuid_bk
mv libuuid* libuuid_bk/问题4
Traceback (most recent call last):
File "/opt/ros/noetic/lib/joint_state_publisher/joint_state_publisher", line 35, in <module>
import rospy
File "/opt/ros/noetic/lib/python3/dist-packages/rospy/__init__.py", line 49, in <module>
from .client import spin, myargv, init_node, \
File "/opt/ros/noetic/lib/python3/dist-packages/rospy/client.py", line 52, in <module>
import roslib
File "/opt/ros/noetic/lib/python3/dist-packages/roslib/__init__.py", line 50, in <module>
from roslib.launcher import load_manifest # noqa: F401
File "/opt/ros/noetic/lib/python3/dist-packages/roslib/launcher.py", line 42, in <module>
import rospkg
ModuleNotFoundError: No module named 'rospkg'
这个错误的原因，还是因为有了conda环境，解决方法：
pip install rospkg问题5
找不到链接库，不管是ladolc，libqpOASES还是其他l开头或者lib开头的。解决办法：
a.打开**/etc/ld.so.conf**文件：
sudo gedit /etc/ld.so.conf
b.加入动态库文件所在的目录：在include ld.so.conf.d/*.conf下方增加
/usr/local/lib
c.保存后，在命令行终端执行：
sudo ldconfig
chmod o+x ./xxx.sh
再执行
sudo ./xxx.sh