Table of Contents:

全局路径规划 - RRT算法原理
基于海图的路径规划
路径二次优化
拐点算法
参考链接

深度优先搜索（DFS）：
- 沿深度遍历
- 可用递归或栈实现
广度优先搜索（BFS）：
- 核心结构：队列。从起点开始逐层访问邻接点；适合无权图最短路径，但效率低 O(n²)，无权重，无法处理障碍物。
Dijkstra 算法：
- 基于 BFS 增加边权，贪心选最小代价顶点；可找带权最短路径，但无方向性，时间复杂度 O(V²)。
- 维护最小优先队列（按当前距离排序），每次取出距离最小的顶点，对其邻接点尝试松弛（if u.d + w < v.d 则更新）。
- 本质：按当前最小代价扩散，比 BFS 多了权重考量。
- 相对于A* 的 h = 0，启发函数始终为0
- 一定是最优的
  - 原因：贪心策略成立 —— 每次从优先队列中取出的节点，其当前距离已经不可能再被其他路径缩短（因为所有边权非负），因此该节点可以永久确定最短路径。
贪婪最佳优先搜索：
- 按到目标点的启发距离排序；速度快，但不保证最短路径。
- 有障碍的情况，路径基本上都不是最优的
- 每次探索优先探索距离目标点最近的格子
- 相对于A* 的 g = 0
A星算法：
- 结合 Dijkstra（实际代价）与贪婪启发（估计代价）；既保证最短路径又减少搜索顶点，高效常用。
- A* 最核心的一句话下一步选择 g+h 最小的点
- openlist：待探索节点集合。通常需要维护一个最小优先队列（Min Heap）
- closelist：已访问集合。
  - 为什么需要 Close List？假设存在回路，如果没有 Close List，会不断重复搜索
- A* 可能不是最优的，如何启发函数 h(n)高估路径，得到的可能不是最优的路径
- 宁可低估，不可高估。低估无损最优性，只牺牲一点速度。

众所周知，无人驾驶大致可以分为三个方面的工作：感知，决策及控制。

路径规划是感知和控制之间的决策阶段，主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下，为车辆提供通往目的地的安全和无碰撞的路径。

路径规划问题可以分为两个方面：

（一）全局路径规划：全局路径规划算法属于静态规划算法，根据已有的地图信息（SLAM）为基础进行路径规划，寻找一条从起点到目标点的最优路径。

通常全局路径规划的实现包括Dijikstra算法，A*算法，RRT算法等经典算法，也包括蚁群算法、遗传算法等智能算法；

（二）局部路径规划：局部路径规划属于动态规划算法，是无人驾驶汽车根据自身传感器感知周围环境，规划处一条车辆安全行驶所需的路线，常应用于超车，避障等情景。通常局部路径规划的实现包括动态窗口算法（DWA），人工势场算法，贝塞尔曲线算法等，也有学者提出神经网络等智能算法。

全局路径规划 - RRT算法原理

RRT算法，即快速随机树算法（Rapid Random Tree），是LaValle在1998年首次提出的一种高效的路径规划算法。RRT算法以初始的一个根节点，通过随机采样的方法在空间搜索，然后添加一个又一个的叶节点来不断扩展随机树。

当目标点进入随机树里面后，随机树扩展立即停止，此时能找到一条从起始点到目标点的路径。算法的计算过程如下：

step1：初始化随机树。将环境中起点作为随机树搜索的起点，此时树中只包含一个节点即根节点；

stpe2：在环境中随机采样。在环境中随机产生一个点，若该点不在障碍物范围内则计算随机树中所有节点到的欧式距离，并找到距离最近的节点，若在障碍物范围内则重新生成并重复该过程直至找到;

stpe3：生成新节点。在和连线方向，由指向固定生长距离生成一个新的节点，并判断该节点是否在障碍物范围内，若不在障碍物范围内则将添加到随机树中，否则的话返回step2重新对环境进行随机采样；

step4：停止搜索。当和目标点之间的距离小于设定的阈值时，则代表随机树已经到达了目标点，将作为最后一个路径节点加入到随机树中，算法结束并得到所规划的路径。

RRT算法由于其随机采样及概率完备性的特点，使得其具有如下优势：
（1）不需要对环境具体建模，有很强空间搜索能力；
（2）路径规划速度快；
（3）可以很好解决复杂环境下的路径规划问题。

但同样是因为随机性，RRT算法也存在很多不足的方面：
（1）随机性强，搜索没有目标性，冗余点多，且每次规划产生的路径都不一样，均不一是最优路径；
（2）可能出现计算复杂、所需的时间过长、易于陷入死区的问题；
（3）由于树的扩展是节点之间相连，使得最终生成的路径不平滑；
（4）不适合动态环境，当环境中出现动态障碍物时，RRT算法无法进行有效的检测；
（5）对于狭长地形，可能无法规划出路径。

基于海图的路径规划

主流程：
1. 根据海图元素（点、线）生成障碍物点集，并构建一个二维网格地图（grid map）。
2. 在每个网格上标记是否可通行（isWalkable）。
3. 提供从起点到终点的路径规划接口（A* 或 DFS）。
4. 对规划出的路径做二次优化（去除冗余拐点）。

栅格化
    栅格的区域的确定
        选择起点到终点区域
        获取所有线和点的信息
        遍历所有点确定minx、miny、maxx、maxy形成的矩形框
            起点和终点必在矩形框中
        确定格子的长度
        按照格子的长度在线段之间补充点，以保证格子可以覆盖到
        进行障碍物膨胀（将障碍物所在格子及其周围 extend_num 范围内的格子都设为不可通行）
    格子的数据结构的定义
        map
            vector<vector<grid>> map
        grid
            index
                x*grid_num + y
            区域类型
            是否可行走
            经纬度坐标,格子的中心点
                相对坐标和绝对经纬度的转换

    格子的绘制和显示
        先画格子的线
        不可行走区的绘制
            灰色
路径规划
    规划算法
        起始点所在的格子
            getGridIndex(double x, double y)   通过经纬度坐标，获取格子的索引，进行得到起始点的格子
        深度优先遍历
        输出为格子的路径
            path
                vector<grid>

        异常情况处理
            起点和终点在不可行走区域
                增加一个检查的函数，航路规划前进行判断，若在不可行走区域则进行提示
            起点终点在封闭区域内
                进行规划，如果探索过的区域只占很小一部分，则可以判断起点在封闭区域内
                如果规划的路径为空，则说明终点在封闭区域内

    绘制规划的路径所在的格子
    绘制初始路径
    绘制探索过的格子

路径二次优化
    根据是否和其他多边形相交来逐步优化路径
    绘制优化后的路径
    绘制优化后的路径

路径二次优化

路径平滑化：通过在已有路径上进行局部调整，使得路径更加平滑和自然，从而降低路径的曲率和不连续性。
路径连接优化：优化路径的连接方式，以减少转折和不必要的拐弯，从而提高路径的连续性。
剪枝算法：通过移除不必要或冗余的路径部分，减少路径的长度和复杂性。
曲线拟合：将原始路径的曲线部分拟合为更平滑的曲线，以减少路径的曲率，例如使用贝塞尔曲线拟合。