В прошлых статьях я тезисно рассказывал о своих роботах для сбора клубники. Теперь хотелось бы показать их подробнее и постепенно разобрать программную часть проекта — от относительно простых вещей до технического зрения, работы манипулятора и непосредственно поиска и сбора ягоды.
В этой статье начну с мобильной платформы и довольно простой задачи: заставим робота самостоятельно пройти заданный маршрут.
Немного о самих роботах
Основная идея проекта - мобильный робот, который может перемещаться вдоль посадок клубники, находить ягоды и собирать их с помощью установленного на платформе манипулятора.
Конструкция сейчас выглядит так:
На мобильной платформе на данный момент установлен 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, чтобы постепенно перейти от такой условной сетки к полноценной навигации.
