Введение
Здравствуй, читатель! Меня зовут Алексей Морозов, и в этой статье мы разберём полный путь по созданию автономного манипулятора, от выбора компонентов до интеграции с компьютерным зрением и моделями 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 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; // Ось Y
Servo grab; // Захват
// Переменные для хранения команд, полученных по Serial
String key = ""; // Ключ команды (например, "x", "y", "grab")
String val = ""; // Значение команды (например, "100")
// --- ФУНКЦИИ ОБРАБОТКИ ДАННЫХ ---
/**
* Функция считывает данные из последовательного порта (Serial).
* Ожидаемый формат: "ключ:значениеn" (например, "x:100n")
*/
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 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/
Автор: ArduDragon
