Полная система для сбора клубники должна объединять сразу несколько достаточно разных задач:

навигация    ↓
поиск рабочей зоны    ↓
техническое зрение    ↓
поиск спелой ягоды    ↓
манипулятор    ↓
захват

Разрабатывать всё одновременно неудобно, поэтому проект был разбит на отдельные модули.

В этой статье займёмся мобильной частью.

Пока робот ещё не ищет клубнику. Его задача проще: определить своё положение, построить маршрут до нужной рабочей зоны, проехать по нему, обнаружить появившееся препятствие и при необходимости перестроить маршрут.

Для экспериментов используются ROS 2, Python и Webots.

Упрощаем теплицу

На первом этапе реальную теплицу заменим сеткой 5×5:

мобильная платформа        +
навигация        +
лидар        +
техническое зрение        +
поиск спелой клубники        +
манипулятор        +
захват

Всего получается 25 возможных положений робота.

Каждой позиции соответствует ArUco-маркер. Например, робот может физически находиться над меткой 7, а рабочая зона располагается около клетки 22, тогда необходимо построить маршрут от клетки 7 до клетки 22.

Определяем положение по ArUco

Снизу мобильной платформы расположена камера.

Отдельный ArUco-детектор обрабатывает её изображение и публикует обнаруженную метку в ROS2. Навигационной программе поэтому вообще не нужно заниматься обработкой изображения. Она получает уже готовый ID. Но принимать первое же распознавание тоже рискованно.

Например, камера может получить:

aruco_17
aruco_17
aruco_8
aruco_17
aruco_17

Единичная ошибка не должна заставлять робота считать, что он внезапно оказался на клетке 8. Поэтому используется простое подтверждение. Нам нужны три одинаковых последовательных определения, теперь можно переходить к построению маршрута.

Представляем поле как граф

За карту отвечает класс Graph.

В нашем случае:

class Graph:    SIZE = 5    MARKER_COUNT = 25

Номер клетки можно довольно просто преобразовать в координаты, где x = 17 % 5, y = 17 // 5, то есть x = 2, y = 3. Операция % возвращает остаток от деления, а // выполняет целочисленное деление. Благодаря этому не требуется вручную хранить таблицу всех 25 координат. Для каждой клетки также определяются соседи. Именно из таких связей постепенно формируется граф, по которому робот будет искать путь.

Ищем маршрут с помощью BFS

Допустим:

START  = 7
TARGET = 22

Теперь нужно найти кратчайший путь.

Для этого используется BFS — Breadth-First Search, или поиск в ширину. Его удобно представить как волну. Если стартовать с клетки 0, сначала проверяются все клетки, находящиеся на расстоянии одного перехода, затем двух, трех и так далее.

Используется очередь, в начале в неё помещается стартовый маршрут:

from collections import deque
queue.append([start])
new_route = route + [next_marker]

Затем берём маршрут и добавляем к нему соседнюю клетку:

Для огромной карты хранить в очереди целые маршруты было бы не самым экономным решением, но для поля из 25 клеток такой вариант очень нагляден. Нам желательно получить не только короткий маршрут, но и уменьшить количество поворотов.

Например, по количеству переходов они могут быть одинаковыми, но второй маршрут требует меньше изменений направления, поэтому среди маршрутов минимальной длины программа дополнительно сравнивает количество поворотов.

Превращаем маршрут в движение

За движение отвечает отдельный Motion, он получает данные одометрии и отправляет команды скорости через стандартное ROS2 сообщение Twist.

Схема получается такой:

маршрут   ↓
следующая клетка   ↓
координаты X/Y   ↓
где робот сейчас?   ↓
куда нужно повернуться?   ↓
Twist   ↓
робот

Управляющий цикл выполняется примерно 20 раз в секунду.

На каждом шаге вычисляется расстояние до следующей точки, целевое направление, после чего происходит сравнение с текущим направлением

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

После выравнивания начинаем движение вперёд. Во время движения остаётся небольшая коррекция курса:

correction = 1.2 * angle_error

Чем больше ошибка направления, тем сильнее робот корректирует курс. Когда расстояние до очередной точки становится достаточно маленьким, считаем клетку достигнутой и переходим к следующей. Так робот последовательно выполняет маршрут, построенный BFS.

Но что произойдёт, если дорога перекрыта?

Теперь добавим вторую часть эксперимента. Пусть робот успешно приехал к рабочей позиции.

Сохраняем показания 2D-лидара, после этого в симуляторе ставим препятствие и получаем новый LaserScan. Сравниваем два измерения, получаем разницу, которая означает, что перед лучом появился новый объект. Каждый луч лидара содержит расстояние и угол, можно перевести их в локальные декартовы координаты так:

local_x = distance * math.cos(angle)
local_y = distance * math.sin(angle)

Но эти координаты относятся к самому роботу. Чтобы понять, какая клетка карты занята, точку необходимо перевести в мировую систему координат с учётом положения и ориентации платформы. Теперь Graph ищет ближайший маркер.

Перестраиваем маршрут

После обнаружения препятствия снова запускается тот же BFS. Но теперь граф выглядит немного иначе. Например, клетка 17 оказалась занята. При запросе соседей Graph просто исключает заблокированную клетку, поэтому планировщик вынужден найти другой путь.

В этом и состоит основной эксперимент.

Зачем здесь одновременно ArUco, одометрия и лидар

На первый взгляд кажется, что датчики частично дублируют друг друга. На самом деле каждый решает свою задачу.

ArUco отвечает на вопрос: На какой логической позиции робот начинает работу?

Одометрия: Где робот находится во время движения?

Лидар: Что находится вокруг робота?

А планировщик работает уже с результатами этих измерений. Получается уже довольно характерная для робототехники архитектура: восприятие окружающей среды отделено от планирования и непосредственного управления приводами.

Как это связано с клубникой

Пока робот всё ещё не умеет собирать ягоды. Текущий модуль решает только задачу доставки будущей системы технического зрения и манипулятора в нужную рабочую область.

В дальнейшем общий цикл должен выглядеть примерно так:

НАВИГАЦИЯ    ↓
робот приехал к растениям    ↓
ТЕХНИЧЕСКОЕ ЗРЕНИЕ    ↓
найдена спелая клубника    ↓
получено положение ягоды    ↓
МАНИПУЛЯТОР    ↓
подвод захвата    ↓
сбор    ↓
укладка ягоды    ↓
следующая рабочая зона

Таким образом, навигация и манипулятор являются двумя разными уровнями одной системы.

В предыдущем эксперименте с рукой мы управляли отдельными joint и захватом. Здесь решается задача более высокого уровня — доставить этот манипулятор к нужному участку растений.

Результат

В результате получился небольшой навигационный прототип, который объединяет сразу несколько базовых элементов робототехники:

КАМЕРА   ↓
ArUco   ↓
локализация   ↓
Graph   ↓
BFS   ↓
маршрут   ↓
Odometry   ↓
управление движением   ↓
Lidar   ↓
обнаружение препятствия   ↓
изменение карты   ↓
повторное планирование

Пока всё это работает на маленькой сетке и в симуляции. Но задача этого этапа и не заключалась в создании готового сельскохозяйственного робота.

А дальше этот навигационный модуль уже можно объединять с манипулятором и системой технического зрения — и постепенно переходить от движения по условной теплице непосредственно к автоматизированному сбору клубники.