1.基于搜索的路径规划：BFS、DFS、Dijkstra、A*、JPS

1.概览

　　可以对比不同算法的小动画 PathFinding.js (qiao.github.io)

• 工作空间规划
  • 机器人有不同的形状和大小
  • 碰撞检测需要了解机器人的几何形状，耗时且难度大
• 我们希望将机器人表示为点，只需要把工作空间转换为配置空间C-obstacle，将原始的空间膨胀，这是一次性的
  • C-space R3空间
  • C-obstacle 膨胀后的障碍空障碍
  • C-space = C-obstacle ∪ C-free

• 图搜索算法的逻辑总是在这个框架下
  • 维护一个容器，该容器记录所有准备去访问的节点，同时一个有一个容器记录了所有已经访问的节点，避免形成环
  • 这个容器刚开始是空的，我们初始化放入起始节点
  • Loop
    • 访问一个节点：在容器里根据一种指标弹出一个节点
    • 扩展：发现该节点所有可能的邻居节点
    • 插入：将这些邻居节点插入到容器中
  • End Loop
  • 容器已经全空就达到了终止条件

2.BFS和DFS

广度优先搜索和深度优先搜索

• 深度优先算法：总是移除容器中最深层的节点（总是把尝试一直把一条路走到底）
  • 首先S进入栈
  • S出栈，发现S的周围元素 d e p，S的周围元素 d e p 进入栈
  • d出栈，发现d的周围元素 b c e，d的周围元素 b c e 进入栈

• 广度优先算法：总是移除容器中最浅层的节点（一层一层的搜索）
  • S 进入队列
  • S 弹出队列，发现S的周围元素 d e p，S的周围元素 d e p 进入队列
  • p 弹出队列，发现p的周围元素 q，p的周围元素 q 进入队列

• 两种算法DFS总是一条路走到黑，所以它找到的路径不是最短路径

3.贪心算法

• DFS或者BFS是通过先入或后入来判定从容器中拿出哪个节点
• 贪心算法是一种启发式搜索，通过认为定义的”最短“判定从容器中弹出哪个节点（不一定有全局最优解）

4.Dijkstra算法

DFS和BFS都没有考虑边的代价问题，Dijkstra算法如果不考虑边的代价实际上就退化成广度优先算法

• g(n)：从起始节点到当前节点n累计的最佳成本
• 更新节点n相邻节点的代价（有可能当前节点n使得它周围的节点m代价降低）
• 保证已扩展/访问的节点开销最小

以下图为例跑一遍流程

• S弹出队列，发现S周围节点 d e p，S的周围节点根据代价排序进入优先级队列 p d e
• p弹出队列，发现p周围节点 q，q的周围节点 q 进入优先级队列
• e弹出队列，发现e周围节点 h r，e的周围节点根据代价排序进入优先级队列 r h

算法伪代码逻辑

• 维护priority queue存储所有已访问的节点
• 将初始节点X_s加入到队列
• 标记 g(X_s)=0，其余的节点代价为无穷大
• Loop
  • if the queue is empty, return FALSE; break;
  • Remove the node "n" with the lowest g(n) from the priority queue
  • mark node "n" as expanded
  • if the node "n" is the goal state, return TRUE; break;
  • For all unexpanded neighbors "m" of node "n"
    • if g(m) = infinite
      • if g(m) = g(n) + COST_nm
      • push node "m" into queue
    • if g(m) > g(n) + COST_nm
      • g(m) = g(n) + COST_nm
• End Loop

5.A*算法

• 对Dijkstra算法进行优化，加上贪心算法的思想就可以得到A*算法
  • h(n)：从当前节点n到目标状态的估计最小代价(即目标代价)
  • g(n)：从起始节点到当前节点n累计的最佳成本
  • Weighted A*：f(n)：g(n)+εh(n), ε>1：经过节点n，从起始节点到目标节点估计的最小代价
  • 如果代价估计大于实际代价，最终找到的路径也不一定是最优路径。ε越大越接近于贪心算法，ε=1就是A*算法，ε=0就是Dijkstra算法
• 算法逻辑与Dijkstra完全相同，但排序将g(n)修改为f(n)
• 以下图为例解释，每个节点的估计代价h已经给出
  • 弹出S节点，发现S周围节点a，将a加入优先队列
  • 弹出a节点，发现a周围节点b d e，通过计算当前代价和估计代价得到应该按照 d e b 加入优先队列

6.A*算法的优化

6.1 The Best Heuristic

　　为了得到最优解，我们必须满足估计最小代价和实际最小代价的关系为h(n)≤h*(n)，但是如果h(n)远远小于真实最小代价的话，就会导致右图所示的情况，进行很多无效的搜索，因为更小的h(n)使得算法误以为会有一条更有的路径

　　但实际上这种规格化的网格只能水平走或斜45度走。所以这里不使用欧氏距离，而是使用更标准的算法会更快速的找到目标

6.2 Tie Breaker

　　因为路径中大量对称的点都会有相同的F(n)值，这些值都需要不断的去遍历进行搜索，这会导致它搜索很多无效的点

　　核心思想：想办法打破这种平衡，比如计算一个常数p让每个点的F(n)值不太相同

　　具体做法为F(n) = g(n) + h(n)*(1+p) ，不过这种做法也可能使得h(n)>h*(n)，使得我们估计最小代价大于实际最小代价，无法找到最优解

　　也可以如果有相同的f(n)，则我们选取更小的h(n)值弹出容器

　　或者是我们添加一种倾向性，让他倾向于去行走一条直线，在无障碍的情况下是很好的，但是有障碍的情况下缺不太符合人的直观体会

7. Jump Point Search

　　先看一下JPS算法和A*算法搜索过程的对比

　　JPS算法思想1：Look Ahead Rule

• 劣质邻居（inferior neighbors）灰色节点：到达这些节点时，没有x节点的路径更优
  • 父节点为4时，如果此时由x到1，它的消耗是4->x->1，不如x->1
  • 父节点为4时，如果此时由x到2，它的消耗是4->x->2，不如4->2
  • 父节点为4时，如果此时由x到3，它的消耗是4->x->3，和4->2->3相同，经过x的路径没有更优
• 自然邻居（natural neighbors）白色节点：我们只需要考虑通过白色节点
• 强制邻居（Forced Neighbors）黑色节点：墙

　　JSP算法思想2：Jumping Rules

• 直线规则：如果周围没有障碍物，就是Look Ahead Rule情况1，它只需要继续走直线
  • 当他走到y节点时，是Look Ahead Rule情况3
• 对角线规则：如果周围没有障碍物，就是Look Ahead Rule情况2，它可以选择三条路线，优先选择水平，然后选择垂直，最后选择对角线运动。