背景：

16-18年做过一阵子无人驾驶，那时候痴迷于移动规划；然而当时可学习的资料非常少，网上的论文也不算太多。基本就是Darpa的几十篇无人越野几次比赛的文章，基本没有成系统的文章和代码讲解实现。所以对移动规划的认识不算全面，这几年随着自动驾驶、无人机的研究和应用的增多，很多的论文课程成体系的开始介绍这方面的内容。对于一个理工男来说机器人并且是能自动的、智能规划的，相信没有多少理工男是可以抗拒不想去做进一步了解的。所以一直在收集资料，筹划这哪一天可以出一个这方面系列，然后在code一个项目出来在机器人上捣腾各种实现。再一次加速本人对这一想法落实是两年前看到fast-lab高飞团队出的一系列飞行走廊解决无人机路径规划的工作视频。第一次看到视频时候真被震惊到，移动规划原来还可以这么玩，如此优美的数学框架。讲了这么多只是想致敬过去的经历，开启这个专题第一讲。这个系列主线就是围绕高飞老师《移动机器人动态规划》课程讲稿，里面会补充一些算法细节和自己的思考。这个课程对移动规划体系框架构建非常棒，内容排布的也非常好，唯一缺憾就是对于动态不确定障碍物的规划会少一些，因为课程本来就是针对无人机设计的。

正文：

现代机器人学和自动驾驶等领域，路径规划是一个重要的主题. 它涉及到在给定的环境中找到从起点到终点的最优路径. 这个过程通常分为两个部分：前端路径搜索和后端轨迹规划. 前端路径搜索在地图中搜索出一条避开障碍物的轨迹，而后端轨迹规划则对搜索到的轨迹进行优化，使其符合机器人的运动学和动力学约束.
实环境中的机器人运动规划是一个比较复杂的问题，对于复杂的问题人类的解法一般都是分步求解：先做个大概、然后在大概轮廓上逐步的复杂精细。机器人运动规划的学院派解法也是如此：

1.前端：路径规划

• 基于搜索的方法
• 通用图搜索：深度优先搜索（DFS），广度优先搜索（BFS）
• Dijkstra 和 A* 搜索
• 跳点搜索
• 基于采样的方法
• 概率路线图（PRM）
• 快速探索随机树（RRT）
• RRT，有信息的 RRT
• 带动力学约束路径规划
• 状态-状态边界值最优控制问题
• 状态栅格搜索
• 动力学RRT*
• 混合A*

2.后端：轨迹生成

• 最小抖动轨迹生成
• 微分平坦性
• 最小抖动优化
• 最小抖动的闭式解
• 时间分配
• 实际应用
• 软硬约束轨迹优化
• 软约束轨迹优化
• 硬约束轨迹优化

3.不确定性状态求解：移动障碍物、突变环境、设备建模变化

• 基于马尔可夫决策过程的规划（MDP）
• 规划中的不确定性和马尔可夫决策过程
• 最小最大成本规划和期望成本最小规划
• 值迭代和实时动态规划
• 机器人规划的模型预测控制（MPC）
• 线性模型预测控制
• 非线性模型预测控制

前端——搜索路径规划

在开始这部分内容介绍前，需要介绍几个概念。介绍这几个概念的目的在于更贴近实际的去理解搜索在业务中应用。搜索路径规划中是把机器人当成一个质点来考虑的，然而实际的机器人是有一定形状和占用空间的，如果把机器人当成质点来考虑很可能是会搜索出一条实际上不可行的（会碰到障碍物的）路径的。为了解决这个问题呢，我们可以简单的物体的形状转移到地图（让地图障碍物区域加上物体占用空间）。在这样的地图里把机器人当成质点来搜索可行路径。

在配置空间中规划¹²³

• 机器人在C-space中被表示为一个点，例如，位置（在R3中的一个点），姿态（在 (3)中的一个点），等等⁴⁷⁸
• 障碍物需要在配置空间中表示（在运动规划之前的一次性工作），称为配置空间障碍物，或C-障碍⁴⁵⁶
• C-space = (C-障碍) ∪ (C-自由)⁴⁵⁶
• 路径规划是在C-自由中找到从起点qstart到目标点qgoal的路径⁹[10]¹⁵

在工作空间中

• 机器人有形状和大小（即，难以进行运动规划）
• 在配置空间：C-space中
• 机器人是一个点（即，易于进行运动规划）⁶
• 在进行运动规划之前，障碍物在C-space中表示⁸[10]
• 在C-space中表示障碍物可能非常复杂。因此，在实践中使用近似（但更保守）的表示。

如果我们保守地将机器人建模为半径为 _ 的球，那么可以通过在所有方向上膨胀障碍物 _ 来构造C-space1。这是一种常见的机器人碰撞检测方法，通过确保球体中心在膨胀地图的自由空间中来实现碰撞评估1。然而，这种保守的方法并未考虑到机器人的形状和大小。

构建地图：

在路径规划中，构建搜索地图是一个关键步骤。这通常涉及到将实际环境抽象为一个图（Graph），其中节点（Nodes）代表可能的位置，边（Edges）代表从一个位置到另一个位置的移动。以下是一个详细的例子：
假设我们有一个机器人需要在一个室内环境中导航。这个环境可以是一个房间，有一些障碍物，比如桌子和椅子。

1. 定义节点（Nodes）：首先，我们需要确定节点的位置。在这个例子中，我们可以将房间的每一个可达的位置定义为一个节点。例如，我们可以创建一个网格（Grid），每一个网格单元都是一个节点。
2. 定义边（Edges）：然后，我们需要确定边。如果机器人可以直接从一个节点移动到另一个节点，那么这两个节点之间就有一条边。在我们的例子中，如果两个网格单元相邻，并且没有障碍物阻挡，那么这两个网格单元（即节点）之间就有一条边。
3. 定义权重（Weights）：最后，我们需要为每一条边定义一个权重。权重可以根据实际的移动成本来确定。例如，如果从一个节点到另一个节点的距离更远，或者路径上有斜坡，那么这条边的权重就应该更大。

地图种类：

栅格地图（Grid Map）则是把环境划分成一系列栅格，在数学视角下是由边联结起来的结点的集合，一个基于图块拼接的地图可以看成是一个栅格图，每个图块(tile)是一个结点，图块之间的连接关系如短线。
概率图（Cost Map）如果在栅格图的基础上，每一栅格给定一个可能值，表示该栅格被占据的概率，则该图为概率图。
特征地图（Feature Map）特征地图用有关的几何特征（如点、直线、面）表示环境。常见于vSLAM（视觉SLAM）技术中。它一般通过如GPS、UWB以及摄像头配合稀疏方式的vSLAM算法产生，优点是相对数据存储量和运算量比较小，多见于最早的SLAM算法中。
拓扑地图（Topological Map）是指地图学中一种统计地图, 一种保持点与线相对位置关系正确而不一定保持图形形状与面积、距离、方向正确的抽象地图。包括有有向图和无向图（字面意思）。

栅格地图

概率图

特征地图

拓扑地图-有向图

拓扑地图-无向图

搜索算法介绍

有了这么多种的地图，那么对应每种图可以用什么对应的算法来做路径的规划呢？下面是地图对应路径搜索算法：

1. 栅格地图 / 概率图
             1. Dijkstra
             2. BFS（Best-First-Search）
             3. A*
             4. hybrid A*
             5. D  *
             6. RRT
             7. RRT*
             8. 蚁群算法
             9. Rectangular Symmetry Reduction (RSR) 
             10. BUG
             11. Beam search
             12. Iterative Deepeningc
             13. Dynamic weighting
             14. Bidirectional search
             15. Dynamic A* and Lifelong Planning A *
             16. Jump Point Search
             17. Theta *
     2. 拓扑地图
              1. Dijkstra
              2. BFS（Best-First-Search）
              3.  A*
              4. CH
              5. HH
              6. CRP