A*算法控制小车python 路径规划代码

以下是一个简单的 A*算法控制小车路径规划的 Python 代码示例，你可以根据自己的实际需求进行修改：

import heapq
import math

# 定义地图大小和障碍物位置
WIDTH = 30
HEIGHT = 20
obstacles = [(5, 5), (6, 5), (7, 5), (8, 5), (9, 5), (10, 5), (5, 15), (6, 15), (7, 15), (8, 15), (9, 15), (10, 15)]

# 定义节点类
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

    def __eq__(self, other):
        return self.x == other.x and self.y == other.y

    def __hash__(self):
        return hash((self.x, self.y))

# 计算两个节点之间的曼哈顿距离
def manhattan_distance(node1, node2):
    return abs(node1.x - node2.x) + abs(node1.y - node2.y)

# 查找起点和终点
def find_start_and_goal(start_pos, goal_pos):
    start_node = Node(start_pos[0], start_pos[1])
    goal_node = Node(goal_pos[0], goal_pos[1])
    return start_node, goal_node

# 计算路径
def a_star(start_pos, goal_pos):
    # 查找起点和终点
    start_node, goal_node = find_start_and_goal(start_pos, goal_pos)

    # 初始化 open 和 close 列表
    open_list = []
    close_list = set()

    # 添加起点到 open 列表
    heapq.heappush(open_list, start_node)

    # 开始搜索
    while open_list:
        # 取出 f 值最小的节点
        current_node = heapq.heappop(open_list)

        # 如果当前节点为终点，返回路径
        if current_node == goal_node:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return list(reversed(path))

        # 将当前节点加入 close 列表
        close_list.add(current_node)

        # 查找当前节点的邻居节点
        for x_offset, y_offset in ((0, -1), (0, 1), (-1, 0), (1, 0)):
            x = current_node.x + x_offset
            y = current_node.y + y_offset
            if x < 0 or x >= WIDTH or y < 0 or y >= HEIGHT:
                continue
            if (x, y) in obstacles:
                continue
            neighbor_node = Node(x, y)

            # 如果邻居节点已经在 close 列表中，跳过
            if neighbor_node in close_list:
                continue

            # 计算邻居节点的 g 值
            new_g = current_node.g + 1

            # 如果邻居节点不在 open 列表中，或者新的 g 值更小，更新邻居节点
            if neighbor_node not in open_list or new_g < neighbor_node.g:
                neighbor_node.g = new_g
                neighbor_node.h = manhattan_distance(neighbor_node, goal_node)
                neighbor_node.f = neighbor_node.g + neighbor_node.h
                neighbor_node.parent = current_node
                if neighbor_node not in open_list:
                    heapq.heappush(open_list, neighbor_node)

    # 如果没有找到路径，返回空列表
    return []

# 测试代码
start_pos = (3, 3)
goal_pos = (25, 15)
path = a_star(start_pos, goal_pos)
print(path)

这个示例代码中，我们通过定义 Node 类来表示地图上的节点，使用 heapq 库来实现优先队列，使用 set 来实现 close 列表，使用 Manhattan 距离来计算节点之间的距离。为了简化问题，我们假设地图大小为 30x20，障碍物的位置已经确定，并且小车只能朝上下左右四个方向行驶，每移动一个格子的代价为 1。在计算路径时，我们先查找起点和终点，然后将起点加入 open 列表，循环取出 f 值最小的节点，将其加入 close 列表，计算邻居节点的 g、h、f 值，并更新邻居节点的父节点，最后将邻居节点加入 open 列表。如果找到了终点，就遍历路径，返回路径列表。如果没有找到路径，就返回空列表。