寻路问题建模

• 定义：建模指的是为地图寻找一个空间表征（spatial representation），即把原始地图转换成计算机数据结构表示的模型，用于后面的寻路。

• 三种建模方式的图形化表示：
  

• (a) 原版地图

• (b) 格子（3D情况下可以对应为体素寻路）

• (c) 路点

• (d) 导航网格

• 三者对比

属性	导航网格	格子	路点
实现复杂度	高	中	低
内存和计算开销	中	高	低
表达精确性	高	中	低
优点	精确表征世界，内存和计算开销适中	容易实现，动态修改简单	实现简单，内存和计算开销低
缺点	复杂，难以实现和修改，建网格耗时耗内存	内存和计算开销大，可能需要额外平滑，不太适合3D地图	需要人工参与，灵活性低，不考虑实际底层地图
适用场景	地图大，精确度要求高	地图中等，不太大，内存充足	路径固定，不考虑真实地形影响
# 基础寻路算法

深度和广度优先

DFS

• 基本思想：DFS从起始节点开始，沿着每条分支尽可能深入地搜索，直到遇到目标节点或无可深入的节点，然后回溯并探索其他分支。

• 伪代码

      DFS(node, goal):
          if node is goal:
              return path to node
          mark node as visited
          for each neighbor in neighbors(node):
              if neighbor is not visited:
                  path = DFS(neighbor, goal)
                  if path is not None:
                      return path
          return None
  

BFS

• 基本思想：BFS从起始节点开始，逐层向外扩展，先访问当前层的所有节点，然后再访问下一层的所有节点，直到找到目标节点。

• 伪代码

      BFS(start, goal):
          create a queue Q
          enqueue start onto Q
          mark start as visited
          while Q is not empty:
              node = dequeue Q
              if node is goal:
                  return path to node
              for each neighbor in neighbors(node):
                  if neighbor is not visited:
                      mark neighbor as visited
                      enqueue neighbor onto Q
          return None
  

Dijkstra算法

• 基本思想：Dijkstra算法用于在加权图中找到从起始节点到目标节点的最短路径。算法维护一个优先队列，存储各节点的当前最短路径估计值，不断更新这些值直到找到最短路径。

• 伪代码

      Dijkstra(graph, start, goal):
          create a priority queue Q
          for each node in graph:
              dist[node] = infinity
              previous[node] = undefined
          dist[start] = 0
          Q.insert(start, dist[start])
          while Q is not empty:
              node = Q.extract_min()
              if node is goal:
                  return reconstruct_path(previous, goal)
              for each neighbor in neighbors(node):
                  alt = dist[node] + cost(node, neighbor)
                  if alt < dist[neighbor]:
                      dist[neighbor] = alt
                      previous[neighbor] = node
                      Q.decrease_key(neighbor, alt)
          return None
      reconstruct_path(previous, goal):
          path = []
          while goal is not undefined:
              path.prepend(goal)
              goal = previous[goal]
          return path
  

Floyd算法（Floyd-Warshall算法）

• 基本思想：Floyd-Warshall算法是一种动态规划算法，用于计算加权图中所有节点对之间的最短路径。它通过不断更新路径矩阵，使得每次更新考虑更多的中间节点。

• 伪代码

      FloydWarshall(graph):
          n = number of nodes in graph
          dist = array[n][n]
          for i from 1 to n:
              for j from 1 to n:
                  if i == j:
                      dist[i][j] = 0
                  else if (i, j) in graph:
                      dist[i][j] = weight of edge (i, j)
                  else:
                      dist[i][j] = infinity
          for k from 1 to n:
              for i from 1 to n:
                  for j from 1 to n:
                      if dist[i][j] > dist[i][k] + dist[k][j]:
                          dist[i][j] = dist[i][k] + dist[k][j]
          return dist
  

Bellman-Ford算法

• 基本思想：Bellman-Ford算法用于计算单源最短路径，能够处理带负权边的图。它通过反复松弛边缘，更新最短路径估计值。如果在V-1次松弛之后还能松弛，说明存在负权环。

• 伪代码

      BellmanFord(graph, source):
          n = number of nodes in graph
          dist = array[n]
          parent = array[n]
          # Step 1: Initialize distances from source to all other nodes as infinity
          for i from 1 to n:
              dist[i] = infinity
              parent[i] = undefined
          dist[source] = 0
          # Step 2: Relax all edges |V| - 1 times
          for i from 1 to n-1:
              for each edge (u, v) in graph:
                  if dist[u] + weight(u, v) < dist[v]:
                      dist[v] = dist[u] + weight(u, v)
                      parent[v] = u
          # Step 3: Check for negative-weight cycles
          for each edge (u, v) in graph:
              if dist[u] + weight(u, v) < dist[v]:
                  print("Graph contains a negative-weight cycle")
                  return None
          return dist, parent
      reconstruct_path(parent, target):
          path = []
          while target is not undefined:
              path.append(target)
              target = parent[target]
          path.reverse()
          return path
  

SPFA算法（Shortest Path Faster Algorithm）

• 基本思想：SPFA算法是对Bellman-Ford算法的一种优化，通过使用队列来选择下一个要处理的节点，从而减少不必要的重复计算，提高效率。它适用于带有负权边但无负权环的图。

• 伪代码

      SPFA(graph, source):
          n = number of nodes in graph
          dist = array[n]
          inQueue = array[n]
          for i from 1 to n:
              dist[i] = infinity
              inQueue[i] = false
          dist[source] = 0
          queue = [source]
          inQueue[source] = true
          while queue is not empty:
              u = queue.pop(0)
              inQueue[u] = false
              for each neighbor v of u:
                  if dist[u] + weight(u, v) < dist[v]:
                      dist[v] = dist[u] + weight(u, v)
                      if not inQueue[v]:
                          queue.append(v)
                          inQueue[v] = true
          return dist
  

贪婪最佳优先搜索

• 基本思想：贪婪最佳优先搜索基于启发式函数进行搜索，每次选择距离目标最近的节点扩展。该算法并不总能找到最短路径，但通常能较快找到一个解决方案。

• 伪代码

      GreedyBestFirstSearch(start, goal, heuristic):
          create a priority queue Q
          Q.insert(start, heuristic(start, goal))
          mark start as visited
          while Q is not empty:
              node = Q.extract_min()
              if node is goal:
                  return path to node
              for each neighbor in neighbors(node):
                  if neighbor is not visited:
                      mark neighbor as visited
                      Q.insert(neighbor, heuristic(neighbor, goal))
          return None
  

总结

算法	时间复杂度	空间复杂度	优点	缺点	适用场景
深度优先搜索 (DFS)	O(V + E)	O(V)	实现简单，内存使用低	可能会陷入死循环，不保证找到最短路径	小规模图，解决迷宫问题，图遍历
广度优先搜索 (BFS)	O(V + E)	O(V)	能找到无权图中的最短路径	内存使用较高	短路径搜索，图遍历，树的层次遍历
Dijkstra算法	O(V^2) 或 O(E + V log V)（使用优先队列）	O(V^2) 或 O(V)（使用优先队列）	适用于无负权边的图，可以找到最短路径	无法处理带负权边的图	带权图的最短路径问题，网络路由
Floyd-Warshall算法	O(V^3)	O(V^2)	能解决所有节点对的最短路径问题，可处理负权边	时间和空间复杂度高	稠密图，解决所有节点对的最短路径问题
Bellman-Ford算法	O(V * E)	O(V)	能处理带负权边的图，检测负权环	时间复杂度较高，效率低于Dijkstra	单源最短路径，带负权边的图
SPFA算法	平均O(V)~O(E)	O(V)	优化了Bellman-Ford算法，效率高于Bellman-Ford	可能会进入死循环，需要检测负权环	单源最短路径，带负权边的图
贪婪最佳优先搜索	O(E)	O(V)	通常能较快找到解决方案	不保证最短路径	路径规划，启发式搜索

A*算法及其改进算法

A*

• 基本思想：A*算法结合了Dijkstra算法和贪婪最佳优先搜索，使用启发式函数（通常是目标节点的估计距离）来指导搜索过程。它同时考虑到当前节点到起始节点的实际距离（g(n)）和从当前节点到目标节点的估计距离（h(n)），总优先级为f(n) = g(n) + h(n)。

• 伪代码

      AStar(graph, start, goal, heuristic):
          create an open set (priority queue) Q
          create a closed set
          g_score = {node: infinity for node in graph}
          f_score = {node: infinity for node in graph}
          g_score[start] = 0
          f_score[start] = heuristic(start, goal)
          Q.insert(start, f_score[start])
          while Q is not empty:
              current = Q.extract_min()
              if current is goal:
                  return reconstruct_path(came_from, current)
              closed_set.add(current)
              for neighbor in neighbors(current):
                  if neighbor in closed_set:
                      continue
                  tentative_g_score = g_score[current] + cost(current, neighbor)
                  if tentative_g_score < g_score[neighbor]:
                      came_from[neighbor] = current
                      g_score[neighbor] = tentative_g_score
                      f_score[neighbor] = g_score[neighbor] + heuristic(neighbor, goal)
                      if neighbor not in Q:
                          Q.insert(neighbor, f_score[neighbor])
          return None
      reconstruct_path(came_from, current):
          path = []
          while current in came_from:
              path.append(current)
              current = came_from[current]
          path.reverse()
          return path
  

JPS和JPS+算法

JPS算法

• 基本思想：JPS是对A_算法的一种优化，专用于_网格图。它通过跳过某些不必要的节点（跳点）来加速路径搜索。这种方法利用对称性和方向性来减少A需要评估的节点数量，从而提高效率。

- 强迫邻居：节点x的8个邻居中有障碍，且x的父节点p经过x到达n的距离代价比不经过x到达的n的任意路径的距离代价小，则称n是x的强迫邻居，即去n必过x点。

- 跳点

    - 节点x是起点/终点。

    - 节点x至少有一个强迫邻居。

    - 如果父节点在斜方向(意味着这是斜向搜索)，节点x的水平或垂直方向上有满足以上2个条件的点

• 伪代码

      JPS(start, goal, grid):
          openSet = priority queue
          closedSet = set
          gScore = dictionary with default value infinity
          fScore = dictionary with default value infinity
          parent = dictionary
          gScore[start] = 0
          fScore[start] = heuristic(start, goal)
          openSet.insert(start, fScore[start])
          while openSet is not empty:
              current = openSet.extract_min()
              if current == goal:
                  return reconstruct_path(parent, current)
              closedSet.add(current)
              for neighbor in jump_neighbors(current, goal, grid):
                  if neighbor in closedSet:
                      continue
                  tentative_gScore = gScore[current] + distance(current, neighbor)
                  if tentative_gScore < gScore[neighbor]:
                      gScore[neighbor] = tentative_gScore
                      parent[neighbor] = current
                      fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goal)
                      if neighbor not in openSet:
                          openSet.insert(neighbor, fScore[neighbor])
          return None
      jump_neighbors(current, goal, grid):
          neighbors = []
          for direction in possible_directions:
              jumpPoint = jump(current, direction, goal, grid)
              if jumpPoint is not None:
                  neighbors.append(jumpPoint)
          return neighbors
      jump(current, direction, goal, grid):
          next = current + direction
          if not in_bounds(next, grid) or is_obstacle(next, grid):
              return None
          if next == goal:
              return next
          if forced_neighbors(next, direction, grid):
              return next
          if direction is diagonal:
              if jump(next, (direction[0], 0), goal, grid) is not None or \\
                 jump(next, (0, direction[1]), goal, grid) is not None:
                  return next
          return jump(next, direction, goal, grid)
      forced_neighbors(node, direction, grid):
          # Implement detection of forced neighbors
          return True or False
      reconstruct_path(parent, current):
          path = []
          while current in parent:
              path.append(current)
              current = parent[current]
          path.reverse()
          return path
  

• 一些优化方案：

  • JPS·Bit使用运算效率更高的位运算和CPU指令运算来优化原始JPS节点扩展过程。

  • JPS-BitPrune将中间跳点全部删除，将中间跳点后继跳点中的非中间跳点的父跳点改为中间跳点的父跳点，可以有效避免冗余的节点拓展运算。由于删除了中间跳点，因此JPS-BitPrune需要在搜索到完整的路径之后以一定的策路在最后寻得的路径中加入中间拐点，使得每两个相邻的路径节点之间都是垂直、水平、对角线方向可达的。

  • Goal Bounding节点裁减技术：为每个节点的边(Edge)计算一个节点集合，该集合至少包含通过该边可到达最短路的所有节点，在执行路径搜索算法（如A*）时，使用预计算的目标节点集合来减少需要考虑的路径，从而加速搜索过程。

JPS+算法

• 基本思想：JPS+ 是 JPS 的改进版，旨在进一步优化搜索过程。JPS+ 通过预处理地图，将跳点信息预先计算并存储，从而在搜索过程中减少实时计算的开销，进一步提升路径搜索的速度。

• 伪代码

      JPSPlus(start, goal, grid):
          preprocessed_grid = preprocess_grid(grid)
          openSet = priority queue
          closedSet = set
          gScore = dictionary with default value infinity
          fScore = dictionary with default value infinity
          parent = dictionary
          gScore[start] = 0
          fScore[start] = heuristic(start, goal)
          openSet.insert(start, fScore[start])
          while openSet is not empty:
              current = openSet.extract_min()
              if current == goal:
                  return reconstruct_path(parent, current)
              closedSet.add(current)
              for neighbor in jump_neighbors_plus(current, goal, preprocessed_grid):
                  if neighbor in closedSet:
                      continue
                  tentative_gScore = gScore[current] + distance(current, neighbor)
                  if tentative_gScore < gScore[neighbor]:
                      gScore[neighbor] = tentative_gScore
                      parent[neighbor] = current
                      fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goal)
                      if neighbor not in openSet:
                          openSet.insert(neighbor, fScore[neighbor])
          return None
      preprocess_grid(grid):
          # Implement preprocessing to compute jump points
          return preprocessed_grid
      jump_neighbors_plus(current, goal, preprocessed_grid):
          # Use precomputed jump points from preprocessed_grid
          neighbors = preprocessed_grid[current]
          return neighbors
      reconstruct_path(parent, current):
          path = []
          while current in parent:
              path.append(current)
              current = parent[current]
          path.reverse()
          return path
  

B*算法

• 基本思想

  • B算法是一种用于树结构搜索的路径优化算法，主要应用于人工智能和游戏开发中的路径规划。它是对经典的A算法的一种改进，结合了最佳优先搜索和界限估计的概念。B*算法通过在搜索过程中同时考虑路径成本和启发式信息，进一步提高了搜索效率。

  • B_算法在搜索过程中，会计算每个节点的综合评价函数_ *f(n) = g(n) + h(n) + b(n)，其中：

    • g(n) 是从起始节点到当前节点的实际路径成本。

    • h(n) 是当前节点到目标节点的启发式估计成本。

    • b(n) 是一个界限函数，用于动态调整启发式信息。
      B_算法通过动态调整界限函数_ *b(n) 来优化搜索路径，避免走冤枉路，从而提高效率。

• 伪代码

      BStar(graph, start, goal, heuristic):
          openSet = priority queue
          closedSet = set
          gScore = dictionary with default value infinity
          fScore = dictionary with default value infinity
          bScore = dictionary with default value 0
          parent = dictionary
          gScore[start] = 0
          fScore[start] = heuristic(start, goal) + bScore[start]
          openSet.insert(start, fScore[start])
          while openSet is not empty:
              current = openSet.extract_min()
              if current == goal:
                  return reconstruct_path(parent, current)
              closedSet.add(current)
              for neighbor in neighbors(current, graph):
                  if neighbor in closedSet:
                      continue
                  tentative_gScore = gScore[current] + distance(current, neighbor)
                  tentativeFScore = tentative_gScore + heuristic(neighbor, goal) + bScore[neighbor]
                  if tentative_gScore < gScore[neighbor]:
                      gScore[neighbor] = tentative_gScore
                      fScore[neighbor] = tentativeFScore
                      parent[neighbor] = current
                      if neighbor not in openSet:
                          openSet.insert(neighbor, fScore[neighbor])
                      else:
                          openSet.update(neighbor, fScore[neighbor])
              # Update the boundary function b(n)
              update_bScore(bScore, current, graph)
          return None
      def update_bScore(bScore, current, graph):
          # Implement the logic to update bScore based on current node and graph
          pass
      def reconstruct_path(parent, current):
          path = []
          while current in parent:
              path.append(current)
              current = parent[current]
          path.reverse()
          return path
  

Theta*算法

• 基本思想：Theta_算法是对A_*算法的改进，通过允许在路径规划中沿对角线方向移动，而不是仅沿网格边缘行走，从而减少路径的节点数，提高路径规划的效率。

• 伪代码

      ThetaStar(graph, start, goal, heuristic):
          openSet = priority queue
          closedSet = set
          gScore = dictionary with default value infinity
          fScore = dictionary with default value infinity
          parent = dictionary
          gScore[start] = 0
          fScore[start] = heuristic(start, goal)
          openSet.insert(start, fScore[start])
          while openSet is not empty:
              current = openSet.extract_min()
              if current == goal:
                  return reconstruct_path(parent, current)
              closedSet.add(current)
              for neighbor in neighbors(current):
                  if neighbor in closedSet:
                      continue
                  tentative_gScore = gScore[current] + distance(current, neighbor)
                  if has_line_of_sight(parent[current], neighbor):
                      new_gScore = gScore[parent[current]] + distance(parent[current], neighbor)
                      if new_gScore < gScore[neighbor]:
                          gScore[neighbor] = new_gScore
                          parent[neighbor] = parent[current]
                          fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goal)
                          if neighbor not in openSet:
                              openSet.insert(neighbor, fScore[neighbor])
                  else:
                      if tentative_gScore < gScore[neighbor]:
                          gScore[neighbor] = tentative_gScore
                          parent[neighbor] = current
                          fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goal)
                          if neighbor not in openSet:
                              openSet.insert(neighbor, fScore[neighbor])
          return None
      reconstruct_path(parent, current):
          path = []
          while current in parent:
              path.append(current)
              current = parent[current]
          path.reverse()
          return path
      has_line_of_sight(node1, node2):
          # Implement line-of-sight check
          return True or False
  

IDA*算法

• 基本思想：IDA_（Iterative Deepening A_）算法结合了A算法的启发式搜索和迭代加深搜索的深度限制。基本思想是使用A算法的启发式函数进行路径评估，但通过逐步增加深度限制来进行搜索，从而减少内存使用。设定一个深度限制（即阈值），从起点开始进行深度优先搜索。每次搜索到当前阈值时，如果没有找到目标，就增加阈值并重新开始搜索，直到找到目标或证明无解。

总结

算法	时间复杂度	空间复杂度	优点	缺点	适用场景
A*	O(E)	O(V)	启发式搜索，效率高，能找到最优路径	需要合理的启发式函数，可能受启发式函数影响较大	需要快速且最优路径规划的场景
JPS	O(E)	O(V)	大幅减少A*需要评估的节点数量，提高搜索效率	仅适用于网格图，需实现复杂的跳点检测算法	网格图的路径规划，启发式搜索
JPS+	O(E)	O(V) + 预处理时间和空间	基于JPS进一步优化，通过预处理减少实时计算开销，提高速度	需要预处理地图，适用范围较局限	大规模网格图的路径规划，启发式搜索
B*	O(E)	O(V)	通过动态调整界限函数，提高搜索效率；避免冤枉路	需要额外维护和计算界限函数，实现复杂度较高	需要高效路径规划的复杂环境，如游戏AI、机器人导航等
Theta*	O(E)	O(V)	将A*的启发式搜索与线性插值相结合，能找到更短且平滑的路径	实现较为复杂，可能需要更多的计算资源	需要平滑路径且高效的路径规划场景，如机器人导航
IDA*	O(b^d)	O(d)	空间效率高，适合内存受限环境，能找到最优路径	时间效率较低，多次迭代增加计算开销	内存受限但需要最优路径的场景，如嵌入式系统、机器人导航

A*算法优化思路

预先计算好每一条路径

• 预先计算好寻路空间中每条可能的路径(Roy-Floyd-Warshal算法)，并把路径数据保存在一张查找表中，通过“换乘点”进行无损压缩或者通过“枢轴pivot节点”有损压缩。

Hierarchical and Dynamic Pathfinding

Hierarchical Pathfinding（层次化寻路）

• 基本思想
  层次化寻路通过将地图或图划分为多个层次，以减少搜索空间并加快路径搜索过程。其基本思想是：

  • 分层结构：

    • 将地图或图划分为多个不同的层次，每个层次表示不同的抽象级别。例如，最低层可以是具体的网格或节点，较高层可以是较大的区域或簇。

    • 每个层次包含较低层次的抽象表示。例如，在游戏中，最低层次是具体的地图网格，中间层次是区域（如房间或走廊），最高层次可能是整个建筑或区域的抽象表示。

  • 路径搜索过程：

    • 在最高层次进行粗略的路径搜索，确定从起点到终点的大致路径。

    • 在较低层次逐步细化路径，直到找到具体的路径。

    • 通过这种分层次的方式，搜索空间显著减少，搜索效率提高。

    

  • 上图中把整个地图按区域来进行一次划分，而 A* 算法也分为两步：

    • local A_，即在单个区域内部进行 A_ 算法

    • tile A_，即在区域间进行 A_ 算法

  • 区域间存在一个路点(Way Point)，而区域间的邻接关系靠的就是这些路点的邻接关系，所以每一次长路径，都是先从源点走到相邻区域的路点，再从当前路点出发，走到下一个路店，直到到达目标区域的目标点

Dynamic Pathfinding（动态寻路）

• 动态寻路处理动态变化的环境，例如地图中某些部分发生变化（如新增障碍物、道路变化等）。动态寻路技术包括：

  • 动态更新路径：

    • 在路径搜索过程中，根据环境的变化实时更新路径。

    • 当环境发生变化时，不必重新计算整个路径，而是只更新受影响的部分。

  • 增量更新：

    • 通过增量更新技术，只计算必要的变化部分，从而提高效率。例如，当一个障碍物出现时，只重新计算受影响的路径节点，而不是整个路径。

预分配所需的内存空间

• 在开始阶段预分配一个内存池，然后反复利用。因为每个节点所占用的内存空间是一样大的，因此这些数据可以很容易从预分配的缓存中进行调用而不会产生任何碎片。

高估或者更好的启发函数

• f(x)=g(x)+ (h(x)*weight)，对weight不断进行实验，同时改进h(x)函数

参考文章

• 寻路建模的三种方式比较

• 深入理解游戏中的寻路算法

• 游戏人工智能