230 lines
8.9 KiB
Markdown
230 lines
8.9 KiB
Markdown
# 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 <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-сообщения с координатами и углами
|
||
|
||
---
|
||
|
||
## Зависимости
|
||
|
||
```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 <id>` (например, `b 15`)
|
||
4. Для разблокировки: повторная команда `b <id>`
|
||
|
||
### Узел центрирования (отдельно)
|
||
|
||
```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
|