深蓝学院-运动规划重点笔记

感觉课程讲的非常有条理，从算法的大分类到细节技术。

基于图搜索的方法

配置空间：维度等于机器人的自由度，可以理解为一个点可以表示一个机器人的位姿。例如小车4自由度(x,y,z,$\theta$)。在配置空间中，机器人表示为点。

在3维空间中，要做碰撞检测，就很麻烦。所以在配置空间中做规划，要对障碍物按照机器人的尺寸做膨胀。

基于图搜索的算法框架：

关键问题：
如果一个结点被弹出容器，就不再会被加入到容器中

BFS使用的容器是队列，DFS使用的是栈。在边的权重都为1的情况下，BFS能保证路径最短，所以搜索算法是基于BFS的。

Dijkstra

与BFS相比，Dijkstra从容器中弹出的规则不同。Dijkstra弹出的是从起点到某点的走过的距离最短的点。也就是说，容器不再使用队列，而是优先队列，弹出的总是走过路径最短的点。

优点：完备的，能保证最优解
缺点：没有启发，就是暴力搜

A*

相比于Dijkstra，A*的容器对元素的排序依赖的是（走过的路径的距离+启发距离）。某节点启发距离是指预测的从该节点到终点的距离。

当启发距离小于真实距离的时候，A*能找到最优解。

工程改进：
1）使用Diagonal Heuristic：用norm做的启发函数都过分的低估了实际距离，使得搜索的区域很大
2）Tie Breaker：可能存在很多条路都是最优解。打破路径的对称性，使得搜索更偏向于一条路

JPS（Jump Point Search）

Look Ahead Rule + Jumping Rule
相比于A*，visit一个结点后，使用Jumping Rule来扩展该节点，把从该节点jumping的后继结点加入到容器中

对于复杂环境，JPS总的来说要比A*好
JPS只能用于uniform grid使用

基于采样的方法

比较高效

三种完备性

• Complete Planner: always answers a path planning query correctly in bounded time.
• Probabilistic Complete Planner: if a solution exists, planner will eventually find it, using random sampling (e.g. Monte Carlo sampling)
• Resolution Complete Planner: same as above but based on a deterministic sampling (e.g sampling on a fixed grid).

概率路图

Learning Phase
先在空间中撒点，去掉与障碍物碰撞的点
然后构建kNN的图，图中的节点包括起始点、终止点和保留下来的撒点。然后去掉边长超出一定阈值的边、去掉与障碍物碰撞的边

Query Phase
使用图搜索的方法，就可以找到一条可行驶路径。

优点：概率完备的，相比于图搜索的比较高效
缺点：边界值问题，不够有效率

优化效率：
碰撞检测：lazy collision-checking
在撒点和构建图的时候，不管碰撞问题。在搜索到轨迹之后，再删去碰撞部分的轨迹，重新搜索部分轨迹。

RRT

Pros:
· Aims to find a path from the start to the goal
· More target-oriented than PRM
Cons:
· Not optimal solution
· Not efficient(leave room for improvement)
· Sample in the whole space

改进：
kd-tree做最近点搜索
Bidirectional RRT/Connect RRT：通过一次采样，完成两颗树的构建。

Note: 这里没讲是怎么构建kd-tree的。按照RRT的思路，采样点是逐渐加进去的，但加进去的过程要使用kd-tree找最近邻。所以，kd-tree也是增量式的构建吗？kd-tree其实本身是针对静态数据集的，在网上我也基本没有搜到kd-tree的增量式的构建方式。问题在于，插入一个点会影响整棵树的结构。所以我查到的方式是使用替罪羊树这种方法来完成增量式构建。或者kd-tree不是增量式构建，就是每一次查找都构建一棵树？这样子是不是有点太慢了。

RRT*

解决生成路径不最优的问题

通过撒点找到的新点$x_{new}$不是直接连接到最近点上，而是先找一个领域内的点，选择一个连接使得$x_{new}$到起始点最近，连接上去。然后更新领域内的点到起始点的距离。

RRT*在生成路径之后，继续采样可以优化路径

Kinodynamic-RRT*

连接$x_{new}$与父节点不用直线，而是用曲线，使得更符合机器人的运动学约束

Anytime-RRT*

在机器人运动过程中，一直在更新RRT*

Advanced Sampling-based Methods

Informed RRT*

当路径生成之后，将采样的范围限定在与生成路径有关的椭圆内，从而完成路径的优化。

Cross-entropy motion planning

首先得到一个路径
然后以路径中的每个点作为一个高斯模型的中心，在多高斯模型中采样，得到多条路径。
然后对多条路径做均值，重新构建多高斯模型。

Note：具体的如何做均值的，可以使用k-means这种方式。但具体的还得看论文

Kinodynamic Path Finding

State Lattice Planning

对空间采样的两种方法：
前向模拟：通过采样控制量，推算下一步车的位置。优点是容易实现，缺点是非任务导向的、效率低
反向计算：通过采样空间中的位置，反算两点之间的控制量，反算边，有点是任务导向，缺点是难实现

在反向计算中，给出采样空间的位置，如何生成一条满足动力学要求的边呢？这就是个Boundary Value Problem

BVP

如何获得一个BVP最好的解

使用经典套路，Pontryain