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

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

 1PROTO TransportRobot [
 2  field SFVec3f translation 0 0 0.05
 3  field SFString name "transport_robot"
 4]
 5{
 6  Robot {
 7    translation IS translation
 8    name IS name
 9    children [
10      # Основной корпус
11      DEF BODY Transform {
12        children [
13          Shape {
14            geometry Box { size 0.30 0.20 0.15 }
15            appearance PBRAppearance {
16              baseColor 0.3 0.3 0.8
17            }
18          }
19        ]
20      }
21      
22      # Левое колесо с мотором
23      DEF LEFT_WHEEL HingeJoint {
24        jointParameters HingeJointParameters {
25          axis 0 1 0
26          anchor -0.075 0.12 0
27        }
28        device RotationalMotor {
29          name "left_motor"
30          maxVelocity 10
31        }
32        endPoint Solid {
33          translation -0.075 0.12 0
34          children [
35            Shape {
36              geometry Cylinder { height 0.025 radius 0.04 }
37            }
38          ]
39          physics Physics { density 1200 }
40        }
41      }
42    ]
43    
44    physics Physics {
45      density -1
46      mass 1.5
47      centerOfMass [0 0 0.02]
48    }
49  }
50}

 1# Подключаем библиотеки
 2from controller import Robot, Motor, DistanceSensor, Camera
 3
 4# Создаем робота
 5robot = Robot()
 6timestep = 32  # миллисекунд между "мыслями" робота
 7
 8# Подключаем устройства
 9left_motor = robot.getDevice('left_motor')
10right_motor = robot.getDevice('right_motor')
11distance_sensor = robot.getDevice('distance_sensor')
12camera = robot.getDevice('camera')
13
14# Настраиваем моторы
15left_motor.setPosition(float('inf'))  # бесконечное вращение
16right_motor.setPosition(float('inf'))
17
18# Включаем датчики
19distance_sensor.enable(timestep)
20camera.enable(timestep)
21
22# Основной цикл - "мышление" робота
23while robot.step(timestep) != -1:
24    # Читаем датчики
25    distance = distance_sensor.getValue()
26    
27    # Принимаем решение
28    if distance > 0.5:  # путь свободен (больше 50 см)
29        move_forward()
30    else:  # препятствие близко
31        avoid_obstacle()

 1def move_forward():
 2    """Едем прямо"""
 3    speed = 2.0  # скорость в радианах/секунду
 4    left_motor.setVelocity(speed)
 5    right_motor.setVelocity(speed)
 6
 7def turn_left():
 8    """Поворачиваем налево"""
 9    left_motor.setVelocity(0.5)    # левое медленно
10    right_motor.setVelocity(2.0)   # правое быстро
11
12def turn_right():
13    """Поворачиваем направо"""  
14    left_motor.setVelocity(2.0)    # левое быстро
15    right_motor.setVelocity(0.5)   # правое медленно
16
17def stop():
18    """Останавливаемся"""
19    left_motor.setVelocity(0)
20    right_motor.setVelocity(0)

 1class PIDController:
 2    def __init__(self, kp=1.0, ki=0.1, kd=0.05):
 3        self.kp = kp  # пропорциональный коэффициент
 4        self.ki = ki  # интегральный коэффициент
 5        self.kd = kd  # дифференциальный коэффициент
 6        self.prev_error = 0
 7        self.integral = 0
 8        self.dt = 0.032  # шаг времени (32 мс)
 9        
10    def compute(self, target, current):
11        error = target - current
12        
13        # Пропорциональная составляющая
14        proportional = self.kp * error
15        
16        # Интегральная составляющая
17        self.integral += error * self.dt
18        # Ограничение интеграла (anti-windup)
19        self.integral = max(-10, min(10, self.integral))
20        integral = self.ki * self.integral
21        
22        # Дифференциальная составляющая
23        derivative = self.kd * (error - self.prev_error) / self.dt
24        
25        # Итоговый выход
26        output = proportional + integral + derivative
27        self.prev_error = error
28        
29        return output
30
31# Пример использования для точного поворота
32angle_pid = PIDController(kp=2.0, ki=0.5, kd=0.1)
33target_angle = 90  # хотим повернуть на 90 градусов
34
35while abs(current_angle - target_angle) > 1:  # точность 1°
36    correction = angle_pid.compute(target_angle, current_angle)
37    
38    base_speed = 1.0
39    left_motor.setVelocity(base_speed - correction)
40    right_motor.setVelocity(base_speed + correction)
41    
42    robot.step(timestep)

 1def avoid_obstacle():
 2    """Объезжаем препятствие по алгоритму 'правой руки'"""
 3    
 4    # Останавливаемся
 5    stop()
 6    time.sleep(0.5)  # пауза на полсекунды
 7    
 8    # Поворачиваем направо
 9    turn_right()
10    time.sleep(1.0)  # поворачиваем 1 секунду
11    
12    # Едем вперед
13    move_forward() 
14    time.sleep(2.0)  # едем 2 секунды
15    
16    # Поворачиваем налево (возвращаемся к исходному направлению)
17    turn_left()
18    time.sleep(1.0)
19    
20    # Продолжаем ехать прямо
21    move_forward()

 1def find_red_cargo():
 2    """Ищем красный груз с помощью камеры"""
 3    
 4    image = camera.getImage()
 5    width = camera.getWidth()
 6    height = camera.getHeight()
 7    
 8    red_pixels = 0
 9    red_center_x = 0
10    
11    # Сканируем каждый пиксель
12    for x in range(width):
13        for y in range(height):
14            # Получаем красную составляющую пикселя
15            red_value = camera.imageGetRed(image, width, x, y)
16            
17            if red_value > 200:  # это красный пиксель
18                red_pixels += 1
19                red_center_x += x
20    
21    if red_pixels > 100:  # нашли достаточно красных пикселей
22        # Вычисляем центр красного объекта
23        red_center_x = red_center_x / red_pixels
24        
25        # Определяем, куда поворачивать
26        image_center = width / 2
27        if red_center_x < image_center - 20:
28            return "turn_left"
29        elif red_center_x > image_center + 20:
30            return "turn_right"
31        else:
32            return "move_forward"  # груз прямо по курсу
33    
34    return "search"  # груз не найден, продолжаем поиск

 1def pickup_cargo():
 2    """Подбираем груз"""
 3    
 4    # Подъезжаем вплотную
 5    while distance_sensor.getValue() > 0.1:  # 10 см до груза
 6        move_forward_slowly()
 7        robot.step(timestep)
 8    
 9    stop()
10    
11    # Активируем захват (если есть)
12    gripper = robot.getDevice('gripper')
13    gripper.setPosition(0.0)  # закрываем захват
14    
15    time.sleep(1.0)  # ждем секунду
16    
17    # Проверяем, захватили ли груз
18    force_sensor = robot.getDevice('force_sensor')
19    if force_sensor.getValue() > 0.5:  # чувствуем вес груза
20        print("Груз захвачен!")
21        return True
22    else:
23        print("Груз не захвачен, пробуем еще раз")
24        return False

 1def rgb_to_hsv(r, g, b):
 2    """Преобразование RGB в HSV"""
 3    r, g, b = r/255.0, g/255.0, b/255.0
 4    
 5    max_val = max(r, g, b)
 6    min_val = min(r, g, b)
 7    diff = max_val - min_val
 8    
 9    # Вычисляем оттенок (Hue)
10    if diff == 0:
11        h = 0
12    elif max_val == r:
13        h = (60 * ((g - b) / diff) + 360) % 360
14    elif max_val == g:
15        h = (60 * ((b - r) / diff) + 120) % 360
16    else:
17        h = (60 * ((r - g) / diff) + 240) % 360
18    
19    # Вычисляем насыщенность (Saturation)
20    s = 0 if max_val == 0 else diff / max_val
21    
22    # Яркость (Value)
23    v = max_val
24    
25    return h, s * 100, v * 100
26
27def detect_red_object_hsv(image):
28    """Более точное обнаружение красного объекта"""
29    
30    red_pixels = []
31    
32    for x in range(camera.getWidth()):
33        for y in range(camera.getHeight()):
34            r = camera.imageGetRed(image, camera.getWidth(), x, y)
35            g = camera.imageGetGreen(image, camera.getWidth(), x, y)
36            b = camera.imageGetBlue(image, camera.getWidth(), x, y)
37            
38            h, s, v = rgb_to_hsv(r, g, b)
39            
40            # Красный цвет в HSV: H=0-10 или H=350-360, S>50, V>50
41            if ((h >= 0 and h <= 10) or (h >= 350 and h <= 360)) and s > 50 and v > 50:
42                red_pixels.append((x, y))
43    
44    return red_pixels
45
46# Кластеризация пикселей для нахождения объектов
47def find_object_clusters(pixels, min_cluster_size=50):
48    """Группируем близкие пиксели в объекты"""
49    
50    if not pixels:
51        return []
52    
53    clusters = []
54    used = set()
55    
56    for pixel in pixels:
57        if pixel in used:
58            continue
59            
60        # Начинаем новый кластер
61        cluster = [pixel]
62        queue = [pixel]
63        used.add(pixel)
64        
65        while queue:
66            current = queue.pop(0)
67            x, y = current
68            
69            # Ищем соседние красные пиксели
70            for dx in [-1, 0, 1]:
71                for dy in [-1, 0, 1]:
72                    neighbor = (x + dx, y + dy)
73                    if neighbor in pixels and neighbor not in used:
74                        cluster.append(neighbor)
75                        queue.append(neighbor)
76                        used.add(neighbor)
77        
78        # Добавляем кластер, если он достаточно большой
79        if len(cluster) >= min_cluster_size:
80            clusters.append(cluster)
81    
82    return clusters

 1def test_basic_movement():
 2    """Проверяем, умеет ли робот двигаться"""
 3    
 4    print("Тест 1: Движение вперед")
 5    start_position = get_robot_position()
 6    
 7    move_forward()
 8    time.sleep(2.0)  # едем 2 секунды
 9    stop()
10    
11    end_position = get_robot_position()
12    distance_traveled = calculate_distance(start_position, end_position)
13    
14    print(f"Проехали: {distance_traveled:.2f} метра")
15    
16    if distance_traveled > 0.5:
17        print("✅ ТЕСТ ПРОЙДЕН")
18        return True
19    else:
20        print("❌ ТЕСТ НЕ ПРОЙДЕН")
21        return False

 1def test_sensors():
 2    """Проверяем работу датчиков"""
 3    
 4    print("Тест 2: Датчики")
 5    
 6    # Тест ультразвукового датчика
 7    distance = distance_sensor.getValue()
 8    print(f"Расстояние до препятствия: {distance:.2f} м")
 9    
10    if 0.1 < distance < 5.0:
11        print("✅ Ультразвуковой датчик работает")
12    else:
13        print("❌ Проблема с ультразвуковым датчиком")
14    
15    # Тест камеры
16    image = camera.getImage()
17    if image is not None:
18        print("✅ Камера работает")
19    else:
20        print("❌ Проблема с камерой")

  1import statistics
  2
  3def statistical_testing(num_tests=10):
  4    """Проводим серию тестов и анализируем результаты"""
  5    
  6    results = {
  7        'mission_times': [],
  8        'success_count': 0,
  9        'collision_count': [],
 10        'path_lengths': []
 11    }
 12    
 13    for test_num in range(num_tests):
 14        print(f"\n🧪 Тест {test_num + 1}/{num_tests}")
 15        
 16        # Сброс в исходное состояние
 17        reset_robot_to_start()
 18        
 19        # Запуск миссии
 20        start_time = time.time()
 21        collisions = 0
 22        path_length = 0
 23        
 24        mission_success = run_transport_mission()
 25        end_time = time.time()
 26        
 27        # Сбор метрик
 28        mission_time = end_time - start_time
 29        results['mission_times'].append(mission_time)
 30        
 31        if mission_success:
 32            results['success_count'] += 1
 33        
 34        results['collision_count'].append(get_collision_count())
 35        results['path_lengths'].append(get_path_length())
 36    
 37    # Статистический анализ
 38    print("\n📊 СТАТИСТИКА ТЕСТИРОВАНИЯ:")
 39    print(f"Успешность: {results['success_count']}/{num_tests} ({results['success_count']/num_tests*100:.1f}%)")
 40    
 41    if results['mission_times']:
 42        avg_time = statistics.mean(results['mission_times'])
 43        std_time = statistics.stdev(results['mission_times']) if len(results['mission_times']) > 1 else 0
 44        print(f"Среднее время: {avg_time:.1f}±{std_time:.1f} сек")
 45        print(f"Лучшее время: {min(results['mission_times']):.1f} сек")
 46        print(f"Худшее время: {max(results['mission_times']):.1f} сек")
 47    
 48    avg_collisions = statistics.mean(results['collision_count'])
 49    print(f"Среднее количество столкновений: {avg_collisions:.1f}")
 50    
 51    return results
 52
 53def calculate_confidence_interval(data, confidence=0.95):
 54    """Вычисляем доверительный интервал"""
 55    import scipy.stats as stats
 56    
 57    n = len(data)
 58    mean = statistics.mean(data)
 59    std_error = statistics.stdev(data) / (n ** 0.5)
 60    
 61    # t-распределение для малых выборок
 62    t_value = stats.t.ppf((1 + confidence) / 2, n - 1)
 63    margin_error = t_value * std_error
 64    
 65    return mean - margin_error, mean + margin_error
 66
 67# Benchmark разных алгоритмов
 68def benchmark_algorithms():
 69    """Сравниваем производительность разных алгоритмов"""
 70    
 71    algorithms = [
 72        ("Простое избежание", simple_obstacle_avoidance),
 73        ("Правая рука", right_hand_rule),
 74        ("Потенциальные поля", potential_fields_navigation),
 75        ("A* планирование", astar_navigation)
 76    ]
 77    
 78    results = {}
 79    
 80    for name, algorithm in algorithms:
 81        print(f"\n🔬 Тестируем алгоритм: {name}")
 82        
 83        times = []
 84        success_rate = 0
 85        
 86        for _ in range(5):  # 5 прогонов каждого алгоритма
 87            reset_environment()
 88            
 89            start_time = time.time()
 90            success = algorithm()
 91            end_time = time.time()
 92            
 93            if success:
 94                times.append(end_time - start_time)
 95                success_rate += 1
 96        
 97        results[name] = {
 98            'avg_time': statistics.mean(times) if times else float('inf'),
 99            'success_rate': success_rate / 5 * 100,
100            'best_time': min(times) if times else float('inf')
101        }
102        
103        print(f"  Успешность: {success_rate}/5 ({success_rate/5*100:.0f}%)")
104        if times:
105            print(f"  Среднее время: {statistics.mean(times):.1f} сек")
106            print(f"  Лучшее время: {min(times):.1f} сек")
107    
108    # Ранжирование алгоритмов
109    print("\n🏆 РЕЙТИНГ АЛГОРИТМОВ:")
110    sorted_algorithms = sorted(results.items(), 
111                              key=lambda x: (x[1]['success_rate'], -x[1]['avg_time']), 
112                              reverse=True)
113    
114    for i, (name, metrics) in enumerate(sorted_algorithms, 1):
115        print(f"{i}. {name}: {metrics['success_rate']:.0f}% успех, {metrics['avg_time']:.1f}с время")
116    
117    return results

 1def spiral_cargo_search():
 2    """Систематический поиск груза по расширяющейся спирали"""
 3    
 4    search_radius = 0.5  # начальный радиус поиска
 5    angle_step = 30      # шаг поворота в градусах
 6    max_radius = 3.0     # максимальный радиус поиска
 7    
 8    while search_radius <= max_radius:
 9        print(f"Поиск в радиусе {search_radius:.1f} м")
10        
11        # Полный оборот на текущем радиусе
12        for angle in range(0, 360, angle_step):
13            turn_to_angle(angle)
14            
15            # Проверяем, видим ли груз
16            cargo_direction = find_red_cargo()
17            if cargo_direction == "move_forward":
18                print("🎯 Груз обнаружен!")
19                return approach_cargo()
20        
21        # Увеличиваем радиус поиска
22        move_forward_distance(0.3)
23        search_radius += 0.3
24    
25    print("❌ Груз не найден в зоне поиска")
26    return False
27
28def approach_cargo():
29    """Точный подход к грузу с корректировкой"""
30    
31    approach_attempts = 0
32    max_attempts = 10
33    
34    while approach_attempts < max_attempts:
35        cargo_direction = find_red_cargo()
36        distance = distance_sensor.getValue()
37        
38        if distance < 0.15:  # достаточно близко для захвата
39            return pickup_cargo()
40        
41        elif cargo_direction == "move_forward":
42            move_forward_slowly()
43        elif cargo_direction == "turn_left":
44            turn_left()
45            time.sleep(0.2)
46        elif cargo_direction == "turn_right":
47            turn_right()
48            time.sleep(0.2)
49        else:
50            # Груз потерялся, делаем небольшой поиск
51            for _ in range(4):
52                turn_right()
53                time.sleep(0.5)
54                if find_red_cargo() != "search":
55                    break
56        
57        approach_attempts += 1
58        robot.step(timestep)
59    
60    return False

 1class AdaptiveSpeedController:
 2    def __init__(self):
 3        self.base_speed = 2.0
 4        self.current_speed = self.base_speed
 5        self.obstacle_history = []
 6        self.max_history = 10
 7        
 8    def calculate_optimal_speed(self, front_distance, side_distances):
 9        """Вычисляем оптимальную скорость на основе окружения"""
10        
11        # Базовая скорость зависит от расстояния до препятствий
12        min_distance = min(front_distance, min(side_distances))
13        
14        if min_distance > 2.0:
15            target_speed = self.base_speed  # полная скорость
16        elif min_distance > 1.0:
17            target_speed = self.base_speed * 0.7  # средняя скорость
18        elif min_distance > 0.5:
19            target_speed = self.base_speed * 0.4  # медленная скорость
20        else:
21            target_speed = 0.2  # очень медленно
22        
23        # Учитываем историю препятствий
24        self.obstacle_history.append(min_distance)
25        if len(self.obstacle_history) > self.max_history:
26            self.obstacle_history.pop(0)
27        
28        # Если часто встречаем препятствия, снижаем базовую скорость
29        avg_distance = sum(self.obstacle_history) / len(self.obstacle_history)
30        if avg_distance < 1.0:
31            target_speed *= 0.8  # адаптивное снижение
32        
33        # Плавное изменение скорости
34        speed_diff = target_speed - self.current_speed
35        self.current_speed += speed_diff * 0.3  # коэффициент сглаживания
36        
37        return self.current_speed
38    
39    def emergency_brake(self, distance):
40        """Экстренное торможение при критическом сближении"""
41        if distance < 0.2:  # критическая дистанция
42            return 0  # полная остановка
43        elif distance < 0.3:
44            return self.current_speed * 0.1  # резкое замедление
45        else:
46            return self.current_speed

 1# Псевдокод этического алгоритма для робота
 2def ethical_decision_making(situation):
 3    """Этический алгоритм принятия решений"""
 4    
 5    # Приоритет 1: Безопасность человека
 6    if human_safety_at_risk(situation):
 7        return prioritize_human_safety()
 8    
 9    # Приоритет 2: Минимизация вреда
10    options = generate_possible_actions(situation)
11    harm_scores = [calculate_potential_harm(option) for option in options]
12    best_option = options[harm_scores.index(min(harm_scores))]
13    
14    # Приоритет 3: Уважение к человеческой автономии
15    if requires_human_consent(best_option):
16        return request_human_approval(best_option)
17    
18    return best_option
19
20# Принципы этичного ИИ:
21ETHICAL_PRINCIPLES = [
22    "Прозрачность: действия робота должны быть объяснимы",
23    "Справедливость: отсутствие дискриминации",
24    "Ответственность: четкое определение ответственных лиц",
25    "Приватность: защита персональных данных",
26    "Благополучие: максимизация пользы для общества"
27]

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

 1def tune_pid_controller():
 2    """Настройте ПИД-регулятор для точного следования по линии"""
 3    
 4    # Задача: найти оптимальные коэффициенты Kp, Ki, Kd
 5    # для следования робота по черной линии
 6    
 7    # Критерии качества:
 8    # - Минимальное отклонение от линии
 9    # - Отсутствие колебаний
10    # - Быстрая реакция на повороты
11    
12    kp_range = [0.5, 1.0, 1.5, 2.0]
13    ki_range = [0.0, 0.1, 0.2, 0.3]
14    kd_range = [0.0, 0.05, 0.1, 0.15]
15    
16    # Проведите серию экспериментов и найдите лучшие параметры
17    pass  # Ваша реализация