🚛 Практическая работа: Программирование модели транспортного робота

Если левое_колесо = правое_колесо → едем прямо
Если левое_колесо > правое_колесо → поворот направо  
Если левое_колесо < правое_колесо → поворот налево

PROTO TransportRobot [
  field SFVec3f translation 0 0 0.05
  field SFString name "transport_robot"
]
{
  Robot {
    translation IS translation
    name IS name
    children [
      # Основной корпус
      DEF BODY Transform {
        children [
          Shape {
            geometry Box { size 0.30 0.20 0.15 }
            appearance PBRAppearance {
              baseColor 0.3 0.3 0.8
            }
          }
        ]
      }
      
      # Левое колесо с мотором
      DEF LEFT_WHEEL HingeJoint {
        jointParameters HingeJointParameters {
          axis 0 1 0
          anchor -0.075 0.12 0
        }
        device RotationalMotor {
          name "left_motor"
          maxVelocity 10
        }
        endPoint Solid {
          translation -0.075 0.12 0
          children [
            Shape {
              geometry Cylinder { height 0.025 radius 0.04 }
            }
          ]
          physics Physics { density 1200 }
        }
      }
    ]
    
    physics Physics {
      density -1
      mass 1.5
      centerOfMass [0 0 0.02]
    }
  }
}

# Подключаем библиотеки
from controller import Robot, Motor, DistanceSensor, Camera

# Создаем робота
robot = Robot()
timestep = 32  # миллисекунд между "мыслями" робота

# Подключаем устройства
left_motor = robot.getDevice('left_motor')
right_motor = robot.getDevice('right_motor')
distance_sensor = robot.getDevice('distance_sensor')
camera = robot.getDevice('camera')

# Настраиваем моторы
left_motor.setPosition(float('inf'))  # бесконечное вращение
right_motor.setPosition(float('inf'))

# Включаем датчики
distance_sensor.enable(timestep)
camera.enable(timestep)

# Основной цикл - "мышление" робота
while robot.step(timestep) != -1:
    # Читаем датчики
    distance = distance_sensor.getValue()
    
    # Принимаем решение
    if distance > 0.5:  # путь свободен (больше 50 см)
        move_forward()
    else:  # препятствие близко
        avoid_obstacle()

def move_forward():
    """Едем прямо"""
    speed = 2.0  # скорость в радианах/секунду
    left_motor.setVelocity(speed)
    right_motor.setVelocity(speed)

def turn_left():
    """Поворачиваем налево"""
    left_motor.setVelocity(0.5)    # левое медленно
    right_motor.setVelocity(2.0)   # правое быстро

def turn_right():
    """Поворачиваем направо"""  
    left_motor.setVelocity(2.0)    # левое быстро
    right_motor.setVelocity(0.5)   # правое медленно

def stop():
    """Останавливаемся"""
    left_motor.setVelocity(0)
    right_motor.setVelocity(0)

class PIDController:
    def __init__(self, kp=1.0, ki=0.1, kd=0.05):
        self.kp = kp  # пропорциональный коэффициент
        self.ki = ki  # интегральный коэффициент
        self.kd = kd  # дифференциальный коэффициент
        self.prev_error = 0
        self.integral = 0
        self.dt = 0.032  # шаг времени (32 мс)
        
    def compute(self, target, current):
        error = target - current
        
        # Пропорциональная составляющая
        proportional = self.kp * error
        
        # Интегральная составляющая
        self.integral += error * self.dt
        # Ограничение интеграла (anti-windup)
        self.integral = max(-10, min(10, self.integral))
        integral = self.ki * self.integral
        
        # Дифференциальная составляющая
        derivative = self.kd * (error - self.prev_error) / self.dt
        
        # Итоговый выход
        output = proportional + integral + derivative
        self.prev_error = error
        
        return output

# Пример использования для точного поворота
angle_pid = PIDController(kp=2.0, ki=0.5, kd=0.1)
target_angle = 90  # хотим повернуть на 90 градусов

while abs(current_angle - target_angle) > 1:  # точность 1°
    correction = angle_pid.compute(target_angle, current_angle)
    
    base_speed = 1.0
    left_motor.setVelocity(base_speed - correction)
    right_motor.setVelocity(base_speed + correction)
    
    robot.step(timestep)

def avoid_obstacle():
    """Объезжаем препятствие по алгоритму 'правой руки'"""
    
    # Останавливаемся
    stop()
    time.sleep(0.5)  # пауза на полсекунды
    
    # Поворачиваем направо
    turn_right()
    time.sleep(1.0)  # поворачиваем 1 секунду
    
    # Едем вперед
    move_forward() 
    time.sleep(2.0)  # едем 2 секунды
    
    # Поворачиваем налево (возвращаемся к исходному направлению)
    turn_left()
    time.sleep(1.0)
    
    # Продолжаем ехать прямо
    move_forward()

def find_red_cargo():
    """Ищем красный груз с помощью камеры"""
    
    image = camera.getImage()
    width = camera.getWidth()
    height = camera.getHeight()
    
    red_pixels = 0
    red_center_x = 0
    
    # Сканируем каждый пиксель
    for x in range(width):
        for y in range(height):
            # Получаем красную составляющую пикселя
            red_value = camera.imageGetRed(image, width, x, y)
            
            if red_value > 200:  # это красный пиксель
                red_pixels += 1
                red_center_x += x
    
    if red_pixels > 100:  # нашли достаточно красных пикселей
        # Вычисляем центр красного объекта
        red_center_x = red_center_x / red_pixels
        
        # Определяем, куда поворачивать
        image_center = width / 2
        if red_center_x < image_center - 20:
            return "turn_left"
        elif red_center_x > image_center + 20:
            return "turn_right"
        else:
            return "move_forward"  # груз прямо по курсу
    
    return "search"  # груз не найден, продолжаем поиск

def pickup_cargo():
    """Подбираем груз"""
    
    # Подъезжаем вплотную
    while distance_sensor.getValue() > 0.1:  # 10 см до груза
        move_forward_slowly()
        robot.step(timestep)
    
    stop()
    
    # Активируем захват (если есть)
    gripper = robot.getDevice('gripper')
    gripper.setPosition(0.0)  # закрываем захват
    
    time.sleep(1.0)  # ждем секунду
    
    # Проверяем, захватили ли груз
    force_sensor = robot.getDevice('force_sensor')
    if force_sensor.getValue() > 0.5:  # чувствуем вес груза
        print("Груз захвачен!")
        return True
    else:
        print("Груз не захвачен, пробуем еще раз")
        return False

def rgb_to_hsv(r, g, b):
    """Преобразование RGB в HSV"""
    r, g, b = r/255.0, g/255.0, b/255.0
    
    max_val = max(r, g, b)
    min_val = min(r, g, b)
    diff = max_val - min_val
    
    # Вычисляем оттенок (Hue)
    if diff == 0:
        h = 0
    elif max_val == r:
        h = (60 * ((g - b) / diff) + 360) % 360
    elif max_val == g:
        h = (60 * ((b - r) / diff) + 120) % 360
    else:
        h = (60 * ((r - g) / diff) + 240) % 360
    
    # Вычисляем насыщенность (Saturation)
    s = 0 if max_val == 0 else diff / max_val
    
    # Яркость (Value)
    v = max_val
    
    return h, s * 100, v * 100

def detect_red_object_hsv(image):
    """Более точное обнаружение красного объекта"""
    
    red_pixels = []
    
    for x in range(camera.getWidth()):
        for y in range(camera.getHeight()):
            r = camera.imageGetRed(image, camera.getWidth(), x, y)
            g = camera.imageGetGreen(image, camera.getWidth(), x, y)
            b = camera.imageGetBlue(image, camera.getWidth(), x, y)
            
            h, s, v = rgb_to_hsv(r, g, b)
            
            # Красный цвет в HSV: H=0-10 или H=350-360, S>50, V>50
            if ((h >= 0 and h <= 10) or (h >= 350 and h <= 360)) and s > 50 and v > 50:
                red_pixels.append((x, y))
    
    return red_pixels

# Кластеризация пикселей для нахождения объектов
def find_object_clusters(pixels, min_cluster_size=50):
    """Группируем близкие пиксели в объекты"""
    
    if not pixels:
        return []
    
    clusters = []
    used = set()
    
    for pixel in pixels:
        if pixel in used:
            continue
            
        # Начинаем новый кластер
        cluster = [pixel]
        queue = [pixel]
        used.add(pixel)
        
        while queue:
            current = queue.pop(0)
            x, y = current
            
            # Ищем соседние красные пиксели
            for dx in [-1, 0, 1]:
                for dy in [-1, 0, 1]:
                    neighbor = (x + dx, y + dy)
                    if neighbor in pixels and neighbor not in used:
                        cluster.append(neighbor)
                        queue.append(neighbor)
                        used.add(neighbor)
        
        # Добавляем кластер, если он достаточно большой
        if len(cluster) >= min_cluster_size:
            clusters.append(cluster)
    
    return clusters

def test_basic_movement():
    """Проверяем, умеет ли робот двигаться"""
    
    print("Тест 1: Движение вперед")
    start_position = get_robot_position()
    
    move_forward()
    time.sleep(2.0)  # едем 2 секунды
    stop()
    
    end_position = get_robot_position()
    distance_traveled = calculate_distance(start_position, end_position)
    
    print(f"Проехали: {distance_traveled:.2f} метра")
    
    if distance_traveled > 0.5:
        print("✅ ТЕСТ ПРОЙДЕН")
        return True
    else:
        print("❌ ТЕСТ НЕ ПРОЙДЕН")
        return False

def test_sensors():
    """Проверяем работу датчиков"""
    
    print("Тест 2: Датчики")
    
    # Тест ультразвукового датчика
    distance = distance_sensor.getValue()
    print(f"Расстояние до препятствия: {distance:.2f} м")
    
    if 0.1 < distance < 5.0:
        print("✅ Ультразвуковой датчик работает")
    else:
        print("❌ Проблема с ультразвуковым датчиком")
    
    # Тест камеры
    image = camera.getImage()
    if image is not None:
        print("✅ Камера работает")
    else:
        print("❌ Проблема с камерой")

import statistics

def statistical_testing(num_tests=10):
    """Проводим серию тестов и анализируем результаты"""
    
    results = {
        'mission_times': [],
        'success_count': 0,
        'collision_count': [],
        'path_lengths': []
    }
    
    for test_num in range(num_tests):
        print(f"\n🧪 Тест {test_num + 1}/{num_tests}")
        
        # Сброс в исходное состояние
        reset_robot_to_start()
        
        # Запуск миссии
        start_time = time.time()
        collisions = 0
        path_length = 0
        
        mission_success = run_transport_mission()
        end_time = time.time()
        
        # Сбор метрик
        mission_time = end_time - start_time
        results['mission_times'].append(mission_time)
        
        if mission_success:
            results['success_count'] += 1
        
        results['collision_count'].append(get_collision_count())
        results['path_lengths'].append(get_path_length())
    
    # Статистический анализ
    print("\n📊 СТАТИСТИКА ТЕСТИРОВАНИЯ:")
    print(f"Успешность: {results['success_count']}/{num_tests} ({results['success_count']/num_tests*100:.1f}%)")
    
    if results['mission_times']:
        avg_time = statistics.mean(results['mission_times'])
        std_time = statistics.stdev(results['mission_times']) if len(results['mission_times']) > 1 else 0
        print(f"Среднее время: {avg_time:.1f}±{std_time:.1f} сек")
        print(f"Лучшее время: {min(results['mission_times']):.1f} сек")
        print(f"Худшее время: {max(results['mission_times']):.1f} сек")
    
    avg_collisions = statistics.mean(results['collision_count'])
    print(f"Среднее количество столкновений: {avg_collisions:.1f}")
    
    return results

def calculate_confidence_interval(data, confidence=0.95):
    """Вычисляем доверительный интервал"""
    import scipy.stats as stats
    
    n = len(data)
    mean = statistics.mean(data)
    std_error = statistics.stdev(data) / (n ** 0.5)
    
    # t-распределение для малых выборок
    t_value = stats.t.ppf((1 + confidence) / 2, n - 1)
    margin_error = t_value * std_error
    
    return mean - margin_error, mean + margin_error

# Benchmark разных алгоритмов
def benchmark_algorithms():
    """Сравниваем производительность разных алгоритмов"""
    
    algorithms = [
        ("Простое избежание", simple_obstacle_avoidance),
        ("Правая рука", right_hand_rule),
        ("Потенциальные поля", potential_fields_navigation),
        ("A* планирование", astar_navigation)
    ]
    
    results = {}
    
    for name, algorithm in algorithms:
        print(f"\n🔬 Тестируем алгоритм: {name}")
        
        times = []
        success_rate = 0
        
        for _ in range(5):  # 5 прогонов каждого алгоритма
            reset_environment()
            
            start_time = time.time()
            success = algorithm()
            end_time = time.time()
            
            if success:
                times.append(end_time - start_time)
                success_rate += 1
        
        results[name] = {
            'avg_time': statistics.mean(times) if times else float('inf'),
            'success_rate': success_rate / 5 * 100,
            'best_time': min(times) if times else float('inf')
        }
        
        print(f"  Успешность: {success_rate}/5 ({success_rate/5*100:.0f}%)")
        if times:
            print(f"  Среднее время: {statistics.mean(times):.1f} сек")
            print(f"  Лучшее время: {min(times):.1f} сек")
    
    # Ранжирование алгоритмов
    print("\n🏆 РЕЙТИНГ АЛГОРИТМОВ:")
    sorted_algorithms = sorted(results.items(), 
                              key=lambda x: (x[1]['success_rate'], -x[1]['avg_time']), 
                              reverse=True)
    
    for i, (name, metrics) in enumerate(sorted_algorithms, 1):
        print(f"{i}. {name}: {metrics['success_rate']:.0f}% успех, {metrics['avg_time']:.1f}с время")
    
    return results

def spiral_cargo_search():
    """Систематический поиск груза по расширяющейся спирали"""
    
    search_radius = 0.5  # начальный радиус поиска
    angle_step = 30      # шаг поворота в градусах
    max_radius = 3.0     # максимальный радиус поиска
    
    while search_radius <= max_radius:
        print(f"Поиск в радиусе {search_radius:.1f} м")
        
        # Полный оборот на текущем радиусе
        for angle in range(0, 360, angle_step):
            turn_to_angle(angle)
            
            # Проверяем, видим ли груз
            cargo_direction = find_red_cargo()
            if cargo_direction == "move_forward":
                print("🎯 Груз обнаружен!")
                return approach_cargo()
        
        # Увеличиваем радиус поиска
        move_forward_distance(0.3)
        search_radius += 0.3
    
    print("❌ Груз не найден в зоне поиска")
    return False

def approach_cargo():
    """Точный подход к грузу с корректировкой"""
    
    approach_attempts = 0
    max_attempts = 10
    
    while approach_attempts < max_attempts:
        cargo_direction = find_red_cargo()
        distance = distance_sensor.getValue()
        
        if distance < 0.15:  # достаточно близко для захвата
            return pickup_cargo()
        
        elif cargo_direction == "move_forward":
            move_forward_slowly()
        elif cargo_direction == "turn_left":
            turn_left()
            time.sleep(0.2)
        elif cargo_direction == "turn_right":
            turn_right()
            time.sleep(0.2)
        else:
            # Груз потерялся, делаем небольшой поиск
            for _ in range(4):
                turn_right()
                time.sleep(0.5)
                if find_red_cargo() != "search":
                    break
        
        approach_attempts += 1
        robot.step(timestep)
    
    return False

class AdaptiveSpeedController:
    def __init__(self):
        self.base_speed = 2.0
        self.current_speed = self.base_speed
        self.obstacle_history = []
        self.max_history = 10
        
    def calculate_optimal_speed(self, front_distance, side_distances):
        """Вычисляем оптимальную скорость на основе окружения"""
        
        # Базовая скорость зависит от расстояния до препятствий
        min_distance = min(front_distance, min(side_distances))
        
        if min_distance > 2.0:
            target_speed = self.base_speed  # полная скорость
        elif min_distance > 1.0:
            target_speed = self.base_speed * 0.7  # средняя скорость
        elif min_distance > 0.5:
            target_speed = self.base_speed * 0.4  # медленная скорость
        else:
            target_speed = 0.2  # очень медленно
        
        # Учитываем историю препятствий
        self.obstacle_history.append(min_distance)
        if len(self.obstacle_history) > self.max_history:
            self.obstacle_history.pop(0)
        
        # Если часто встречаем препятствия, снижаем базовую скорость
        avg_distance = sum(self.obstacle_history) / len(self.obstacle_history)
        if avg_distance < 1.0:
            target_speed *= 0.8  # адаптивное снижение
        
        # Плавное изменение скорости
        speed_diff = target_speed - self.current_speed
        self.current_speed += speed_diff * 0.3  # коэффициент сглаживания
        
        return self.current_speed
    
    def emergency_brake(self, distance):
        """Экстренное торможение при критическом сближении"""
        if distance < 0.2:  # критическая дистанция
            return 0  # полная остановка
        elif distance < 0.3:
            return self.current_speed * 0.1  # резкое замедление
        else:
            return self.current_speed

# Псевдокод этического алгоритма для робота
def ethical_decision_making(situation):
    """Этический алгоритм принятия решений"""
    
    # Приоритет 1: Безопасность человека
    if human_safety_at_risk(situation):
        return prioritize_human_safety()
    
    # Приоритет 2: Минимизация вреда
    options = generate_possible_actions(situation)
    harm_scores = [calculate_potential_harm(option) for option in options]
    best_option = options[harm_scores.index(min(harm_scores))]
    
    # Приоритет 3: Уважение к человеческой автономии
    if requires_human_consent(best_option):
        return request_human_approval(best_option)
    
    return best_option

# Принципы этичного ИИ:
ETHICAL_PRINCIPLES = [
    "Прозрачность: действия робота должны быть объяснимы",
    "Справедливость: отсутствие дискриминации",
    "Ответственность: четкое определение ответственных лиц",
    "Приватность: защита персональных данных",
    "Благополучие: максимизация пользы для общества"
]

def implement_astar():
    """Реализуйте алгоритм A* для поиска оптимального пути"""
    
    # Задача: создать функцию, которая находит кратчайший путь
    # от стартовой точки до цели, обходя препятствия
    
    # Подсказки:
    # 1. Используйте эвристическую функцию (например, Манхэттенское расстояние)
    # 2. Ведите списки открытых и закрытых узлов
    # 3. Для каждого узла храните g(n), h(n) и f(n) = g(n) + h(n)
    
    pass  # Ваша реализация

def tune_pid_controller():
    """Настройте ПИД-регулятор для точного следования по линии"""
    
    # Задача: найти оптимальные коэффициенты Kp, Ki, Kd
    # для следования робота по черной линии
    
    # Критерии качества:
    # - Минимальное отклонение от линии
    # - Отсутствие колебаний
    # - Быстрая реакция на повороты
    
    kp_range = [0.5, 1.0, 1.5, 2.0]
    ki_range = [0.0, 0.1, 0.2, 0.3]
    kd_range = [0.0, 0.05, 0.1, 0.15]
    
    # Проведите серию экспериментов и найдите лучшие параметры
    pass  # Ваша реализация