1 路径规划的方法
最直接的是离散路径规划，另外两种方法建立于离散路径规划之上，是sample-based路径规划和概率路径规划。
离散路径规划
离散路径规划将机器人工作空间明确划分为连接的图(graph)，然后应用图搜索(graph serch)算法来计算最佳路径。
这种方式非常精确和彻底，因为是直接对环境进行离散化，而且可以通过调节离散化程度来改变精度。
但结果是，这种方式具有极大的计算成本，也就不适用于大型路径规划的问题。
离散路径规划具有较好的精确性，不过只适用于低维问题，在高维度路径规划问题上，使用基于样本的路径规划就更合适。
基于样本的路径规划
基于样本的路径规划通过探测工作空间并增量地构建图。结果精度没有离散路径规划高，但由于使用到的样本量相对较小其运行速度快很多。
其生成出来的路径可能不是最优的，但绝大多数情况下生成可用的路径，好于等待数小时或数天。
概率路径规划
概率路径规划与前二者主要不同之处在于，考虑机器人运动的不确定性。在一些高度受限、敏感、高风险环境特别有用。
路径规划算法的评价指标
完整：对任何可解的问题均能提供结果，对不可解的问题不提供结果。
最优：能够找到最佳路径。最佳情况可以是：最短路径、最快速度、最少风险等。
2 离散路径规划
第一步：构建连续的表达，如构建C-space(Configuration Space)
第二步：离散化
第三步：应用graph search算法
连续表达Continuous Representation
使用机器人的半径膨胀所有障碍物，然后将机器人缩小成一个点，构成C-space这使得应用路径搜索算法更容易。
C-space机器人所有位姿的集合，分为C_free和C_obs。
闵可夫斯基和Minkowski Sum
闵可夫斯基和是一个数学工具，给定障碍物和机器人的几何形状即可计算其C-space.
其背后的思想就是，将机器人几何作为笔刷，沿着障碍物描一圈轮廓，新描后内容就是C_obs.
对于凸多边形，计算极快，而对于非凸多边形，计算代价高很多。
3D Configuration Space
在原来的C-space中增加一个旋转维度，形成3D C-space.
形成的方法很简单，机器人绕Z轴旋转一个角度就是一个图层，在该角度下使用闵可夫斯基和就能得到该层的C-space,
多层的叠加就形成了3D C-space.
离散化的方法
路线图Roadmap、单元分解Cell Decomposition、梯度场Gradient Field.
可视性图法 visibility graph
可视性图法通过连接起点到所有可见障碍物的顶点生成路线图。是完整的、最短距离的算法。
其缺陷是，容错率较低，因为会有一些线段非常接近障碍物，容易发生撞击。
Voronoi Diagram
Voronoi Diagram法的路径离开障碍物。实现方式是将障碍物之间的自由空间二分，所有路径与其周围的障碍物等距。是完整算法。
Cell Decompositon（exact）
在障碍物的每一个顶点增加一条竖直线，将C-space隔成非重叠单元区域。对于非常规形状，会增加计算复杂度。
以下方法可以解决计算复杂度问题。
Approximate Cell Decompostion
将C-space分解为简单且常规的形状。同时，还支持空间的等级分解。
Simple Decomposition
一个C-space被分解为由矩形单元组成的网格，从而每个单元可以标记成空或占有。算法就能在起点和终点之间找到一条空单元连线。
这种方式比精确分解更高效，却也丢失了其完整性。因为偶尔有存在路径的图被标记为占有，把99%占用和1%占用做一样看，不实用。
Iterative Decompostion
该方法不同于直接分解，而是将空间迭代地分解为4个象限。每个象限被分类为空、占有和混合的，混合即指部分占有情况。
如果从起点到目标都找不到一串空的单元格，就将混合的单元再次划分为4个新的象限。2维里叫做四叉树分解，3维中叫八叉树分解。
Approximate Cell Decompostion常用于实践，因为计算量较Exact Cell Decompostion低很多。但是与其他的离散/组合型的
路径规划算法一样，碰到多维问题时就计算量爆炸。
势场法 Potential Field
势场法生成两个函数，一个吸引机器人去目标，一个将机器人排离障碍物，两个函数可以加起来为一个离散化的表达。
应用梯度下降算法，就能将机器人导向目的。
吸引的势场：一个在目的地全局最小的方程
排斥的势场：在障碍物周围有斜坡
其缺陷是：即不完整也不最佳
图搜索算法 graph search
分为不知情的盲搜和启发式搜索。
盲搜：广度优先搜索BFS、深度优先搜索DFS、等成本搜索Uniform cost search
启发式搜索：A*
算法评价指标：时间复杂度、空间复杂度、泛化能力
广度优先搜索BFS
以树结构为例，从根部结点开始，将探索到的所有根放入队列尾端，然后从队列前端依次取出一个节点，将其根都放入队列尾端。
complete and optimal
深度优先搜索DFS
以树结构为例，从根部节点开始，将探索到的所有根压入栈，然后栈顶元素出栈，探索其根也压入栈。
nither complete or optimal
等成本搜索Uniform
如果把每两个节点之间的路加上一个权重，就能计算某一条路的成本。
complete and optimal
A*算法
引入一个启发函数h(n) = 到目标的距离
探索图的时候计算的成本g(n) = 当前的路径成本
算法目标是最小化函数 f(n) = g(n) + h(n)
算法流程：计算每个节点到目标的启发函数h(n), 计算起点周围点的路径成本g(n), 计算各周围点的f(n)后排序（或用priority queue），探索f(n)最小的点，
再次计算f(n)排序-探索，直到目标。
A*不保证每次都是optimal的，满足以下条件则可以达到optimal：
每条边都要比某个量长，否则会卡住；启发函数要满足三角不等式，即一边始终小于另两边之和；启发函数的值不能超过真实成本（可能导致非最优路径）。
附加资料
启发式
Path Finding Visualization
A* 变种
一些考虑
应用双向搜索，即从起点和终点一起搜，当二者汇合就产生了路径。
A*搜出来的通常是路径比较短，但是与障碍物之间很近，容易发生撞击，可以将靠近障碍物的路径的成本提高。
离散的折现状路径在现实中需要做一些smoothing，平滑的时候也要考虑与障碍物保持安全距离。
3 基于样本的路径规划
处理更高维度的问题时，比如三维地图、多自由度的机器人等，做全面的离散化较难，成本较高。
对于n维空间，每个节点的连接边有3^n-1个。
此外，在使用运动学受限的机器人时也会有困难，例如汽车不能侧向移动等。
完整系统，所有约束都只与当前姿态和时间有关。
非完整系统，有约束和微分有关，例如安全转弯半径与速度有关。路径规划会难一点。
基于样本的路径规划算法从C-space随机取样本以建立对空间的表达，具有概率完整性，在高维空间非常高效，缺陷是面对长条缝隙中的路径需要较长
时间发现，或者当作没有路径。
两种类型：概率地图方法 PRM probabilistic roadmap method、RRT rapidly exploring random tree method
PRM
从工作空间随机取样构建一个graph，跳过c-space构建和离散化。只要一个碰撞检测函数检查一个随机生成配置点是在自由空间还是在障碍物。
建图的过程叫做学习阶段：生成点，判断是否在障碍物中，在给定半径内找邻居或者k近邻，检查是否能够建立到邻居的路径，建立路径，
当真个采样点或者路径边数达到目标结束。
参数设定：迭代次数决定最终地图的精细度和计算用时；最近邻选择算法。
RRT
从起点开始，随机生成配置点，每个配置点只有一个父节点，如果配置点很长，在方向上新建一个距离短的点，可行路线连接。可以双向进行。
参数设定：取样方法（如何生成随机配置点）；growth rate 距离；
可以在几毫秒内解决7维的问题，几分钟解决20维问题，效率比PRM高很多
一些考虑
在生成路径后可能是非常曲折的，这时候要用到路径平滑。
所谓的路径平滑，也就是检查相邻的两个点之间的距离，如果小于路径中的距离，就判断两个点的连线是否有障碍物，如果没有就用连线代替原路径段。
非完整：基于样本的路径规划算法不是完整的(仅概率完整)，通常在复杂的环境中（例如狭长的通道）无法找到结果。
非最优：虽然可以在PRM图中用A*获取图内最优路径，但是图本身就不是环境的全面表达。
至少，基于样本的方法在多维空间中路径规划是可行的。
RRT*: 变种，通过查看子节点能否和其父节点或父节点的父节点交换，来生成更直接的路径
Anytime algorithm: 就算搜索被中止，也有路径返回，搜的越久越接近最优
4 概率路径规划
马尔可夫决策过程MDP
- 一个状态集合：S
- 初始状态：s_0
- 一个动作集合：A
- 过渡模型：T(s, a, s’)，从状态s执行动作a后到达s’的概率函数，在MDP中认为此概率只与前一状态s有关而与路径无关。
- 一个奖励集合R
概率路径规划中的MDP和强化学习RL之间的主要区别是，前者对以上的要素都已知，而在RL中，机器人只已知状态和可执行的操作，但不知道奖励和过渡模型。
策略Policy
策略是从状态到动作的映射。对于每个状态，策略都会告知机器人应该采取哪些动作。
最佳策略，表示为π∗，告知机器人在任何状态下应采取的 最佳 行动，以最大化整体回报
每个状态可以计算出其状态效用值，在某种状态下策略会选择最大效用值