Создание манипулятора на Arduino с openCV

Введение

Здравствуй, читатель! Меня зовут Алексей Морозов, и в этой статье мы разберём полный путь по созданию автономного манипулятора, от выбора компонентов до интеграции с компьютерным зрением и моделями YOLO! Приступим к работе.

Концепция

Техническое задание такое:

Сделать манипулятор способный брать кубик 10*5*5 см, поднимать его и перемещать его по оси X

Итак, по ТЗ нам не нужен точный контроль по осям кроме x, как и расположение в любой точке рабочей зоны. При таких критериях идеально подходит концепция палки с захватом.

Подбор комплектующих. Более детальная «прорисовка»

В качестве направляющей оси x я советую использовать квадратный конструкционный профиль. Для небольшого манипулятора, как этот, я взял профиль 20×20 мм. Он обладает отличной стойкостью на изгиб, он дешёвый и его можно купить на любом маркетплейсе.

Комплектующие в картинках (трафик)

В качестве привода оси Х я советую использовать шаговые двигатели, например, nema17, которые тоже отличаются своей доступностью и распространённостью. На мотор не будет больших нагрузок, так что short-версия мотора справится с задачей.

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

Для приводов манипулятора я буду использовать сервоприводы mg996r, т.к они относительно дешёвые при достаточной мощности. Брать нужно обязательно с металлическим редуктором.

Для манипулятора я решил взять готовый вариант с озона и модернизировать ему клещи.

Моддинг захвата

Манипулятор и шаговый двигатель надо куда‑то крепить, для этого используется каретка, которая подходит к вашему профилю.

Проектирование

Давайте по порядку, что нужно спроектировать:

1. Крепление мотора.

2. Крепление манипулятора.

3. Мод захвата.

---

Крепление мотора

Всё зависит от вашего мотора, но вот краткая текстовая инструкция:

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

• Печатаете или изготавливаете его другим способом, находим недостатки и косяки

• Исправляем то, что нашли.

Получается колобок уголок!

Манипулятор с креплением

Сборка

Итак, у меня есть всё, чтобы собрать манипулятор. На этом шаге останавливаться не будем, но сразу скажу что лучше манипулятор расположить на условном листе фанеры, для удобного программирования. У меня это были перфорированные вафли, как у turtle bot 3.

Программирование Low‑level

В качестве контроллера взял arduino uno с proto sheld’ом v3.

Вот такой код я использую для этого уровня:

// --- НАСТРОЙКИ И ПОДКЛЮЧЕНИЕ БИБЛИОТЕК ---// Коэффициент пересчета для оси X (умножает входное значение на 1)#define Kx 1// Пин, к которому подключен сервопривод оси Y#define Y_PIN 3// Пин, к которому подключен сервопривод захвата (грейфера)#define GRAB_PIN 8// Подключение библиотеки для управления шаговым двигателем (в режиме 2-х проводов)#include "GyverStepper2.h"// Подключение стандартной библиотеки для управления сервоприводами#include "Servo.h"// --- ОБЪЯВЛЕНИЕ ПЕРЕМЕННЫХ И ОБЪЕКТОВ ---// Создание объекта шагового двигателя.// <STEPPER2WIRE> - режим работы (2 управляющих провода).// 2048 - количество шагов на один полный оборот вала.// 2, 5 - пины управления драйвером.GStepper2<STEPPER2WIRE> x(2048, 2, 5);// Создание объектов для управления сервоприводамиServo y;      // Ось YServo grab;   // Захват// Переменные для хранения команд, полученных по SerialString key = "";   // Ключ команды (например, "x", "y", "grab")String val = "";   // Значение команды (например, "100")// --- ФУНКЦИИ ОБРАБОТКИ ДАННЫХ ---/** * Функция считывает данные из последовательного порта (Serial). * Ожидаемый формат: "ключ:значение\n" (например, "x:100\n") */void get_data() {  if (Serial.available()) {    // Читаем строку до двоеточия — это ключ команды    key = Serial.readStringUntil(':');    // Читаем строку до символа новой строки — это значение команды    val = Serial.readStringUntil('\n');    // Выводим полученные данные в монитор порта для отладки    Serial.println(key);    Serial.println(val);    Serial.println();  }}/** * Функция парсинга (разбора) полученной команды. * В зависимости от ключа вызывает соответствующую функцию установки. */void parse_data(String key, String val) {  switch (key) {    case "x":      // Если ключ "x" — управление шаговым двигателем      set_x(val);      break;    case "y":      // Если ключ "y" — управление сервоприводом оси Y      set_y(val);      break;    case "grab":   // Если ключ "grab" — управление захватом      set_grab(val);      break;  }}/** * Устанавливает целевую позицию для шагового двигателя по оси X. * @param val Строка с координатой. */void set_x(String val) {  // Преобразуем строку в целое число  int pos = val.toInt();  // Проверяем, не занят ли двигатель выполнением предыдущей команды  if (x.ready()) {    // Устанавливаем новую цель. Значение умножается на коэффициент Kx.    x.setTarget(pos * Kx);  }}/** * Устанавливает угол поворота сервопривода оси Y. * @param val Строка с углом в градусах. */void set_y(String val) {  int angle = val.toInt();  y.write(angle); // Поворот сервопривода на указанный угол}/** * Устанавливает угол поворота сервопривода захвата. * @param val Строка с углом в градусах. */void set_grab(String val) {  int angle = val.toInt();  grab.write(angle); // Поворот захвата на указанный угол}// --- НАСТРОЙКА ОБОРУДОВАНИЯ (setup) ---void setup() {  // Запуск последовательного порта для связи с компьютером  Serial.begin(115200);  // Подключение сервоприводов к указанным пинам  y.attach(Y_PIN);  grab.attach(GRAB_PIN);  // Настройка параметров движения шагового двигателя  x.setAcceleration(200);   // Устанавливаем ускорение (шагов/сек^2)  x.setMaxSpeed(100);       // Устанавливаем максимальную скорость (шагов/сек)  x.setTarget(0);           // Инициализируем двигатель в нулевой позиции}// --- ОСНОВНОЙ ЦИКЛ РАБОТЫ (loop) ---void loop() {  // Метод tick() необходимо вызывать постоянно.  // Он проверяет текущее положение двигателя и плавно двигает его к цели.  x.tick();  // Используем таймер для опроса порта не в каждом цикле loop(),  // а с интервалом в 10 миллисекунд, чтобы не перегружать процессор.  static uint32_t tmr;  if (millis() - tmr >= 10) {    tmr = millis();         // Обновляем таймер    get_data();             // Проверяем наличие новых данных в Serial    parse_data(key, val);   // Обрабатываем полученные данные, если они есть  }}

Программирование Highg‑Level

У меня есть собственная дообученная модель yolo8n на цветные блоки, но она работала довольно медленно в формате .pt, я конвертировал в .onnx и сквантовал из fp32 в fp16. Стало работать в 2 раза быстрее.

Вот код для конвертации и квантования:

from ultralytics import YOLOmodel = YOLO('best.pt')# half=True включает FP16. Никаких датасетов и калибровки не нужно!model.export(format='onnx', half=True, dynamic=False)

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

import cv2import numpy as npimport serialimport timefrom ultralytics import YOLOfrom typing import Tuple, List, Optionalfrom enum import Enum# --- НАСТРОЙКИ ---MODEL_PATH = "best.onnx"SERIAL_PORT = "COM3"  # Измените на ваш портBAUD_RATE = 115200TARGET_CLASS = "red_block"  # Имя класса для отслеживанияCONFIDENCE_THRESHOLD = 0.5  # Порог уверенности# Коэффициент P-регулятора для оси XKP_X = 0.8# Диапазоны для сервоприводовSERVO_X_MIN = 0SERVO_X_MAX = 180# Позиции для захватаY_UP_POSITION = 90      # Поднятое положение (начальное)Y_DOWN_POSITION = 0     # Опущенное положение (для захвата)GRAB_OPEN = 90          # Грейфер открытGRAB_CLOSED = 0         # Грейфер закрыт (зажат)# Задержки для последовательности захвата (в секундах)DELAY_AFTER_DOWN = 1.0  # Задержка после опусканияDELAY_AFTER_GRAB = 0.5  # Задержка после захватаclass GrabState(Enum):    """Состояния процесса захвата"""    IDLE = "idle"                    # Ожидание объекта    MOVING_DOWN = "moving_down"      # Опускание к объекту    GRABBING = "grabbing"            # Захват объекта    MOVING_UP = "moving_up"          # Подъем с объектом    COMPLETED = "completed"          # Захват завершенclass ServoController:    """Класс для управления сервоприводами через Arduino"""        def __init__(self, port: str, baud_rate: int):        """        Инициализация контроллера                Args:            port: Serial порт            baud_rate: Скорость передачи        """        try:            self.serial = serial.Serial(port, baud_rate, timeout=1)            time.sleep(2)  # Ждем инициализации Arduino            print(f"Serial порт {port} открыт")        except serial.SerialException as e:            print(f"Ошибка открытия serial порта: {e}")            self.serial = None        def send_command(self, key: str, value: int):        """        Отправка команды на Arduino                Args:            key: Ключ команды (x, y, grab)            value: Значение команды        """        if self.serial and self.serial.is_open:            command = f"{key}:{value}\n"            self.serial.write(command.encode())            print(f"Отправлено: {command.strip()}")        def close(self):        """Закрытие serial порта"""        if self.serial and self.serial.is_open:            self.serial.close()            print("Serial порт закрыт")class CameraTracker:    """Основной класс для отслеживания объекта"""        def __init__(self):        """Инициализация трекера"""        # Загрузка YOLO модели из ONNX файла        print(f"Загрузка модели: {MODEL_PATH}")        self.model = YOLO(MODEL_PATH)        print("Модель успешно загружена!")                # Получаем имена классов из модели        self.class_names = self.model.names        print(f"Классы модели: {self.class_names}")                # Инициализация контроллера сервоприводов        self.controller = ServoController(SERIAL_PORT, BAUD_RATE)                # Инициализация камеры        self.cap = cv2.VideoCapture(0)        if not self.cap.isOpened():            raise RuntimeError("Не удалось открыть камеру")                # Получаем размер кадра        self.frame_width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))        self.frame_height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))        self.center_x = self.frame_width // 2                # Текущие позиции сервоприводов        self.current_x = 90  # Центральное положение        self.current_y = Y_UP_POSITION  # Начальное положение - поднято                # Состояние захвата        self.grab_state = GrabState.IDLE        self.state_timer = 0        self.target_locked = False  # Флаг блокировки цели во время захвата                # Инициализация начального состояния        self.controller.send_command('y', self.current_y)        self.controller.send_command('grab', GRAB_OPEN)                print(f"Камера инициализирована: {self.frame_width}x{self.frame_height}")        def calculate_servo_x_position(self, bbox: List[int]) -> int:        """        Расчет позиции сервопривода оси X для наведения на объект                Args:            bbox: Bounding box [x1, y1, x2, y2]                    Returns:            Позиция сервопривода X        """        x1, y1, x2, y2 = bbox                # Центр объекта по X        obj_center_x = (x1 + x2) // 2                # Отклонение от центра кадра        error_x = self.center_x - obj_center_x                # P-регулятор        delta_x = error_x * KP_X                # Обновляем позицию с ограничением диапазона        new_x = int(np.clip(self.current_x + delta_x, SERVO_X_MIN, SERVO_X_MAX))                return new_x        def start_grab_sequence(self):        """Начало последовательности захвата"""        print("Начало последовательности захвата")        self.grab_state = GrabState.MOVING_DOWN        self.state_timer = time.time()        self.target_locked = True  # Блокируем цель        def update_grab_sequence(self):        """Обновление состояния последовательности захвата"""        current_time = time.time()        elapsed = current_time - self.state_timer                if self.grab_state == GrabState.MOVING_DOWN:            # Опускаем сервопривод Y            self.current_y = Y_DOWN_POSITION            self.controller.send_command('y', self.current_y)            print(f"Опускание к объекту: Y={self.current_y}")                        # Переход к захвату после задержки            if elapsed >= DELAY_AFTER_DOWN:                self.grab_state = GrabState.GRABBING                self.state_timer = current_time                elif self.grab_state == GrabState.GRABBING:            # Закрываем грейфер            self.controller.send_command('grab', GRAB_CLOSED)            print("Захват объекта")                        # Переход к подъему после задержки            if elapsed >= DELAY_AFTER_GRAB:                self.grab_state = GrabState.MOVING_UP                self.state_timer = current_time                elif self.grab_state == GrabState.MOVING_UP:            # Поднимаем сервопривод Y            self.current_y = Y_UP_POSITION            self.controller.send_command('y', self.current_y)            print(f"Подъем с объектом: Y={self.current_y}")                        # Завершение последовательности            self.grab_state = GrabState.COMPLETED            print("Последовательность захвата завершена")        def reset_grab_sequence(self):        """Сброс последовательности захвата"""        self.grab_state = GrabState.IDLE        self.target_locked = False        # Открываем грейфер для следующего захвата        self.controller.send_command('grab', GRAB_OPEN)        print("Сброс последовательности захвата")        def draw_visualization(self, frame: np.ndarray, results) -> Tuple[np.ndarray, Optional[List[int]]]:        """        Отрисовка визуализации на кадре                Args:            frame: Кадр для отрисовки            results: Результаты детекции от YOLO                    Returns:            Кортеж (кадр с визуализацией, целевой bbox или None)        """        # Рисуем центр кадра        cv2.circle(frame, (self.center_x, self.frame_height // 2), 5, (255, 255, 255), -1)        cv2.circle(frame, (self.center_x, self.frame_height // 2), 10, (255, 255, 255), 2)                # Обрабатываем результаты детекции        target_bbox = None                if results and len(results) > 0:            for result in results:                boxes = result.boxes                if boxes is not None:                    for box in boxes:                        # Получаем координаты                        x1, y1, x2, y2 = map(int, box.xyxy[0].tolist())                        confidence = float(box.conf[0])                        class_id = int(box.cls[0])                        class_name = self.class_names[class_id]                                                # Определяем цвет в зависимости от класса                        if class_name == TARGET_CLASS:                            color = (0, 255, 0)  # Зеленый для целевого класса                            target_bbox = [x1, y1, x2, y2]                        else:                            color = (0, 0, 255)  # Красный для других классов                                                # Рисуем bounding box                        cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2)                                                # Рисуем центр объекта                        obj_center_x = (x1 + x2) // 2                        obj_center_y = (y1 + y2) // 2                        cv2.circle(frame, (obj_center_x, obj_center_y), 5, color, -1)                                                # Рисуем линию от центра кадра к центру объекта                        cv2.line(frame, (self.center_x, self.frame_height // 2),                                 (obj_center_x, obj_center_y), color, 1)                                                # Подпись                        label = f"{class_name}: {confidence:.2f}"                        cv2.putText(frame, label, (x1, y1 - 10),                                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)                # Информация о состоянии        state_text = f"State: {self.grab_state.value}"        cv2.putText(frame, state_text, (10, 30),                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)                pos_text = f"X: {self.current_x}°  Y: {self.current_y}°"        cv2.putText(frame, pos_text, (10, 60),                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)                return frame, target_bbox        def run(self):        """Основной цикл работы"""        print("Запуск отслеживания...")        print("Нажмите 'q' для выхода, 'r' для сброса захвата")                while True:            ret, frame = self.cap.read()            if not ret:                print("Ошибка захвата кадра")                break                        # Обнаружение объектов с помощью YOLO            results = self.model(frame, conf=CONFIDENCE_THRESHOLD, verbose=False)                        # Визуализация и получение целевого bbox            frame, target_bbox = self.draw_visualization(frame, results)                        # Обработка состояний захвата            if self.grab_state != GrabState.IDLE and self.grab_state != GrabState.COMPLETED:                # Выполняем последовательность захвата                self.update_grab_sequence()            else:                # Режим поиска и наведения                if target_bbox and not self.target_locked:                    # Объект найден, наводимся по оси X                    new_x = self.calculate_servo_x_position(target_bbox)                                        # Обновляем позицию только если она изменилась значительно                    if abs(new_x - self.current_x) > 1:                        self.current_x = new_x                        self.controller.send_command('x', self.current_x)                                        # Если объект достаточно близко к центру, начинаем захват                    obj_center_x = (target_bbox[0] + target_bbox[2]) // 2                    if abs(self.center_x - obj_center_x) < 20:  # Порог центрирования                        self.start_grab_sequence()                        # Показываем кадр            cv2.imshow('Red Block Tracker', frame)                        # Обработка клавиш            key = cv2.waitKey(1) & 0xFF            if key == ord('q'):                break            elif key == ord('r'):                # Сброс последовательности захвата                self.reset_grab_sequence()            elif key == ord('+'):                # Увеличить порог уверенности                global CONFIDENCE_THRESHOLD                CONFIDENCE_THRESHOLD = min(0.95, CONFIDENCE_THRESHOLD + 0.05)                print(f"Порог уверенности: {CONFIDENCE_THRESHOLD:.2f}")            elif key == ord('-'):                # Уменьшить порог уверенности                CONFIDENCE_THRESHOLD = max(0.1, CONFIDENCE_THRESHOLD - 0.05)                print(f"Порог уверенности: {CONFIDENCE_THRESHOLD:.2f}")                # Освобождение ресурсов        self.cleanup()        def cleanup(self):        """Освобождение ресурсов"""        print("Остановка...")        self.cap.release()        cv2.destroyAllWindows()        self.controller.close()        print("Завершено")if __name__ == "__main__":    try:        tracker = CameraTracker()        tracker.run()    except KeyboardInterrupt:        print("\nПрервано пользователем")    except Exception as e:        print(f"Ошибка: {e}")        import traceback        traceback.print_exc()

Итоги

Видео с тестами появится на *канале в течении 4–5 дней.

Идеи по доработке:

1. Интеграция с ROS2.

2. Использование всего датасета, а не одного блока.

3. Создание мобильной платформы для манипулятора.

Ссылки

Ссылка на мой канал — https://rutube.ru/channel/37099536/