A*和bilstm结合的路径栅格地图路径规划代码

以下是A*和BiLSTM结合的路径栅格地图路径规划的Python代码，其中使用PyTorch实现BiLSTM模型。

import numpy as np
import heapq
import torch
import torch.nn as nn

# 定义BiLSTM模型
class BiLSTM(nn.Module):
    def __init__(self, input_size, hidden_size, num_layers, output_size):
        super(BiLSTM, self).__init__()
        self.hidden_size = hidden_size
        self.num_layers = num_layers
        self.lstm = nn.LSTM(input_size, hidden_size, num_layers, batch_first=True, bidirectional=True)
        self.fc = nn.Linear(hidden_size*2, output_size)

    def forward(self, x):
        h0 = torch.zeros(self.num_layers*2, x.size(0), self.hidden_size).to(device)
        c0 = torch.zeros(self.num_layers*2, x.size(0), self.hidden_size).to(device)
        out, _ = self.lstm(x, (h0, c0))
        out = self.fc(out[:, -1, :])
        return out

# 定义A*算法
def A_star(start, goal, obstacles, model):
    # 定义节点类
    class Node:
        def __init__(self, x, y, g, h, parent=None):
            self.x = x
            self.y = y
            self.g = g
            self.h = h
            self.parent = parent

        def f(self):
            return self.g + self.h

        def __lt__(self, other):
            return self.f() < other.f()

    # 定义启发函数
    def heuristic(node):
        return np.sqrt((node.x - goal[0])**2 + (node.y - goal[1])**2)

    # 定义邻居节点生成函数
    def get_neighbors(node):
        neighbors = []
        for dx, dy in [(1, 0), (-1, 0), (0, 1), (0, -1)]:
            x = node.x + dx
            y = node.y + dy
            if x < 0 or x >= map_width or y < 0 or y >= map_height:
                continue
            if (x, y) in obstacles:
                continue
            neighbors.append(Node(x, y, 0, 0))
        return neighbors

    # 初始化起点和终点节点
    start_node = Node(start[0], start[1], 0, heuristic(Node(start[0], start[1], 0, 0)))
    goal_node = Node(goal[0], goal[1], 0, 0)

    # 定义开放列表和关闭列表
    open_list = [start_node]
    closed_list = set()

    # 迭代查找最优路径
    while open_list:
        # 取出开放列表中f值最小的节点
        current_node = heapq.heappop(open_list)
        # 如果当前节点是终点，则返回路径
        if current_node.x == goal_node.x and current_node.y == goal_node.y:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1]
        # 加入关闭列表
        closed_list.add((current_node.x, current_node.y))
        # 生成邻居节点
        neighbors = get_neighbors(current_node)
        for neighbor in neighbors:
            # 如果邻居节点已在关闭列表中，则跳过
            if (neighbor.x, neighbor.y) in closed_list:
                continue
            # 计算邻居节点的g值和h值
            neighbor.g = current_node.g + 1
            x, y = neighbor.x, neighbor.y
            input_data = torch.tensor([map_data[x-1:x+2, y-1:y+2]], dtype=torch.float32).to(device)
            neighbor.h = model(input_data).item()
            # 设置邻居节点的父节点为当前节点
            neighbor.parent = current_node
            # 如果邻居节点已在开放列表中，则更新g值和h值
            for node in open_list:
                if node.x == neighbor.x and node.y == neighbor.y:
                    if node.g > neighbor.g:
                        node.g = neighbor.g
                        node.h = neighbor.h
                        node.parent = neighbor.parent
                    break
            else:
                # 否则将邻居节点加入开放列表
                heapq.heappush(open_list, neighbor)

    # 如果无法到达终点，则返回空路径
    return []

# 生成地图数据
map_width, map_height = 10, 10
map_data = np.zeros((map_width, map_height))
obstacles = [(2, 3), (3, 3), (4, 3), (5, 3), (6, 3), (6, 4), (6, 5), (5, 5), (4, 5), (3, 5)]
for obstacle in obstacles:
    map_data[obstacle[0], obstacle[1]] = 1

# 定义起点和终点
start = (1, 1)
goal = (8, 8)

# 加载BiLSTM模型
device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
model = BiLSTM(9, 32, 2, 1).to(device)
model.load_state_dict(torch.load('bilstm_model.pth'))

# 路径规划
path = A_star(start, goal, obstacles, model)

# 输出路径
print(path)