多机器人寻路与避障代码

多机器人寻路与避障是一个复杂的问题，需要考虑到多个机器人之间的相互作用和碰撞避免。以下是一个简单的多机器人寻路与避障的代码示例：

import numpy as np

class Robot:
    def __init__(self, position, velocity, radius):
        self.position = np.array(position)
        self.velocity = np.array(velocity)
        self.radius = radius

    def move(self, dt):
        self.position += self.velocity * dt

    def avoid_collisions(self, robots):
        for other in robots:
            if other != self:
                distance = np.linalg.norm(other.position - self.position)
                if distance < (self.radius + other.radius):
                    self.velocity = self.velocity + (self.position - other.position) / distance ** 2
    
    def seek_goal(self, goal, max_speed):
        desired_velocity = (goal - self.position)
        speed = np.linalg.norm(desired_velocity)
        if speed > max_speed:
            desired_velocity = desired_velocity / speed * max_speed
        self.velocity = desired_velocity

class MultiRobot:
    def __init__(self, num_robots, world_size, goal, dt):
        self.robots = []
        self.world_size = world_size
        self.goal = np.array(goal)
        self.dt = dt
        for i in range(num_robots):
            position = [np.random.uniform(0, world_size[0]), np.random.uniform(0, world_size[1])]
            velocity = [np.random.uniform(-1, 1), np.random.uniform(-1, 1)]
            radius = 1.0
            robot = Robot(position, velocity, radius)
            self.robots.append(robot)

    def update(self):
        for robot in self.robots:
            robot.seek_goal(self.goal, 10)
            robot.avoid_collisions(self.robots)
            robot.move(self.dt)
            self.wrap_position(robot)

    def wrap_position(self, robot):
        if robot.position[0] > self.world_size[0]:
            robot.position[0] -= self.world_size[0]
        elif robot.position[0] < 0:
            robot.position[0] += self.world_size[0]
        if robot.position[1] > self.world_size[1]:
            robot.position[1] -= self.world_size[1]
        elif robot.position[1] < 0:
            robot.position[1] += self.world_size[1]

这个代码示例中，`Robot` 类表示一个机器人，具有位置、速度和半径属性，以及 `move`、`avoid_collisions` 和 `seek_goal` 方法。`MultiRobot` 类表示多个机器人的集合，具有一个目标点、世界大小和时间步长属性，以及 `update` 和 `wrap_position` 方法。在 `update` 方法中，每个机器人都会寻找目标点、避免碰撞和移动，同时通过 `wrap_position` 方法将机器人的位置限制在世界边界内。