diff --git a/README.md b/README.md index 196719a..777de64 100644 --- a/README.md +++ b/README.md @@ -1,229 +1,89 @@ -# RMC-2 Navigation System +# Умная навигация по сетке ArUco-маркеров (RMC2) -Система автономной навигации для робота RMC-2 на основе ROS2 с использованием ArUco-маркеров и одометрии. +Данный проект представляет собой надежную систему автономной навигации для робота с дифференциальным приводом. Робот перемещается по координатной сетке (поле 6х6), узлы которой обозначены метками 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) -``` - -**Знак ошибки:** `+` — справа от линии, `−` — слева. +1. **Гибридная система управления (Сенсорный фьюжн)** + - **Одометрия** используется для удержания базового курса (Heading) и плавных поворотов. + - **Компьютерное зрение (ArUco)** используется для коррекции бокового сноса (Cross-track error) и предотвращения накопления ошибки одометрии. +2. **Память визуальной ошибки (Visual Error Decay)** + - Когда робот проезжает над маркером, он вычисляет визуальное смещение. Если маркер выходит из поля зрения, система "запоминает" ошибку и плавно сводит её на нет (коэффициент `K_VISUAL_DECAY`). Это избавляет робота от резких рывков при потере маркера из кадра. +3. **Динамическое построение маршрутов (Pathfinding)** + - Встроенный алгоритм BFS (Поиск в ширину) автоматически прокладывает кратчайший маршрут по клеткам. + - Поддержка **динамических препятствий**: пользователь может блокировать и разблокировать узлы сетки (команда `b `), и робот автоматически построит маршрут в обход. +4. **Прецизионное центрирование (Visual Servoing)** + - Встроенный модуль визуального сервопривода обеспечивает финальную парковку. Робот использует П-регулятор по осям X и Y камеры для идеального позиционирования над целевой клеткой (погрешность до пары пикселей/миллиметров). +5. **Универсальность и отказоустойчивость OpenCV** + - Код автоматически определяет версию библиотеки `cv2` и использует корректный API (поддерживаются как старые версии `< 4.7`, так и новые `>= 4.7`), что снимает головную боль при развертывании на разных версиях Ubuntu/ROS2. --- -### `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 # Макс. скорость (м/с) -``` +### Узел навигатора (`nav_test.py`) +Главный ROS2 узел (`RMC2Navigator`), управляющий конечным автоматом (State Machine) робота. +**Состояния (States):** +* `IDLE`: Режим ожидания. Опрос консоли в отдельном потоке для получения цели от пользователя. +* `TURN_TO_TARGET`: Поворот на месте к следующей точке маршрута (waypoint). +* `DRIVE_STRAIGHT`: Движение вперед по отрезку с одновременным удержанием курса и визуальным подруливанием над промежуточными маркерами. +* `CENTER_MARKER`: Точная остановка и выравнивание камеры строго над центром финального маркера (используются функции работы с ArUco). +* `ALIGN_ORIENTATION`: Корректировка финального угла (Yaw) робота после центрирования. --- -## Преимущества системы +## ROS-Интерфейсы (Топики) -### 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-сообщения с координатами и углами +| Имя топика | Тип сообщения | Направление | Описание | +| :--- | :--- | :--- | :--- | +| `/RMC2/cmd_vel` | `geometry_msgs/Twist` | **Publisher** | Управляющие команды скорости на колеса | +| `/RMC2/aruco_id` | `std_msgs/String` | **Subscriber** | ID текущего/ближайшего маркера | +| `/RMC2/camera_bottom/image_color` | `sensor_msgs/Image` | **Subscriber** | Видеопоток с нижней камеры робота | +| `/RMC2/odometry` | `nav_msgs/Odometry` | **Subscriber** | Данные одометрии для расчета текущего угла (Yaw) | --- -## Зависимости +## Инструкция по использованию -```bash -# ROS2 пакеты -rclpy -geometry_msgs -sensor_msgs -std_msgs -nav_msgs -cv_bridge - -# Python библиотеки -opencv-contrib-python # ArUco + cv2 -numpy -``` +1. Запустите ROS2-окружение и симуляцию (или систему реального робота). +2. Сделайте скрипт исполняемым (если еще не сделали): + ```bash + chmod +x nav_test.py + ``` +3. Запустите основной скрипт навигации: + ```bash + python3 nav_test.py + # или через ros2 run, если скрипт лежит в пакете: + # ros2 run <имя_вашего_пакета> nav_test.py + ``` +4. Обратите внимание на терминал. Робот дождется, пока обнаружит стартовый маркер, и предложит ввести команду: + - Введите число (например, `15`), чтобы робот построил маршрут и поехал на клетку с этим маркером. + - Введите `b 8`, чтобы добавить клетку 8 в "черный список" (заблокировать проезд через неё). + - Введите `b 8` повторно, чтобы разблокировать клетку. --- -## Запуск +## Тонкая настройка (Параметры управления) -### Основной узел навигации +В начале файла `nav_test.py` вынесены ключевые коэффициенты управления, которые можно тюнинговать под вес и моторы вашего конкретного робота: -```bash -python3 navigator_node.py -``` - -После запуска: -1. Дождитесь сообщения `"Узел навигации RMC2 запущен"` -2. Введите ID целевого маркера в консоль -3. Для блокировки клетки: `b ` (например, `b 15`) -4. Для разблокировки: повторная команда `b ` - -### Узел центрирования (отдельно) - -```bash -python3 aruco_centering.py -``` +* `K_VISUAL = 0.8` — Резкость реакции на боковое смещение от центра маркера при движении прямо. +* `K_VISUAL_DECAY = 0.92` — Скорость "забывания" ошибки (1.0 = помнит всегда, 0.0 = забывает мгновенно). +* `K_HEADING = 0.6` — П-регулятор удержания направления по одометрии (компасу). +* `MAX_ANGULAR = 0.35` — Лимит угловой скорости в движении (рад/с). +* `K_TURN = 1.5` — Агрессивность разворота на месте. +* `DRIVE_SPEED = 0.3` — Крейсерская линейная скорость движения (м/с). --- -## Структура поля - -``` -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 +## Как работает визуальная коррекция курса +Во время состояния `DRIVE_STRAIGHT` робот непрерывно анализирует кадры с камеры: +1. Если маркера в кадре нет, скорость поворота рассчитывается только по одометрии (`K_HEADING * yaw_err`). +2. Как только внизу появляется маркер маршрута, камера фиксирует нормализованное боковое отклонение X (от -1.0 до 1.0). +3. Алгоритм вычитает из одометрического регулятора значение `K_VISUAL * err_x`. Это заставляет робота физически "наезжать" на центр маркера, даже если одометрия думает, что робот едет прямо. Это полностью нивелирует дрейф датчиков на длинных дистанциях! \ No newline at end of file