first commit
This commit is contained in:
258
README.md
258
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). Робот строит маршрут с учётом заблокированных клеток, выполняет движение по линии с визуальной коррекцией и точное центрирование над целевым маркером.
|
## Главные преимущества
|
||||||
|
|
||||||
## Файлы проекта
|
1. **Гибридная система управления (Сенсорный фьюжн)**
|
||||||
|
- **Одометрия** используется для удержания базового курса (Heading) и плавных поворотов.
|
||||||
### `navigator_node.py` — основной узел навигации
|
- **Компьютерное зрение (ArUco)** используется для коррекции бокового сноса (Cross-track error) и предотвращения накопления ошибки одометрии.
|
||||||
|
2. **Память визуальной ошибки (Visual Error Decay)**
|
||||||
Главный ROS2-узел, реализующий полный цикл навигации:
|
- Когда робот проезжает над маркером, он вычисляет визуальное смещение. Если маркер выходит из поля зрения, система "запоминает" ошибку и плавно сводит её на нет (коэффициент `K_VISUAL_DECAY`). Это избавляет робота от резких рывков при потере маркера из кадра.
|
||||||
|
3. **Динамическое построение маршрутов (Pathfinding)**
|
||||||
- **Построение маршрута** — BFS-алгоритм для поиска кратчайшего пути по сетке 6×6
|
- Встроенный алгоритм BFS (Поиск в ширину) автоматически прокладывает кратчайший маршрут по клеткам.
|
||||||
- **Движение по линии** — комбинированное управление по одометрии и визуальной обратной связи
|
- Поддержка **динамических препятствий**: пользователь может блокировать и разблокировать узлы сетки (команда `b <id>`), и робот автоматически построит маршрут в обход.
|
||||||
- **Cross-track коррекция** — расчёт бокового отклонения от идеальной траектории
|
4. **Прецизионное центрирование (Visual Servoing)**
|
||||||
- **Визуальная память** — затухающее сохранение последней визуальной ошибки (K_VISUAL_DECAY = 0.92)
|
- Встроенный модуль визуального сервопривода обеспечивает финальную парковку. Робот использует П-регулятор по осям X и Y камеры для идеального позиционирования над целевой клеткой (погрешность до пары пикселей/миллиметров).
|
||||||
- **Финальное центрирование** — точное позиционирование над маркером по камере
|
5. **Универсальность и отказоустойчивость OpenCV**
|
||||||
|
- Код автоматически определяет версию библиотеки `cv2` и использует корректный API (поддерживаются как старые версии `< 4.7`, так и новые `>= 4.7`), что снимает головную боль при развертывании на разных версиях Ubuntu/ROS2.
|
||||||
#### Архитектура узла
|
|
||||||
|
|
||||||
```
|
|
||||||
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`
|
### Узел навигатора (`nav_test.py`)
|
||||||
|
Главный ROS2 узел (`RMC2Navigator`), управляющий конечным автоматом (State Machine) робота.
|
||||||
Работает независимо от ROS2, требует только OpenCV:
|
**Состояния (States):**
|
||||||
|
* `IDLE`: Режим ожидания. Опрос консоли в отдельном потоке для получения цели от пользователя.
|
||||||
- **`detect_markers(image)`** — обнаружение всех маркеров на кадре
|
* `TURN_TO_TARGET`: Поворот на месте к следующей точке маршрута (waypoint).
|
||||||
- **`get_marker_center(corners)`** — вычисление центра маркера
|
* `DRIVE_STRAIGHT`: Движение вперед по отрезку с одновременным удержанием курса и визуальным подруливанием над промежуточными маркерами.
|
||||||
- **`get_centering_error(image, target_id)`** — ошибка позиционирования (пиксели)
|
* `CENTER_MARKER`: Точная остановка и выравнивание камеры строго над центром финального маркера (используются функции работы с ArUco).
|
||||||
- **`get_marker_orientation(corners)`** — угол ориентации маркера (радианы)
|
* `ALIGN_ORIENTATION`: Корректировка финального угла (Yaw) робота после центрирования.
|
||||||
- **`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 # Макс. скорость (м/с)
|
|
||||||
```
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## Преимущества системы
|
## ROS-Интерфейсы (Топики)
|
||||||
|
|
||||||
### 1. **Гибридная навигация**
|
Узел подписывается и публикует данные в следующие топики:
|
||||||
Комбинирование одометрии и компьютерного зрения обеспечивает устойчивость:
|
|
||||||
- Одометрия — долгосрочная стабильность
|
|
||||||
- Камера — быстрая коррекция отклонений
|
|
||||||
|
|
||||||
### 2. **Визуальная память**
|
| Имя топика | Тип сообщения | Направление | Описание |
|
||||||
Коэффициент `K_VISUAL_DECAY = 0.92` сохраняет последнюю визуальную ошибку при временной потере маркера, предотвращая резкие колебания.
|
| :--- | :--- | :--- | :--- |
|
||||||
|
| `/RMC2/cmd_vel` | `geometry_msgs/Twist` | **Publisher** | Управляющие команды скорости на колеса |
|
||||||
### 3. **Адаптивное управление**
|
| `/RMC2/aruco_id` | `std_msgs/String` | **Subscriber** | ID текущего/ближайшего маркера |
|
||||||
- Автоматическое снижение усиления при малых ошибках (< 5°)
|
| `/RMC2/camera_bottom/image_color` | `sensor_msgs/Image` | **Subscriber** | Видеопоток с нижней камеры робота |
|
||||||
- Ограничение угловой скорости (`MAX_ANGULAR`)
|
| `/RMC2/odometry` | `nav_msgs/Odometry` | **Subscriber** | Данные одометрии для расчета текущего угла (Yaw) |
|
||||||
- Выбор ближайшего эквивалента целевого угла (±2π)
|
|
||||||
|
|
||||||
### 4. **Обход препятствий**
|
|
||||||
Динамическая блокировка клеток через консольную команду `b <id>` с автоматическим перестроением маршрута.
|
|
||||||
|
|
||||||
### 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-сообщения с координатами и углами
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## Зависимости
|
## Инструкция по использованию
|
||||||
|
|
||||||
|
1. Запустите ROS2-окружение и симуляцию (или систему реального робота).
|
||||||
|
2. Сделайте скрипт исполняемым (если еще не сделали):
|
||||||
```bash
|
```bash
|
||||||
# ROS2 пакеты
|
chmod +x nav_test.py
|
||||||
rclpy
|
|
||||||
geometry_msgs
|
|
||||||
sensor_msgs
|
|
||||||
std_msgs
|
|
||||||
nav_msgs
|
|
||||||
cv_bridge
|
|
||||||
|
|
||||||
# Python библиотеки
|
|
||||||
opencv-contrib-python # ArUco + cv2
|
|
||||||
numpy
|
|
||||||
```
|
```
|
||||||
|
3. Запустите основной скрипт навигации:
|
||||||
---
|
|
||||||
|
|
||||||
## Запуск
|
|
||||||
|
|
||||||
### Основной узел навигации
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
python3 navigator_node.py
|
python3 nav_test.py
|
||||||
```
|
# или через ros2 run, если скрипт лежит в пакете:
|
||||||
|
# ros2 run <имя_вашего_пакета> nav_test.py
|
||||||
После запуска:
|
|
||||||
1. Дождитесь сообщения `"Узел навигации RMC2 запущен"`
|
|
||||||
2. Введите ID целевого маркера в консоль
|
|
||||||
3. Для блокировки клетки: `b <id>` (например, `b 15`)
|
|
||||||
4. Для разблокировки: повторная команда `b <id>`
|
|
||||||
|
|
||||||
### Узел центрирования (отдельно)
|
|
||||||
|
|
||||||
```bash
|
|
||||||
python3 aruco_centering.py
|
|
||||||
```
|
```
|
||||||
|
4. Обратите внимание на терминал. Робот дождется, пока обнаружит стартовый маркер, и предложит ввести команду:
|
||||||
|
- Введите число (например, `15`), чтобы робот построил маршрут и поехал на клетку с этим маркером.
|
||||||
|
- Введите `b 8`, чтобы добавить клетку 8 в "черный список" (заблокировать проезд через неё).
|
||||||
|
- Введите `b 8` повторно, чтобы разблокировать клетку.
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## Структура поля
|
## Тонкая настройка (Параметры управления)
|
||||||
|
|
||||||
```
|
В начале файла `nav_test.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
|
|
||||||
```
|
|
||||||
|
|
||||||
**Формулы:**
|
* `K_VISUAL = 0.8` — Резкость реакции на боковое смещение от центра маркера при движении прямо.
|
||||||
- `row = marker_id % 6`
|
* `K_VISUAL_DECAY = 0.92` — Скорость "забывания" ошибки (1.0 = помнит всегда, 0.0 = забывает мгновенно).
|
||||||
- `col = marker_id // 6`
|
* `K_HEADING = 0.6` — П-регулятор удержания направления по одометрии (компасу).
|
||||||
- `x = col * 1.0`
|
* `MAX_ANGULAR = 0.35` — Лимит угловой скорости в движении (рад/с).
|
||||||
- `y = 5 - row * 1.0`
|
* `K_TURN = 1.5` — Агрессивность разворота на месте.
|
||||||
|
* `DRIVE_SPEED = 0.3` — Крейсерская линейная скорость движения (м/с).
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## Пример маршрута
|
## Как работает визуальная коррекция курса
|
||||||
|
Во время состояния `DRIVE_STRAIGHT` робот непрерывно анализирует кадры с камеры:
|
||||||
```
|
1. Если маркера в кадре нет, скорость поворота рассчитывается только по одометрии (`K_HEADING * yaw_err`).
|
||||||
Маршрут: 0 → 1 → 2 → 8 → 14 → 15
|
2. Как только внизу появляется маркер маршрута, камера фиксирует нормализованное боковое отклонение X (от -1.0 до 1.0).
|
||||||
Еду к точке 1 (waypoint 1/5), курс 90.0°
|
3. Алгоритм вычитает из одометрического регулятора значение `K_VISUAL * err_x`. Это заставляет робота физически "наезжать" на центр маркера, даже если одометрия думает, что робот едет прямо. Это полностью нивелирует дрейф датчиков на длинных дистанциях!
|
||||||
Курс взят (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
|
|
||||||
Reference in New Issue
Block a user