Motion Planning for Mobile Robots：总览

What is autonomous robot？

Definition: an autonomous robot is a robot that performs behaviors or tasks with a high degree of autonomy (without external influence).

一个自动机器人在其运动过程中应该包含状态估计（estimation）、感知（perception）、规划（planning）和控制（control）等环节，其action-loop如下：

Perception-Planning-Control action loop

Sensing代表机器人的传感器获取外界信息，Localization and Mapping代表机器人定位与建图，即SLAM环节，Planning代表机器人运动规划，Control代表通过控制理论对机器人进行控制。

• Estimation

• Low latency 低延迟

• High accuracy & consistency 高精度&高前后一致性

• Perception

• 3D sensing & dense perception 使用机载传感器对周围环境进行3D的稠密的建图

• Map fusion & integration for planning 地图的信息融合以用于机器人运动规划

• Planning

• Complex & unknown environment 在复杂和未知的环境中

• Safety & dynamical feasibility 生成安全的、动力学可行的移动机器人运动轨迹

• Limited sensing & computation 考虑到机器人具备很有限的感知和计算能力

• Control

• Aggressive maneuvers 激进的运动控制

• Smooth trajectory tracking 平滑的轨迹跟踪

What is motion planning?

Basic requirement

1. Safety: Collision avoidance 避免碰撞且要求躲避所有可能的障碍物。

2. Smoothness: energy saving, comfort 运动过程节能、生成轨迹要求保证其光滑性和平滑性。

3. Kinodynamic feasible: executable, controllable要求在机器人运动学动力学约束上是可行可控的。

Front and Back

• Front-end path finding 前端路径搜索

• Search for an initial safe path 寻找一个安全的初始的（可能质量并不高）路径，通常不包含时间、高维信息

• Low dimensional 低维的

• Discrete space 离散的状态空间

• Back-end trajectory generation 后端轨迹优化

• Search for an executable trajectory 生成一个可行的高质量的轨迹

• High dimensional 高维的

• continuous space 连续的状态空间

Front-end: Path Finding

• Search-based Path Finding 基于搜索的路径规划

• Graph Search Basis 将移动器人路径规划问题转变为一个图搜索问题

• Dijkstra and A* Dijkstra 和 A* 算法

• Jump Point Search JPS算法

• Sampling-based Path Finding 基于随机采样的路径规划

• Probabilistic Road Map (PRM) 概率路线图

• Rapidly-exploring Random Tree (RRT) 快速搜索随机树（RRT）算法

• Optimal Sampling-based Methods 使具有渐进最优性的改良RRT算法（RRT*）

• Advanced Sampling-based Methods 进阶的基于采样的路径规划方法

• Kinodynamic Path Finding 考虑机器人动力学模型的路径规划

• State-state Boundary Value Optimal Control Problem 满足两点边界值的最优控制问题

• State Lattice Search 状态图搜索（高维的Dijkstra或者A*算法）

• Kinodynamic RRT* 引入动力学模型的RRT*

• Hybrid A* 混合A*

Back-end: Trajectory Generation

• Minimum Snap Trajectory Generation 最小捕捉轨迹生成

• Differential Flatness 差分平坦

• Minimum Snap Optimization 最小捕捉优化

• Closed-form Solution to Minimum Snap 最小捕捉优化的闭式解的求解

• Time Allocation 时间分配问题

• Implementation in Practice 实现

• Soft and Hard Constrained Trajectory Optimization 硬约束与软约束轨迹优化

• Soft Constrained Trajectory Optimization 软约束：带有的倾向性很强的约束，但不一定始终满足该约束

• Hard Constrained Trajectory Optimization 硬约束：始终满足的约束

MDP & MPC

• Markov Decision Process-based Planning 马尔可夫决策过程

• Uncertainties in Planning and MDP

• Minimax Cost Planning and Expected Cost Minimal Planning

• Value Iteration and Real-Time Dynamic Programming

• Model Predictive Control for Robotics Planning 模型预测控制

• Linear MPC

• Non-linear MPC

Search-based Method

• For every search problem, there's a corresponding state space graph.

对于任意一个搜索问题，都有与之对应的一个搜索图，例如：

Search Graph

将搜索问题抽象成一个在拓扑上互相连接起来的图。

• Connectivity between nodes in the graph is represented by (directed or undirected) edges.

节点中的连接可以是有向的，也可以是无向的

Dijkstra vs. A*

• Dijkstra's algorithm expanded in all directions.

Dijkstra

• A* expand mainly towards the goal, but does not hedge its bets to ensure optimality.

A*

A* vs. JPS

A* and JPS Comparsion

• A*算法的过程中需要拓展的点的数量很多，拓展这些点需要花费大量的时间，在实际情况中可能不利

• JPS算法使用了独特的跳跃机制，能大大减少拓展节点的数量，使用很长的边取而代之。

Sampling-based Method

Probabilistic Roadmap (PRM)

基于随机采样的概率路线图法，先在图中撒很多随机的采样点，随后尽量将每个点与其拓扑结构或欧式距离相邻的采样点进行边的连接，同时检验这些边是否会经过障碍物以及这些节点是否本身就落在障碍物中，如果与障碍物有接触则删除这些节点/边，最后得到一张由节点和边组成的图。

RRT vs RRT*

• 概率路线图是一次撒好所有的随机采样点，再尽可能的将每一个采样点与其周围相邻点进行连接再进行图搜索。而RRT相当于是PRM的增量版本，随着采样的进行，Random Tree（随机树）就不断向外拓展，假设采样拓展树的步长尽量的小，算法期望最后会收敛到一条连接起点和终点的最优路径。但是，RRT被证明不具有渐进最优性，即使拓展树的步长尽可能的短，采样数量接近无穷，最后找到的路径也很难收敛到理论上的最优路径，如上图RRT所示。

• 而RRT*会在每次采样过后，对采样点的周围进行树的重新组织，会对已经存在的采样点进行判断：能否通过新采样点来维护一个更短的到达起点的路径，也叫rewire操作。通过这个过程使得RRT*算法具备了渐进最优性，最后得到的路径也能更加接近理论上的最佳路径，如上图RRT*所示。

Informed RRT*

启发式的RRT*算法：构造一个椭圆，以起点和终点为椭圆的焦点，以已经找到的初始路径的长度作为椭圆的长轴，如下图所示：

Informed RRT*

之后的采样就只会在椭圆范围内进行采样，而不会生成新的采样了。最后椭圆范围不断缩小，一直保持在椭圆范围内进行采样，最后一直逼近它的最优路径。如下图所示：

Informed RRT* Process

Informed RRT*和RRT*对比优势在哪呢？由于RRT*是完全随机的采样，在某些特殊的地图上可能不会发现最短路径。例如下图，障碍物中间有一条很窄的缝，随机采样能通过这条缝的概率是很低的，RRT*很有可能找不到最短路径。但是Informed RRT*的方法会不断提升最优解的质量，会在已有的暂时最优的Path的周围去采样，因此它将很有概率能采样到通过这条缝的最优Path。同时也能很轻易的看出来Informed RRT*的采样点数量远少于RRT，其花费时间更少。

RRT* and Informed RRT* Comparsion

Kinodynamic Path Finding

State Lattice Search

建立移动机器人的状态空间模型（或非线性模型），同时将它的控制量离散化，给定一些控制量进而驱动其状态的转移来构造一个状态图，其cost很高。

State Lattice Search

Hybrid A*

混合A*算法的框架与State Latiice Search类似，不同之处在于混合A*算法还维护了一个栅格网络地图，会在Lattice Graph拓展的过程中依据栅格网络地图本身的结构对树进行暴力的剪枝。

Hybrid A*

每个网格中一定只有一个状态，当出现一个更好的状态时就会取代之前存储的状态。

Kinodynamic RRT*

遵循RRT*算法的基本框图，但是有别于经典的欧式空间中的RRT*，这里的初始状态具有高维信息，它不只是初始的位置，还有初始的速度、加速度等状态信息。所以Kinodynam RRT*采样点需要包括多种信息，然后需要解从初始状态到终止状态的两点边界的最优控制问题，需要把两采样点的边给连上。

Kinodynamic RRT*

与State Lattice Search和Hybrid A*算法的区别在于这里离散的不是控制量，而是状态量。因此要恢复出状态转移路径就相当于先有了边界，再去算中间的路径。

Back-end: Trajectory Optimization

Basic Minimum-snap

Basic Minimum-snap

给个若干个目标点，包括起始点和终止点，生成一条曲线依次通过这些路标点，同时尽可能地减少运动过程中的能量消耗。

Hard constrained Minimum-snap

在复杂的环境中发现其中的安全的可通行区域，并将其表示成一个一个连接起来的凸的解空间，在解空间里可以对Minimum-snap问题施加一些硬性的约束。

Hard constrained Minimum-snap

将环境中的安全区域用一些凸的解空间表示，如上图中的绿色四边形。

Soft constrained Minimum-snap

软约束一般用于做局部的轨迹规划或轨迹重规划，它的思想是在某一个环境中，每一个障碍物通常会提供一个距离场，给我们距离梯度信息，可以理解为离障碍物越近，障碍物就会对机器人产生一个排斥力，就会将机器人往路径的中间推。

Soft constrained Minimum-snap