2 基于搜索的路径发现

2.1 图搜索理论

2.1.1 配置空间

1. 配置空间 Configuration Space

   • 机器人配置：机器人所有点位的坐标参数
   • 机器人自由度：表示机器人构型所需的实值坐标最小 $n$
   • 机器人配置空间：一个包含所有可能机器人构型的 $n$ 维空间，记为 C-Spcae
   • 每一个机器人姿态 pose 都是 C-space 中的一个点

2. 在配置空间中进行规划

   • 在 C-space 中，机器人被表示为一个点，如 position（$R^3$ 中的一个点），pose（$SO(3)$ 中的一个点）

   • 障碍物也需要在配置空间中表示（这是在运动规划之前的一次性工作），称为配置空间障碍，或 C-obstacle

   • C-space = C-obstacle $\cup$ C-free

   • 路径规划就是在 C-free 中找到起始点 $q_{start}$ 到目标点 $q_{goal}$ 的一条路径

   • 将障碍按机器人的体积膨胀，在 C-space 就只需要按质点考虑了
     

2.1.2 图搜索理论

1. 图有节点和边：无向图、权重、有向图
   

2. 状态空间图 State space graph：搜索算法 search algorithm 的数学表达

   • 图中节点之间的连接性 connectivity 被表示为（有向或无向的）边

   • 基于网格的方法可以直接看到图的节点与边
     

   • 基于采样的方法障碍物被表示为黑色椭圆，需要使用算法如RPM生成图
     

3. 搜索树 Search tree：搜索总是从起始状态 $X_S$ 开始

   • 在图中搜索产生一个搜索树
     

   • 在搜索树中回溯 back-trace 一个节点给出一条从起始点到该节点的路径

   • 对大多数问题，无法实际构造完整的树，我们只希望尽快搜索到目标节点

4. 图搜索方法的基本框架

   • 维护一个存储了所有访问节点的容器 container
   • 该容器从起始状态 $X_S$ 被初始化
   • 循环
     • 根据预定义的目标函数 score function 移除 remove 一个节点（访问节点）
     • 展开 expansion：获得该节点的所有邻居（发现其所有邻居）
     • 压入 push：将这些邻居压入容器
   • 循环结束

2.1.3 图遍历（DFS和BFS）

1. 广度优先搜索和深度优先搜索：Breadth First Search (BFS) 和 Depth First Searcg (DFS)
   

2. 深度优先搜索 DFS

   • 策略：移除 / 展开容器中最深的节点

   • 实践：维护一个后进先出 LIFO 的容器，如栈
     

3. 广度优先搜索 BFS

   • 策略：移除 / 展开容器中最浅的节点

   • 实践：维护一个先入先出 FIFO 的容器，如队列
     

4. BFS 能搜索到最短路径，DFS 不一定
   

2.1.4 启发式算法

1. 贪心算法 Greedy Best First Search

   • 启发：贪心算法根据某些规则来选取“最优”节点，这些节点被称为启发

   • 定义：启发是对距离目标有多近的一个猜测 guess

     • 启发指引一个正确的方向
     • 启发应当易于计算

   • 使用欧式距离或曼哈顿距离的例子

     

2. 贪心算法的效果距离

   • 在没有障碍的图中，贪心算法用更少的时间找到了和BFS一样的最短路径（耗时可由探索过的节点数量看出）
     

   • 在有障碍的图中，贪心算法耗时短，但路径不优
     

2.2 Dijkstra and A*

2.2.1 动作的代价

1. 实际的搜索问题从一个节点到其邻居有代价 cost C，如长度、时间、能量等
2. 当所有权重都是 1 时，BFS 是最优解。
3. 对于一般的例子，怎么尽快找到代价最优路径 least-cost path 呢？

2.2.2 Dijkstra 算法

1. 策略：扩展 / 访问具有最小累计代价 cheapest accumulated cost g(n) 的节点
   

   • g(n)：从起始点到节点 n 的累计代价的当前最优估计
   • 对节点 n 的所有未展开邻居 m 更新累计代价 g(m)
   • 一个已经被扩展/访问过的节点被保证具有从起始状态的最小代价

2. 算法伪代码：维护一个优先级队列 priority queue
   

3. 流程描述：在 dep 被压入优先级队列时，会自动排序，p 具有最小的代价，在下一次迭代时优先弹出 p
   

4. 优缺点

   • 优点：完备且最优
   • 缺点：只能看到当前的累计代价，因此探索下一个状态时会从所有方向探索；不知道目标位置的信息

2.2.3 A* 算法（具有启发的Dijkstra）

1. 启发式算法

   • 回顾基于启发的贪心算法

   • 通过推断目标最小成本（即目标成本）来克服统一成本搜索的缺陷

   • 针对特定搜索问题设计

   • 例如：使用欧氏距离或曼哈顿距离
     

2. A 算法关键词*

   • 累计代价 Accumulated cost
     • g(n)：从起始状态到节点 n 的累计代价的当前最优估计
   • 启发 Heuristic
     • h(n)：从节点 n 到目标状态的估计最小代价，如目标代价
   • 从起点到终点途径当前节点 n 的最优估计代价为：f(n)=g(n)+h(n)
   • 策略：通过最小的 f(n)=g(n)+h(n) 展开一个节点
     • 对节点 n 的所有未展开节点 m 更新累计代价 g(n)
     • 一个已经展开的节点被保证有最小的从起点出发的代价

3. A 算法伪代码*：与 Dijkstra 只有目标函数不同
   

4. A 算法示例*
   

5. A 的最优性问题*

   • 在这个例子中，由 S 经过 A 的代价是 f(n)=1+6=7，而直接去G的代价是 f(n)=5+0=5，因此 A* 会选择直接去 G，而实际代价分别是4和5
     

   • 问题：对于节点 A，实际去目标的最小代价（目标代价） < 估计最小代价（启发代价）

   • A 的可采纳性 Admissibility*：A* 只有在所有节点的启发函数 h(n) ≤ 实际最小代价时，才能保证找到最优解。

6. 可采纳的启发式函数 Admissible Heuristics

   • 一个启发函数是可接受的，如果
     $$h(n)\le h^{\ast }(n),\forall n$$
     其中，$h(n)$ 是节点 n 到目标的真实最小代价

   • 如果启发函数是可接受的，那么 A* 搜索是最优的

   • 在实际应用中，使用 A* 算法的核心工作就是设计出合适的启发式算法。

   • 可接受的启发式函数必须根据具体情况设计

     • 欧氏距离（L2 norm）：总是可接受的
     • 曼哈顿距离（L1 norm）：视具体情况而定
     • L∞ norm 距离：总是可接受的
     • 0 距离：总是可接受的（退化成 Dijkstra）

7. Dijkstra 与 A 对比*
   

8. 次优解决方案

   • 有意使用过高估计的启发：weighted A*
     

     $$f=g+\epsilon h,\epsilon >1$$
     系数越大，效果越接近 A*
     

   • 其它方法
     

   • 效果对比
     

2.2.4 工程细节

1. 使用栅格地图 grid

   • 考虑连通性（平面 4/8联通，立体 6/26联通）
     

   • 栅格地图的实施细节

     • 创建稠密图 dense graph
     • 将网格图中存储的占用状态进行关联
     • 通过网格索引发现邻居
     • 实施 A* 搜索

   • C++ 中的优先队列结构

     • std::priority_queue
     • std::make_heap
     • std::multimap

2. 最好的启发函数

   • 回顾
     

   • 这些很有用，但都不是最优选择

     • 它们不是紧的 tight
     • 紧的意味着它们能测量真实最短距离

   • 如欧式距离会扩展许多无用节点，因为欧式距离和真实理论距离相差很远
     

   • 真实理论最佳方法：对角启发 diagonal heuristic

     • 当栅格地图是高度结构化的
       

     • 真实距离有闭环解 closed-form solution
       $$\begin{gathered} \mathrm{d}x=abs(node.x-goal.x) \\ \mathrm{d}y=abs(node.y-goal.y) \\ h=(\mathrm{d}x+\mathrm{d}y)+(\sqrt{2}-2)\ast min(\mathrm{d}x,\mathrm{d}y) \end{gathered}$$

     • 对角启发的效果
       

3. 平局打破器 Tie Breaker

   • 许多路径有相同的 f 值，使得 A* 算法等价的探索它们

   • 操纵 f 值打破这个平局，让它们不一样来减少探索

   • 比如增加其它系数让 h 不一样
     $$\begin{gathered} h=h\times (1.0+p) \\ p<\frac{\text{minimum cost of one step}}{\text{expected maximum path cost}} \end{gathered}$$
     

   • 平局打破器的核心思想：为相同代价的路径增加倾向性

   • 可用的方法

     • 当 f 相同时，比较 h

     • 在启发式算法或边成本中加入确定性随机数（坐标哈希值）

     • 优先选择从起点到终点的直线路径
       $$\begin{gathered} \text{dx1 = abs(node.x - goal.x)} \\ \text{dy1 = abs(node.y - goal.y)} \\ \text{dx2 = abs(start.x - goal.x)} \\ \text{dx2 = abs(start.y - goal.y)} \\ \text{cross = abs(dx1×dy2 - dx2×dy1)} \\ \text{h = h + cross×0.001} \end{gathered}$$
       

2.3 跳点搜索 Jump Point Search

2.3.1 算法流程

1. 前瞻规则 Look Ahead Rule

   • 邻居剪枝 Neighbor Pruning
     

     • 自然邻居 Natural neighbors：白色节点，可以直接走到，且不绕路
     • 劣等邻居 Inferior neighbors：灰色节点，走过去反而更贵，因为路径可以绕开当前节点 x
     • 结论：在扩展搜索中，剪掉劣等邻居，只考虑自然邻居

   • 强制邻居 Forced Neighbors
     

     • 当 x 的某个邻居被障碍物组当时（红色），原本可以直走的路必须绕行
     • 强制邻居：那些由于障碍物导致无法继续沿原方向前进的邻居
     • 即使路径看起来是“直线”，但如果遇到障碍，就必须重新规划。
     • JPS 会在这些“转折点”处插入新节点进行搜索，而不是盲目跳过。

2. 跳跃规则 Jumping Rules

   • 直线跳跃 Jumping Straight
     

     • 递归地应用 straight pruning rule（直线剪枝规则），并识别 y 为节点 x 的一个 jump point successor（跳点后继）。
     • 这个节点很有趣，因为它有一个邻居 z，该邻居只能通过一条访问 x 再到 y 的路径才能被最优到达。

   • 对角跳跃 Jumping Diagonally
     

     • 递归地应用 diagonal pruning rule（对角线剪枝规则），并识别 y 为 x 的一个 jump point successor。
     • 在每次对角线移动前，我们首先进行一次“直线递归”。只有当两个方向的直线递归都失败（找不到跳点）时，才再次尝试对角线移动（对角一格一格移动）。
     • 节点 w 是 x 的一个 forced neighbor（强制邻居），会像普通节点一样被正常展开（也推入优先队列）。

3. JPS 算法伪代码
   

   • 和 A* 算法几乎一致
   • A* 算法将所有几何邻居加入 open list
   • JPS 算法只将跳跃邻居加入 open list

4. 示例

   • 绿色节点为起始点，红色节点为终点，黑色节点为障碍
     

   • 先沿 X 方向扩展 -> 遇到障碍返回，沿 Y 方向扩张 -> 遇到障碍返回，沿对角线移动一格；沿 X 方向移动 … 在黄色节点时，向 X 方向扩展两格遇到带强制邻居的节点，于是将黄色节点（关键节点）加入 open list，将绿色节点从 open list 弹出
     

   • 沿 X 方向扩展，遇到带强制邻居的紫色节点，将紫色节点加入 open list
     

   • 沿 Y 方向扩展 -> 遇到障碍返回，沿对角线移动一格；沿 X 方向扩展 … 当前黄色节点搜索完成，从 open list 弹出
     

   • 检查 open list，从新的黄色节点开始，沿 X 方向扩展 -> 遇到障碍返回，不用沿 Y 方向扩展，因为 Y 方向是劣等邻居
     

   • 沿对角线移动一格，沿 X 方向扩展 … 再沿 Y 方向扩展 …
     

   • 沿对角线移动一格，沿 X 方向扩展 -> 遇到障碍返回，再沿 Y 方向扩展，发现带终点红色节点的紫色节点（对终点的兴趣等同于障碍），将紫色节点加入 open list，当前节点没有自然邻居剩余所以结束当前搜索，将黄色节点从 open list 弹出
     

   • 检查 open list，从新的黄色节点开始，沿 X 轴方向扩展 … 最终发现终点
     

   • 最终路径
     

2.3.2 扩展

1. 可视化网站：

   • https://zerowidth.com/2013/a-visual-explanation-of-jump-point-search.html
   • http://qiao.github.io/PathFinding.js/visual/

2. 3D JPS：https://github.com/KumarRobotics/jps3d

3. JPS 一定更优吗？
   

   • 大多数情况下，尤其是在复杂环境中，JPS 表现更优，但远非“总是”如此。为什么？
   • JPS 减少了 Open List 中的节点数量，但增加了状态查询（如地图访问）的次数。
   • JPS 的局限性：仅适用于均匀网格地图（即所有可通行格子的移动代价相同）。