深蓝 运动规划

【CH1】 Introduction

一. Motion Planning 概念

机器人运动规划：

前端： path finding

• search for an initial safe path
• low dimensional
• discrete space

path finding 找到一个 collision free 的运动路径。

后端： trajectory generation/optimization

• search for an executable trajectory
• high dimensional
• continuous space

从前端得到一个低维，粗略的路径之后，使用优化的技巧，把它变成高维的，满足机器人动力学要求的，光滑连续，安全保证等要求的一个路径，称为轨迹 motion planning.

二. Front-end：Path finding

1. Search-based methods

2. Sampling-based methods

• PRM
• RRT
• RRT*
• Informed RRT*
  

3. Kinodynamic Path Finding

• State lattice seach
• Hybrid A *
• Kinodynamic RRT*
  

三. Back-end：Trajectory Optimization

1. minimum-snap

2. Hard constrained minimum-snap

3. Soft constrained minimum-snap

四. Map representation

1. grid map

2. octomap

3. voxel hashing

4. point cloud map

5. TSDF map

Truncated Signed Distance Functions
体素存储的是传感器射线到测量物体表面的距离。具体：一个视锥的范围内，存储物体表面[-delta,delta]距离内的值。

6. ESDF map

Euclidean Signed Distance Functions

【CH2】 Search-based path finding

一. Configuration Space

机器人原来是在工作空间中，根据机器人的大小对障碍物进行膨胀，此时机器人可以看成一个点，都转到配置空间中。

二. Graph and Search method

1. overview

2. graph traversal

广度优先搜索（BFS）维护的容器是队列，深度优先搜索（DFS）维护的容器是堆栈。

DFS：维护的是堆栈，节点先进后出。所以后加进来的节点会首先弹出来，然后扩展，再压进堆栈，不断循环，直观的理解就是会朝着某一个方向搜索，即深度优先。

BFS：维护的是队列，节点先进先出。先加进来的节点会首先弹出来，然后扩展，再压进堆栈，不断循环，直观的理解就是它是一层一层不断扩展搜索的，即广度优先。

三. Heuristic search

1. Greedy Best First Search

BFS 和 DFS 从容器中弹出节点是根据 " first in" 或者 “ last in ” 的规则。
贪心算法中，从容器中弹出节点的规则是自己定义的，叫做 heuristic

2. Djikstra

3. A *

4. JPS

Jump Point Search
核心：find symmetry and break them

【CH3】 Sample-based path finding

一. PRM

Probabilistic Road Map

第一个阶段：Learning phase

• Sample N points in C-space
• Delete points that are not collision-free
• Connect to nearest points and get collision-free segments.
• Delete segments that are not collision free

如下图所示：采样N个点，删除那些在障碍物内的点。然后把点与点连接起来，但是连接时对距离做一定的限制，如果连线超过一定的距离，就不连了。此外连线中经过障碍的也要删去。

第二个阶段：Query phase

• Search on the road map to find a path from the start to the goal (using Dijkstra’s algorithm or the A* algorithm).
  

此外：Lazy collision-checking

在learning 阶段判断采样点落入障碍物中比较费时，这里就不考虑采样点落入障碍物中。

具体过程： 这种 lazy 的方式，只采样点，然后生成连线，但是不考虑采样点碰到障碍物。首先搜索一条路径不考虑障碍物，搜索到路径之后把碰到障碍物的边和障碍物内的采样点删去，再重新搜索。

二. RRT

Rapidly-exploring Random Tree

实现步骤：首先得到一个采样点（蓝色的点），从起点向采样点移动一定的距离 $\delta$ ,得到一个新的红色点，如若这个点是 collision free 的，就把这个点个这条边加入树中。

然后再采样得到一个点 $X_{rand}$, 找到树上距离 $X_{rand}$ 最近的一个点 $X_{near}$，然后 $X_{rand}$ 移动一定的距离得到 $X_{new}$， 如果 $X_{new}$ 是 collision free 的就把 $X_{new}$ 和 $E_i$ 添加到树中。

三. Optimal sampling-based path planning methods

1. RRT*

2. Kinodynamic RRT*

3. Anytime-RRT*

四. Advanced Sampling-based Methods

1. Informed RRT*

最左边是路径轨迹的生成部分，但是生成之后不是在整个空间中进行随机点的采样了。把采样范围限制在一个椭圆内。以起点和终点做为椭圆的焦点，生成的路径做为椭圆的常数。随着在限制范围内采样，路径变的优化，同时随着路径变短，椭圆也会变扁，采样范围变小。

2. Cross-entropy motion planning

【CH4】Kinodynamic path finding

希望每两个节点之前是 feasible motion connections

两种方法：

• forward direction ：discrete in control space
  把控制空间离散

• reverse direction ：discrete in state space
  把机器人周围的状态离散，再找一条当前状态到周围状态的连接

两种方法比较：

一. State Lattice Planning

1. sample in control space

问题：离散出来的状态有一条碰到障碍物，其他的几条也很有可能碰到，如何让离散的状态尽量分得开。

2. sample in state space

直接把周围的状态离散出来，反算这条边是怎么连接上的。

二. Boundary Value Problem

通过边界条件（0时刻和T时刻）来解，会得到很多组解，不知道哪一个才是最优的。

1.Optimal Boundary Value Problem

(…一些上学期学的最优控制里的东西)。

三. Hybrid A*

实现的步骤: 和 A * 类似

启发函数的选择：

和 A * 的不同点：

1. 启发函数的选择
2. 根据离散状态，寻找临近的点，不像A * 直接找上下左右相邻的点
3. 不仅需要记录代价，还要记录和更新状态

四. Kinodynamic RRT*