В предыдущих материалах я рассказывал о разработке отдельных робототехнических систем. В этот раз задача связана с мобильным роботом для автоматизированного сбора клубники.
Полноценный робот-сборщик должен состоять как минимум из нескольких крупных подсистем:
мобильная платформа + навигация + лидар + техническое зрение + поиск спелой клубники + манипулятор + захват
Разрабатывать всё сразу неудобно, поэтому систему я разбил на отдельные модули.
В этой статье рассматривается именно навигационная часть.
На текущем этапе робот ещё не ищет и не собирает ягоды. Задача мобильной платформы проще:
самостоятельно определить, где она находится;
получить заданную конечную точку;
построить маршрут;
проехать по нему;
определить появившееся препятствие;
исключить занятую область из карты;
построить новый маршрут.
Для разработки используются ROS 2, Python, OpenCV и Webots.
Постановка задачи
На первом этапе теплица представлена сильно упрощённо.
Рабочее пространство — сетка 5×5:
20 21 22 23 24 15 16 17 18 19 10 11 12 13 14 5 6 7 8 9 0 1 2 3 4
Всего получается 25 возможных позиций мобильной платформы.
Каждой позиции соответствует ArUco-маркер.
Раньше начальную точку я задавал вручную:
START = 0 TARGET = 22
Но у такого подхода есть очевидная проблема.
Если физически поставить робота, например, на маркер №17, программа сама об этом не узнает.
Одометрия здесь не решает задачу. При старте она вполне может сообщить:
x = 0 y = 0
но из этого невозможно сделать вывод:
робот находится на маркере 17
Поэтому в новой версии начальная точка вообще не передаётся программе.
Робот должен определить её самостоятельно.
Получается:
робота поставили на поле ↓ нижняя камера ↓ ArUco detector ↓ распознан ID маркера ↓ START определяется автоматически ↓ TARGET задаётся пользователем ↓ строится маршрут
Например, робот физически установлен над маркером 7.
Камера распознаёт:
aruco_7
Навигационная программа получает:
START = 7
Пользователь при запуске задаёт только:
TARGET = 22
После этого робот сам строит:
7 → ... → 22
Общая архитектура
В итоге система выглядит так:
Webots │ ┌────────┼─────────┐ │ │ │ ↓ ↓ ↓ камера odometry lidar │ │ │ ↓ │ │ ArUco detector │ │ │ │ │ ↓ │ │ /RMC2/camera_bottom/aruco_id │ │ │ │ └────┐ │ │ ↓ ↓ ↓ main.py │ START определён │ ↓ Planner │ ↓ Graph │ ↓ список маркеров │ ↓ Motion │ /RMC2/cmd_vel │ ↓ робот │ ↓ рабочая позиция │ ↓ ObstacleDetector ↑ /RMC2/scan │ ↓ занятая клетка │ ↓ Planner │ ↓ новый маршрут
Сам Python-проект остаётся небольшим:
module_b_navigation/ │ ├── __init__.py ├── main.py ├── graph.py ├── planner.py ├── motion_controller.py └── obstacle_detector.py
Отдельный Python-модуль распознавания ArUco здесь не нужен.
Распознавание уже выполняет существующая ROS 2-нода на C++.
Как определяется начальная позиция
Это главное изменение новой версии.
ArUco-детектор получает изображения нижней камеры:
/RMC2/camera_bottom/image/compressed
и после распознавания публикует ID найденной метки:
/RMC2/camera_bottom/aruco_id
Тип сообщения:
std_msgs/msg/String
Например:
aruco_7
или:
aruco_17
Таким образом, мы разделяем две задачи.
Первая программа занимается компьютерным зрением:
изображение ↓ поиск квадратного маркера ↓ декодирование ArUco ↓ ID
А навигационная программа работает уже с готовым результатом:
ID ↓ номер клетки ↓ планирование
Это гораздо лучше, чем повторно реализовывать OpenCV-детектор внутри main.py.
Почему одного распознавания недостаточно
Предположим, камера несколько кадров подряд сообщает:
aruco_17 aruco_17 aruco_8 aruco_17 aruco_17
Если принимать первое попавшееся значение, единичная ошибка распознавания потенциально может определить неправильную стартовую клетку.
Поэтому в программе используется подтверждение.
По умолчанию:
aruco_confirmations = 3
Нам нужны три последовательных одинаковых определения:
aruco_17 ↓ 1/3 aruco_17 ↓ 2/3 aruco_17 ↓ 3/3 START_MARKER_CONFIRMED = 17
Если между ними появилась другая метка:
17 17 8
счётчик для 17 сбрасывается.
Логирование
Вторая существенная доработка — постоянный журнал работы.
Раньше часть информации просто выводилась через:
print()
или стандартный ROS logger.
Но при отладке робототехнической системы важно потом восстановить всю последовательность событий:
какую метку увидела камера? какой старт определился? какой маршрут построился? когда началось движение? какие координаты были у робота? где появился объект? какую клетку заблокировал планировщик?
Поэтому теперь информация одновременно отправляется:
событие / \ / \ ↓ ↓ терминал log-файл
Файлы сохраняются в:
~/module_b_logs/
Например:
module_b_20260915_110412.log
Типичный журнал выглядит так:
[2026-09-15 11:04:12.031] SYSTEM: запуск навигации [2026-09-15 11:04:12.032] SYSTEM: ArUco topic=/RMC2/camera_bottom/aruco_id [2026-09-15 11:04:12.033] SYSTEM: target_id=22 [2026-09-15 11:04:12.034] LOCALIZATION: ожидаем начальную ArUco-метку [2026-09-15 11:04:12.251] LOCALIZATION: ArUco message='aruco_7' [2026-09-15 11:04:12.252] LOCALIZATION: candidate=7; confirmations=1/3 [2026-09-15 11:04:12.351] LOCALIZATION: candidate=7; confirmations=2/3 [2026-09-15 11:04:12.451] LOCALIZATION: candidate=7; confirmations=3/3 [2026-09-15 11:04:12.452] LOCALIZATION: START_MARKER_CONFIRMED=7 [2026-09-15 11:04:12.454] PLANNER: планирование 7 -> 22 [2026-09-15 11:04:12.455] PLANNER: маршрут найден
При этом нет смысла писать состояние управления каждые 0,05 секунды — файл быстро разрастётся.
Поэтому состояния и важные события записываются сразу, а телеметрия движения — примерно раз в секунду.
1. graph.py — представление поля
Начнём с самой простой части.
class Graph: SIZE = 5 MARKER_COUNT = 25
SIZE задаёт ширину и высоту сетки:
5 × 5
а:
MARKER_COUNT = 25
задаёт количество допустимых маркеров.
Проверка номера
def valid(self, marker): return 0 <= marker < self.MARKER_COUNT
Для нашей карты:
0 ... 24
поэтому:
graph.valid(12)
вернёт:
True
а:
graph.valid(30)
даст:
False
Это особенно важно теперь, когда номер стартовой клетки приходит не из параметра программы, а извне — от ArUco-детектора.
Даже если пришло:
aruco_135
планировщик не должен воспринимать 135 как клетку нашей карты.
Номер маркера → координаты
Метод:
def position(self, marker): if not self.valid(marker): raise ValueError( f"Неверный номер маркера: {marker}" ) x = marker % self.SIZE y = marker // self.SIZE return float(x), float(y)
переводит дискретный номер клетки в координаты.
Например:
marker = 17
Получаем:
x = 17 % 5 y = 17 // 5
То есть:
x = 2 y = 3
Следовательно:
17 → (2, 3)
На карте:
20 21 22 23 24 15 16 17 18 19 ↑ (2,3)
Операция % возвращает остаток от деления, а // выполняет целочисленное деление.
Таким образом, нам не нужна отдельная таблица:
{ 0: (0, 0), 1: (1, 0), ... }
Соседи клетки
Для планирования нужно знать, куда можно попасть за один шаг.
def neighbors(self, marker, blocked=None):
Например, для центральной клетки 12:
17 ↑ 11 ← 12 → 13 ↓ 7
Получаем:
[13, 17, 11, 7]
Но необходимо учитывать границы.
Например:
0 1 2 3 4
у 4 нет соседа справа.
Нельзя просто сделать:
4 + 1
потому что получим 5, который на нашей карте находится уже в следующем ряду.
Поэтому используются проверки строки и столбца.
Заблокированные клетки
blocked — множество клеток, через которые ехать запрещено.
Например:
blocked = {12}
означает:
маркер 12 занят препятствием
В конце neighbors() выполняется:
return [ m for m in result if m not in blocked ]
Если:
result = [13, 17, 11, 7] blocked = {17}
останется:
[13, 11, 7]
Для планировщика клетка 17 фактически исчезает из доступного графа.
Поиск ближайшего маркера
Лидар работает не с номерами клеток.
После обработки мы получаем координату:
x = 2.08 y = 3.11
Нужно определить, возле какого маркера она находится.
Для каждого маркера вычисляется:
distance = math.hypot( x - mx, y - my )
Это евклидово расстояние:
d = √((x-mx)² + (y-my)²)
После перебора всех 25 клеток выбирается ближайшая.
Таким образом Graph связывает:
ArUco ID ↕ номер клетки ↕ координаты X/Y ↕ данные лидара
2. planner.py — поиск маршрута
Теперь старт известен.
Например:
START = 7 TARGET = 22
Нужно построить путь.
Для этого используется BFS — Breadth-First Search, или поиск в ширину.
Почему BFS
Все переходы между соседними клетками имеют одинаковую условную стоимость:
1 клетка = 1 переход
В такой ситуации BFS гарантирует поиск минимального маршрута по количеству переходов.
Используется:
from collections import deque
deque позволяет выполнять:
queue.append(...)
и:
queue.popleft()
Получается обычная очередь:
первым пришёл ↓ первым обработан
FIFO — First In, First Out.
Как работает поиск в ширину
Пусть робот находится на 0.
На первом уровне доступны:
1 5
На следующем:
2 6 10
Затем:
3 7 11 15
Поиск распространяется как волна.
Сначала рассматриваются все клетки на расстоянии одного перехода, затем двух, трёх и так далее.
Почему очередь хранит целые маршруты
В нашей реализации:
queue.append([start])
То есть туда кладётся не просто:
7
а:
[7]
При расширении:
new_route = route + [next_marker]
Например:
route = [7, 12, 17] next_marker = 22
получим:
[7, 12, 17, 22]
Для огромной карты такой вариант был бы не самым экономным по памяти.
Но для:
5 × 5 = 25 клеток
он очень удобен для понимания и отладки.
Почему ищется не просто первый маршрут
Нам желательно минимизировать не только расстояние, но и количество поворотов.
Предположим, существуют два маршрута одинаковой длины:
A: → ↑ → ↑ → ↑ B: → → ↑ ↑ ↑
Оба содержат одинаковое число переходов.
Но второй требует только одного изменения направления.
Для мобильной платформы это обычно предпочтительнее.
Поэтому программа сначала собирает все минимальные маршруты, а затем выбирает лучший.
Подсчёт поворотов
Метод:
count_turns(route)
получает направления соседних участков.
Например:
0 → 1 → 2 → 7 → 12 → 17 → 22
Направления:
RIGHT RIGHT UP UP UP UP
Изменение произошло один раз:
RIGHT → UP
следовательно:
turns = 1
Выбор маршрута
В конце:
best_route = min( found_routes, key=lambda route: ( self.count_turns(route), route ) )
Для каждого маршрута Python формирует ключ:
( количество поворотов, список ID )
Сначала сравнивается число поворотов.
Если оно одинаковое, сравниваются сами списки ID.
Благодаря этому результат получается детерминированным.
Логирование Planner
В новой версии Planner получает функцию журналирования:
def __init__(self, graph, logger=None): self.graph = graph self.logger = logger
А затем:
def log(self, message): if self.logger is not None: self.logger( f"PLANNER: {message}" )
Получается интересное разделение.
Planner вообще не обязан знать:
куда записывается файл как устроен ROS logger какое сейчас время
Он просто сообщает:
PLANNER: планирование 7 -> 22
А main.py уже решает, куда это сообщение отправить.
3. motion_controller.py — движение платформы
Планировщик возвращает:
[7, 12, 17, 22]
Но робот не умеет выполнять Python-списки.
Нужно превратить маршрут в команды скорости.
Publisher Twist
Создаётся:
self.publisher = node.create_publisher( Twist, "/RMC2/cmd_vel", 10 )
Через:
/RMC2/cmd_vel
робот получает сообщения:
geometry_msgs/msg/Twist
Нас интересуют две составляющие:
cmd.linear.x cmd.angular.z
Первая отвечает за движение вперёд.
Вторая — за вращение.
Получение Odometry
Одновременно Motion подписывается на:
/RMC2/odometry
Из сообщения берутся:
self.x self.y
и ориентация.
Но ROS хранит ориентацию в виде кватерниона:
x y z w
Для движения по плоскости удобнее использовать один угол:
yaw
Поэтому выполняется преобразование кватерниона в угол.
Например:
yaw = 0° → +X yaw = 90° → +Y yaw = 180° → -X
Почему ArUco и Odometry выполняют разные задачи
Это важный момент.
ArUco отвечает на вопрос:
На какой логической клетке находится робот?
Например:
aruco_17
Одометрия отвечает на другой вопрос:
Как изменяется физическое положение робота во время движения?
То есть:
ArUco ↓ START = 17 Odometry ↓ x, y, yaw ↓ контроль движения
Одно не заменяет другое.
Цикл управления
Создаётся таймер:
self.timer = node.create_timer( 0.05, self.control )
Следовательно:
1 / 0.05 = 20 Гц
Функция управления выполняется примерно двадцать раз в секунду.
На каждом шаге задаются вопросы:
какая следующая клетка? ↓ какие у неё координаты? ↓ где робот сейчас? ↓ какое расстояние до цели? ↓ куда нужно повернуться? ↓ что отправить в cmd_vel?
Почему route_index начинается с 1
Когда вызывается:
motion.start(route)
используется:
self.route_index = 1
Если:
route = [7, 12, 17, 22]
робот уже находится на:
route[0] = 7
Следующая физическая цель:
route[1] = 12
Поэтому индекс начинается с единицы.
Расстояние до цели
Получаем координаты следующей клетки:
target_x, target_y = self.graph.position( target_marker )
Затем:
dx = target_x - self.x dy = target_y - self.y
Расстояние:
distance = math.hypot( dx, dy )
А направление:
target_yaw = math.atan2( dy, dx )
Ошибка ориентации
Нужно сравнить:
куда робот смотрит
с:
куда находится цель
Поэтому:
angle_error = target_yaw - self.yaw
Но углы циклические.
Например:
+179° -179°
математически отличаются почти на 360°, хотя физически между ними всего около 2°.
Поэтому угол нормализуется в:
[-π, +π]
Поворот
Если:
abs(angle_error) > self.angle_tolerance
робот сначала стоит на месте:
cmd.linear.x = 0.0
и вращается:
cmd.angular.z = ±0.60
Направление зависит от знака ошибки.
Получается:
цель ↑ робот → │ └── сначала поворот робот ↑ │ └── затем движение
Движение вперёд
Когда ошибка угла достаточно мала:
cmd.linear.x = self.linear_speed
и робот начинает двигаться.
При этом сохраняется небольшая коррекция:
correction = 1.2 * angle_error
Это простейший P-регулятор:
ω = Kp × e
где:
ω — угловая скорость Kp — 1.2 e — ошибка направления
Коррекция ограничивается:
-0.30 ... +0.30
чтобы робот не начал слишком резко вращаться во время движения.
Когда клетка считается достигнутой
Используется:
self.distance_tolerance = 0.12
То есть попадать точно в математическую точку:
(2.000000, 3.000000)
не требуется.
Если расстояние меньше 12 см:
MARKER_REACHED
индекс увеличивается и выбирается следующая клетка.
Телеметрия
Во время движения полезно видеть:
x y target distance angle_error
Например:
MOTION: x=1.53 y=2.01 target=17 distance=0.47 angle_error=3.2deg
Но цикл управления работает 20 раз в секунду.
Если записывать каждую итерацию, за одну минуту получится около:
20 × 60 = 1200
строк только телеметрии.
Поэтому она ограничена примерно одним сообщением в секунду.
События вроде:
ROTATING DRIVING MARKER_REACHED ROUTE_COMPLETE
при этом записываются сразу.
4. obstacle_detector.py — обнаружение нового препятствия
После прибытия в рабочую точку используется 2D-лидар.
В текущем эксперименте задача специально упрощена:
сначала измеряем свободную сцену ↓ запоминаем LaserScan ↓ ставим препятствие ↓ получаем новый LaserScan ↓ сравниваем
Что содержит LaserScan
Упрощённо:
ranges = [ 2.31, 2.29, 2.25, ... ]
Каждый элемент — расстояние для определённого направления.
Угол луча:
angle = ( scan.angle_min + i * scan.angle_increment )
То есть лидар возвращает набор:
(угол, расстояние)
Baseline
До появления препятствия:
self.baseline = list( self.scan.ranges )
Например:
луч 120 → 3.8 м луч 121 → 3.9 м луч 122 → inf
После установки объекта:
луч 120 → 1.7 м луч 121 → 1.72 м луч 122 → 1.75 м
Система ищет именно такие изменения.
Условие появления объекта
Используются два случая.
Первый:
раньше = inf сейчас = 1.7
То есть раньше луч ничего не видел, а теперь увидел объект.
Второй:
old_range - new_range >= 0.20
Например:
было 3.4 м стало 1.9 м 3.4 - 1.9 = 1.5 м
Это значительно больше порога:
0.20 м
Луч считается принадлежащим новому объекту.
Из LaserScan в X/Y
Лидар даёт полярные координаты:
r α
Переводим их:
local_x = new_range * math.cos(angle) local_y = new_range * math.sin(angle)
Получаем точку относительно самого робота.
Но наш Graph существует в мировой системе координат.
Поэтому выполняется преобразование:
world_x = robot_x + ( local_x * cos(robot_yaw) - local_y * sin(robot_yaw) ) world_y = robot_y + ( local_x * sin(robot_yaw) + local_y * cos(robot_yaw) )
Математически:
[world_x] [robot_x] [ cosθ -sinθ ] [local_x] [world_y] = [robot_y] + [ sinθ cosθ ] [local_y]
После этого можно вызвать:
graph.nearest_marker( world_x, world_y )
Голосование
Один ящик создаёт не одну lidar-точку.
Например:
(1.93, 2.04) (1.97, 2.08) (2.01, 2.05) (2.06, 1.99)
Для каждой определяется ближайшая клетка.
Получается:
votes[marker] = ( votes.get(marker, 0) + 1 )
В результате может получиться:
{ 11: 1, 12: 14, 13: 2 }
Максимум:
marker 12 → 14 голосов
Значит предполагаем:
blocked = {12}
При этом требуется минимум три подтверждающие lidar-точки:
self.min_votes = 3
Это защищает от единичного шума.
5. main.py — связываем всё вместе
Именно main.py изменился сильнее всего.
Теперь это не просто последовательный скрипт.
Создаётся собственная ROS-нода:
class NavigationNode(Node):
Она отвечает за:
ROS-параметры ArUco subscriber определение START лог-файл вывод сообщений
Параметры запуска
Теперь параметра:
start_id
нет.
Есть:
target_id aruco_topic aruco_confirmations
По умолчанию:
target_id = 22
ArUco topic:
/RMC2/camera_bottom/aruco_id
Количество подтверждений:
3
Подписка на ArUco
Используется:
self.create_subscription( String, self.aruco_topic, self.aruco_callback, 10 )
Когда ArUco-нода публикует:
aruco_17
ROS автоматически вызывает:
aruco_callback()
Извлекаем число из строки
Получаем:
raw = msg.data.strip()
Например:
aruco_17
Используется регулярное выражение:
match = re.search( r"(\d+)$", raw )
Разберём его.
\d
означает цифру.
+
— одну или несколько.
$
— конец строки.
Следовательно:
aruco_17 ^^
получаем:
17
Затем:
marker_id = int( match.group(1) )
строка "17" превращается в число 17.
Подтверждаем старт
Если текущий ID совпадает с предыдущим:
self.aruco_count += 1
Если изменился:
self.last_aruco_id = marker_id self.aruco_count = 1
Когда:
self.aruco_count >= self.required_confirmations
выполняется:
self.start_id = marker_id
После этого старт определён.
Почему main ждёт через spin_once
Используется цикл:
while ( rclpy.ok() and node.start_id is None ): rclpy.spin_once( node, timeout_sec=0.1 )
Мы не можем просто написать:
while start_id is None: pass
Потому что тогда ROS не будет нормально обрабатывать входящие callback.
spin_once() означает примерно:
обработать одну порцию ожидающих ROS-событий.
Благодаря этому работают одновременно:
ArUco callback Odometry callback LaserScan callback Motion timer
Полный сценарий запуска
Теперь последовательность выглядит так:
SYSTEM_START ↓ создание Graph ↓ создание Planner ↓ создание Motion ↓ создание ObstacleDetector ↓ ожидание ArUco ↓ aruco_7 aruco_7 aruco_7 ↓ START = 7 ↓ TARGET = 22 ↓ ожидание Odometry ↓ Planner ↓ маршрут ↓ Motion ↓ TARGET ↓ baseline LaserScan ↓ установка препятствия ↓ новый LaserScan ↓ ObstacleDetector ↓ blocked = {marker} ↓ Planner ↓ новый маршрут ↓ Motion ↓ возвращение к автоматически определённому START
Запуск
Теперь команда стала проще:
ros2 run module_b_navigation module_b \ --ros-args \ -p target_id:=22
Обратите внимание: здесь больше нет:
-p start_id:=0
Робот должен определить старт самостоятельно.
Проверка ArUco отдельно
Перед запуском всей навигации удобно проверить интерфейс:
ros2 topic echo /RMC2/camera_bottom/aruco_id
Если робот стоит над меткой №7, должны появляться сообщения примерно такого вида:
data: aruco_7 --- data: aruco_7 ---
После этого можно запускать навигационный модуль.
Что должно появиться в журнале
Например:
SYSTEM: запуск навигации SYSTEM: ArUco topic=/RMC2/camera_bottom/aruco_id SYSTEM: target_id=22 LOCALIZATION: ожидаем начальную ArUco-метку LOCALIZATION: ArUco message='aruco_7' LOCALIZATION: candidate=7; confirmations=1/3 LOCALIZATION: candidate=7; confirmations=2/3 LOCALIZATION: candidate=7; confirmations=3/3 LOCALIZATION: START_MARKER_CONFIRMED=7 SYSTEM: стартовая клетка=7 PLANNER: планирование 7 -> 22 PLANNER: маршрут найден MOTION: начало движения
Это сильно облегчает поиск ошибок.
Если робот не поехал, можно посмотреть:
получена ли ArUco? получена ли Odometry? нашёлся ли маршрут? вызван ли Motion.start()?
Как теперь работают пять Python-файлов вместе
Обновлённая схема:
ArUco detector │ ↓ /RMC2/camera_bottom/aruco_id │ ↓ main.py │ START определяется │ ↓ Planner │ ↓ Graph │ ↓ список клеток │ ↓ Motion ↙ ↘ /RMC2/odometry /RMC2/cmd_vel ↘ ↙ мобильный робот │ ↓ TARGET │ ↓ ObstacleDetector ↑ /RMC2/scan │ ↓ LaserScan diff │ ↓ world X / Y │ ↓ Graph │ ↓ ближайший marker │ ↓ blocked={...} │ ↓ Planner │ ↓ новый маршрут │ ↓ Motion │ ↓ возврат к исходной ArUco-позиции
Как это связано со сбором клубники
На данном этапе робот ещё не распознаёт и не собирает клубнику.
Сейчас решается задача мобильной базы.
Будущий цикл должен выглядеть примерно так:
НАВИГАЦИЯ ↓ робот определил положение ↓ поехал к рабочей точке ↓ остановился возле растений ↓ ТЕХНИЧЕСКОЕ ЗРЕНИЕ ↓ найдена клубника ↓ определена спелость ↓ получены XYZ ↓ МАНИПУЛЯТОР ↓ подвод захвата ↓ сбор ягоды ↓ укладка ↓ следующая ягода ↓ НАВИГАЦИЯ ↓ следующий участок
Таким образом, текущий модуль решает задачу:
доставить будущую систему зрения и манипулятор в нужное место.
Что уже реализовано
На текущем этапе прототип умеет:
представлять рабочую область сеткой 5×5;
работать с 25 логическими позициями;
получать ID физической ArUco-метки через ROS 2;
автоматически определять начальную клетку;
подтверждать старт несколькими распознаваниями;
принимать только целевой маркер как параметр запуска;
искать кратчайший маршрут BFS;
выбирать среди равных маршрутов вариант с меньшим количеством поворотов;
выполнять маршрут по данным одометрии;
отправлять команды через
Twist;корректировать направление движения;
получать
LaserScan;сравнивать состояние среды до и после появления препятствия;
переводить lidar-точки из локальной системы в мировую;
определять ближайшую занятую клетку;
исключать её из графа;
повторно запускать BFS;
возвращаться другим маршрутом;
записывать основные действия одновременно в терминал и log-файл.
Что пока упрощено
Текущую систему нельзя воспринимать как готовую промышленную навигацию тепличного робота.
Есть несколько серьёзных упрощений.
Во-первых, карта:
5 × 5
дискретная.
В реальной теплице потребуется карта проходов и рядов.
Во-вторых, препятствие сейчас определяется сравнением:
baseline scan ↓ new scan
Это удобно для эксперимента, но не является полноценным динамическим obstacle avoidance.
В-третьих, текущий контроллер движения очень простой.
Фактически:
повернись ↓ езжай ↓ подправляй курс
Для реального робота логично будет перейти к более полноценному локальному планированию.
Что дальше
Следующий этап навигации — уйти от сценария:
робот остановился ↓ сохранили lidar ↓ поставили препятствие ↓ сравнили
к непрерывной работе:
робот движется │ ↓ lidar работает │ ↓ есть препятствие? ↙ ↘ нет да ↓ ↓ едем дальше STOP ↓ обновление карты ↓ новый маршрут ↓ движение
После этого можно переходить к интеграции с системой технического зрения.
Следующий крупный модуль уже будет связан непосредственно с клубникой:
RGB / RGB-D камера ↓ изображение ↓ детектор ягод ↓ спелая / неспелая ↓ сегментация ↓ центр ягоды ↓ XYZ ↓ манипулятор
В перспективе простые компоненты текущего прототипа можно постепенно заменить:
сетка 5×5 ↓ карта теплицы / occupancy grid BFS ↓ A* / Nav2 простое управление по точкам ↓ Nav2 Controller baseline LaserScan ↓ постоянное обнаружение препятствий фиксированные рабочие позиции ↓ позиции вдоль реальных рядов ручной эксперимент ↓ автономный цикл навигация + техническое зрение + манипулятор
Результат
В новой версии навигационная система стала ближе к автономному роботу.
Самое важное изменение — робот больше не получает информацию:
ты находишься в клетке 0
от пользователя.
Теперь цепочка начинается с восприятия окружающего мира:
камера ↓ ArUco ↓ START ↓ Graph ↓ Planner ↓ Motion
А после движения подключается второй сенсорный канал:
Lidar ↓ ObstacleDetector ↓ blocked marker ↓ Graph ↓ Planner ↓ новый маршрут
То есть получается уже достаточно характерная для робототехники схема:
ВОСПРИЯТИЕ ↓ ЛОКАЛИЗАЦИЯ ↓ ПРЕДСТАВЛЕНИЕ МИРА ↓ ПЛАНИРОВАНИЕ ↓ УПРАВЛЕНИЕ ↓ ДВИЖЕНИЕ ↓ НОВЫЕ ДАННЫЕ ↓ КОРРЕКЦИЯ РЕШЕНИЯ
Пока всё это реализовано на маленькой сетке 5×5 и в симуляции Webots. Но именно такой вариант позволяет отдельно проверить логику каждого уровня перед переходом к более сложной навигации и непосредственно к автоматизированному сбору клубники.
Полный код graph.py
import math class Graph: """ Карта поля 5x5. Нумерация: 20 21 22 23 24 15 16 17 18 19 10 11 12 13 14 5 6 7 8 9 0 1 2 3 4 """ SIZE = 5 MARKER_COUNT = 25 def valid(self, marker): return 0 <= marker < self.MARKER_COUNT def position(self, marker): if not self.valid(marker): raise ValueError( f"Неверный номер маркера: {marker}" ) x = marker % self.SIZE y = marker // self.SIZE return float(x), float(y) def neighbors(self, marker, blocked=None): if blocked is None: blocked = set() if not self.valid(marker): return [] row = marker // self.SIZE col = marker % self.SIZE result = [] # Вправо if col < self.SIZE - 1: result.append(marker + 1) # Вверх if row < self.SIZE - 1: result.append(marker + self.SIZE) # Влево if col > 0: result.append(marker - 1) # Вниз if row > 0: result.append(marker - self.SIZE) return [ m for m in result if m not in blocked ] def direction(self, a, b): ax, ay = self.position(a) bx, by = self.position(b) if bx > ax: return "RIGHT" if bx < ax: return "LEFT" if by > ay: return "UP" if by < ay: return "DOWN" raise ValueError( f"Маркеры {a} и {b} не являются соседними" ) def nearest_marker( self, x, y, max_distance=0.75, excluded=None ): if excluded is None: excluded = set() best_marker = None best_distance = float("inf") for marker in range(self.MARKER_COUNT): if marker in excluded: continue mx, my = self.position(marker) distance = math.hypot( x - mx, y - my ) if distance < best_distance: best_distance = distance best_marker = marker if best_distance > max_distance: return None return best_marker
Полный код main.py
import re from datetime import datetime from pathlib import Path import rclpy from rclpy.node import Node from std_msgs.msg import String from module_b_navigation.graph import Graph from module_b_navigation.planner import Planner from module_b_navigation.motion_controller import Motion from module_b_navigation.obstacle_detector import ObstacleDetector class NavigationNode(Node): def __init__(self): super().__init__( "module_b_navigation" ) # ----------------------------------------- # Параметры # ----------------------------------------- # Стартового параметра БОЛЬШЕ НЕТ. self.declare_parameter( "target_id", 22 ) self.declare_parameter( "aruco_topic", "/RMC2/camera_bottom/aruco_id" ) self.declare_parameter( "aruco_confirmations", 3 ) self.target_id = ( self.get_parameter( "target_id" ).value ) self.aruco_topic = ( self.get_parameter( "aruco_topic" ).value ) self.required_confirmations = ( self.get_parameter( "aruco_confirmations" ).value ) # ----------------------------------------- # Лог # ----------------------------------------- self.log_path = ( self.create_log_file() ) # ----------------------------------------- # Состояние локализации # ----------------------------------------- self.start_id = None self.last_aruco_id = None self.aruco_count = 0 # ----------------------------------------- # Подписка на существующий # ArUco detector # ----------------------------------------- self.aruco_subscription = ( self.create_subscription( String, self.aruco_topic, self.aruco_callback, 10 ) ) self.write_log( "SYSTEM: запуск навигации" ) self.write_log( f"SYSTEM: ArUco topic=" f"{self.aruco_topic}" ) self.write_log( f"SYSTEM: target_id=" f"{self.target_id}" ) self.write_log( "LOCALIZATION: ожидаем " "начальную ArUco-метку" ) def create_log_file(self): log_directory = ( Path.home() / "module_b_logs" ) log_directory.mkdir( parents=True, exist_ok=True ) timestamp = ( datetime.now().strftime( "%Y%m%d_%H%M%S" ) ) path = ( log_directory / f"module_b_{timestamp}.log" ) return path def write_log(self, message): timestamp = ( datetime.now().strftime( "%Y-%m-%d " "%H:%M:%S.%f" )[:-3] ) # Терминал ROS. self.get_logger().info( message ) # Файл. with self.log_path.open( "a", encoding="utf-8" ) as file: file.write( f"[{timestamp}] " f"{message}\n" ) def aruco_callback(self, msg): # После определения стартовой # клетки дальнейшие сообщения # нам для старта не нужны. if self.start_id is not None: return raw = msg.data.strip() self.write_log( f"LOCALIZATION: " f"ArUco message='{raw}'" ) # Ожидаем: # # aruco_17 # # Но извлекаем последнее число, # поэтому tf_namespace тоже # не должен мешать. match = re.search( r"(\d+)$", raw ) if match is None: self.write_log( "LOCALIZATION: " "не удалось извлечь ID" ) return marker_id = int( match.group(1) ) # Проверяем диапазон нашего поля. if not ( 0 <= marker_id < 25 ): self.write_log( f"LOCALIZATION: " f"метка {marker_id} " f"не относится к полю" ) return # ----------------------------------------- # Подтверждение нескольких # одинаковых распознаваний # ----------------------------------------- if ( marker_id == self.last_aruco_id ): self.aruco_count += 1 else: self.last_aruco_id = ( marker_id ) self.aruco_count = 1 self.write_log( f"LOCALIZATION: " f"candidate={marker_id}; " f"confirmations=" f"{self.aruco_count}/" f"{self.required_confirmations}" ) if ( self.aruco_count < self.required_confirmations ): return self.start_id = marker_id self.write_log( f"LOCALIZATION: " f"START_MARKER_CONFIRMED=" f"{self.start_id}" ) def wait_for_start_marker( node ): while ( rclpy.ok() and node.start_id is None ): rclpy.spin_once( node, timeout_sec=0.1 ) return node.start_id def wait_for_odometry( node, motion ): node.write_log( "SYSTEM: ожидаем odometry" ) while ( rclpy.ok() and not motion.odometry_received ): rclpy.spin_once( node, timeout_sec=0.1 ) node.write_log( "SYSTEM: odometry готова" ) def wait_robot( node, motion ): while ( motion.active and rclpy.ok() ): rclpy.spin_once( node, timeout_sec=0.1 ) def wait_for_new_scan( node ): node.write_log( "OBSTACLE: ожидаем свежий LaserScan" ) for _ in range(10): rclpy.spin_once( node, timeout_sec=0.1 ) def main(args=None): rclpy.init( args=args ) node = NavigationNode() graph = Graph() planner = Planner( graph, logger=node.write_log ) motion = Motion( node, graph, logger=node.write_log ) detector = ObstacleDetector( node, graph, logger=node.write_log ) try: # ========================================= # 1. ОПРЕДЕЛЯЕМ СТАРТ ПО ARUCO # ========================================= start = wait_for_start_marker( node ) node.write_log( f"SYSTEM: стартовая клетка={start}" ) target = node.target_id if not graph.valid(target): raise ValueError( f"Неверная целевая " f"клетка: {target}" ) # ========================================= # 2. ЖДЁМ ОДОМЕТРИЮ # ========================================= wait_for_odometry( node, motion ) blocked = set() # ========================================= # 3. ПЛАНИРУЕМ ПУТЬ # ========================================= node.write_log( "SYSTEM: OUTBOUND_PLANNING_START" ) route, distance, turns = ( planner.plan( start, target, blocked ) ) node.write_log( f"SYSTEM: OUTBOUND_ROUTE=" f"{route}" ) node.write_log( f"SYSTEM: distance=" f"{distance}; turns={turns}" ) # ========================================= # 4. ЕДЕМ К ЦЕЛИ # ========================================= node.write_log( "SYSTEM: OUTBOUND_MOVEMENT_START" ) motion.start( route ) wait_robot( node, motion ) node.write_log( "SYSTEM: OUTBOUND_MOVEMENT_DONE" ) # ========================================= # 5. СОХРАНЯЕМ BASELINE ЛИДАРА # ========================================= # На случай, если scan ещё не пришёл. while ( rclpy.ok() and detector.scan is None ): rclpy.spin_once( node, timeout_sec=0.1 ) if not detector.save_baseline(): node.write_log( "SYSTEM: невозможно " "сохранить baseline" ) return # ========================================= # 6. СТАВИМ ПРЕПЯТСТВИЕ # ========================================= print() print( "Поставьте ОДНО препятствие " "в Webots." ) print( "После установки введите y." ) print() while True: answer = input( "Препятствие установлено? [y]: " ) if ( answer .strip() .lower() == "y" ): break node.write_log( "SYSTEM: пользователь " "подтвердил установку препятствия" ) # ========================================= # 7. ПОЛУЧАЕМ НОВЫЙ SCAN # ========================================= wait_for_new_scan( node ) # ========================================= # 8. ОПРЕДЕЛЯЕМ ПРЕПЯТСТВИЕ # ========================================= blocked_marker = ( detector.find_obstacle( motion.x, motion.y, motion.yaw, excluded={ start, target } ) ) if blocked_marker is None: node.write_log( "SYSTEM: препятствие " "не обнаружено" ) return blocked = { blocked_marker } node.write_log( f"SYSTEM: BLOCKED_MARKERS=" f"{sorted(blocked)}" ) # ========================================= # 9. СТРОИМ ОБРАТНЫЙ МАРШРУТ # ========================================= node.write_log( "SYSTEM: RETURN_PLANNING_START" ) return_route, distance, turns = ( planner.plan( target, start, blocked ) ) node.write_log( f"SYSTEM: RETURN_ROUTE=" f"{return_route}" ) # ========================================= # 10. ВОЗВРАЩАЕМСЯ # ========================================= node.write_log( "SYSTEM: RETURN_MOVEMENT_START" ) motion.start( return_route ) wait_robot( node, motion ) node.write_log( "SYSTEM: RETURN_MOVEMENT_DONE" ) node.write_log( "SYSTEM: MODULE_B_COMPLETE" ) except KeyboardInterrupt: node.write_log( "SYSTEM: остановлено пользователем" ) except Exception as error: node.write_log( f"SYSTEM: ERROR: {error}" ) raise finally: motion.stop() node.write_log( "SYSTEM: shutdown" ) node.write_log( f"SYSTEM: log_file=" f"{node.log_path}" ) node.destroy_node() rclpy.shutdown() if __name__ == "__main__": main()
Полный код motion_controller.py
import math from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry class Motion: def __init__( self, node, graph, logger=None ): self.node = node self.graph = graph self.logger = logger self.publisher = node.create_publisher( Twist, "/RMC2/cmd_vel", 10 ) self.subscription = node.create_subscription( Odometry, "/RMC2/odometry", self.odometry_callback, 10 ) self.timer = node.create_timer( 0.05, self.control ) self.x = 0.0 self.y = 0.0 self.yaw = 0.0 self.odometry_received = False self.route = [] self.route_index = 0 self.active = False self.linear_speed = 0.20 self.angular_speed = 0.60 self.distance_tolerance = 0.12 self.angle_tolerance = math.radians( 5.0 ) # Для ограничения количества # телеметрических сообщений. self.last_telemetry_time = 0 self.last_state = None def log(self, message): if self.logger is not None: self.logger( f"MOTION: {message}" ) def odometry_callback(self, msg): self.x = ( msg.pose.pose.position.x ) self.y = ( msg.pose.pose.position.y ) q = msg.pose.pose.orientation siny_cosp = ( 2.0 * ( q.w * q.z + q.x * q.y ) ) cosy_cosp = ( 1.0 - 2.0 * ( q.y * q.y + q.z * q.z ) ) self.yaw = math.atan2( siny_cosp, cosy_cosp ) if not self.odometry_received: self.odometry_received = True self.log( "первое сообщение odometry получено: " f"x={self.x:.3f}, " f"y={self.y:.3f}, " f"yaw={math.degrees(self.yaw):.1f} deg" ) @staticmethod def angle_to_pi(angle): while angle > math.pi: angle -= 2.0 * math.pi while angle < -math.pi: angle += 2.0 * math.pi return angle def start(self, route): if not route: self.log( "получен пустой маршрут" ) self.stop() return self.route = list(route) # route[0] — клетка, # на которой робот уже находится. self.route_index = 1 if len(self.route) == 1: self.log( "движение не требуется" ) self.stop() return self.active = True self.last_state = None self.log( f"начало движения; route={self.route}" ) self.log( f"следующая клетка={self.route[1]}" ) def stop(self): cmd = Twist() self.publisher.publish(cmd) was_active = self.active self.active = False if was_active: self.log( "робот остановлен" ) def set_state(self, state): if state == self.last_state: return self.last_state = state self.log( f"state={state}" ) def telemetry( self, target_marker, distance, angle_error ): now_ns = ( self.node.get_clock() .now() .nanoseconds ) # Раз в секунду. if ( now_ns - self.last_telemetry_time < 1_000_000_000 ): return self.last_telemetry_time = now_ns self.log( f"x={self.x:.2f} " f"y={self.y:.2f} " f"target={target_marker} " f"distance={distance:.2f} " f"angle_error=" f"{math.degrees(angle_error):.1f}deg" ) def control(self): if not self.active: return if not self.odometry_received: return if self.route_index >= len( self.route ): self.log( "маршрут завершён" ) self.stop() return target_marker = ( self.route[ self.route_index ] ) target_x, target_y = ( self.graph.position( target_marker ) ) dx = target_x - self.x dy = target_y - self.y distance = math.hypot( dx, dy ) target_yaw = math.atan2( dy, dx ) angle_error = self.angle_to_pi( target_yaw - self.yaw ) self.telemetry( target_marker, distance, angle_error ) # ----------------------------------------- # Маркер достигнут # ----------------------------------------- if distance <= self.distance_tolerance: self.log( f"клетка достигнута: " f"{target_marker}" ) self.route_index += 1 if self.route_index >= len( self.route ): self.log( "конечная точка маршрута достигнута" ) self.stop() return next_marker = ( self.route[ self.route_index ] ) self.log( f"следующая клетка={next_marker}" ) return cmd = Twist() # ----------------------------------------- # Сначала разворачиваемся # ----------------------------------------- if ( abs(angle_error) > self.angle_tolerance ): self.set_state( "ROTATING" ) cmd.linear.x = 0.0 if angle_error > 0: cmd.angular.z = ( self.angular_speed ) else: cmd.angular.z = ( -self.angular_speed ) # ----------------------------------------- # Потом едем # ----------------------------------------- else: self.set_state( "DRIVING" ) cmd.linear.x = ( self.linear_speed ) correction = ( 1.2 * angle_error ) correction = max( -0.30, min( 0.30, correction ) ) cmd.angular.z = correction self.publisher.publish(cmd)
Полный код obstacle_detector.py
import math from sensor_msgs.msg import LaserScan from rclpy.qos import qos_profile_sensor_data class ObstacleDetector: def __init__( self, node, graph, logger=None ): self.node = node self.graph = graph self.logger = logger self.scan = None self.baseline = None self.min_change = 0.20 self.max_marker_distance = 0.75 self.min_votes = 3 self.subscription = ( node.create_subscription( LaserScan, "/RMC2/scan", self.scan_callback, qos_profile_sensor_data ) ) self.log( "детектор препятствий создан" ) def log(self, message): if self.logger is not None: self.logger( f"OBSTACLE: {message}" ) def scan_callback(self, msg): first_scan = self.scan is None self.scan = msg if first_scan: self.log( "первый LaserScan получен" ) def save_baseline(self): if self.scan is None: self.log( "baseline сохранить невозможно: " "LaserScan отсутствует" ) return False self.baseline = list( self.scan.ranges ) self.log( f"baseline сохранён; " f"rays={len(self.baseline)}" ) return True def find_obstacle( self, robot_x, robot_y, robot_yaw, excluded=None ): if excluded is None: excluded = set() self.log( "начало поиска нового препятствия" ) if self.scan is None: self.log( "LaserScan отсутствует" ) return None if self.baseline is None: self.log( "baseline отсутствует" ) return None votes = {} points = {} changed_points = 0 scan_count = min( len(self.baseline), len(self.scan.ranges) ) for i in range(scan_count): old_range = ( self.baseline[i] ) new_range = ( self.scan.ranges[i] ) # Новый луч должен быть валидным. if not math.isfinite( new_range ): continue if ( new_range < self.scan.range_min or new_range > self.scan.range_max ): continue changed = False # Раньше ничего не было, # теперь появился объект. if not math.isfinite( old_range ): changed = True # Объект стал минимум на # min_change ближе. elif ( old_range - new_range >= self.min_change ): changed = True if not changed: continue changed_points += 1 angle = ( self.scan.angle_min + i * self.scan.angle_increment ) # Координаты точки # относительно робота. local_x = ( new_range * math.cos(angle) ) local_y = ( new_range * math.sin(angle) ) # Переводим в мировые координаты. world_x = ( robot_x + local_x * math.cos(robot_yaw) - local_y * math.sin(robot_yaw) ) world_y = ( robot_y + local_x * math.sin(robot_yaw) + local_y * math.cos(robot_yaw) ) marker = ( self.graph.nearest_marker( world_x, world_y, max_distance=( self.max_marker_distance ), excluded=excluded ) ) if marker is None: continue votes[marker] = ( votes.get(marker, 0) + 1 ) if marker not in points: points[marker] = [] points[marker].append( (world_x, world_y) ) self.log( f"изменившихся lidar-точек=" f"{changed_points}" ) self.log( f"голоса по клеткам={votes}" ) if not votes: self.log( "препятствие не найдено" ) return None best_marker = max( votes, key=votes.get ) best_votes = votes[ best_marker ] if best_votes < self.min_votes: self.log( f"недостаточно голосов: " f"marker={best_marker}; " f"votes={best_votes}" ) return None marker_points = points[ best_marker ] average_x = sum( point[0] for point in marker_points ) / len(marker_points) average_y = sum( point[1] for point in marker_points ) / len(marker_points) self.log( f"препятствие обнаружено: " f"marker={best_marker}; " f"votes={best_votes}; " f"world≈(" f"{average_x:.2f}, " f"{average_y:.2f})" ) return best_marker
Полный код planner.py
from collections import deque class Planner: def __init__(self, graph, logger=None): self.graph = graph self.logger = logger def log(self, message): if self.logger is not None: self.logger( f"PLANNER: {message}" ) def count_turns(self, route): if len(route) < 3: return 0 turns = 0 old_direction = self.graph.direction( route[0], route[1] ) for i in range(1, len(route) - 1): new_direction = self.graph.direction( route[i], route[i + 1] ) if new_direction != old_direction: turns += 1 old_direction = new_direction return turns def plan( self, start, target, blocked=None ): if blocked is None: blocked = set() self.log( f"планирование {start} -> {target}; " f"blocked={sorted(blocked)}" ) if not self.graph.valid(start): raise ValueError( f"Неверный стартовый маркер: {start}" ) if not self.graph.valid(target): raise ValueError( f"Неверный целевой маркер: {target}" ) if start in blocked: raise ValueError( "Стартовая клетка заблокирована" ) if target in blocked: raise ValueError( "Целевая клетка заблокирована" ) if start == target: self.log( "робот уже находится в целевой клетке" ) return [start], 0, 0 queue = deque() queue.append([start]) found_routes = [] shortest_length = None while queue: route = queue.popleft() # Если уже нашли кратчайшие пути, # более длинные больше не рассматриваем. if ( shortest_length is not None and len(route) > shortest_length ): break current = route[-1] if current == target: if shortest_length is None: shortest_length = len(route) found_routes.append(route) continue for next_marker in self.graph.neighbors( current, blocked ): if next_marker in route: continue new_route = route + [next_marker] queue.append(new_route) if not found_routes: self.log( "маршрут не найден" ) raise RuntimeError( f"Нет маршрута {start} -> {target}" ) best_route = min( found_routes, key=lambda route: ( self.count_turns(route), route ) ) distance = len(best_route) - 1 turns = self.count_turns( best_route ) self.log( f"маршрут найден: {best_route}; " f"distance={distance}; " f"turns={turns}" ) return ( best_route, distance, turns )
