87 lines
8.5 KiB
Markdown
87 lines
8.5 KiB
Markdown
# Умная навигация по сетке ArUco-маркеров (RMC2)
|
||
|
||
Данный проект представляет собой надежную систему автономной навигации для робота с дифференциальным приводом. Робот перемещается по координатной сетке (поле 6х6), узлы которой обозначены метками ArUco. Навигация осуществляется с использованием одометрии и визуальной обратной связи от направленной вниз камеры.
|
||
|
||
## Главные преимущества
|
||
|
||
1. **Гибридная система управления (Сенсорный фьюжн)**
|
||
- **Одометрия** используется для удержания базового курса (Heading) и плавных поворотов.
|
||
- **Компьютерное зрение (ArUco)** используется для коррекции бокового сноса (Cross-track error) и предотвращения накопления ошибки одометрии.
|
||
2. **Память визуальной ошибки (Visual Error Decay)**
|
||
- Когда робот проезжает над маркером, он вычисляет визуальное смещение. Если маркер выходит из поля зрения, система "запоминает" ошибку и плавно сводит её на нет (коэффициент `K_VISUAL_DECAY`). Это избавляет робота от резких рывков при потере маркера из кадра.
|
||
3. **Динамическое построение маршрутов (Pathfinding)**
|
||
- Встроенный алгоритм BFS (Поиск в ширину) автоматически прокладывает кратчайший маршрут по клеткам.
|
||
- Поддержка **динамических препятствий**: пользователь может блокировать и разблокировать узлы сетки (команда `b <id>`), и робот автоматически построит маршрут в обход.
|
||
4. **Прецизионное центрирование (Visual Servoing)**
|
||
- Встроенный модуль визуального сервопривода обеспечивает финальную парковку. Робот использует П-регулятор по осям X и Y камеры для идеального позиционирования над целевой клеткой (погрешность до пары пикселей/миллиметров).
|
||
5. **Универсальность и отказоустойчивость OpenCV**
|
||
- Код автоматически определяет версию библиотеки `cv2` и использует корректный API (поддерживаются как старые версии `< 4.7`, так и новые `>= 4.7`), что снимает головную боль при развертывании на разных версиях Ubuntu/ROS2.
|
||
|
||
---
|
||
|
||
## Архитектура проекта
|
||
|
||
Весь функционал собран в модулях, главный из которых:
|
||
|
||
### Узел навигатора (`nav_test.py`)
|
||
Главный ROS2 узел (`RMC2Navigator`), управляющий конечным автоматом (State Machine) робота.
|
||
**Состояния (States):**
|
||
* `IDLE`: Режим ожидания. Опрос консоли в отдельном потоке для получения цели от пользователя.
|
||
* `TURN_TO_TARGET`: Поворот на месте к следующей точке маршрута (waypoint).
|
||
* `DRIVE_STRAIGHT`: Движение вперед по отрезку с одновременным удержанием курса и визуальным подруливанием над промежуточными маркерами.
|
||
* `CENTER_MARKER`: Точная остановка и выравнивание камеры строго над центром финального маркера (используются функции работы с ArUco).
|
||
* `ALIGN_ORIENTATION`: Корректировка финального угла (Yaw) робота после центрирования.
|
||
|
||
---
|
||
|
||
## ROS-Интерфейсы (Топики)
|
||
|
||
Узел подписывается и публикует данные в следующие топики:
|
||
|
||
| Имя топика | Тип сообщения | Направление | Описание |
|
||
| :--- | :--- | :--- | :--- |
|
||
| `/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) |
|
||
|
||
---
|
||
|
||
## Инструкция по использованию
|
||
|
||
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` вынесены ключевые коэффициенты управления, которые можно тюнинговать под вес и моторы вашего конкретного робота:
|
||
|
||
* `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` — Крейсерская линейная скорость движения (м/с).
|
||
|
||
---
|
||
|
||
## Как работает визуальная коррекция курса
|
||
Во время состояния `DRIVE_STRAIGHT` робот непрерывно анализирует кадры с камеры:
|
||
1. Если маркера в кадре нет, скорость поворота рассчитывается только по одометрии (`K_HEADING * yaw_err`).
|
||
2. Как только внизу появляется маркер маршрута, камера фиксирует нормализованное боковое отклонение X (от -1.0 до 1.0).
|
||
3. Алгоритм вычитает из одометрического регулятора значение `K_VISUAL * err_x`. Это заставляет робота физически "наезжать" на центр маркера, даже если одометрия думает, что робот едет прямо. Это полностью нивелирует дрейф датчиков на длинных дистанциях! |