along/graduation-project
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

format

Browse Source
This commit is contained in:
weixin_46229132 2025-05-04 21:32:13 +08:00
parent b626f667d9
commit 1833258f34
1 changed files with 24 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
navigate.py
View File

@ -1,15 +1,5 @@
import heapq import heapq
# 停车场状态矩阵
parking_lot = [
[3, 2, 2, 0, 2, 2, 2],
[3, 1, 1, 0, 1, 1, 1],
[0, 0, 0, 0, 0, 0, 0]
]
# 起点和终点（行, 列）
start = (0, 3)
goal = (1, 1)
# 可行走的位置（即值为0，或目标位置） # 可行走的位置（即值为0，或目标位置）
def is_walkable(pos): def is_walkable(pos):
@ -18,10 +8,12 @@ def is_walkable(pos):
return parking_lot[r][c] == 0 or pos == goal return parking_lot[r][c] == 0 or pos == goal
return False return False
# 曼哈顿距离作为启发函数 # 曼哈顿距离作为启发函数
def heuristic(a, b): def heuristic(a, b):
return abs(a[0] - b[0]) + abs(a[1] - b[1]) return abs(a[0] - b[0]) + abs(a[1] - b[1])
# A*搜索 # A*搜索
def a_star(start, goal): def a_star(start, goal):
open_set = [] open_set = []
@ -56,6 +48,18 @@ def a_star(start, goal):
return None # 找不到路径 return None # 找不到路径
if __name__ == "__main__":
# 停车场状态矩阵
parking_lot = [
[3, 2, 2, 0, 2, 2, 2],
[3, 1, 1, 0, 1, 1, 1],
[0, 0, 0, 0, 0, 0, 0]
]
# 起点和终点（行, 列）
start = (0, 3)
goal = (1, 1)
# 执行 A* 路径搜索 # 执行 A* 路径搜索
path = a_star(start, goal) path = a_star(start, goal)