Комплексное робототехническое решение для сбора клубники. Часть 1

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

В этой статье начну с мобильной платформы и довольно простой задачи: заставим робота самостоятельно пройти заданный маршрут.

Немного о самих роботах

Основная идея проекта - мобильный робот, который может перемещаться вдоль посадок клубники, находить ягоды и собирать их с помощью установленного на платформе манипулятора.

Конструкция сейчас выглядит так:

На мобильной платформе на данный момент установлен 3-осевой манипулятор с плоским захватом. Планируется установка полноразмерного 6-осевого манипулятора, но платформа не подходит для такого. В конструкции используются магнитные концевики и энкодеры. Подробнее механику манипулятора, кинематику и нижний уровень управления я хочу разобрать отдельно - здесь они нам пока практически не понадобятся.

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

При этом делать манипулятор с огромной рабочей зоной нет особого смысла. Гораздо удобнее разделить движение системы на два уровня.

Мобильная платформа выполняет грубое позиционирование всего робота относительно грядки, а манипулятор — точное позиционирование относительно конкретной ягоды.

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

Именно этим и займёмся дальше.

Что будем делать

В данном случае мы начнем рассматривать комплексную навигацию от простого к сложному. Для эксперимента я максимально упростил пространство, в котором работает робот.

Представим его как поле 5×5:

24  19  14   9   4
23  18  13   8   3
22  17  12   7   2
21  16  11   6   1
20  15  10   5   0

Каждая точка соответствует определённому положению мобильной платформы. Расстояние между соседними точками в текущем варианте принимается равным одному метру. Именно такая нумерация заложена в программе.

У робота будет три основных положения: Стартовая позиция, место сбора и место складирования. Кроме этого, некоторые клетки можно объявить запрещёнными, чтобы робот их избегал. Это служит эмуляцией ситуаций, когда часть пространства перекрыта. Программа должна сама найти маршрут в обход запрещённых клеток, физически провести по нему робота, выполнить операцию с лифтом и затем вернуть платформу обратно. Ее вполне можно разбить на несколько блоков.

Часть 1. Поле и поиск маршрута

Начнем вообще без ROS2. Сначала нам нужно описать наше поле и научиться находить путь между двумя клетками. Для этого был использован обычный BFS - поиск в ширину.

import math
import time
from collections import deque

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import String, Float64

GRID_SIZE = 5
CELL_STEP = 1.0

LINEAR_SPEED = 0.15
ANGULAR_SPEED = 0.35

TARGET_DISTANCE = 0.10
ANGLE_TOLERANCE = math.radians(5)

LIFT_UP_VALUE = 1.0
LIFT_DOWN_VALUE = 0.0
LIFT_TIMEOUT = 15.0

robot = {
    "x": 0.0,
    "y": 0.0,
    "yaw": 0.0,
    "odom_received": False
}

start_cell = 0

start_odom_x = 0.0
start_odom_y = 0.0
start_odom_yaw = 0.0

def cell_to_grid(cell):
    grid_y = cell % GRID_SIZE
    grid_x = GRID_SIZE - 1 - cell // GRID_SIZE
    return grid_x, grid_y

def get_neighbors(cell):
    neighbors = []

    row = cell % GRID_SIZE
    column = cell // GRID_SIZE

    if row < GRID_SIZE - 1:
        neighbors.append(cell + 1)

    if row > 0:
        neighbors.append(cell - 1)

    if column < GRID_SIZE - 1:
        neighbors.append(cell + GRID_SIZE)

    if column > 0:
        neighbors.append(cell - GRID_SIZE)

    return neighbors

def bfs(start, target, forbidden):
    if start == target:
        return [start]

    queue = deque([[start]])
    visited = {start}

    while queue:
        route = queue.popleft()
        current = route[-1]

        for neighbor in get_neighbors(current):
            if neighbor in forbidden or neighbor in visited:
                continue

            new_route = route + [neighbor]

            if neighbor == target:
                return new_route

            visited.add(neighbor)
            queue.append(new_route)

    return []

Не буду заострять внимание на работе BFS, скажу лишь, что суть алгоритма в том, что поиск начинает распространяться по карте как волна. Если какая-то клетка находится в forbidden, алгоритм просто не рассматривает её. В результате мы получаем не координаты движения, а пока только последовательность номеров. Теперь понятно куда ехать, но пока не ясно как именно.

Часть 2. Переходим от клеток к реальным координатам

Следующая проблема - BFS работает с номерами клеток, а мобильный робот ничего о клетках не знает. От ROS2 мы получаем обычную одометрию, содержащую x, y, yaw. В связи с этим появляется необходимость перевода маршрута из клеток в реальные координаты.

Для этого используются три функции:

def quaternion_to_yaw(q):
    sin_yaw = 2.0 * (q.w * q.z + q.x * q.y)
    cos_yaw = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
    return math.atan2(sin_yaw, cos_yaw)

def normalize_angle(angle):
    while angle > math.pi:
        angle -= 2.0 * math.pi

    while angle < -math.pi:
        angle += 2.0 * math.pi

    return angle

def cell_to_world(cell):
    target_grid_x, target_grid_y = cell_to_grid(cell)
    start_grid_x, start_grid_y = cell_to_grid(start_cell)

    dx_cells = target_grid_x - start_grid_x
    dy_cells = target_grid_y - start_grid_y

    forward = dy_cells * CELL_STEP
    left = -dx_cells * CELL_STEP

    forward_x = forward * math.cos(start_odom_yaw)
    forward_y = forward * math.sin(start_odom_yaw)

    left_x = -left * math.sin(start_odom_yaw)
    left_y = left * math.cos(start_odom_yaw)

    world_x = start_odom_x + forward_x + left_x
    world_y = start_odom_y + forward_y + left_y

    return world_x, world_y

Здесь есть важный момент. Нет привязки сетки к какой-то заранее известной глобальной системе координат. В момент запуска запоминаются реальные показания одометрии и уже относительно этой позиции строится наше условное поле, то есть стартовая ориентация робота принимается за направление «вверх» по сетке. Именно такую схему использует исходный алгоритм преобразования клетки в odom.

Часть 3. ROS2 и физическое движение робота

Теперь появляется непосредственно мобильная платформа. Стоит сразу оговориться, что действие будет происходить в симуляторе, на реальном роботе покажу тесты немного позднее. Для управления создадим ноды и подключимся к двум основным топикам: cmd_vel и odometry. Первый используется для управления скоростью, второй - для получения текущего положения.

class RMC2Controller(Node):

    def __init__(self):
        super().__init__("rmc2_task")

        self.cmd_pub = self.create_publisher(
            Twist,
            "/RMC2/cmd_vel",
            10
        )

        self.create_subscription(
            Odometry,
            "/RMC2/odometry",
            self.odom_callback,
            10
        )

        self.lift_pub = self.create_publisher(
            Float64,
            "/RMC2/lift",
            10
        )

        self.lift_status = None

        self.create_subscription(
            String,
            "/RMC2/lift_status",
            self.lift_status_callback,
            10
        )

    def odom_callback(self, msg):
        robot["x"] = msg.pose.pose.position.x
        robot["y"] = msg.pose.pose.position.y
        robot["yaw"] = quaternion_to_yaw(
            msg.pose.pose.orientation
        )
        robot["odom_received"] = True

    def lift_status_callback(self, msg):
        self.lift_status = msg.data.strip().lower()

    def send_speed(self, linear, angular):
        msg = Twist()
        msg.linear.x = linear
        msg.angular.z = angular
        self.cmd_pub.publish(msg)

    def stop(self):
        self.send_speed(0.0, 0.0)

    def move_to_cell(self, cell):
        target_x, target_y = cell_to_world(cell)

        while rclpy.ok():
            rclpy.spin_once(self, timeout_sec=0.02)

            x = robot["x"]
            y = robot["y"]
            yaw = robot["yaw"]

            dx = target_x - x
            dy = target_y - y

            distance = math.hypot(dx, dy)

            if distance < TARGET_DISTANCE:
                self.stop()
                return

            target_angle = math.atan2(dy, dx)
            angle_error = normalize_angle(target_angle - yaw)

            if abs(angle_error) > ANGLE_TOLERANCE:
                angular = (
                    ANGULAR_SPEED
                    if angle_error > 0
                    else -ANGULAR_SPEED
                )

                self.send_speed(0.0, angular)

            else:
                self.send_speed(LINEAR_SPEED, 0.0)

    def follow_route(self, route):
        for cell in route[1:]:
            self.move_to_cell(cell)

        self.stop()

Для очередной точки вычисляется вектор: текущее положение - цель, по нему получаем расстояние до цели и необходимый угол.

Дальше используется очень простой алгоритм управления:

угол неправильный?        ↓ да
поворачиваемся        ↓ нет
едем прямо        ↓
проверяем расстояние        ↓
точка достигнута

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

Часть 4. Работа с лифтом

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

    def move_lift(self, value):
        self.stop()

        self.lift_status = None

        msg = Float64()
        msg.data = float(value)
        self.lift_pub.publish(msg)

        moving_seen = False
        start_time = time.monotonic()

        while rclpy.ok():
            rclpy.spin_once(self, timeout_sec=0.1)

            status = self.lift_status

            if status == "moving":
                moving_seen = True

            elif moving_seen and status in ("raised", "lowered"):
                return True

            if time.monotonic() - start_time > LIFT_TIMEOUT:
                return False

    def lift_up(self):
        return self.move_lift(LIFT_UP_VALUE)

    def lift_down(self):
        return self.move_lift(LIFT_DOWN_VALUE)

Здесь я специально не продолжаю движение сразу после отправки команды. Сначала платформа останавливается, затем отправляется команда лифту, и программа ждёт подтверждения его движения. Дополнительно используется таймаут. Если за 15 секунд механизм не подтвердил завершение операции, выполнение не должно бесконечно зависнуть в ожидании.

Часть 5. Собираем всё в одно задание

Осталось соединить поиск пути, одометрию, движение и лифт.

Для статьи я убрал большую часть консольного интерфейса исходной программы и оставил саму последовательность работы.

def main():
    global start_cell
    global start_odom_x
    global start_odom_y
    global start_odom_yaw

    start_cell = int(input("Начальная точка: "))
    storage_cell = int(input("Точка хранения: "))
    picking_cell = int(input("Точка комплектации: "))

    text = input("Запретные точки через пробел: ")
    forbidden = set(map(int, text.split())) if text.strip() else set()

    route_storage = bfs(
        start_cell,
        storage_cell,
        forbidden
    )

    route_picking = bfs(
        storage_cell,
        picking_cell,
        forbidden
    )

    route_home = bfs(
        picking_cell,
        start_cell,
        forbidden
    )

    if not route_storage or not route_picking or not route_home:
        print("Маршрут не найден")
        return

    rclpy.init()
    controller = RMC2Controller()

    while rclpy.ok() and not robot["odom_received"]:
        rclpy.spin_once(controller, timeout_sec=0.1)

    start_odom_x = robot["x"]
    start_odom_y = robot["y"]
    start_odom_yaw = robot["yaw"]

    try:
        controller.follow_route(route_storage)

        if not controller.lift_up():
            return

        controller.follow_route(route_picking)

        if not controller.lift_down():
            return

        controller.follow_route(route_home)

    finally:
        controller.stop()
        controller.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

Теперь уже хорошо видна логика задания целиком. Сначала программа получает три точки и список препятствий. BFS независимо строит три маршрута: старт - сбор, сбор - складирование, складирование - старт После подключения к ROS2 программа ждёт первую одометрию и запоминает реальное начальное положение платформы. А затем выполняется само задание - сбор и складирование уже подготовленных ягод.

Что получилось

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

И это пока намеренно простой вариант. Здесь ещё нет полноценной локализации по карте, сложного планировщика траекторий и других вещей, которые обычно ожидаешь увидеть в автономном мобильном роботе.

Следующий шаг уже интереснее: роботу недостаточно знать, что его стартовая точка — условный 0. Он должен сам определить, где находится, а затем уже построить маршрут. И здесь в систему можно добавить лидар и SLAM, чтобы постепенно перейти от такой условной сетки к полноценной навигации.