Глава 11. Полное прохождение задания

1. Цель занятия и обзор плана

На заключительном занятии курса вы соберёте воедино все навыки, полученные на предыдущих уроках: работу с камерой, цветовую сегментацию, нейросетевую детекцию, регуляторы и машину состояний. Задача — полностью автономно пройти соревновательную трассу: найти метку, пройти через ворота и припарковаться в гараже.

После завершения занятия вы сможете:

  • спроектировать машину состояний для комплексного задания;

  • интегрировать модули компьютерного зрения (цветовая сегментация + YOLO) в единую систему;

  • реализовать стратегию автономного прохождения задания;

  • отладить взаимодействие между состояниями, детекцией и управлением.

1.1. Обзор плана занятия

  1. Описание задания и ограничения.

  2. Этап 1 — поиск метки на дне.

  3. Этап 2 — движение к воротам и их прохождение.

  4. Этап 3 — поиск гаража и парковка.

  5. Структура рабочего пакета.

2. Описание задания

Задание воспроизводит дисциплину открытых первенств Санкт-Петербурга по морской робототехнике, организованных СПбГМТУ. Катамаран должен автономно выполнить три последовательных этапа:

  1. Найти метку на дне — определить цветную фигуру (красный треугольник, жёлтый круг или зелёный квадрат), которая задаёт целевой гараж.

  2. Пройти через ворота — обнаружить два буя (красный и жёлтый) и пройти между ними.

  3. Припарковаться в гараже — найти нужный гараж по запомненной фигуре и заехать в него.

2.1. Ограничения

Эти ограничения важно понимать при проектировании стратегии:

  • Нет абсолютной позиции. У катамарана нет GNSS или другой системы позиционирования — навигация только по камерам. Вы не знаете, где находитесь. Единственный ориентир — то, что видит камера.

  • Ограниченные вычислительные мощности. YOLO на бортовом компьютере обрабатывает не более 15 кадров в секунду. Это значит, что решения нужно принимать с учётом задержки.

  • Объекты не имеют точной позиции. Из попытки в попытку объекты могут смещаться. Нельзя заранее «запомнить» координаты ворот — их нужно искать каждый раз.

  • Окружающие условия могут меняться. Освещение, блики на воде, случайные объекты в поле зрения — всё это источники помех.

3. Этап 1. Поиск метки на дне

На дне по ходу движения катамарана расположена фигура на листе A4: красный треугольник, жёлтый круг или зелёный квадрат. Одновременно присутствует только одна фигура. Её нужно обнаружить и запомнить — она определяет, в какой гараж нужно заехать на третьем этапе.

3.1. Стратегия

  1. Двигайтесь прямо с постоянной скоростью. Метка расположена где-то по ходу движения.

  2. Нижняя камера катамарана направлена вниз — используйте цветовую сегментацию или YOLO (или оба метода) для детекции фигуры.

  3. Реагируйте на систематическое обнаружение, а не на единичный кадр. Если один и тот же класс объекта обнаружен в 5–10 кадрах подряд — можно считать результат достоверным. Одиночный кадр может быть ложным срабатыванием.

  4. Запомните класс обнаруженной фигуры в переменной состояния — он понадобится на третьем этапе.

Совет: используйте простой счётчик. При каждом обнаружении объекта — инкремент. При отсутствии — декремент (или сброс). Когда счётчик превысит порог — фиксируем результат.

4. Этап 2. Движение к воротам

Ворота — это два буя (красный и жёлтый) диаметром 0.5–0.6 м. Расстояние между ними — несколько метров. Этот этап состоит из трёх шагов.

4.1. 2.1 Поиск ворот

Ворота трудно различимы на расстоянии более 15–17 м. Если после обнаружения метки ворота не видны — продолжайте сближение по прямой. Когда ворота приблизятся, YOLO начнёт их обнаруживать.

Если после длительного движения ворота всё ещё не найдены — начните разворот (поиск). Вращайтесь, пока оба буя не появятся в кадре.

4.2. 2.2 Движение к воротам

Когда оба буя обнаружены:

  1. Рассчитайте средний азимут между буями — это направление на центр ворот. Мы уже делали это на прошлом занятии: суммируете азимуты всех найденных буёв и делите на их количество.

  2. При помощи регулятора сводите ошибку азимута к нулю — катамаран будет двигаться в центр ворот.

  3. Определяйте дистанцию до ворот. Самый простой способ — по видимому размеру буёв: чем ближе, тем крупнее они в кадре. Когда видимый размер превысит порог — считайте, что ворота близко.

4.3. 2.3 Прохождение ворот

Когда катамаран приблизился к воротам вплотную, буи выходят из поля зрения камеры. Это нормально — в этот момент продолжайте движение по прямой заданное время, чтобы гарантированно пройти между ними.

Совет: не пытайтесь отслеживать буи до последнего момента. Переключитесь в режим «движение по прямой» заранее, пока курс ещё стабилен.

5. Этап 3. Движение к гаражу

У одного из бортов бассейна расположены три гаража. Над каждым — одна из трёх фигур: красный треугольник, жёлтый круг, зелёный квадрат. Задача: заехать в гараж, соответствующий фигуре, обнаруженной на первом этапе.

5.1. 3.1 Поиск гаража

Гаражи расположены не по траектории движения — после прохождения ворот необходимо развернуться. Вращайтесь, пока камера не обнаружит нужный гараж (по фигуре над ним).

Внимание: рамки гаражей могут бликовать на свету. Если вы включите в обучающий датасет изображения с бликами, нейросеть будет работать устойчивее. Также старайтесь, чтобы катамаран подходил к гаражам перпендикулярно — под косым углом фигуры плохо распознаются.

5.2. 3.2 Движение к гаражу

Аналогично движению к воротам:

  1. Рассчитайте азимут на нужный гараж (по фигуре или по входу).

  2. При помощи регулятора наводитесь на него.

5.3. 3.3 Парковка в гараже

Когда гараж выйдет из поля зрения камеры, катамаран может ещё не находиться внутри. Продолжайте движение вперёд заданное время, чтобы аппарат гарантированно оказался в гараже.

6. Структура рабочего пакета

Вам будет предоставлен пакет с готовой структурой. Большая часть кода уже написана — вам нужно реализовать миссию.

  • cv/ — ноды компьютерного зрения: цветовая сегментация нижней камеры, YOLO-детекция передней камеры. Эти файлы менять не нужно — они публикуют результаты в топики, которые вы читаете из состояний.

  • pilot/ — нода управления движением (регулятор). Принимает целевой азимут, управляет моторами.

  • state_machine/ — машина состояний. Содержит стейты, наследуемые от DefaultState, аналогично уроку 08. Сюда вы добавляете свои состояния.

  • mission/ — описание вашей миссии: какие состояния, в каком порядке, с какими переходами. Именно этот файл вы редактируете для прохождения задания.

  • config/ — конфигурационные файлы: параметры цветовой сегментации, настройки регулятора, путь к нейросети.

В папку config необходимо поместить вашу дообученную модель YOLO (файл *.pt из урока 09).

6.1. Примерная архитектура миссии

[WaitStart] --start--> [MoveForward] --timeout--> [SearchMarker]
    |                                                     |
    |                                              marker_found
    |                                                     v
    |                                             [SearchGate]
    |                                                     |
    |                                              gate_found
    |                                                     v
    |                                             [MoveToGate]
    |                                                     |
    |                                              gate_close
    |                                                     v
    |                                            [PassGate]
    |                                                     |
    |                                               timeout
    |                                                     v
    |                                            [SearchGarage]
    |                                                     |
    |                                             garage_found
    |                                                     v
    |                                            [MoveToGarage]
    |                                                     |
    |                                              garage_close
    |                                                     v
    |                                              [ParkInGarage]
    |                                                     |
    v                                                     v
[SM_ABORTED]                                     [SM_SUCCEEDED]

Это ориентировочная структура. Вы можете менять количество состояний, добавлять промежуточные шаги, реализовать повторный поиск при потере объекта.

7. Домашнее задание

  1. Пройдите полное задание в симуляторе: обнаружьте метку, пройдите ворота, припаркуйтесь в гараже.

  2. Визуализируйте вашу машину состояний через smach-viewer.

  3. Убедитесь, что миссия проходится стабильно при нескольких запусках подряд.

8. Дополнительные материалы

  • Все предыдущие уроки курса содержат необходимый теоретический и практический материал для выполнения этого задания.

  • Начните с простого: сначала реализуйте только прохождение ворот. Когда это заработает — добавьте поиск метки. Когда и это заработает — добавьте парковку.