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

以下是使用Pygame实现A*算法与DWA算法结合的路径规划的示例代码：

import pygame
import math
import random

# 定义地图和机器人模型
WIDTH = 800
HEIGHT = 600
ROBOT_SIZE = 20
ROBOT_SPEED = 2
ROBOT_MAX_SPEED = 5
ROBOT_MAX_ROTATION = math.pi / 4
GOAL_RADIUS = 10
OBSTACLES = [(200, 200, 100, 50), (500, 400, 50, 100)]

# 定义颜色
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
RED = (255, 0, 0)
GREEN = (0, 255, 0)
BLUE = (0, 0, 255)

# 实现A*算法
class Node:
    def __init__(self, x, y, parent=None):
        self.x = x
        self.y = y
        self.parent = parent
        self.g = 0
        self.h = 0
        self.f = 0

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

def get_distance(node1, node2):
    return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)

def get_path(node):
    path = [(node.x, node.y)]
    while node.parent:
        node = node.parent
        path.append((node.x, node.y))
    return path[::-1]

def astar(start, goal):
    open_list = [start]
    closed_list = []

    while open_list:
        current = min(open_list, key=lambda node: node.f)

        if current == goal:
            return get_path(current)

        open_list.remove(current)
        closed_list.append(current)

        for neighbor in get_neighbors(current):
            if neighbor in closed_list:
                continue

            new_g = current.g + get_distance(current, neighbor)

            if neighbor not in open_list:
                open_list.append(neighbor)
            elif new_g >= neighbor.g:
                continue

            neighbor.g = new_g
            neighbor.h = get_distance(neighbor, goal)
            neighbor.f = neighbor.g + neighbor.h
            neighbor.parent = current

    return None

def get_neighbors(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 or x >= WIDTH or y < 0 or y >= HEIGHT:
                continue
            if collides_with_obstacle(x, y):
                continue
            neighbors.append(Node(x, y))
    return neighbors

def collides_with_obstacle(x, y):
    for obstacle in OBSTACLES:
        if x < obstacle[0] or x > obstacle[0] + obstacle[2]:
            continue
        if y < obstacle[1] or y > obstacle[1] + obstacle[3]:
            continue
        return True
    return False

# 实现DWA算法
class Robot:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.vx = 0
        self.vy = 0
        self.rotation = 0

    def update(self, path):
        goal = Node(path[0][0], path[0][1])
        distance = get_distance(Node(self.x, self.y), goal)
        if distance < GOAL_RADIUS:
            path.pop(0)
            if not path:
                return

        speed, rotation = dwa(self, goal)
        self.vx = speed * math.cos(self.rotation)
        self.vy = speed * math.sin(self.rotation)
        self.rotation += rotation

        if self.vx > ROBOT_MAX_SPEED:
            self.vx = ROBOT_MAX_SPEED
        elif self.vx < -ROBOT_MAX_SPEED:
            self.vx = -ROBOT_MAX_SPEED

        if self.rotation > ROBOT_MAX_ROTATION:
            self.rotation = ROBOT_MAX_ROTATION
        elif self.rotation < -ROBOT_MAX_ROTATION:
            self.rotation = -ROBOT_MAX_ROTATION

        self.x += self.vx
        self.y += self.vy

    def draw(self, screen):
        pygame.draw.circle(screen, BLUE, (int(self.x), int(self.y)), ROBOT_SIZE)

def dwa(robot, goal):
    # 随机生成速度和角速度
    v = random.uniform(0, ROBOT_MAX_SPEED)
    w = random.uniform(-ROBOT_MAX_ROTATION, ROBOT_MAX_ROTATION)

    # 计算代价函数
    cost = 0
    for i in range(10):
        x = robot.x + robot.vx * i
        y = robot.y + robot.vy * i
        theta = robot.rotation + w * i
        cost += get_distance(Node(x, y), goal)
    cost /= 10

    return v, w

# 初始化Pygame
pygame.init()
screen = pygame.display.set_mode((WIDTH, HEIGHT))
pygame.display.set_caption("Path Planning with A* and DWA")

# 创建机器人和目标
robot = Robot(100, 100)
goal = Node(700, 500)

# 计算路径
path = astar(Node(robot.x, robot.y), goal)

# 游戏循环
clock = pygame.time.Clock()
running = True
while running:
    clock.tick(60)

    # 处理事件
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False

    # 更新机器人位置和朝向
    robot.update(path)

    # 绘制屏幕
    screen.fill(WHITE)

    # 绘制障碍物
    for obstacle in OBSTACLES:
        pygame.draw.rect(screen, BLACK, obstacle)

    # 绘制机器人和目标
    robot.draw(screen)
    pygame.draw.circle(screen, RED, (goal.x, goal.y), GOAL_RADIUS)

    # 绘制路径
    if path:
        pygame.draw.lines(screen, GREEN, False, path)

    pygame.display.flip()

# 退出Pygame
pygame.quit()

在此示例中，我们首先定义了地图和机器人模型的参数。障碍物用一个列表来表示，每个障碍物是一个元组，包含障碍物的左上角坐标、宽度和高度。机器人用一个类来表示，包含位置、速度、朝向等信息。我们还定义了A*算法和DWA算法的实现，以及用于绘制屏幕的Pygame代码。在游戏循环中，我们首先计算路径，然后在每个帧中更新机器人位置和朝向，绘制障碍物、机器人、目标和路径，最后更新屏幕。