first commit
This commit is contained in:
229
README.md
Normal file
229
README.md
Normal file
@@ -0,0 +1,229 @@
|
||||
# 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
|
||||
Reference in New Issue
Block a user