使用广度优先搜索算法找到最短路径，然后绘制路径图

from collections import deque
import matplotlib.pyplot as plt

# 定义网格环境的参数
GRID_SIZE = 5
GRID_WIDTH = 4
AGENT_SPEED = 2
START_STATE = (0, 0)
GOAL_STATE = (GRID_SIZE - 1, GRID_SIZE - 1)

# 定义动作集合：上、下、左、右
ACTIONS = ['UP', 'DOWN', 'LEFT', 'RIGHT']
NUM_ACTIONS = len(ACTIONS)

# 定义参数
EPSILON = 0.1  # 探索率

# 定义动作对应的坐标增量
ACTION_DELTA = {
    'UP': (-AGENT_SPEED, 0),
    'DOWN': (AGENT_SPEED, 0),
    'LEFT': (0, -AGENT_SPEED),
    'RIGHT': (0, AGENT_SPEED)
}

# 执行广度优先搜索（BFS）算法找到最短路径
def bfs():
    queue = deque([(START_STATE, [])])
    visited = set()
    
    while queue:
        current_state, path = queue.popleft()
        if current_state == GOAL_STATE:
            return path
        visited.add(current_state)
        for action in ACTIONS:
            next_state = take_action(current_state, action)
            if next_state not in visited and is_valid_state(next_state):
                queue.append((next_state, path + [action]))

# 检查状态是否有效（在网格范围内）
def is_valid_state(state):
    x, y = state
    return 0 <= x < GRID_SIZE and 0 <= y < GRID_SIZE

# 执行动作
def take_action(state, action):
    dx, dy = ACTION_DELTA[action]
    x, y = state
    next_state = (x + dx, y + dy)
    return next_state

# 绘制路径图
def plot_path(path):
    x_coords = [START_STATE[0]]  # 起点 x 坐标
    y_coords = [START_STATE[1]]  # 起点 y 坐标
    z_coords = [0]  # 时间轴

    for action in path:
        dx, dy = ACTION_DELTA[action]
        x_coords.append(x_coords[-1] + dx)
        y_coords.append(y_coords[-1] + dy)
        z_coords.append(len(z_coords))

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(x_coords, y_coords, z_coords)
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Time Step')
    plt.title('Shortest Path of the Agent')
    plt.show()

# 执行广度优先搜索找到最短路径
shortest_path = bfs()

# 输出最短路径
print("Shortest Path:", shortest_path)

# 绘制最短路径的三维图形
plot_path(shortest_path)

寻优路线图！