self_drive car_学习笔记–第10课：路径规划

前言：这节课围绕无人车的路径规划开讲，包含规划时采用的一些算法思路、一些规划方式，最后以Apollo项目规划部分为例子，介绍一下其中所使用的一些规划算法以及方式。很菜现在，有些理解错误地方，还望大大们不吝赐教。觉得写得还行，麻烦赏个赞哈。好了，不废话，开始主题。

概要：
1 什么是规划
2 传统的规划方法
3无人车的规划
4Apollo如何求解规划问题

1 什么是规划

1）规划planning目前是无人车最困难也最有挑战的部分

1.1 规划的本质

1）求解：argminf(x)
获取最小值就是规划；f 可以理解为成本，x可理解为轨迹，追求最小成本就是规划的本质。

2）规划问题也可理解为搜索问题，
—-【比如】Google：关键词，返回最优结果
—-无人车：当前环境和当前状态，返回当前路径上的最优选择

3）什么是好的规划？
—-“好”，取决于目标函数，f(x)的最优解

4）运动规划 motion planning 的三个领域

4-1）机器人领域robot fileds：
—-规划领域理解：生成轨迹，实现目标
—-经典的实现算法：RRT、A*、D*、D* Lite

4-2）控制领域control theory：
—-规划领域理解：动态系统理论，实现目标状态（从而完成planning）
—-经典实现方法：MPC、LQR

4-3）AI
—-规划领域理解：生成状态和Action的一个映射
—-实现方法：Reinforcement learing(强化学习),Imitation learing(模仿学习)

参考材料：http://planning.cs.uiuc.edu/par1.pdf

1.2 如何解决一个规划问题motion planning

1）突破口：规划问题简化成一个简单问题：path finding problem
—-简化后的问题特征：不考虑速度，不考虑怎么走过去；周围环境固定

2）简化后，问题转变为：路径选择问题
—-A simple shortest path example: http://qiao.github.io/PathFinding.js/visual/【在线最短路径展示，使用的原理有DFS、BFS、Dijkstra】

—-重点理解最好的path是怎么样的？
——–路径最短的path
【DFS、BFS方法查找最短路径有缺陷的，假设地图无限大，就有可能无法搜索到目标点，也就是说有可能无法找到路径】

—-展示的看到的search属于Non-informative search，效果较低

—-A* search：基于Dijkstra的改进算法：

https://www.redblobgames.com/pathfinding/a-star/introduction.html

—-该算法，大概知道了终点位置
—-然后，设置Heuristic func，其可以估计下一移动点为更可能性的点【到达目标点可能性】
—-以上就是A*算法的思路

【不离散化，对连续空间的求解比较困难，这是因为连续空间求解会涉及解析式，该式求解比较困难；所以，离散化网格处理是机器人通用的路径求解方式之一。高效的离散化，高效的网格化，是研究的重点】【A算法，面试时候，公司都会要求至少写出一个A思路】

3）无人车的规划和A* Search相差多远？
—-无人车属于部分感知【上面展示的A*算法，是知道了全局、周边环境以及终点位置，而无人车只是局部信息，也不知道终点具体在哪】

—-无人车周边的障碍物属于动态的，而且该障碍物不确定性极高【这里就需要通过前面课程学习的预测等来降低不确定性】
—-无人车的环境比较复杂，例如有交通规则

4）A* 本身是一个Global Algorithm【目前的导航地图的基本思想就是A*】
—-Global rounting【将rounting路由信息传给无人车，从而进行一个细节的规划】

5）Partial observed situation【部分感知情况解决方式】
—-贪心算法
———incremental search增量式搜索：把目前获取的状态信息，求解出最优解

—–D* star算法【对A* 进行改进】，对部分环境信息的一种search，典型运用是，Apollo登月小车；后面，D* star 算法发展成更轻量级算法D* lite。

—-D* lite算法，利用当前信息对全局信息进行一个修正

6）利用5）中的算法，可以求得全局最优吗？有可能，但是难度不小；进一步考虑，有必要全局最优吗？没有必要。

Optimal and Efficient Path Planning for Partially-Known Environments【部分最优从而接近全局最优的理论，对应的论文链接：】

https://www2.cs.sfu.ca/~vaughan/teaching/415/papers/dstar_icra94.pdf

7）上面介绍了搜索方式：informative &non-informative search【有提供信息与无提供信息】
两种感知方式：Global & partial observed【全局与局部方式】

8）根据上面 1）-7）的讨论，解决无人车搜索问题，可以尝试有如下方法：
—-【首先定义一个目标函数】目标函数并且结合了平滑性和目标cost
—-使用通用的Search方法来最小化cost，从而找到一个最优解
—-通过partial observed information 来做局部planning

9）对于8），对于无人车规划而言，还缺少一些东西：

—-处理动态障碍物，动态环境【如果将动态环境也作为静态处理，会导致很大问题】
—-处理交通规则【涉及公共安全问题】

—-实时计算【无人车要求的planing是在100ms-150ms之间；人的反应时间一般在300ms-500ms，酒驾时会达到1000ms;实时计算，也就是在有限时间内找到最优解；这也是无人车开发，一般使用C++，而不是使用python，由于C++算法有更多的实时性优化】

10）无人车motion planning的定义涵盖：
—-safely安全性
—-smoothly平滑性【保证乘客舒适性】

—-Achieve to destination到达目的地
—-X、Y，Time:3D trajectory optimization problem【规划问题，也可视为三维轨迹优化问题】
—-无人车硬件信息：定位设备、感知设备

—-无人车软件信息：动态信息【处理程序】、静态信息【HP MAP（其是实时性的保证）】【如果静态信息不是使用HP MAP提供的，而是通过感知获取的，那么实时性大大降低】

—-如何设计出一个合理的轨迹？：路径path【运动路线设计】、速度speed【到达目的地速度设计】

路径规划【不局限无人车】参考书籍推荐：
Steve Lavelle,Motion Planning Algorithms【全面介绍Motion Planning】
【http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.225.1874&rep=rep1&type=pdf】

Principles of Robot Motion：Theory, Algorithms, and Implementations【机器人里面综述类书籍】

经典文献：A Review of Motion Planning Techniques for Automated Vehicles【综述类】
【https://www.researchgate.net/publication/284766879_A_Review_of_Motion_Planning_Techniques_for_Automated_Vehicles】

2 传统的规划方法

2.1机器人学基础

基本planning方法：
1）经典基于环境建模的方法
—-RRT
—-Lattice【网格化】

2）现代无人车planning方法
—-Darpa【无人车开发的方法】
—-Lattice in Frenet Frame【这里的Lattice和上面的是有区别的】
—-Spiral polynomial【解决无人车平滑性所采用的一种思路】
【这里涉及的方法都在下面文章中有介绍，如文献中截图如下】

来自：A Review of Motion Planning Techniques for Automated Vehicles

3）质点模型
—-将物体看成一个质点
—-点与点不碰撞

4）刚体问题【无人车可视为一个刚体】
—-BycicleModel自行车模型【其是无人车建模的一种基本方式】
—XY Heading【这是有方向的X、Y，可理解为矢量】
—-Collision【碰撞条件比点与点的更严格；而且，碰撞检测更复杂】

5)Planning限制条件
—-避免碰撞
—-边界阈值【这么理解，理论人和车距离3-8米，但是由于车的传感器并没有人的感觉器官反应能力那么优秀，由于传感器数据传输有延迟这些，为了避免安全事故，车与人的距离应该更远，这个安全距离可视为阈值】

6）连续空间问题解决方式：
–离散化
–网格化

2.2 经典算法

传统的机器人基础：

1）PRM（Probabilistic RoadMap Planning）【非常实用的一个方法】

【开始点s,目标点g，由于连续空间中计算能力比较弱，而且也不知道全局环境，所以该方法采用撒点方式，如图中紫色点，如果撒点和障碍物重合了，就把该点去掉；洒的点多了，自然可运行空间就出来了；但是撒的点不会过多，这是因为只是为了达到目标点g,而不是走每一个角落】
—-连续空间离散化：
———随机撒点【全空间撒点】
———【撒在】Obstacle上的点就删除

—-连接可行点，形成可行空间
—-A*【获取运动轨迹】

2）RRT（incremental version of PRM【上面的PRM方法的改进版本】）

【增量式解析：图中点T想在空白区域行走，先撒两个点，】
2-1）使用增量搜索方式进行

2-2）找附近可行点的最优点：
—-F（x）最小，也就是成本cost最小
—-走过程中也不能有阻碍【这个也放在cost里面描述】【如路面不平】，尽量使得这个cost小

2-3）运动过程中，还有可能碰到障碍物

2-4）撒点搜索距离不能太远【上图就是撒点太远，导致碰到障碍物】
—-【撒点】需要一步一步移动

【这里撒点移动路线属于折线，对于质点移动还行；但是对于刚体移动来说，属于曲率无穷大，是不行的；此外，这种撒点方式还不能解决动态障碍物问题】

3）Lattice方法

3-1）该方法改进了RRT的折线问题
3-2）目的是，给出path的平滑曲线

3-3）采用的思路是，网格化【将搜索空间替换成一个一个小格子；每个采样格都是用曲线连接】【例如上左图中每个网格内，连接曲线都是平滑的】

3-4）该方法属于指数级别的搜索算法（NP-Hard）【这是该算法的一个缺点】

【指数级别可以理解为，一开始无人车所处的点有5根曲线，当达预测到下一个点时候，每个预测点又有多根曲线，这就是指数级别的增长了；为了改进这种搜索缺点，学者们改进了曲线搜索方式，比如，第二级中的曲线到达的点相隔很近，那么，没有必要再取这么多点的，把相近的点合并一个处理，加上撒点范围的约束（道路两侧约束），就可以避免了指数级的搜索空间增长】【最后，形成一种点线连接的搜索方式】

4）DP（动态规划）

【结合上述 3-4）后一草图可知，后面采用的点线搜索方式降维，这是由于使用了动态规划】

4-1）该算法减少了搜索空间：通过复用已有结果【搜索过的路径空间保存下来】

4-2）Lattice DP曲线的平滑度情况：曲率连续、曲率导数不一定连续【联系到无人车例子，曲率不连续，有可能出现，上一瞬间打方向盘90°，下一瞬间打180°，明显不符合驾驶习惯的】

5）QP（二次规划）【解决 4-2）中的曲率导数不连续情况】
5-1）凸优化问题最优化求解
5-2）公式表达：

5-3）二次规划性质：在凸优化中的凸空间问题，用QP，有最优解
5-4）QP如何找到平滑曲线？
—-min|f’|^2
—-min|f’’|^2
—-min|f’’’|^2
【上面所示意思对应的导数都是最优的，此时可以保证平滑曲线存在；个人理解为高维导数的连续来保证平滑】

5-4)其他的平滑曲线方法还有贝塞尔曲线、样条插值方法
【贝塞尔曲线多阶多项式都平滑，从而保证曲线平滑】

6）刚体模型

【建模时候，后轮轴线中点设置为车的原点（0，0）；前轮轴中心设置为前轮中心的拐点】【前后轮和转向的关系：】

6-1）前轮转向和Heading的关系：

—-车轮是沿着切线方向行驶
—-前后轮是同一个旋转中心

—-左右轮的结构相同【排除差速情况】【假设前后轮绕转同一个中心旋转，那么会是一个环形状，由于前后轮旋转半径不一致导致的】

6-2）Bicycle Model

【由于6-1）的已知关系，那么将左右轮简化，以中轴轴线为中心建立车轮关系，得到上图的单车模型；w表示的是前轮转角，】
—-曲率公式：1/R=kappa=(tan(w)) /L

3 无人车的规划

无人车planning定义：

—-A点到B点，构建一个车辆运动轨迹，结合 HD MAP，Localization和Prediction【高精地图、定位、预测】

—-输入【结合模块的信息】
—-输出：可行性轨迹，有一系列点组成【点使用X，Y，time来表示】【注意要轨迹要避免碰撞，体感舒适】

—-两个层面：导航层面【获取开始点到终点的基本路线；可视为一个粗略的planning】、运动轨迹层面【给出在路线前进时，运动的细节轨迹；可视为一个细节的或者多层面的planning】

3.1Routing

1）Routing定义：导航一条从A到B的全局路径
2）输入：地图（路网信息、交通状态【如道路拥堵情况】）、当前位置、目的地（乘客决定）
3）输出：可行驶道路的连接线

4）搜索：【将高精地图】地图数据转化成图网络

—-节点表示道路【node的划分可以自定义的，并不是一条道就是node，比如靠左靠右划分两个node也是可以的】
—-边表示道路连接

【里面道路权重是不一样的，左右转不一样，拥堵情况也会影响权重】【左转的cost比较高原因是，其碰撞风险更高，左转还需要等红绿灯】

5）经典算法A*

—-其是最经典的路径查找算法
—-F(n)=G(n)+H(n) : F(n)表示道路的routing的总cost；G(n)表示起始点到候选点的cost；H(n)表示候选点通过启发函数得到的目标点cost

【启发函数怎么得到，就是A算法的重点】【每个格子，中间值就是F值，上两个值就是G和H的值，亮橙色为起始点，启发式搜索一圈，也就是围绕这个点搜索一圈每格子的总cost，发现是其下面的cost40为最小，则移动到下面的格子，循环往复达到绿色格子上】
【关于A算法学习，https://www.redblobgames.com/pathfinding/a-star/introduction.html】

6）真实地图中的应用

6-1）左转cost最大【结合F=G+H函数思考下面图】【结合上图来看，红色表示左转，蓝绿色表示直行，淡蓝色表示右转】

6-2）通过比对发现，右转的总cost最小，那么选择右转。【这也是导航地图路线搜索的基本思路】

3.2 Planning

1）planning可理解为高精度、低级别的一个search，又可称为trajectory planning【轨迹生成】【结合上图，轨迹由点组成，该点带有相对当前的时间以及速度；提供速度的目的是，为了方便后面做跟踪】

2）轨迹点：X、Y、time、Velocity【（好像是指）道路坐标系中的坐标XY、相对当前的时间、速度】

【这里还需要一个速度的标签，这是因为实际情况，速度是存在抖动情况的，也就是速度平滑性不能保证的，所以不能简单利用轨迹点和时间来求取速度；所以，这里使用了速度信息，然后让control来跟踪。同时，这里要明白，并不是一定是给定速度，而是，给出速度，可以更好让后续模块跟踪轨迹点】

3）规划的约束条件
—-Collision 碰撞：躲避任何障碍物
—-Comfortable 舒适性：路径点必须相对平滑、速度也要平滑

—-运动学约束：高速转弯【应该避免，因为有侧翻可能】、掉头曲率角度【符合常规】
—-illegal 合法：符合交通法规、符合人类约定俗成规则

4）cost function成本函数

4-1）真实情况下有多条可行轨迹

4-2）cost由许多条件组成
—-道路偏移中心线距离【越偏离，cost越大】
—-碰撞或者靠得太近【对应cost越大】
—-速度太快，超速【对应cost越大】
—-curvature曲率太大，打方向盘太急【对应cost越大】

【对上述得三种运动路线进行cost比对，可以发现中间得cost最小，那么可以选择中间方式的路线】

4-3)不同场景下我们可能有多个不同的cost function
–高速场景【速度慢是一个大cost】
–停车场【速度慢是一个小cost】
–不同车辆【卡车和私家车的cost不一样】

5）Frenet坐标系（车道坐标系）

5-1）一般使用笛卡尔坐标系（世界坐标系）的话，
—-xy坐标无法知道无人车开了多远
—-不知道有没有偏离中心线
—-也不知道道路在哪

5-2）更好的坐标系：Frenet【车道坐标系】

—-注意和Track坐标系的区别：
——–L方向不同
——–Track是基于Road级别【高精地图介绍的track坐标系，就是道路坐标系】
——–Frenet是基于Lane级别【这里是路线坐标系】

—-S表示走了多远
—-L表示距离车道有多偏

6）Path VS Speed 解耦【轨迹生成和速度生成需要解耦；只有解耦，才能使得每个部分的形成凸问题，只有凸问题才能使用QP来处理；当混合一起，就不是凸问题，求解空间会非常丰富】

—-Path planning
——–生成可行轨迹路径
——–cost：path平滑性、安全性、道路中心偏移距离

—-选择出成本最低的一个path planning

—-speed planning
——–每个path上选择一系列速度（每个点都有速度），生成速度轨迹

7）Path Planning
—-首先生成道路网格（GridMap网格地图）
—-每个网格单元中随机采样（撒点）
—-每个网格选一个点
—-组成多条候选path
—-基本样式如图下

Cost Function对这些轨迹进行评估，需要考虑：
—-找到一个成本最低的一个
—-中心线距离 la
—-障碍物距离da1

—-速度变化率acca2
—-曲率kappaa3
——–为什么是kappa？个人感觉是，舒适性原因。如果曲率不平滑，必然会有打方向盘很急，乘客难受，也有翻车可能性。

F(x)=l* a0+d* a1+acc* a2+kappa* a3+a4【上述几项cost相加，获取cost function函数】
【cost线性加权，如果在参数可调情况下，就可以达到局部最优】【这里的权重需要人为来调节】

8）speed planning

【ST图中的S并不是指道路S，而是指给定path的S，如上面图中红色线path的S；颜色表示的是斜率，也就是速度；】【ST图将三维XY、time空间，降维到ST的二维空间，其中path锁定了XY，这里理解图中的S，这样可以使得无人车规划求解过程中，有了数学工具，如QP，进行求解】
—-S表示Path上纵向距离
—-T表示运动时间

9）如何规划ST轨迹

9-0）连续空间离散化（grid map网格地图）
9-1）每个单元格速度保持不变【每个单元格的速度线，在图中不能是曲线连接，只能使用折线连接】

9-2）把障碍物投影进来【投影到ST图】
—-挡住给定的path轨迹的部分画进ST图中
—-因此必须要有良好的预测轨迹
—-（下图，t0，t1时刻障碍物会在我们的path轨迹中挡柱s0,s1部分）

【无人车直走，白车前方掉头，那么，白车视为障碍物，并在ST图中表示出来】【t0、t1信息是由预测模块提供的】

9-3）速度曲线不能碰撞这个区域【个人觉得这里速度曲线说法有点不合适】

【上图就是为了避免速度曲线碰到障碍区域，设置了白色的轨迹路线】【个人感觉这里的速度曲线相碰，倒不如说ST曲线相碰比较合适，有点迷糊这里；有大佬知道的话，麻烦告知一下哈】【而且，感觉无人车加速通过的】

【上图是，多障碍物时候的路线规划情况】

9-4）凸优化求解得到最优的速度曲线
—-search搜索
—-限制条件：速度限制、距离限制（安全距离）、车辆动力学限制（车的加速度、刹车性能）

10）如何优化？【这个折线不平滑，需要优化】

10-0）对不平滑的速度线进行优化
—-使用QP（二次规划）进行优化
【通过QP约束速度，约束加速度，约束加速度导数，使得平滑非线性曲线和平滑的加速度曲线，达到一个拟合程度；拟合程度多少，由QP约束条件来确定】

10-1）这个方法很大程度依赖于连续空间离散化

10-2）网格、单元格方法
—-但是不平滑【解决方式：】
——–Quadratic Programming 二次规划
——–将平滑的非线性曲线与这些线段进行拟合
——–有现成工具qpOASES
【将折线拟合成曲线思路，最后使得path结果可用，例子看轨迹规划例子】

11）轨迹规划
11-1）实例，遇到一辆很慢的车，如何超车

11-2）生成很多轨迹线（撒点采样）

11-3）cost function对其进行评估，选择cost最小的一条【比如安全距离太小导致cost过大，直接去掉】

11-4）生成一个ST图表述速度规划
11-5）生成多条速度曲线【撒点】

11-6）使用优化工具对多条速度采样进行最优化求解（cost func成本函数，constraints约束）【优化，就是保证每个点的加速度最小，加速度平方最小，加速度导数平方也最小】
—-让整个路线和速度曲线变得平滑

11-7）最后将每个轨迹点（跟自定义的轨迹点resolution）的path、speed合并得到最终结果

【红色表示速度较大，青蓝色表示速度低】

【上面规划思路是，path和speed是分开规划】

3.3Lattice Planner【网格规划】

【这里介绍的方法的规划思路是，path和speed合并一起规划】

1）SL轨迹和ST轨迹

2）Lattice将两个图合并处理，同时进行path和speed的采样

3）实例：一个切车场景

3-1）先对整个候选轨迹进行采样

【红色线表示红车的预测轨迹范围，短的表示速度慢，轨迹分布情况，长的表示速度快，轨迹分布情况】

3-2）设计一个合理的cost
3-3）选择一个cost最小的轨迹

3-4）条件检查和碰撞检查

【左侧路线有急刹情况；右侧有碰撞情况】

3-5）检查失败，则返回继续找cost次优选项

3-6）成功输出结果

—-Lattice因为其采样计算量大，为了优化其采样效果，需要先进行场景化以简化计算量【这是Lattice的一个弊端，计算量很大，如果预测5s到8秒时，计算量指数爆炸增长】

【lattice这种规划方式在2006年时候使用后，因为计算量大而逐渐退出无人车领域；但是在物流小车比较适用，由于物流小车的速度没有那么快，采样量比较小，这种比较适合lattice】

4）Cruising，Following and Stopping：对S方向进行优化【特定场景使用lattice】
【下面三种场景，减低了lattice的采样点，从而减低计算量】

4-1）Cruising：定速行驶
—-v=vc
—-a=0
—-这种状态，不需要采样

4-2）Following：跟车行驶

—-需要对s和t同时采样
—-规定时间t跟在某个车的后面【减低可选点范围】
—-保证安全距离
—-速度v=v1
—-加速度a=a1

4-3)Stopping：停止

—-对车辆在哪里停下来进行采样
—-s和t同时采样
—-加速度a=0
—-速度v=0

5）Lattice对L方向进行优化
—-需要保证车辆以一个稳定的状态进行终点状态，比如与本车道线平齐
—-H’=0 H’’=0【横向的 速度、加速度都为0】【这是lattice对L方向优化】
—-S’=0 S’’=0【纵向的 速度、加速度都为0】

6）合并ST和SL坐标：

—-转化到Cartesian坐标系
—-生成X Y time 三维轨迹
——–两个坐标系都有S；找同一个S对应的L和T

【ST曲线也是先简化为凸函数】
【凸问题感性认识：以前面有车掉头为例，无人车有两种情况，一种是加速赶在掉头车前通过，另外一种情况，等待掉头完毕再通过，这时候求解path规划不是凸问题；假设，我们告诉无人车只能超速通过，这时候，path规划求解就是一个凸问题求解】

3.4上述两种无人车规划方式小结

1）path和speed分开规划【两者交替进行】

2）path和speed合并规划【lattice方式】

3）小答疑：
【二次规划，主要是对非平滑曲线进行平滑处理；二次规划QP可以处理任何凸优化问题】
【超车时，如果最优的cost还是很大，系统会不进行超车操作，这是决策层决定方案的执行与否的；也就是说，方案的cost出来了，最后是否实施，是由决策层决定的；并不是最优了就一定会执行】

4 APOLLO如何求解规划问题

【阿波罗规划，采用了上述两种方式，但主要是以交替方式为主】

1）Constrains约束
1-1）soft contraints&hard constraints【软、硬约束】
1-2）例子：

1-3）constraints:
—-交通规则：Hard constraints【硬约束】
—-用QP或者Hard code 方式精细处理【这些约束，都会放到二次规划QP求解过程中】

1-4）Decision
—-决策：soft constraints
—-用DP的方式来处理一些人为设置的软约束【例如不往障碍物右侧撒点，就不会往右边走了】

1-5）最优轨迹
—-通过二次规划QP生成最优轨迹

2）如何换道

2-1）无人车想要换道怎么办

2-2）换道考虑很多安全性问题
—-给出两种轨迹结果，让后续模块判断【一种换道轨迹，一种不换道轨迹】

2-3）Reference Line Decider
—-判断换道时，是否安全
—-拿到信息比planning丰富【例如，观测后方是否有来车，目的保证安全】
—-做很多准备工作【例如决定换道时，提供打方灯的指令】

4.1 EM Planner（Apollo）

1）什么叫EM【EM，期待最大化】

【EM思路，单个维度求解最优解，然后叠加所有维度的最优解，得到的最优解（和一开始使用多维度计算最优解得到结果差不多）】

2）上图基本流程：
【下面就是Apollo的EM操作】

2-1）首先生成一条Optimal Path（理想path）
2-2）再生成一条当前（理想）path情况下的optimal speed

2-3）再将目前的speed返回给path进行一次tuning（调试）
2-4）将tuning的path返回给speed做优化

2-5）最后迭代到最优解
2-6）贪心算法：Local Optimum

3）Apollo规划基本框架图

【这就是Apollo规划思路】

4）上述3）框架总结：

4-1）三个关键步骤：

—-目标函数：线性加权的cost
—-限制条件：交通规则、碰撞条件（无穷大）、动态特性（车辆能力）

—-优化求解：如何计算最优解（DP+QP）

——–DP：不要求问题是凸的
——–QP：是一种凸优化

4.2 DP、QP求解

1）path QP

1-1）问题抽象：根据当前驾驶信息和道路状况建立平滑的SL坐标轨迹
1-2）模型建立：合理优化目标函数和约束条件
1-3）优化方法：二次优化求解带约束的二次规划问题

【上面的path是已经撒完点后拟合的结果；这个公式就是方便求解平滑曲线】

2）speed DP

2-1）问题抽象：使用ST图
2-2）模型建立：cost函数（障碍物、曲率、无人车状态）
2-3）优化方法DP最优求解

2-4）s’=v 速度
2-5）s’’=acc 加速度
2-6）s’’’=jerk 踩油门或刹车的速度

3）计算提速思路【Apollo给出的一些规划提速方案】

3-1）SL坐标系离散化处理
—-粗分辨率使用时机、细分辨率使用时机【障碍物复杂，分辨率低一些；不复杂时分辨率则大一些】

3-2）GPU并行计算
—-同时计算多条reference lane【参考路线】的结果

3-3）QP Hotstart
—-QP的性质【QP的原理是泰勒展开】
—-两帧之间差距不大【上一帧结果启发下一帧开始也是可以的】【个人没有理解】

3-4）精通C++【利用编码思路来提升处理速度】

5 小结

1）规划的定义
—-规划本质就是一个搜索过程。【理解是一个搜索过程，那么就可以利用很多思路，通过现有的数学方式、图模型方式来求解】

2）机器人学的基本规划思路
PRM(撒点)->RRT(增量式撒点)->lattice（平滑折线）->动态规划DP（降维）->二次规划QP（平滑曲线）

3）无人车规划特点
—-先用path求解，再用speed规划【path和speed交替求解】
—-path和speed结合求解

4）Apollo中EM Planner
—-path、speed规划中，添加了EM（期待最大化）思想，求解当前维度下的最优，循环迭代找到更优的结果，这样子防止出现局部最优的带来的限制。【个人理解是，目的获取全局最优结果】

答疑环节

1）预测模块在规划的作用体现就是，使得障碍物可以被绘制到ST图中

2）轨迹规划流程：网格化、撒点、利用搜索找到折线轨迹，再利用QP进行平滑处理

3）撒点有效性取决于撒点密度，撒点密度取决于计算效率。

4）撒点不一定每个网格都要有点，有些空格也可以是没有点，其为了减低采样频率。这些方式，应该根据实际项目来思考的，没有孰优孰劣一概而定的。

阅读资料：
A Review of Motion Planning Techniques for Automated Vehicles
Baidu Apollo EM Motion Planner【https://arxiv.org/pdf/1807.08048.pdf】
A*: https://www.redblobgames.com/pathfinding/a-star/introduction.html【这个式面试必写的】

Coding：
PythonRobotics: https://github.com/AtsushiSakai/PythonRobotics【代码，关于规划示例，关键式学习算法思路，因为其使用的是python写的，c++写的话，会有更好的优化】

思考：
Everything is trade-off?【trade-off平衡。开发过程，做的是找一个平衡。我们并不是找某个最优点或者速度最快的解，而是在效率和结果上取一个平衡的解】

No model is perfect,but useful【无人车很难找到一个最优的解，每个模型都有其优势和劣势；充分利用优势和劣势，逐步找到当前环境下的最优解】

个人小结：
这节课介绍了路径规划。首先，介绍规划的本质是搜索。然后，介绍机器人学基本规划思路，循进介绍了PRM、RRT、lattice、DP、QP特性。接着介绍无人车路径规划采用的两种方式，path、speed交替进行方式，以及path、speed合并进行方式。最后，介绍了Apollo项目的规划情况，其采用了上述两种方式，但主要是以交替方式为主；此外，也介绍了其规划融合了EM的思想。不过，这些理论东西，还是觉得很迷。菜是原罪！

#####################
感恩授课老师的付出
图片版权归原作者所有
不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈

今天的文章self_drive car_学习笔记–第10课：路径规划分享到此就结束了，感谢您的阅读。