无人驾驶路径规划python代码

以下一个简单的无人驾驶路径规划的 Python 代码示例，使用 A* 算法：

import heapq

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.g = 0
        self.h = 0
        self.f = 0
        self.parent = None

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

class AStar:
    def __init__(self, grid):
        self.grid = grid
        self.width = len(grid[0])
        self.height = len(grid)
        self.open_list = []
        self.closed_list = set()

    def heuristic(self, node, goal):
        return abs(node.x - goal.x) + abs(node.y - goal.y)

    def get_neighbors(self, node):
        neighbors = []
        for dx in [-1, 0, 1]:
            for dy in [-1, 0, 1]:
                if dx == 0 and dy == 0:
                    continue
                x = node.x + dx
                y = node.y + dy
                if x >= 0 and x < self.width and y >= 0 and y < self.height and self.grid[y][x] == 0:
                    neighbors.append(Node(x, y))
        return neighbors

    def get_path(self, start, goal):
        start_node = Node(start[0], start[1])
        goal_node = Node(goal[0], goal[1])
        heapq.heappush(self.open_list, start_node)
        while len(self.open_list) > 0:
            current_node = heapq.heappop(self.open_list)
            if current_node.x == goal_node.x and current_node.y == goal_node.y:
                path = []
                while current_node.parent is not None:
                    path.append((current_node.x, current_node.y))
                    current_node = current_node.parent
                path.append((start_node.x, start_node.y))
                path.reverse()
                return path
            self.closed_list.add((current_node.x, current_node.y))
            neighbors = self.get_neighbors(current_node)
            for neighbor in neighbors:
                if (neighbor.x, neighbor.y) in self.closed_list:
                    continue
                if neighbor not in self.open_list:
                    neighbor.g = current_node.g + 1
                    neighbor.h = self.heuristic(neighbor, goal_node)
                    neighbor.f = neighbor.g + neighbor.h
                    neighbor.parent = current_node
                    heapq.heappush(self.open_list, neighbor)
                else:
                    if current_node.g + 1 < neighbor.g:
                        neighbor.g = current_node.g + 1
                        neighbor.f = neighbor.g + neighbor.h
                        neighbor.parent = current_node
        return None

# 示例使用
grid = [
    [0, 0, 0, 0, 0],
    [0, 1, 0, 1, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 0, 1, 0],
    [0, 0, 0, 0, 0]
]
start = (0, 0)
goal = (4, 4)
astar = AStar(grid)
path = astar.get_path(start, goal)
print(path)

在这个示例中，我们使用了一个 5x5 的网格，并在其中定义了起点和终点。我们还定义了一个 `AStar` 类，它包含了 A* 算法的主要逻辑，包括启发式函数、获取邻居节点等。最后，我们将起点和终点传递给 AStar 类的 `get_path` 方法，它将返回一条可行的路径。