原理部分参考：
JPS(jump point search)寻路算法-CSDN博客
JPS/JPS+ 寻路算法 - KillerAery - 博客园 (cnblogs.com)
算法篇 — JPS 寻路- (fallingxun.github.io)
代码部分参考：
JPS 跳点算法源码分析_jps算法c++代码-CSDN博客
一文弄懂基于图搜索的路径规划算法JPS（有python代码）_jps路径规划算法-CSDN博客
Jump Point Search-跳点搜索法-原理&matlab代码-与A*算法比较（路径规划）_跳点搜索算法-CSDN博客
我的代码：
from __future__ import annotations # 延迟评估注解(类型声明)
import matplotlib.pyplot as plt
import numpy as np
from typing import List, Union
import math
class Node:
def __init__(self, grid_pos: tuple[int, int], g: float, h: float, parent: Node = None):
self.grid_pos = grid_pos
self.g = g
self.h = h
self.parent = parent
self.f = self.g + self.h
class JPS:
def __init__(self, width: int, height: int):
self.start_grid_pos = (0, 0)
self.goal_grid_pos = (0, 0)
self.width = width
self.height = height
self.open = {} # pos:node
self.close = {}
self.motion_directions = [[1, 0], [0, 1], [0, -1], [-1, 0], [1, 1], [1, -1], [-1, 1], [-1, -1]]
def run(self, start_grid_pos: tuple[int, int], goal_grid_pos: tuple[int, int]):
self.start_grid_pos = start_grid_pos
self.goal_grid_pos = goal_grid_pos
start_node = Node(start_grid_pos, 0, self.getH(self.start_grid_pos, self.goal_grid_pos))
self.open[start_node.grid_pos] = start_node
while True:
# 寻路失败，结束循环
if not self.open:
print("未找到路径")
break
current_node = min(self.open.values(), key=lambda x: x.f) # f值最小的节点
# 找到路径， 返回结果
if current_node.grid_pos == self.goal_grid_pos:
path = self.findPath(current_node)
print("找到路径！")
return path
# 扩展节点
self.extendNode(current_node)
# 更新节点
self.close[current_node.grid_pos] = current_node
del self.open[current_node.grid_pos]
def extendNode(self, current_node: Node):
"""
根据当前节点，扩展节点（只有跳点才可以扩展）
input
----------
current_node: 当前节点对象
"""
neighbours = self.getPruneNeighbours(current_node)
for n in neighbours:
jp = self.getJumpNode(n, current_node.grid_pos) # 跳点
if jp:
if jp in self.close:
continue
new_node = Node(jp, current_node.g + self.getG(jp, current_node.grid_pos),
self.getH(jp, self.goal_grid_pos), current_node)
if jp in self.open:
if new_node.f < self.open[jp].f:
self.open[jp].parent = current_node
self.open[jp].f = new_node.f
else:
self.open[jp] = new_node
def getJumpNode(self, now: tuple[int, int], pre: tuple[int, int]) -> Union[tuple[int, int], None]:
"""
计算跳点
input
----------
now: 当前节点坐标
pre: 上一节点坐标
output
----------
若有跳点，返回跳点坐标；若无跳点，则返回None
"""
x_direction = int((now[0] - pre[0]) / abs(now[0] - pre[0])) if now[0] - pre[0] != 0 else 0
y_direction = int((now[1] - pre[1]) / abs(now[1] - pre[1])) if now[1] - pre[1] != 0 else 0
if now == self.goal_grid_pos: # 如果当前节点是终点，则为跳点（条件1）
return now
if self.hasForceNeighbours(now, pre): # 如果当前节点包含强迫邻居，则为跳点（条件2）
return now
if abs(x_direction) + abs(y_direction) == 2: # 若为斜线移动，则判断水平和垂直方向是否有满足上述条件1和2的点，若有，则为跳点（条件3）
if (self.getJumpNode((now[0] + x_direction, now[1]), now) or
self.getJumpNode((now[0], now[1] + y_direction), now)):
return now
if self.isPass(now[0] + x_direction, now[1] + y_direction): # 若当前节点未找到跳点，朝当前方向前进一步，继续寻找跳点，直至不可达
jp = self.getJumpNode((now[0] + x_direction, now[1] + y_direction), now)
if jp:
return jp
return None
def hasForceNeighbours(self, now: tuple[int, int], pre: tuple[int, int]) -> bool:
"""
根据当前节点坐标和上一节点坐标判断当前节点是否拥有强迫邻居
input
----------
now: 当前坐标
pre: 上一坐标
output
----------
若拥有强迫邻居，则返回True；否则返回False
"""
x_direction = now[0] - pre[0]
y_direction = now[1] - pre[1]
# 若为直线移动
if abs(x_direction) + abs(y_direction) == 1:
if abs(x_direction) == 1: # 水平移动
if (self.isPass(now[0] + x_direction, now[1] + 1) and not self.isPass(now[0], now[1] + 1)) or \
(self.isPass(now[0] + x_direction, now[1] - 1) and not self.isPass(now[0], now[1] - 1)):
return True
else:
return False
elif abs(y_direction) == 1: # 垂直移动
if (self.isPass(now[0] + 1, now[1] + y_direction) and not self.isPass(now[0] + 1, now[1])) or \
(self.isPass(now[0] - 1, now[1] + y_direction) and not self.isPass(now[0] - 1, now[1])):
return True
else:
return False
else:
raise Exception("错误，直线移动中只能水平或垂直移动！")
# 若为斜线移动
elif abs(x_direction) + abs(y_direction) == 2:
if (self.isPass(now[0] + x_direction, now[1] - y_direction) and not self.isPass(now[0],
now[1] - y_direction)) or \
(self.isPass(now[0] - x_direction, now[1] + y_direction) and not self.isPass(now[0] - x_direction,
now[1])):
return True
else:
return False
else:
raise Exception("错误，只能直线移动或斜线移动！")
def getH(self, current: tuple[int, int], goal: tuple[int, int], func: str = "Euclidean") -> float:
"""
根据当前坐标和终点坐标计算启发值，包含：
曼哈顿距离（Manhattan Distance）
欧几里得距离（Euclidean Distance） 默认
切比雪夫距离（Chebyshev Distance）
input
----------
current: 当前节点坐标
goal: 目标节点坐标
func: 启发函数，默认为欧几里得距离
"""
current_x = current[0]
current_y = current[1]
goal_x = goal[0]
goal_y = goal[1]
if func == "Manhattan": # 适用于格点地图上，移动限制为上下左右四个方向的情况
h = abs(current_x - goal_x) + abs(current_y - goal_y)
elif func == "Euclidean": # 适用于可以自由移动至任何方向的情况，例如在平面上自由移动
h = math.hypot(current_x - goal_x, current_y - goal_y)
elif func == "Chebyshev": # 适用于八方向移动的格点地图（上下左右及对角线）
h = max(abs(current_x - goal_x), abs(current_y - goal_y))
else:
raise Exception("错误，不支持该启发函数。目前支持：Manhattan、Euclidean（默认）、Chebyshev。")
return h
def getG(self, pos1: tuple[int, int], pos2: tuple[int, int]) -> float:
"""
根据两坐标计算代价值（直走或斜走，直接计算欧几里得距离就可）
input
----------
pos1: 坐标1
pos2: 坐标2
output
----------
返回代价值
"""
return math.hypot(pos1[0] - pos2[0], pos1[1] - pos2[1])
def getPruneNeighbours(self, current_node: Node) -> list[tuple[int, int]]:
"""
获得裁剪之后的邻居节点坐标列表（包含自然邻居和强迫邻居，其相关概念参考：https://fallingxun.github.io/post/algorithm/algorithm_jps/）
input
----------
current_node: 当前节点对象
output
----------
prune_neighbours: 裁剪邻居坐标列表
"""
prune_neighbours = []
if current_node.parent:
motion_x = int((current_node.grid_pos[0] - current_node.parent.grid_pos[0]) / abs(
current_node.grid_pos[0] - current_node.parent.grid_pos[0])) if current_node.grid_pos[0] - \
current_node.parent.grid_pos[
0] != 0 else 0 # 方向不分大小，所以要除以长度
motion_y = int((current_node.grid_pos[1] - current_node.parent.grid_pos[1]) / abs(
current_node.grid_pos[1] - current_node.parent.grid_pos[1])) if current_node.grid_pos[1] - \
current_node.parent.grid_pos[
1] != 0 else 0 # 方向不分大小，所以要除以长度
if abs(motion_x) + abs(motion_y) == 1: # 直线
# 自然邻居
if self.isPass(current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] + motion_y):
prune_neighbours.append((current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] + motion_y))
# 强迫邻居
if abs(motion_x) == 0: # 垂直走
if not self.isPass(current_node.grid_pos[0] + 1, current_node.grid_pos[1]) and self.isPass(current_node.grid_pos[0] + 1, current_node.grid_pos[1] + motion_y):
prune_neighbours.append((current_node.grid_pos[0] + 1, current_node.grid_pos[1] + motion_y))
if not self.isPass(current_node.grid_pos[0] - 1, current_node.grid_pos[1]) and self.isPass(current_node.grid_pos[0] - 1, current_node.grid_pos[1] + motion_y):
prune_neighbours.append((current_node.grid_pos[0] - 1, current_node.grid_pos[1] + motion_y))
else: # 水平走
if not self.isPass(current_node.grid_pos[0], current_node.grid_pos[1] + 1) and self.isPass(current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] + 1):
prune_neighbours.append((current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] + 1))
if not self.isPass(current_node.grid_pos[0], current_node.grid_pos[1] - 1) and (current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] - 1):
prune_neighbours.append((current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] - 1))
elif abs(motion_x) + abs(motion_y) == 2: # 对角线
# 自然邻居
if self.isPass(current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] + motion_y):
prune_neighbours.append((current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] + motion_y))
if self.isPass(current_node.grid_pos[0] + motion_x, current_node.grid_pos[1]):
prune_neighbours.append((current_node.grid_pos[0] + motion_x, current_node.grid_pos[1]))
if self.isPass(current_node.grid_pos[0], current_node.grid_pos[1] + motion_y):
prune_neighbours.append((current_node.grid_pos[0], current_node.grid_pos[1] + motion_y))
# 强迫邻居
if not self.isPass(current_node.grid_pos[0] - motion_x, current_node.grid_pos[1]) and self.isPass(
current_node.grid_pos[0] - motion_x, current_node.grid_pos[1] + motion_y):
prune_neighbours.append((current_node.grid_pos[0] - motion_x, current_node.grid_pos[1] + motion_y))
if not self.isPass(current_node.grid_pos[0], current_node.grid_pos[1] - motion_y) and self.isPass(
current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] - motion_y):
prune_neighbours.append((current_node.grid_pos[0] + motion_x, current_node.grid_pos[1] - motion_y))
else:
raise Exception("错误，只能对角线和直线行走！")
else:
for dir in self.motion_directions:
if self.isPass(current_node.grid_pos[0] + dir[0], current_node.grid_pos[1] + dir[1]):
prune_neighbours.append((current_node.grid_pos[0] + dir[0], current_node.grid_pos[1] + dir[1]))
return prune_neighbours
def isPass(self, grid_x: int, grid_y: int) -> bool:
"""
判断该栅格坐标是否可以通过
input
----------
grid_x: 栅格x坐标
grid_y: 栅格y坐标
output
----------
若可以通过，为True，反之为False
"""
if 0 <= grid_x < self.width and 0 <= grid_y < self.height:
if map_test[grid_x][grid_y] != 1 or [grid_x, grid_y] == self.goal_grid_pos:
return True
else:
return False
else:
return False
def findPath(self, node: Node) -> list[list[int]]:
"""
根据给定节点回溯到开始点，找到路径
"""
path_x = [node.grid_pos[0]]
path_y = [node.grid_pos[1]]
while node.parent:
node = node.parent
path_x.append(node.grid_pos[0])
path_y.append(node.grid_pos[1])
return [path_x[::-1], path_y[::-1]]
if __name__ == '__main__':
# 地图数据；0：能通过；1:障碍，不能通过(7X7)
map_test = np.array(
[[0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0],
[0, 1, 0, 1, 1, 0, 1],
[0, 1, 0, 1, 1, 0, 0],
[0, 1, 0, 1, 1, 1, 0],
[0, 1, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0],
]
)
start = (6, 0)
goal = (6, 6)
jps = JPS(7, 7)
path = jps.run(start, goal)
fig = plt.figure()
ax = fig.add_subplot(111)
ax.imshow(map_test, cmap='binary', vmin=0, vmax=1)
ax.scatter(start[1], start[0]) # 绘制图像的坐标轴与np相反
ax.scatter(goal[1], goal[0])
px, py = path
ax.plot(py, px)
plt.show()
7328
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
