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

Введение

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

Концепция

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

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

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

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

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

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

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

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

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

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

Пример готового манипулятора "Сделай сам" — Пример готового манипулятора «Сделай сам»

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

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

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

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

Крепление мотора.
Крепление манипулятора.
Мод захвата.

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

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

При проектировании возникла проблема: при том расположении мотора, которое я выбрал из соображений простоты сборки, на каретке не осталось места для крепления манипулятора, поэтому я решил спроектировать съёмную платформу‑расширитель. Она будет крепиться на болт m5 и упираться в грань каретки(при правильных настройках печати и выборе материала, деталь не изгибается).

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

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

Это также зависит от конфигурации, но вот тоже краткая инструкция по проектированию:

Замер всех углов и габаритов деталей которые влияют на работу манипулятора.

Изменение тех параметров, которые не влияют на принцип работы.

Не забывайте о сцеплении и изгибе щупов.

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

Сборка

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

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

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

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

// --- НАСТРОЙКИ И ПОДКЛЮЧЕНИЕ БИБЛИОТЕК ---

// Коэффициент пересчета для оси X (умножает входное значение на 1)
#define Kx 1

// Пин, к которому подключен сервопривод оси Y
#define Y_PIN 11

// Пин, к которому подключен сервопривод захвата (грейфера)
#define GRAB_PIN 10

// Подключение библиотеки для управления шаговым двигателем (в режиме 2-х проводов)
#include <GyverStepper2.h>

// Подключение стандартной библиотеки для управления сервоприводами
#include <Servo.h>

// --- ОБЪЯВЛЕНИЕ ПЕРЕМЕННЫХ И ОБЪЕКТОВ ---

// Создание объекта шагового двигателя.
// <STEPPER2WIRE> - режим работы (2 управляющих провода).
// 2048 - количество шагов на один полный оборот вала.
// 2, 5 - пины управления драйвером.
GStepper2<STEPPER2WIRE> x(2048, 2, 5);

// Создание объектов для управления сервоприводами
Servo y;      // Ось Y
Servo grab;   // Захват

// Переменные для хранения команд, полученных по Serial
String 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) {
  if (key == "x") {        // Если ключ "x" — управление шаговым двигателем
    set_x(val);
  } else if (key == "y") { // Если ключ "y" — управление сервоприводом оси Y
    set_y(val);
  } else if (key == "grab") { // Если ключ "grab" — управление захватом
    set_grab(val);
  }
}

/**
 * Устанавливает целевую позицию для шагового двигателя по оси 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 YOLO

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

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


import cv2
import numpy as np
import serial
import time
from ultralytics import YOLO
from typing import Tuple, List, Optional
from enum import Enum

# --- НАСТРОЙКИ ---
MODEL_PATH = "best.onnx"
SERIAL_PORT = "COM3"  # Измените на ваш порт
BAUD_RATE = 115200
TARGET_CLASS = "red_block"  # Имя класса для отслеживания
CONFIDENCE_THRESHOLD = 0.5  # Порог уверенности

# Коэффициент P-регулятора для оси X
KP_X = 0.8

# Диапазоны для сервоприводов
SERVO_X_MIN = 0
SERVO_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 дней.

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

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

Ссылки

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