机器人运动规划：
前端： path finding
path finding 找到一个 collision free 的运动路径。
后端： trajectory generation/optimization
从前端得到一个低维，粗略的路径之后，使用优化的技巧，把它变成高维的，满足机器人动力学要求的，光滑连续，安全保证等要求的一个路径，称为轨迹 motion planning.
Truncated Signed Distance Functions
体素存储的是传感器射线到测量物体表面的距离。具体：一个视锥的范围内，存储物体表面[-delta,delta]距离内的值。
Euclidean Signed Distance Functions
机器人原来是在工作空间中，根据机器人的大小对障碍物进行膨胀，此时机器人可以看成一个点，都转到配置空间中。
广度优先搜索（BFS）维护的容器是队列，深度优先搜索（DFS）维护的容器是堆栈。
DFS：维护的是堆栈，节点先进后出。所以后加进来的节点会首先弹出来，然后扩展，再压进堆栈，不断循环，直观的理解就是会朝着某一个方向搜索，即深度优先。
BFS：维护的是队列，节点先进先出。先加进来的节点会首先弹出来，然后扩展，再压进堆栈，不断循环，直观的理解就是它是一层一层不断扩展搜索的，即广度优先。
BFS 和 DFS 从容器中弹出节点是根据 " first in" 或者 “ last in ” 的规则。
贪心算法中，从容器中弹出节点的规则是自己定义的，叫做 heuristic
Jump Point Search
核心：find symmetry and break them
Probabilistic Road Map
第一个阶段：Learning phase
如下图所示：采样N个点，删除那些在障碍物内的点。然后把点与点连接起来，但是连接时对距离做一定的限制，如果连线超过一定的距离，就不连了。此外连线中经过障碍的也要删去。
第二个阶段：Query phase
此外：Lazy collision-checking
在learning 阶段判断采样点落入障碍物中比较费时，这里就不考虑采样点落入障碍物中。
具体过程： 这种 lazy 的方式，只采样点，然后生成连线，但是不考虑采样点碰到障碍物。首先搜索一条路径不考虑障碍物，搜索到路径之后把碰到障碍物的边和障碍物内的采样点删去，再重新搜索。
Rapidly-exploring Random Tree
实现步骤：首先得到一个采样点（蓝色的点），从起点向采样点移动一定的距离
δ
\delta
δ ,得到一个新的红色点，如若这个点是 collision free 的，就把这个点个这条边加入树中。
然后再采样得到一个点
X
r
a
n
d
X_{rand}
Xrand, 找到树上距离
X
r
a
n
d
X_{rand}
Xrand 最近的一个点
X
n
e
a
r
X_{near}
Xnear，然后
X
r
a
n
d
X_{rand}
Xrand 移动一定的距离得到
X
n
e
w
X_{new}
Xnew， 如果
X
n
e
w
X_{new}
Xnew 是 collision free 的就把
X
n
e
w
X_{new}
Xnew 和
E
i
E_{i}
Ei 添加到树中。
最左边是路径轨迹的生成部分，但是生成之后不是在整个空间中进行随机点的采样了。把采样范围限制在一个椭圆内。以起点和终点做为椭圆的焦点，生成的路径做为椭圆的常数。随着在限制范围内采样，路径变的优化，同时随着路径变短，椭圆也会变扁，采样范围变小。
希望每两个节点之前是 feasible motion connections
两种方法：
forward direction ：discrete in control space
把控制空间离散
reverse direction ：discrete in state space
把机器人周围的状态离散，再找一条当前状态到周围状态的连接
两种方法比较：
问题：离散出来的状态有一条碰到障碍物，其他的几条也很有可能碰到，如何让离散的状态尽量分得开。
直接把周围的状态离散出来，反算这条边是怎么连接上的。
通过边界条件（0时刻和T时刻）来解，会得到很多组解，不知道哪一个才是最优的。
(…一些上学期学的最优控制里的东西)。
实现的步骤: 和 A * 类似
启发函数的选择：
和 A * 的不同点：
