基于搜索的路径规划

基于搜索的路径规划

1.0 图搜索基础

1.1 Configuration Space(配置空间)

Robot configuration
机器人配置，简单来说就是将机器人在空间中用一个点来描述，忽略了其外观信息，例如形状不管是圆形还是方形，都均会抽象成一个点进行表示。
Robot degree of freedom (DOF)
在进行轨迹规划的时候是要考虑机器人的一个实际的运动模型的，不同的运动模型需要的输入和输出变量是不同的，即对应的控制变量的维度是不同的，尽可能的使用一个较小的自由度去表示机器人在配置空间中的位姿。
Robot configuration space
一个 \(n\) 维的空间，包含着机器人的所有可能位置，称为 C-Space
简单来说，每个机器人的位姿在 C-space 中就是一个点而已。

1.2 C-space Obstacle

Planning in workspace
首选解释一下什么事工作空间即什么是 workspace，简单来说其实就是机器人真实的一个工作空间，就是我们所处的物理世界。如果在物理世界中进行机器人的一个路径规划，其实是一个比较麻烦的事情，有如下两个原因：
- 每个机器人拥有不同的形状和大小(different shape and size)
- 碰撞检测需要指导机器人的几何信息
  这样进行规划是非常耗时和困难的，因此引入 C-space 这一概念
Planning in C-space
- 在 C-space 中，机器人被抽象为了一个 C-space 中的一个点，例如三维空间中机器人可以由 position(a point in \(R^3\))，pose (a point in \(SO(3)\))
- 至于真实世界中感知到的障碍物信息则需要提前展示部署到 C-space 中去，是一个在规划前要完成的工作，我们称这些障碍物为 C-space 中的障碍物，或者称为 C-obstacle
- \(C_{space} = (C_{obstacle})\ U\ (C_{free})\)
- 而我们通常所说的路径规划此时就可以说成是在 C-free 中找一个连接 \(q_{start}\) 和 \(q_{goal}\) 的路径

1.3 总结

In workspace
机器人是一个拥有形状和大小的实体 (hard for motion planning)
In configuration space
- 机器人是一个点 (easy for motion planning)
- 障碍物提前加入到 C-space 中去，其实严格上来说也不算是加入，只是以某种方式对规划时使用的地图进行了一定方式的变动，使得在后续的路径规划中可以忽略机器人的一个形状和大小的约束，从而简化规划问题的难度。
在 C-space 表示障碍物可能非常复杂。因此在实践中使用了近似（但更保守）的表示，例如下图所示是将机器人通建模为一个半径为 \(r\) 的球形：

2.0 图和搜索技术

2.1 图

首先图这个概念相信大家或多或少的在本科学习中都会接触到图论相关的内容，简单来说图是由节点和边构成的。根据节点和边的不同形式还可以分成无向图，有向图，赋权图等等。下面是一些例子。
- 无向图
- 有向图
- 赋权图
状态空间图 (state sapce graph)
搜索算法的数学表示，即将一张图抽象成数学上的表达形式
- 对于每一个搜索问题，都有一个对应的 state space graph
- 图中节点之间的联系自然由链接两个节点之间的边所表示
- 下面两张图分别代表了在基于栅格地图搜索是和基于概率路线图搜索是所对应的 state space graph

2.2 图搜索概述

图搜索技术总是从一个起点 \(X_S\) 出发
- 可以将搜索图画成一颗树（数据结构中的树），树的父节点就是起点
- 然后对该树进行遍历，当在树中找到目标点的时候，通过该节点进行回溯，即可找到一条从起点到终点的路径。
- 然而对于许多问题，我们永远无法真正构建整棵树，太大或效率低下——我们只想尽快到达目标节点。
- 下面展示的则是一个简单的将图转换为树的过程

一个基于图搜索的大致流程可以归结为如下步骤

维持一个 container 去存储将要访问的节点
container 初始化的时候只有 \(X_S\) 节点
LOOP
- 从 container 中 Remove 一个节点，移除的规则需要遵循提前定义好的 score function
  - Visit a node
- Expansion: 获得移除节点的邻居节点
  - Discover all its neighbors
- Push them (neighbors) into the container
End LOOP

2.3 图搜索回顾

Question 1: When to end the loop? 什么时候终止上述的 LOOP
- Possible Option: 当所维护的 container 为空的时候
Question 2: What if the graph is cyclic? 如果图是循环的呢？
- 当一个节点从容器中移除（扩展/访问）时，它不应该再次添加回容器。
Question 3: 以什么方式删除正确的节点，以便尽快达到目标状态，从而减少图节点的扩展。

\(\quad\)显而易见，问题3则是解决图搜索问题或者说基于搜索的路径规划问题的核心问题，起初，我们并不知道目标点在图中的哪个位置，因此我们只能尝试去遍历图中的所有节点，当到达目标节点的时候则停止遍历即可，那么下面的一个问题则变成了如何高效的进行图的遍历，从而快速的找到目标点。

2.4 图的遍历 (Graph Traversal)

\(\quad\)学过一点关于图论的同学可能知道，图的遍历遍历一般是基于两种方法，即我们通常所说的深度优先遍历(DFS)和广度优先遍历(BFS)，两者的遍历方式有一定的区别，首先在编程实现的时候，所对应的数据结构一个对应的是堆，一个对应的则是栈，具体如下图所示：

2.4.1 Depth First Search (DFS)

\(\quad\)策略：移除/扩展容器中深度最大的节点，若深度一样，则根据自定义的策略进行选择移除(即后文所说的 Heuristic Function)
\(\quad\)具体的实现如下图所示，维护一个 last in first output container (i.e. statck)

\(\quad\)下面通过一个动图来展示这一过程，根据图中的编号，节点会依次的进行扩展和遍历。

2.4.2 Breadth First Search (BFS)

\(\quad\)广度优先搜索的策略：移除/扩展容器中最浅层的节点。
\(\quad\)实现方式：维护一个 first in first output container (i.e. queue)

\(\quad\)下面通过一个动图来展示这一过程，根据图中的编号，节点会依次的进行扩展和遍历。

2.4.3 BFS VS DFS

BFS 和 DFS 的对比如下图所示：

\(\quad\) 从图中可以明显的看到虽然两个方法都可以找到最终的目标点，但是 DFS 明显找到的不是最优解，即并不是最短路。这里我们只是直观上说明 DFS 不能找到最优路径，事实上也确实不行因此在后面的讨论中，我们值讨论基于 BFS 的遍历方式。

3.0 启发式搜索 (Heuristic Search)

3.1 贪心搜索

\(\quad\) BFS 和 DFS 在选择下一个节点的时候，仅仅是依靠当前 container 中谁是 "first in" 或者谁是 last in 进行判断。

\(\quad\) 贪心算法进行搜索则是根据一种 heuristic function 进行选择，从而选择认为(最好的节点)

Heuristic Definition:

A heuristic is a guess of how close to you are to the target。即启发式是一个对当前点距离目标点多远的猜测，那么我们自然就可以使用两点之间的一个范式距离来判断猜测了，下图就表示了利用欧式距离和曼哈顿距离当作 heuristic 时所猜测的距离。

heuristic 应当满足以下两点：
启发式搜索可以指导你朝着一个尽可能正确的方向向目标点靠近。
启发式函数应当是非常简单进行计算的，因为本身我们在不使用启发式搜索时，单单通过广度优先遍历是可以找到一个起点到目标点的最有解的，但是其速度比较慢，因此才加入启发式搜索，但如果启发式函数计算较为复杂，反而会增加计算时间，得不偿失，启发式搜索也就是去了意义。