# RMC-2 Navigation System Система автономной навигации для робота RMC-2 на основе ROS2 с использованием ArUco-маркеров и одометрии. ## Описание Проект реализует полноценную систему навигации по полю 6×6 клеток, размеченному ArUco-маркерами (DICT_6X6_50). Робот строит маршрут с учётом заблокированных клеток, выполняет движение по линии с визуальной коррекцией и точное центрирование над целевым маркером. ## Файлы проекта ### `navigator_node.py` — основной узел навигации Главный ROS2-узел, реализующий полный цикл навигации: - **Построение маршрута** — BFS-алгоритм для поиска кратчайшего пути по сетке 6×6 - **Движение по линии** — комбинированное управление по одометрии и визуальной обратной связи - **Cross-track коррекция** — расчёт бокового отклонения от идеальной траектории - **Визуальная память** — затухающее сохранение последней визуальной ошибки (K_VISUAL_DECAY = 0.92) - **Финальное центрирование** — точное позиционирование над маркером по камере #### Архитектура узла ``` RMC2Navigator (Node) ├── Подписки: │ ├── /RMC2/aruco_id (String) — текущий маркер под роботом │ ├── /RMC2/camera_bottom/image_color (Image) — видеопоток │ └── /RMC2/odometry (Odometry) — данные одометрии ├── Публикации: │ └── /RMC2/cmd_vel (Twist) — команды скорости ├── Таймер: 20 Гц (control_loop) └── Поток ввода: консольный ввод команд пользователя ``` #### States (конечный автомат) | Состояние | Описание | |-----------|----------| | `IDLE` | Ожидание команды | | `TURN_TO_TARGET` | Поворот на целевой угол | | `DRIVE_STRAIGHT` | Движение прямо с коррекцией | | `CENTER_MARKER` | Визуальное центрирование над маркером | | `ALIGN_ORIENTATION` | Финальное выравнивание ориентации | #### Ключевые коэффициенты ```python K_VISUAL = 0.8 # П-коэффициент визуальной коррекции K_VISUAL_DECAY = 0.92 # Затухание визуальной ошибки K_HEADING = 0.6 # Удержание курса K_CROSS_P = 2.0 # Odom cross-track MAX_ANGULAR = 0.35 # Макс. угловая скорость (рад/с) K_TURN = 1.5 # Поворот на месте DRIVE_SPEED = 0.3 # Скорость движения (м/с) CELL_SIZE_M = 0.65 # Размер клетки (м) ``` #### Алгоритм cross-track ошибки Метод `_cross_track_error()` вычисляет знаковое расстояние от текущей позиции до идеальной линии движения: ```python dx = x - start_x dy = y - start_y cross_track = -dx * sin(target_yaw) + dy * cos(target_yaw) ``` **Знак ошибки:** `+` — справа от линии, `−` — слева. --- ### `aruco_centering.py` — модуль центрирования Библиотека и ROS2-узел для точного центрирования над ArUco-маркером. #### Класс `ArucoCentering` Работает независимо от ROS2, требует только OpenCV: - **`detect_markers(image)`** — обнаружение всех маркеров на кадре - **`get_marker_center(corners)`** — вычисление центра маркера - **`get_centering_error(image, target_id)`** — ошибка позиционирования (пиксели) - **`get_marker_orientation(corners)`** — угол ориентации маркера (радианы) - **`compute_velocity(error_x, error_y)`** — расчёт скорости для центрирования #### Класс `ArucoCenteringNode` ROS2-узел для автономного центрирования: - Подписка: `/RMC2/camera_bottom/image_color` - Публикация: `/RMC2/cmd_vel` - Параметр: `target_marker_id` (−1 = любой маркер) #### Настройки ```python CAMERA_WIDTH = 640 # Ширина кадра (пикс) CAMERA_HEIGHT = 480 # Высота кадра (пикс) CENTER_TOLERANCE = 20 # Допуск центрирования (пикс) CENTERING_KP = 0.002 # Пропорциональный коэффициент MAX_CENTER_SPEED = 0.1 # Макс. скорость (м/с) ``` --- ## Преимущества системы ### 1. **Гибридная навигация** Комбинирование одометрии и компьютерного зрения обеспечивает устойчивость: - Одометрия — долгосрочная стабильность - Камера — быстрая коррекция отклонений ### 2. **Визуальная память** Коэффициент `K_VISUAL_DECAY = 0.92` сохраняет последнюю визуальную ошибку при временной потере маркера, предотвращая резкие колебания. ### 3. **Адаптивное управление** - Автоматическое снижение усиления при малых ошибках (< 5°) - Ограничение угловой скорости (`MAX_ANGULAR`) - Выбор ближайшего эквивалента целевого угла (±2π) ### 4. **Обход препятствий** Динамическая блокировка клеток через консольную команду `b ` с автоматическим перестроением маршрута. ### 5. **Кроссплатформенность OpenCV** Поддержка старых (< 4.7) и новых (≥ 4.7) версий OpenCV через детектирование API: ```python # Старый API cv2.aruco.Dictionary_get() cv2.aruco.DetectorParameters_create() # Новый API cv2.aruco.getPredefinedDictionary() cv2.aruco.DetectorParameters() cv2.aruco.ArucoDetector() ``` ### 6. **Многозадачность** - Главный цикл: 20 Гц (таймер ROS2) - Ввод пользователя: отдельный поток - Обработка изображений: асинхронный колбэк ### 7. **Отладочные инструменты** - Логирование с троттлингом - Сохранение кадров (`/tmp/aruco_debug_gray.png`) - Детальные DEBUG-сообщения с координатами и углами --- ## Зависимости ```bash # ROS2 пакеты rclpy geometry_msgs sensor_msgs std_msgs nav_msgs cv_bridge # Python библиотеки opencv-contrib-python # ArUco + cv2 numpy ``` --- ## Запуск ### Основной узел навигации ```bash python3 navigator_node.py ``` После запуска: 1. Дождитесь сообщения `"Узел навигации RMC2 запущен"` 2. Введите ID целевого маркера в консоль 3. Для блокировки клетки: `b ` (например, `b 15`) 4. Для разблокировки: повторная команда `b ` ### Узел центрирования (отдельно) ```bash python3 aruco_centering.py ``` --- ## Структура поля ``` ID маркеров (6×6): col 0 1 2 3 4 5 row 5 0 1 2 3 4 5 4 6 7 8 9 10 11 3 12 13 14 15 16 17 2 18 19 20 21 22 23 1 24 25 26 27 28 29 row 0 30 31 32 33 34 35 ``` **Формулы:** - `row = marker_id % 6` - `col = marker_id // 6` - `x = col * 1.0` - `y = 5 - row * 1.0` --- ## Пример маршрута ``` Маршрут: 0 → 1 → 2 → 8 → 14 → 15 Еду к точке 1 (waypoint 1/5), курс 90.0° Курс взят (90.0°). Еду 1 кл. прямо. [VIS-MEM] err_x=+0.120, yaw=+2.3°, tw.z=-0.096 Промежуточный waypoint 1 достигнут ... Финальный маркер 15 в кадре (err_y=-0.15). Центрирование завершено (err=0.02,-0.03). Выравниваю ориентацию. Все точки маршрута пройдены! ``` --- ## Лицензия MIT