Search Based Path Planning
Search Based Pathing Planning
参考：https://www.bilibili.com/video/BV1yT4y1T7Eb
Dijkstra
流程
- 从起点开始扩展，每一步为一个节点找到最短路径
- cost：f(n) = g(n)
- g(n)：当前路径，从起点到n的代价（距离）
- While true：
- 从open list选择代价最小的节点，收录进closed list
- 收录后遍历该节点的所有可访问邻节点，更新cost
- 最优性证明：反证法
特性
完备
最优
算法
开始
将起点放入 open list
While 1
if open list 为空
无可行路径，搜索失败
取出 open list 中 cost=g(n) 最小的节点为 current
将 current 放入 closed list
if current 为终点
找到路径，结束
遍历 current 的邻节点 neighbour
if neighbour为障碍物，跳过
if neighbour 已被 closed list 收录，跳过
if neighbour 在 open list 中
更新 neighbour g(n)
else
计算 neighbour g(n)，加入 open list
结束
效果
A*
motivation
减少收录的栅格数目，增加搜索速度
在Dijkstra上的改进
-
cost：f(n) = g(n) + h(n)
-
g(n)：当前路径，从起点到n的代价（距离）
-
h(n)：启发式函数，一般取当前点到终点的距离
-
While true：
- 从open list选择代价最小的节点，收录进closed list
- 收录后遍历该节点的所有可访问邻节点，更新cost
-
最优性保证：h(n) ≤ \le ≤ *h(n)
特性
完备
最优
算法
开始
将起点放入 open list
While 1
if open list 为空
无可行路径，搜索失败
取出 open list 中 cost=g(n)+h(n) 最小的节点为 current
将 current 放入 closed list
if current 为终点
找到路径，结束
遍历 current 的邻节点 neighbour
if neighbour为障碍物，跳过
if neighbour 已被 closed list 收录，跳过
if neighbour 在 open list 中
更新 neighbour g(n)
else
计算 neighbour g(n)，加入 open list
结束
效果
Jump Point Search
有众多的算法能够在代价一致的均匀网格中（uniform-cost grid maps）搜索最短线性路径，常见的有A*，Dijkstra’s等。而JPS算法相比其他算法，省略了众多点插入和点删除的操作，有选择性地扩展必要的节点。因此有着很高的搜索效率。
原论文：Online Graph Pruning for Pathfinding on Grid Maps by Daniel Harabor and Alban Grastien
符号和术语
- JPS算法工作于不定向的均匀网格地图中。
- 每一次直线运动的代价为1，对角运动的代价为 2 \sqrt{2} 2。
- 涉及不可穿越节点（障碍物）的运动是禁止的。
- y = x + k d ⃗ y=x+k\vec{d} y=x+kd 代表从 x x x 沿 d ⃗ \vec{d} d 方向运动 k k k 个单位可到达 y y y，当 d ⃗ \vec{d} d 为对角方向时，可分解为 d 1 ⃗ \vec{d_1} d1 和 d 2 ⃗ \vec{d_2} d2 。
- π = < n 0 , n 1 , . . . , n k > \pi=
- l e n ( π ) len(\pi) len(π) 代表路径长度， d i s t ( n 0 , n k ) dist(n_0,n_k) dist(n0,nk) 代表两节点间的距离。
Jump Point
Neighbour Pruning Rules
考虑从父节点 p ( x ) p(x) p(x) 到节点 x x x 的两种运动：直线运动与对角运动。
直线运动：我们修剪所有满足以下约束的 n ∈ n e i g h b o u r ( x ) n\in neighbour(x) n∈neighbour(x) ：
l e n ( ⟨ p ( x ) , . . . , n ⟩ ∖ x ) ≤ l e n ( ⟨ p ( X ) , x , n ⟩ ) len(\left\langle p(x),...,n\right\rangle \setminus x)\le len(\left\langle p(X),x,n\right\rangle) len(⟨p(x),...,n⟩∖x)≤len(⟨p(X),x,n⟩)
对角运动：与直线运动相似，但不包含 x x x 的路径必须严格主导，即：
l e n ( ⟨ p ( x ) , . . . , n ⟩ ∖ x ) < l e n ( ⟨ p ( X ) , x , n ⟩ ) len(\left\langle p(x),...,n\right\rangle \setminus x)< len(\left\langle p(X),x,n\right\rangle) len(⟨p(x),...,n⟩∖x)<len(⟨p(X),x,n⟩)
假设 n e i g h b o u r ( x ) neighbour(x) neighbour(x) 不包含任何障碍物，则修剪过后留下的节点称为natural neighbour。当 n e i g h b o u r ( x ) neighbour(x) neighbour(x) 中存在障碍物时，则我们可能无法修剪所有的non-natural neighbour，这些因为障碍物而无法修剪的neighbour称为forced neighbour。
Definition 1 如果节点 n ∈ n e i g h b o u r ( x ) n\in neighbour(x) n∈neighbour(x) 满足以下条件：
-
n n n 不是 x x x 的natural neighbour
-
l e n ( ⟨ p ( x ) , x , n ⟩ ) < l e n ( ⟨ p ( x ) , . . . , n ⟩ ∖ x ) len(\left\langle p(x),x,n\right\rangle)
则节点 n n n 是节点 x x x 的forced neighbour。
Definition 2 如果 y y y 最小化了 k k k，使 y = x + k d ⃗ y=x+k\vec{d} y=x+kd，且下列条件之一成立：
-
节点 y y y 是终点
-
节点 y y y 至少存在一个 forced neighbour
-
d ⃗ \vec{d} d 是对角运动，且存在节点 z = y + k i d i ⃗ z=y+k_i\vec{d_i} z=y+kidi， z z z 根据前两个条件被判定为是 y y y 的跳点
则节点 y y y 是节点 x x x 在 d ⃗ \vec{d} d 方向的跳点。
Algorithmic Description
3D扩展
原论文：Planning Dynamically Feasible Trajectories for Quadrotors Using Safe Flight Corridors in 3-D Complex Environments by Sikang Liu, Michael Watterson et.l
效果
