常用地图结构
occupancy grid map - 栅格地图
- Most Dense;
- Structural;
- Direct Index Query;
octo-map - 栅格地图的优化，在障碍物存在的地方稠密，无障碍物的地方稀疏
- Sparse;
- Structural;
- Indirect Index Query;
voxel hashing - 将坐标哈希化，通过哈希表查询地图信息
- Most Sparse;
- Structural;
- Indirect Index Query;
- Un-ordered;
- No Index Query;
基于图搜索的算法
基础概念
- 机器人配置（Robot configuration）：机器人上所有点的位置的描述；
- 机器人自由度（Robot degree of freedom, DOF）：能够表示机器人配置空间最少需要的值的数量；
- 机器人配置空间（Robot configuration space）：包含机器人所有可能的配置的 n 维向量空间，又称为 C-space；
- 机器人任何一个可能的位置都是在配置空间中的一个点；
2.1 Graph Search Basis
算法核心思想
- Maintain a container to store all the nodes to be visited;
- The container is initialized with the start state \(X_s\) ;
- Loop:
- Remove a node from the container according to some pre-defined score function;
- Visit a node;
- Expansion: Obtain all neighbors of the node;
- Discover all its neighbors;
- Push them (neighbors) into the container;
- Remove a node from the container according to some pre-defined score function;
- End loop;
Question 1：When to end the loop?
Answer：容器为空，或者目标节点已经被访问到；
Question 2：What if the graph is cyclic?
Answer：维护一个新的容器，存储被访问过的节点，避免节点被再次添加到容器中重复访问；
图的遍历
- 广度优先搜索（Breadth First Search, BFS）
- 先进先出，使用 queue；
- Strategy: 优先移除和扩张容器中最浅的节点；
- 深度优先搜索（Depth First Search, DFS）
- 先进后出，使用 stack；
- Strategy: 优先移除和扩张容器中最深的节点；
Dijkstra 算法
- Strategy: expand/visit the node with cheapest accumulated cost \(g(n)\) ;
- \(g(n)\)：The current best estimates of the accumulated cost from the start state to node \(n\) ;
- Update the accumulated costs \(g(m)\) for all unexpanded neighbors \(m\) of node \(n\) ;
- A node that has been expanded/visited is guaranteed to have the smallest cost from the start state.
算法流程：
- 初始时，S 集只包含起点 s，U 集包含除 s 外的其他节点，U 集中的节点 v 与起点 s 相邻，则该节点存储值为距起点 s 的距离，若与起点 s 不相邻，则距离为无限大；
- 从 U 集中选出距离起点最短的节点 k，并将节点 k 加入到 S 集中，同时从 U 集中移除节点 k；
- 更新 U 集中各个节点到起点 s 的距离。之所以更新 U 集中节点的距离，是因为由于上一步确定了节点 k 是求出最短路径的节点，从而可以利用节点 k 来更新其他节点的距离；例如，（s, v）的距离可能大于(s, k) + (k, v)的距离。
- 重复步骤 2 和步骤 3，直到遍历完所有节点。
2.3 A star 算法
算法简介：
- A*算法是一种静态路网中求解最短路径最有效的直接搜索方法。广泛应用于室内机器人的路径搜索、游戏动画路径搜索等；
- A*算法结合了贪心算法（深度优先）和 Dijkstra 算法（广度优先），是一种启发式的搜索算法；
- 路径优劣评价公式为：\(f\left(n\right)=g\left(n\right)+h\left(n\right)\)，\(g\left(n\right)\)是在状态空间中从初始状态到状态 \(n\) 的实际代价，\(h\left(n\right)\)是从状态 \(n\) 到目标状态的最佳路径的估计代价。
- A*算法使用了两个状态表，分别称为OpenList和CloseList，OpenList存储待考察的节点，CloseList存储已考察过的节点。
地图预处理：
- 将地图栅格化，把每一个正方形格子的中央称为节点；
- 确定栅格属性，即每一个格子有两种状态：可走和不可走；
- 定义两个列表集合：OpenList 和 CloseList，OpenList 可以存储节点的 g 值；
- 确定起始节点和目标节点。
A* 算法流程：
- 初始时，定义起始节点为父节点，移入 CloseList 中；
- 逐个检查与父节点 \(i\) 相邻的周围的节点 \(j\)，忽略不可走节点和已经存在于 CloseList 中的节点：
- 若节点 \(j\) 不在 OpenList 中，则将节点 \(j\) 的父节点记为节点 \(i\)，计算节点 \(i\) 到节点 \(j\) 的距离 \(d\)，计算 \(g\left(j\right)=g\left(i\right)+d\)，并把它们加入到 OpenList 中成为待考察的对象；
- 若节点 \(j\) 已经在 OpenList 中，则检查这条路径是否更优，即经由节点 \(i\) 到达节点 \(j\) 是否有更小的 \(g\) 值：计算节点 \(i\) 到节点 \(j\) 的距离 \(d\)，若 \(g\left(i\right)+d\) 小于 OpenList 中节点 \(j\) 存储的 \(g\left(j\right)\)，则这条路径更优，更新 OpenList 中节点 \(j\) 存储的 \(g\) 值 \(g\left(j\right)=g\left(i\right)+d\)，并将节点 \(j\) 的父节点记为节点 \(i\)，若这条路径不是更优，则不进行任何操作；
- 计算 OpenList 中所有节点的 \(f\) 值：\(f\left(n\right)=g\left(n\right)+h\left(n\right)\)，从 OpenList 中取出 \(f\) 值最小的节点，放到 CloseList 中，并把它作为新的父节点 \(i\)；
- 这里启发式函数 \(h\left(n\right)\)一般用曼哈顿距离或者欧氏距离来替代。
- 重复步骤 2 和步骤 3，不断重复，直到搜索到目标节点，完成路径搜索。搜索出路径的结果可以直接遍历父节点得到。
Question 1：欧式距离（L2）作为启发式函数一定是最优的吗？
Amswer：是的
Question 2：曼哈顿（L1）作为启发式函数一定是最优的吗？
Amswer：不一定
Question 3：\(L \infty\) 作为启发式函数一定是最优的吗？
Amswer：是
Question 4：常值 0 作为启发式函数一定是最优的吗？
Amswer：退化成 Dijkstra 算法，保证最优
思考：
- 如果启发式函数 \(h(n) < h^*(n)\) ，即启发式函数估计的距离小于真实距离，则保证路径的最优性。如果设计出的 \(h(n)\ge h^*(n)\)，可以视为更加贪心，牺牲路径的最优性来换取计算时间，这就是 weight A*。
weighted A*：
将 A* 中的 \(f(n)=g(n)+h(n)\) 替换为 \(f(n)=g(n)+\epsilon \cdot h(n), \epsilon > 0\) ，以牺牲路径最优性来换取计算时间上的效率。
若有 \(f(n)=a\cdot g(n)+b\cdot h(n)\)，则：
- \(a=0\)，\(b=1\) 时为最贪心的路径；
- \(a=1\)，\(b=\epsilon>1\) 时为偏向贪心的路径；
- \(a=1\)，\(b=1\) 时为最优路径；
- \(a=1\)，\(b=0\) 时退化为 Dijkstra 算法；
Tie Breaker：
在寻找路径时，有时会出现多个节点的 \(f(n)\) 相同的情况，这可能会在搜索的过程中导致 expand 很多节点，导致搜索效率低下。
解决：
- 方案一：当几个节点拥有相同的 \(f(n)\) 值时，对 \(h(n)\) 进行比较；
- 方案二：我们倾向于对角线抵达终点，在计算 \(h(n)\) 时进行如下修正：\[\begin{align} dx1 &= abs(node.x-goal.x) \\ dy1 &=abs(node.y-goal.y) \\ dx2 &= abs(start.x-goal.x) \\ dy2 &= abs(start.y - goal.y) \\ cross &= abs(dx1\times dy2-dx2\times dy1) \\ h &= h+cross\times0.001 \end{align} \]
Jump Point Search 算法
与 A* 的区别：
- A* 在扩张时考虑自然的邻居节点，而 JPS 考虑的是跳跃的邻居点；
- JPS 可以在空旷的区域进行大范围的跳跃；
搜索流程：
Question：JPS 一定比 A* 好吗？
Answer：不是，如果在障碍物多的场景，JPS 具有较好的效果，但是如果在障碍物较少的场景，JPS 需要扩展大量的节点；
总结：
- 在大多数情况下，尤其是复杂环境，JPS 的效果比 A*好，因为 JPS 减少了加入到 Open List 中节点的数量，减少了内存消耗，和 Open List 中排序的次数等，但是 JPS 增加了对地图中节点状态（有无障碍物）查询的次数；
- JPS 只适用于 uniform grid map；