first commit

navigate.py

@@ -0,0 +1,68 @@
+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，或目标位置）
+def is_walkable(pos):
+    r, c = pos
+    if 0 <= r < len(parking_lot) and 0 <= c < len(parking_lot[0]):
+        return parking_lot[r][c] == 0 or pos == goal
+    return False
+
+# 曼哈顿距离作为启发函数
+def heuristic(a, b):
+    return abs(a[0] - b[0]) + abs(a[1] - b[1])
+
+# A*搜索
+def a_star(start, goal):
+    open_set = []
+    heapq.heappush(open_set, (0 + heuristic(start, goal), 0, start))
+    came_from = {}  # 路径追踪
+    g_score = {start: 0}
+
+    while open_set:
+        _, current_cost, current = heapq.heappop(open_set)
+
+        if current == goal:
+            # 还原路径
+            path = [current]
+            while current in came_from:
+                current = came_from[current]
+                path.append(current)
+            return path[::-1]  # 反转
+
+        # 邻接节点（上下左右）
+        for dr, dc in [(-1,0), (1,0), (0,-1), (0,1)]:
+            neighbor = (current[0] + dr, current[1] + dc)
+
+            if not is_walkable(neighbor):
+                continue
+
+            tentative_g = current_cost + 1
+            if neighbor not in g_score or tentative_g < g_score[neighbor]:
+                g_score[neighbor] = tentative_g
+                f = tentative_g + heuristic(neighbor, goal)
+                heapq.heappush(open_set, (f, tentative_g, neighbor))
+                came_from[neighbor] = current
+
+    return None  # 找不到路径
+
+# 执行 A* 路径搜索
+path = a_star(start, goal)
+
+# 输出结果
+if path:
+    print("找到路径:")
+    for step in path:
+        print(step)
+else:
+    print("找不到路径。")