在经过一个项目完整周期的开发与学习，觉得有必要写一个阶段性总结，也算是将之前开发和调试中遇到的问题进行归纳、整理，也是对我所了解的范畴中的规划算法做一个概述，有不足或错误之处还往指正。
首先笔者声明，我的工作中的自动驾驶场景不是典型的结构化道路，而是半结构化道路，可以理解为道路边界不清晰，且没有道路中心线和高精地图，主要场景涉及山地、沙漠、丘陵、草原、城市等，我们的工作便是需要在此环境下，依旧找出道路可通行区域（感知范畴，这里不做过多赘述），且在多维空间中输出一条安全、平滑且合理的路径。下面，我们从整体架构、地图、规划算法几大块分别讨论。
目前主流的规划算法架构基本可以用以下模式概括：
注意，这里前后端都使用了生成二字，意为不限制参考路径和平滑路径的生成过程。在前端参考路径生成过程中，可以使用1.A*、D*、Dijkstra等搜索算法；2.RRT、RRT*、PRM等采样算法；3.动态规划（dp）；
具体使用哪种方法，需要结合场景和算力。举个最简单的例子：dp在基于横纵向采样点生成多项式曲线后，曲线形状较为固定，比较适合于简单环境，而A*搜索的路径往往拐点较多，形状多样，比较适合于复杂环境，但是自由度和稳定性往往是trade off的关系，想要更高的自由度，路径形状就会变的千奇百怪，难免出现一些并不是我们想要的路径。所以在复合环境中，某一种算法往往难以满足需求，如何使算法根据不同场景自适应，也是需要我们认真思考的议题。
由于前端生成的轨迹只是大致找到一条通往目的地的路，往往不考虑运动学和动力学约束，也就是说，车辆无法按照此轨迹进行行驶（如曲率超限，速度、加速度超限等等），或者说，得到的轨迹不够平滑，使得车辆按照轨迹行驶时，出现猛打方向盘、猛踩油门这样的情况，总之不符合我们对轨迹的要求。
针对这样的问题，衍生出了很多将纯搜索或采样的方法与运动学、动力学相结合的算法，例如hybrid A*，Kinodynaimic RRT*等，核心要点都是在原算法基础上，修改节点扩展方式，使得扩展的节点满足车辆非完整性约束，这里的变化有很多，你也可以探索不同的扩展模式，比如使用三次多项式、B样条、螺旋线进行节点的连接与拓展，创造出属于你自己的变种算法。使用满足约束的搜索或采样的路径规划算法可以一步到位，在一定程度上可以不需要后端优化，可直接发给控制执行。
在这里我们仍继续讨论前端+后端的架构模式：
后端中，生成过程也可以多种多样，比如，如何使用参考路径，决定了最终生成平滑轨迹的方法。这里列举三种大类。
1.使用参考路径离散点作为初始值，建立各项车辆运动学、动力学、安全性约束，进行求解，此类方法往往高度非线性，遇到复杂场景时，如何满足实时性，算力是一个挑战，目前比较流行的方法是使用微分平坦性质，重新formulate约束，可以大大简化问题，提高求解效率，具体可参考fast lab论文：An Efficient Spatial-Temporal Trajectory Planner for Autonomous Vehicles in Unstructured Environments；
2.使用参考路径部分属性，进行二次采样，注意二次采样是满足车辆非完整性约束的，此类方法根本上来说不算严格意义上的采样，而是在横纵向采样后，通过空间映射，使用优化的方法生成参数化曲线，实时性高、稳定性强，但由于此类方法是使用后验的方式判断生成的参数化曲线是否安全，而不是在一开始就考虑障碍物，所以在一些极端情况下，会出现没有路径生成的情况，具体可见论文：Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control；
3.使用参考路径开辟凸空间，在此凸空间上建立线性约束，进行求解，得到最终轨迹，典型代表：Apollo EM Planner。需注意和1的区别是，此类方法不使用参考路径作为初始解，而是在此空间上进行求解。由于是凸优化问题，一定能得到全局最优解，且计算高效，笔者实测，使用osqp_eigen求解50m长度的路径耗时在1-2ms左右（x86 i7处理器）。在实际应用中由于我们项目地图格式的差异，凸空间开辟我是在cartesian坐标系下做，然后映射到frenet下，优化完后再转回cartesian，此类方法实时性很高，且由于在优化时就考虑了障碍物，所以基本不会出现2中无路径生成的情况，但frenet与cartesian坐标系转换在一些大曲率的地方往往存在问题，这也是使用frenet下进行规划的弊端，有知道如何解决的朋友也请赐教。
总而言之，近些年自动驾驶得到了长足发展，规控作为自动驾驶直接和底盘相连的模块也变得尤为重要，上述架构只是比较通用的简单概括，也不乏有更加新颖的架构没有涉及到，但是经过这几年的工作与学习，我的感受是“万变不离其中”，我们都是踩在巨人的肩膀上，这些人类的智慧已经摆在了我们的面前，如何将已有的成果融会贯通，根据自己的需求去工程化，可能是目前大部分自动驾驶工程师首当其冲需要的的能力。
2 地图引用浙大高飞老师的话：“移动机器人做planning，首先要把地图搞明白。机器人理解的地图，并不等价于你看到的地图，怎样设计针对导航最有效的地图？对某种特定传感器，什么样的地图又是最简洁高效的。”当谈及路径规划算法，地图是不可避免的，它不仅在系统上决定了环境的表达形式，也在一定程度上决定了哪些算法更适配。
地图形式千千万，如何选择或者创造出最适合你项目的地图，也是需要深思的，选择合适的地图，可以大大减小planning的开发难度。目前市面上有多种形式的地图，主要分为全局和局部两大类：1.全局地图：全局地图是导航生成全局参考路径的依赖，常用的全局地图有：
高精度地图信息稠密，数据量大，大多数乘用车都使用图商提供的高精度地图，但正是由于其夸张的数据量，导致数据冗余不可避免，加载起来也相对复杂，且在某些野外环境，并没有可直接使用的高精度地图。
拓扑地图信息稀疏，只是简单的描述道路节点之前的连接关系和部分属性，制作和加载起来相对简单，但不可避免的存在导航信息量不够的问题，需要使用其他手段进行补充。
2.局部地图全局地图只为导航提供了参考全局路径，作为指引，而车辆实际行驶的路径都需要在局部地图中进行规划。局部地图中包含车辆行驶需要避让的动静态障碍物信息以及真实的道路边界信息，目前比较常用的几种局部地图有：
同样的，使用哪种地图也需要结合具体项目需求来看。比如，当你的环境都是一些规则的障碍物，如车辆或人为放置的障碍物时，感知比较容易将障碍物抽象成bounding box，只需在矢量图中描述其几何形状和位置，就可以表示障碍物和车辆的相对位置关系，从而在几何方面设计碰撞检测算法（如矩形相交判断），进行规划。但是，如果置身于戈壁，障碍物种类多样，形状复杂（如杂草、碎石等），感知很难将障碍物抽象成规则的几何形状，此时选择栅格地图，会提高障碍物信息表达的自由度，当然，你还可以根据系统要求，设计相应的cost（例如多帧使用概率值），来生成你所需要的代价栅格图用于规划。
以上只是笼统的概括了几种地图模式，细分下去还有很多，比如栅格图grid map在2.5d维度下还有高程图，在3D下可以用oct-map，还有多层的cost map，每层都可以人为定义不同的cost，来达到不同的规划算法目的。但大体形式是统一的，还是那句话，“万变不离其中”，我们需要做的还是融会贯通后，有选择合适地图的能力，进而去创造。
这些所谓的选择，也体现了一个工程师的设计能力，结合起来就叫工程设计能力吧，当然前提是你有足够多的知识储备，才有的选。就像上述例子中，一名不合格的工程师在复杂障碍物环境下选择使用了矢量地图，那大概率在局部地图中会出现满屏幕bounding box，并且由于bouding box在一定程度上增加了冗余，拓展了膨胀距离，很可能导致明明有路，而规划不出来的情况，因为碰撞检测发现哪哪都会发生碰撞。
