Построение точного линейного серво-привода с обратной связью — пошаговое руководство

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

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

Смотрите видео или читайте текст ниже.

Обзор

Вход управления — аналоговый или цифровой. Аналоговый: любой потенциометр (линейный, вращательный, джойстик). Цифровой: RC-передатчик (нужен приёмник на входе привода). Оба режима требуют всего 3 провода: 2 — питание входного устройства, 1 — сигнал.

Крутая фича: можно задать пользовательские начальную и конечную позиции штока, а также чувствительность (скорость отклика).

Любимая фича: управление с ПК/ноутбука через последовательный порт. Вводим позицию в мм в Serial Monitor Arduino IDE — привод едет туда. Ещё круче: повторяемые движения — вводим «save» в каждой точке, потом «run» — привод циклически повторяет сохранённые позиции.

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

Принцип работы

Замкнутая система управления основана на магнитном энкодере AS5600 и ПИД-регуляторе для DC-двигателя.

Использую ту же кастомную плату серво-контроллера, что и в прошлом видео — она включает свой микроконтроллер и всё нужно, чтобы превратить любой DC-двигатель в автономный сервопривод. Подробности работы сервопривода и замкнутого контура — в прошлом туториале.

Кратко: сервопривод — замкнутая система, где входной сигнал (желаемая позиция) сравнивается с актуальной позицией от датчика обратной связи. Разница (ошибка) обрабатывается контроллером, который командует двигателем до достижения цели.

Этот линейный серво-привод работает по тому же принципу, но с дополнительным шагом: преобразование вращения двигателя в линейное движение через винтовую пару (lead screw).

Проектирование линейного серво-привода

3D-модель показывает устройство. AS5600 сзади отслеживает вращение винта. Шаг винта 8 мм — за оборот гайка движется на 8 мм.

AS5600 — 12-битный энкодер: 4096 позиций на оборот. 8 / 4096 = 0.001953 мм — минимальное обнаруживаемое изменение позиции. Очень точно.

DC-двигатель 12V с редуктором 480 RPM. 480/60 = 8 об/с × 8 мм = 64 мм/с линейная скорость. Максимальный ход штока 150 мм → ~2.5 с на макс. скорости (с разгоном/торможением ~3 с). Поэтому использовал прямую передачу 1:1 на винт.

Конструкция вытекает из размеров кастомной PCB серво-контроллера (40×40 мм) и винтовой пары. Гайка винта 22 мм наружно — под неё спроектирован шток. Гайка и шток соединены 4 болтами M3 и вставками. Верх штока — подшипник, скользящий по цилиндровому блоку (гайка от вращения). На выходной крышке 4 мелких подшипника для плавного выхода штока.

Компактно для данного набора компонентов. Успел втиснуть микро-концевой выключатель внутрь цилиндра — для хоминга и установки стартовой позиции.

Скачать 3D-модель

Модель доступна в браузере Onshape (нужен аккаунт, бесплатно для домашнего использования). STL для печати и STEP-файл — на Cults3D.

Максимальный ход легко увеличить: удлините цилиндровый блок и шток. Размеры выбрал, чтобы всё поместилось на стол 220×220 мм. Крупнейшая деталь — шток 215 мм.

3D-печать

Новый Creality Ender-3 V3 SE отлично справился с печатью штока горизонтально по Y — чуть шлифовки, но плавнее работа и прочнее деталь.

Важно: Horizontal Expansion в слайсере для компенсации расширения пластика и точных размеров. Использовал –0.1 мм, но подбирайте тестами под свой принтер.

Цилиндровый блок печатал вертикально (по Z) — без саппортов. Ender-3 V3 SE удивил качеством за цену < $200: автокалибровка, прямой экструдер, до 250 мм/с. Один из лучших бюджетных принтеров. Подробный обзор на сайте. Магазины: Creality store | Amazon.

Сборка

Все 3D-детали готовы — собираем.

Список комплектующих

Механика:

• Винт Tr8×8 (шаг 8 мм) — Amazon / AliExpress
• Подшипник 8×22×7 мм — Amazon / AliExpress
• Подшипники 4×9×4 мм (×4) — Amazon / AliExpress
• Подшипник 6×13×5 мм 686-2RS (×1) — Amazon / AliExpress
• Резьбовые вставки M3×5, M4×5 — Amazon / AliExpress
• Болты/гайки M3, M4 — Amazon / AliExpress
  Болты: M3×8 (10); M3×10 (2); M3×10 зыбк. (1); M4×6 (2); M4×25 (2); M4×30 (2); M2×8 (6); M4×5 прижим. (5)

Электроника:

• AS5600 магнитный энкодер — Amazon / AliExpress
• DRV8871 драйвер DC — Amazon / AliExpress
• ATmega328p-AU — Amazon / AliExpress
• Кварц 16 МГц — Amazon / AliExpress
• Стабилизатор AMS1117 5V — Amazon / AliExpress
• Подстроечный резистор 3386P — Amazon / AliExpress
• Набор конденсаторов 0805 — Amazon / AliExpress
• DC-двигатель 12V ~50 RPM — Amazon / AliExpress

Партнёрские ссылки.

Сборка винтового механизма

Сначала устанавливаем винт в базовый блок цилиндра. Ввинчиваем 3D-напечатанную гайку с тем же шагом — плотная посадка (натяжка), это критично: гайка воспринимает всю силу толкания штока. В гайке отверстие под резьбовую вставку для прижимного винта на вал.

Винт держат два подшипника 22 мм наружного диаметра в базовом блоке. Сзади — ведущая шестерня с тем же резьбовым профилем и двумя вставками для прижимных винтов. Соединение передаёт весь крутящий момент — не должно проскальзывать.

Подготовим подсборку: впрессуем вставки в шестерню, гайку, базовый блок. Вкручиваем шестерню и гайку навстречу друг другу (не затягивая сильно — осевая нагрузка на подшипники). Прижимными винтами фиксируем на валу. Добавляем пластину на базовый блок — удерживает подшипники. Винт теперь твёрдо стоит и свободно вращается.

Готовим шток: полый на всю длину под винт. Впрессуем вставки для связи с гайкой винта. Сверху — направляющий подшипник 13/6 мм на 3D-напечатанном валу 6 мм, закреплён контроболтом M3×10. Шток скользит по направляющим цилиндра.

Прикручиваем цилиндр к базовому блоку. Но сначала ставим микро-концевой выключатель. Паяем провода (~15 см) на NC-контакт. Провода через отверстие наверху цилиндра, фиксируем выключатель двумя болтами M2×8. Нужна именно эта модель — чтобы срабатывала от направляющего подшипника вовремя. Если другой — модифицируйте отверстия/механизм.

Цилиндр к базовому блоку: вставки, два болта M4×25 (верхние пары; нижние позже — те же отверстия для крышки редуктора/PCB). Вкручиваем шток в гайку винта — направляющий подшипник входит в рельсы цилиндра. Вращая шестерню сзади, отводим шток до срабатывания концевого выключателя.

Надеваем выходную крышку цилиндра: 4 мелких подшипника 9 мм наружного диаметра (валы для них 3D-напечатаны). Аккуратно вставляйте — тонкое место, ломается. Подшипники поддерживают шток. Крышка фиксируется 4 болтами M4.

Установка DC-двигателя

Вставляем DC-двигатель, 6 болтов M3. Шестерня на вал — две вставки и прижимные винты. При правильном зацеплении шестерён накрываем заднюю крышку редуктора/PCB. Сначала вставки в базовый блок. Провода двигателя (~20 см) паяю прямо к контактам. Провода через отверстие в базовом блоке (и от концевого выключателя), через держатели на крышке — чтобы не мешали шестерням. Крепим крышку: нижние два болта M4 не до конца (~2–3 мм зазор), кладем держатель крышки, затягиваем вместе. Немного грязная операция, но крышка компактная и цельная.

Установка кастомной PCB серво-контроллера

Это тот же контроллер из прошлого видео (как сделать из DC-двигателя сервопривод). Главное — AS5600, следящий за угловой позицией магнита на выходном валу. Здесь магнит на выходной шестерне винта — идеально выравнивается при установке PCB.

Фиксация PCB: гайки M2 в слоты держателей, 4 болта M2. Проводка: двигатель в клеммник (проверьте полярность по прошивке). Перед подключением к PCB можно подать напряжение на двигатель — проверить механику. Концевой выключатель: земля на панель электролита, NC — на ножку SCK (D13 ATmega328). Клеммник питания у борта крышки — отверстие под разъём 5.5 мм. Радиатор на драйвере. Защёлкиваем заднюю крышку — готово.

Теперь можно подключить любой потенциометр или RC-приёмник к входам — управление позициию готово. Подробности контроллера, схема, PCB — в прошлом туториале.

Схема

AS5600 отслеживает позицию выходного штока. Данные в ATmega328 — расчёт ошибки — PWM на DRV8871 — DC-двигатель до нужной позиции. DRV8871 до 3.6 А пик. Питание 12V → AMS1117 → 5V для микроконтроллера и 5V-компонентов. DIP-переключатель 2-канальный — выбор режима входа: аналоговый, цифровой (RC), последовательный порт. Один подстроечный резистор — чувствительность, кнопка — установка старт/финиш позиций.

PCB заказывал у PCBWay: 4 слоя (средние — GND), цвет белый, позолота (immersion gold) бесплатно. Gerber в сообществе PCBWay.

Можно собрать и без кастомной платы: AS5600 на breakout + Arduino для управления DC-двигателем.

Код

Код линейного серво-привода (библиотеки: AS5600 — RobTillaart, PID — br3ttb):

/* Linear Servo Actuator - Arduino Code by Dejan, www.HowToMechatronics.com
   Libraries: AS5600 encoder: https://github.com/RobTillaart/AS5600
              PID controller: https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h */
#include "AS5600.h"
#include "Wire.h"
#include 
AS5600 as5600;
double Pk1 = 0.65, Ik1 = 0, Dk1 = 0.1;
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, Pk1, Ik1, Dk1, DIRECT);
#define motor_IN1 5
#define motor_IN2 6
#define ch1 2
#define setButton 7
#define inputSwitch 3
#define modeSwitch 4
#define limitSwitch 13
int ch1Value;
long encoderValue, desiredValue, pwmValue;
String serialInput = "";
int serialIntInput;
double totalDistance = 0;
long startPosition = 358;
long endPosition = 6750;
long rangeAdjustment = 0;
float sensitivityAdjustment = 0;
float angle = 0, angleValue = 0, rodPosition;
float positionsArray[100];
int positionsCounter = 0;
long quadrantNumber = 2, previousQuadrantNumber = 3, numberOfTurns = 0;
float totalAngle = 0;
char incomingByte = 0;
int intInput = 0;

void setup() {
  Serial.begin(115200);
  Serial.println(__FILE__);
  Serial.print("AS5600_LIB_VERSION: "); Serial.println(AS5600_LIB_VERSION);
  Wire.begin();
  pinMode(motor_IN1, OUTPUT); pinMode(motor_IN2, OUTPUT);
  pinMode(setButton, INPUT_PULLUP);
  pinMode(inputSwitch, INPUT_PULLUP);
  pinMode(modeSwitch, INPUT_PULLUP);
  pinMode(limitSwitch, INPUT_PULLUP);
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-255, 255);
  myPID.SetSampleTime(20);
  // HOMING
  while (digitalRead(limitSwitch) != 1) {
    digitalWrite(motor_IN1, LOW); analogWrite(motor_IN2, 70);
    encoderValue = as5600.readAngle();
  }
  while (digitalRead(limitSwitch) != 0) {
    analogWrite(motor_IN1, 50); digitalWrite(motor_IN2, LOW);
  }
  digitalWrite(motor_IN1, LOW); digitalWrite(motor_IN2, LOW);
  startPosition = as5600.readAngle() * 0.087890625;
  endPosition = 6000;
  Setpoint = startPosition;
}

void loop() {
  // Текущая позиция в мм
  rodPosition = as5600.readAngle() / 0.001953125; // 8 мм/оборот, 4096 RAW/оборот

  // Режим Serial
  if (digitalRead(modeSwitch) == 0) {
    while (Serial.available() > 0) {
      serialInput = Serial.readString(); serialInput.trim();
      if (serialInput == "save") {
        positionsArray[positionsCounter] = totalDistance;
        delay(1000); positionsCounter++;
      }
      if (serialInput == "clear") {
        memset(positionsArray, 0, sizeof(positionsArray));
        positionsCounter = 0;
      }
      serialIntInput = serialInput.toInt();
      if (serialIntInput != 0) {
        if (serialIntInput < 0) serialIntInput = 0;
        if (serialIntInput > 150) serialIntInput = 150;
        Setpoint = serialIntInput * 45; // 1 мм = 45° вращения
      }
    }
    // Run saved positions
    if (serialInput == "run") {
      while (serialInput != "stop") {
        for (int i = 0; i <= positionsCounter - 1; i++) {
          if (serialInput == "stop") break;
          while (positionsArray[i] > totalDistance + 25 || positionsArray[i] < totalDistance - 25 || pwmValue != 0) {
            Setpoint = positionsArray[i];
            readEncoder(); runMotor();
          }
          delay(2000);
          while (Serial.available() > 0) {
            serialInput = Serial.readString(); serialInput.trim();
          }
        }
      }
    }
  }
  // Режим потенциометр/RC
  else if (digitalRead(modeSwitch) == 1) {
    if (digitalRead(inputSwitch) == 0) { // Аналоговый
      desiredValue = analogRead(A0);
      if (desiredValue < 15) desiredValue = 15;
      if (desiredValue > 1008) desiredValue = 1008;
      Setpoint = map(desiredValue, 15, 1008, startPosition, endPosition);
    } else if (digitalRead(inputSwitch) == 1) { // RC
      desiredValue = pulseIn(ch1, HIGH, 30000);
      if (desiredValue < 1000 || desiredValue > 2000) desiredValue = 1000;
      Setpoint = map(desiredValue, 1000, 2000, startPosition, endPosition);
    }
    if (Setpoint > endPosition) Setpoint = endPosition;
    if (Setpoint < startPosition) Setpoint = startPosition;
    // Чувствительность (P-член)
    // Pk1 = analogRead(A2) * 0.002; myPID.SetTunings(Pk1, Ik1, Dk1);
    readEncoder(); runMotor();
    // Установка старт/финиш кнопкой
    if (digitalRead(setButton) == LOW) {
      delay(3000);
      if (digitalRead(setButton) == LOW) { endPosition = totalDistance; while (digitalRead(setButton) != HIGH); }
      else { startPosition = totalDistance; }
    }
  }
}

void readEncoder() {
  encoderValue = as5600.readAngle() * 0.087890625; // в градусах
  if (encoderValue >= 0 && encoderValue <= 90) quadrantNumber = 1;
  else if (encoderValue >= 90 && encoderValue <= 180) quadrantNumber = 2;
  else if (encoderValue >= 180 && encoderValue <= 270) quadrantNumber = 3;
  else if (encoderValue >= 270 && encoderValue <= 360) quadrantNumber = 4;
  if (quadrantNumber != previousQuadrantNumber) {
    if (quadrantNumber == 1 && previousQuadrantNumber == 4) numberOfTurns++;
    if (quadrantNumber == 4 && previousQuadrantNumber == 1) numberOfTurns--;
    previousQuadrantNumber = quadrantNumber;
  }
  totalDistance = (numberOfTurns * 360) + encoderValue;
  Input = totalDistance;
}

void runMotor() {
  myPID.Compute();
  if (Output > 1) {
    pwmValue = Output;
    if (pwmValue < 50 && pwmValue > 15) pwmValue += 40;
    if (pwmValue <= 15) pwmValue = 0;
    analogWrite(motor_IN1, pwmValue); digitalWrite(motor_IN2, LOW);
  } else if (Output < -1) {
    pwmValue = abs(Output);
    if (pwmValue < 50 && pwmValue > 15) pwmValue += 40;
    if (pwmValue <= 15) pwmValue = 0;
    digitalWrite(motor_IN1, LOW); analogWrite(motor_IN2, pwmValue);
  } else {
    pwmValue = 0;
    digitalWrite(motor_IN1, LOW); digitalWrite(motor_IN2, LOW);
  }
}

Разбор кода

Цикл начинается с чтения энкодера — текущая позиция в мм.

rodPosition = as5600.readAngle() / 0.001953125; // 8 мм/оборот, 4096 RAW/оборот

В режиме Serial читаем ввод. «save» — сохраняем текущую позицию. «clear» — чистим массив.

if (serialInput == "save") { positionsArray[positionsCounter] = totalDistance; ... }
if (serialInput == "clear") { memset(positionsArray, 0, sizeof(positionsArray)); positionsCounter = 0; }

Число 0–150 → Setpoint в градусах (×45, так как 1 мм = 45° при шаге 8 мм). При другом шаге коэффициент меняется.

Setpoint = serialIntInput * 45;

«run» — циклическое выполнение сохранённых позиций с паузой 2 с.

В режиме потенциометр/RC: аналоговый вход маппится в startPosition–endPosition; цифровой (RC) — 1000–2000 в тот же диапазон.

Функция readEncoder() переводит RAW в углы, отслеживает квадранты, считает полные обороты (numberOfTurns) — итоговый угол (totalDistance) — Input для ПИД.

runMotor() — ПИД вычисляет Output, по которому двигаем мотор PWM влево/вправо/стоп. Пороговые поправки на малых PWM для преодоления трения.

Коэффициенты ПИД вверху: P=0.65, I=0, D=0.1. Качество работы напрямую зависит от их настройки.

Тестирование

Проверка точности: возврат в точку нормальный. Движение по 1 мм: первое ~0.8 мм, следующие 4 — близко к 1 мм, 4 мм — погрешность ~0.15 мм. Есть люфт ~0.25 мм между винтом и гайкой + люфт в 3D-шестернях и редукторе двигателя. Под нагрузкой ошибка растёт, но улучшается настройкой ПИД.

Вот и всё. Надеюсь, понравилось и пригодилось.