pygame实现A*算法与DWA算法结合

好的，这里我们提供一种简单的实现方式，仅供参考。在这个实现中，我们使用 pygame 库来实现可视化界面，并结合 A* 算法和 DWA 算法来实现机器人路径规划和控制。

首先，需要安装 pygame 库。在命令行中输入以下命令：

pip install pygame

接下来，我们需要创建一个 pygame 窗口和地图。这里我们创建一个 600x600 的窗口，并在窗口中绘制一个简单的地图，包括起点、终点和障碍物：

import pygame

# 初始化 Pygame
pygame.init()

# 创建 Pygame 窗口
screen = pygame.display.set_mode((600, 600))

# 设置窗口标题
pygame.display.set_caption("A* + DWA Demo")

# 定义地图大小和格子大小
MAP_SIZE = (500, 500)
GRID_SIZE = 10

# 定义起点、终点和障碍物
start_pos = (50, 50)
end_pos = (450, 450)
obstacles = [(200, 200), (250, 350), (300, 200), (350, 350)]

# 绘制地图
def draw_map():
    # 绘制背景
    screen.fill((255, 255, 255))
    
    # 绘制起点和终点
    pygame.draw.circle(screen, (0, 255, 0), start_pos, 5)
    pygame.draw.circle(screen, (255, 0, 0), end_pos, 5)
    
    # 绘制障碍物
    for obs in obstacles:
        pygame.draw.rect(screen, (0, 0, 0), pygame.Rect(obs[0], obs[1], GRID_SIZE, GRID_SIZE))
    
    # 更新显示
    pygame.display.flip()

# 绘制初始地图
draw_map()

接下来，我们实现 A* 算法来计算从起点到终点的最短路径。这里我们使用一个二维数组来表示地图，并使用一个字典来保存每个格子的父节点和到起点的距离。具体实现如下：

# 定义地图数组
map_array = [[0] * (MAP_SIZE[0] // GRID_SIZE) for i in range(MAP_SIZE[1] // GRID_SIZE)]

# 定义节点类
class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.g = float('inf')
        self.h = float('inf')
        self.f = float('inf')
    
    def __eq__(self, other):
        return self.x == other.x and self.y == other.y
    
    def __hash__(self):
        return hash((self.x, self.y))

# 定义 A* 算法
def a_star(start_pos, end_pos, obstacles):
    # 创建起点和终点节点
    start_node = Node(start_pos[0] // GRID_SIZE, start_pos[1] // GRID_SIZE)
    end_node = Node(end_pos[0] // GRID_SIZE, end_pos[1] // GRID_SIZE)
    
    # 初始化开放列表和关闭列表
    open_list = set()
    closed_list = set()
    
    # 将起点加入开放列表
    start_node.g = 0
    start_node.h = abs(end_node.x - start_node.x) + abs(end_node.y - start_node.y)
    start_node.f = start_node.g + start_node.h
    open_list.add(start_node)
    
    # 开始搜索
    while open_list:
        # 选择 f 值最小的节点
        current_node = min(open_list, key=lambda node: node.f)
        
        # 判断是否到达终点
        if current_node == end_node:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1]
        
        # 将当前节点从开放列表中删除，并加入关闭列表
        open_list.remove(current_node)
        closed_list.add(current_node)
        
        # 搜索相邻节点
        for x, y in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
            neighbor_x, neighbor_y = current_node.x + x, current_node.y + y
            
            # 判断是否越界或为障碍物
            if neighbor_x < 0 or neighbor_y < 0 or neighbor_x >= len(map_array[0]) or neighbor_y >= len(map_array) or (neighbor_x, neighbor_y) in obstacles:
                continue
            
            # 创建相邻节点
            neighbor_node = Node(neighbor_x, neighbor_y)
            
            # 判断是否已在关闭列表中
            if neighbor_node in closed_list:
                continue
            
            # 计算 g、h、f 值
            g = current_node.g + 1
            h = abs(end_node.x - neighbor_node.x) + abs(end_node.y - neighbor_node.y)
            f = g + h
            
            # 判断是否已在开放列表中
            if neighbor_node in open_list:
                # 如果新的路径更优，则更新节点信息
                if f < neighbor_node.f:
                    neighbor_node.g = g
                    neighbor_node.h = h
                    neighbor_node.f = f
                    neighbor_node.parent = current_node
            else:
                # 将节点加入开放列表
                neighbor_node.g = g
                neighbor_node.h = h
                neighbor_node.f = f
                neighbor_node.parent = current_node
                open_list.add(neighbor_node)
    
    # 如果找不到路径，则返回空列表
    return []

# 计算 A* 算法得到的最短路径
path = a_star(start_pos, end_pos, obstacles)

现在，我们已经得到了从起点到终点的最短路径，接下来我们使用 DWA 算法来控制机器人沿着这条路径平滑移动。DWA 算法需要计算机器人的速度和角速度，这里我们简单地使用了一个恒定的速度和角速度：

# 定义机器人类
class Robot:
    def __init__(self, pos):
        self.pos = pos
        self.angle = 0
    
    # 更新机器人位置和角度
    def update(self, v, w):
        self.pos = (self.pos[0] + v * GRID_SIZE * math.cos(self.angle),
                    self.pos[1] + v * GRID_SIZE * math.sin(self.angle))
        self.angle += w
    
    # 绘制机器人
    def draw(self):
        pygame.draw.circle(screen, (0, 0, 255), (int(self.pos[0]), int(self.pos[1])), 5)
        pygame.draw.line(screen, (0, 0, 255), (int(self.pos[0]), int(self.pos[1])),
                         (int(self.pos[0] + GRID_SIZE * math.cos(self.angle)),
                          int(self.pos[1] + GRID_SIZE * math.sin(self.angle))))
    
    # 计算机器人和路径的距离
    def distance_to_path(self, path):
        min_distance = float('inf')
        for i in range(len(path) - 1):
            p1, p2 = path[i], path[i+1]
            x1, y1 = p1[0] * GRID_SIZE, p1[1] * GRID_SIZE
            x2, y2 = p2[0] * GRID_SIZE, p2[1] * GRID_SIZE
            distance = abs((y2 - y1) * self.pos[0] - (x2 - x1) * self.pos[1] + x2 * y1 - y2 * x1) / math.sqrt((y2 - y1) ** 2 + (x2 - x1) ** 2)
            min_distance = min(min_distance, distance)
        return min_distance

# 创建机器人对象
robot = Robot(start_pos)

# 定义恒定速度和角速度
v = 1
w = 0.1

# 计算机器人和路径的距离
distance_to_path = robot.distance_to_path(path)

最后，我们在游戏循环中实现主要逻辑。在每次循环中，我们先判断是否到达终点，如果到达则停止运动，否则计算机器人和路径的距离，根据距离调整机器人的速度和角速度，并更新机器人的位置和角度。同时，我们还需要重新绘制地图和机器人：

# 进入游戏循环
while True:
    # 处理事件
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            sys.exit()
    
    # 判断是否到达终点
    if robot.pos[0] >= end_pos[0] - GRID_SIZE and robot.pos[1] >= end_pos[1] - GRID_SIZE:
        break
    
    # 计算机器人和路径的距离
    distance_to_path = robot.distance_to_path(path)
    
    # 根据距离调整速度和角速度
    if distance_to_path > 50:
        v = 1
    elif distance_to_path > 20:
        v = 0.5
    else:
        v = 0.1
    if path:
        dx = path[0][0] * GRID_SIZE - robot.pos[0]
        dy = path[0][1] * GRID_SIZE - robot.pos[1]
        target_angle = math.atan2(dy, dx)
        w = max(-0.3, min(0.3, target_angle - robot.angle))
    else:
        w = 0
    
    # 更新机器人位置和角度
    robot.update(v, w)
    
    # 绘制地图和机器人
    draw_map()
    for p in path:
        pygame.draw.circle(screen, (0, 255, 255), (p[0] * GRID_SIZE, p[1] * GRID_SIZE), 2)
    robot.draw()
    
    # 更新显示
    pygame.display.flip()
    
    # 延迟一段时间
    pygame.time.delay(50)

完整代码如下：