Открытый проект серво‑контроллера MC50 продолжает развиваться. На этот раз поговорим о создании сервопривода.

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

Предыдущие статьи по теме проекта MC50

В прошлой статье было показано как заставить BLDC мотор крутиться с помощью периферии микроконтроллера S5D9 семейства Renesas Synergy™

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

Конструкция

Конструкция состоит из мотор‑редуктора, дополнительного редуктора 29/40, простого узла с серво‑потенциометром для контроля за скоростью выходного вала и термистором для контроля за температурой мотора. Как видно, внешний редуктор даёт удобно разместить серво‑потенциометр. Дополнительно он увеличивает момент силы 1.3 раза Такая конструкция в разных масштабах встречается во множестве сервоприводов.

Конструкция сервопривода
Конструкция сервопривода

Схема подключения

Плата серво‑контроллера рассчитана на питание от внешнего источника напряжением от 20 до 30 В. Но силовой драйвер продолжает функционировать при понижении напряжения до 7 В. При отсутствии внешнего источника напряжения плата может питаться от USB, но силовой драйвер при этом функционировать не будет.

Схема подключения серво-контроллера MC50
Схема подключения серво-контроллера MC50

О частоте вращения BLDC мотора

Частота вращения BLDC двигателя определяется приложенным к нему напряжением при условии постоянной нагрузки на валу (считаем что 6-шаговая коммутация в это время работает безукоризненно, напряжение меняем ШИМ‑ом). Эта частота записывается в даташите на мотор. Если момент силы нагрузки будет больше или меньше, то частота будет разная при неизменном напряжении. Но мы не знаем точно момент силы нагрузки и он скорее всего будет переменным.

Для идеального двигателя без нагрузки, без трения и без сопротивления обмоток частоту вращения при определенном напряжении можно узнать вращая мотор внешним приводом до достижения заданного напряжения. Частота вращения в этот момент и будет искомой частотой. Но тут важна форма напряжения. Применив 6-шаговую коммутацию к мотору с синусоидальной обратной ЭДС, не получится получить ту же частоту.

Идеальный BLDC двигатель будет крутиться с бесконечной частотой при подаче на него бесконечного напряжения. У реального же BLDC двигателя в какой‑то момент сначала наступит насыщение магнитопровода, резко повысится ток через обмотки, обмотки раскалятся и спустя некоторое время мотор загорится. Поэтому частота вращения и допустимый момент силы ещё ограничиваются температурой мотора.

Разработка архитектуры управления.

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

Для начала отметим, что частота вращения ротора не равна частоте вращения сервопривода, а в динамике с реальной механикой с люфтами и упругими реакциями даже не пропорциональна.

Для управления частотой вращения самое лёгкое решение — это просто менять скважность широтно‑импульсной модуляции (ШИМ).

Простейшая модель сервопривода в Simulink. Здесь реализована 6-шаговая коммутация и вращение только в одном направлении
Простейшая модель сервопривода в Simulink. Здесь реализована 6-шаговая коммутация и вращение только в одном направлении

Файл модели здесь

При постоянном моменте силы нагрузки на мотор или ручном управлении все довольно хорошо получается. Мы линейно меняем скважность и добиваемся нужной частоты вращения. Коммутация автоматическая с помощью датчиков Холла находящихся в моторе. Метод отлично работает в самокатах.

Осциллограмма сигналов  при вращении груза в вертикальной плоскости
Осциллограмма сигналов при вращении груза в вертикальной плоскости

Но как только момент силы начнёт меняться частота вращения начнёт пульсировать, как показано на видео ниже.

Непостоянство частоты вращения при отсутствии автоматического регулирования
Непостоянство частоты вращения при отсутствии автоматического регулирования

Самое очевидное решение — это применить PID управление с обратной связью по частоте вращения. К сожалению все не так просто. Серво‑приводы могут предназначаться для управления угловой скоростью или для управления положением, в обоих случаях нужны разные алгоритмы. И в обоих случаях простой одноконтурный PID очень плохо работает. В случае со стабилизацией положения нужен регулятор с переходной характеристикой не менее третьего порядка. А PID имеет второй порядок. т. е. оставаясь в рамках PID нужно две петли управления, вложенные одна в другую. Нам нужна хорошая, быстрая, оптимизируемая, масштабируемая реализация PID алгоритма.

Такая реализация есть в среде MATLAB Simulink в виде модели конвертируемой в исходники на C. Ниже представлена модель в виде открытой расширяемой подсистемы.

Файлы модели здесь.

Модель универсального PID регулятора
Модель универсального PID регулятора

Модель повторяет функциональность встроенного PID компонента Simulink, но при этом выводит все коэффициенты PID как входные аргументы в одной шине. Аргументы можно менять и в стандартном компоненте используя символьные обозначения переменных из рабочего пространства, но это влечёт за собой неявные зависимости приводящие к ошибкам. А шина обеспечивает генерацию аргумента как передаваемой структуры. Это уменьшает объем кода при интеграции модели в embedded проект и упрощает рефакторинг моделей.

Диалог настройки PID регулятора в Simulink
Диалог настройки PID регулятора в Simulink

Для предотвращения бесконечного нарастания интегральной составляющей (anti‑windup) применён метод clamping.

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

Сгенерированные из модели исходники находятся в директории MATLAB_PID.

Выбор одноконтурного или двухконтурного управления скоростью вращения серво-привода.

Чтобы сделать выбор архитектуры петли управления проведём эксперимент. Сделаем в среде FreeMaster две записи сигналов в разных режимах управления мотором. Записывать будем одновременно четыре сигнала:

  • сигнала включения,

  • тока протекающего через мотор,

  • состояния датчиков Холла, отражающих скорость вращения ротора,

  • скорость выходного вала привода.

Первая запись ниже сделана во время скачкообразного включения вращения мотора в режиме 6-step с максимальным напряжением.

Вторая запись ниже сделана во время скачкообразного включения вращения мотора в режиме 6-step с удержанием постоянного тока через мотор с помощью петли управления током.

Если посмотреть на реакцию скорости выходного вала на скачок воздействия, то на первой записи видна крайне нелинейная кривая скорости. На второй записи скорость достаточно линейно нарастает, но нарастание начинается с некоторой задержкой.

Очевидно в первом случае практически трудно будет проводить регулировку скорости регулируя алгоритмом PID скважность ШИМ. Во всяком случае с интервалом регулирования меньшим 0.1 сек. Поскольку скорость откликается совершенно не похожим на линейную систему образом. А классический PID хорош только для линейных систем.

Во втором случае отклик гораздо более линейный и PID здесь подойдёт. Вернее тут подойдёт простой PI контур, поскольку отклик демонстрирует инерциальную нагрузку без упругости (осцилляций). Но надо помнить — тут уже применён высокоскоростной контур поддержки заданного тока. т. е. в данном случае у нас получится 2-контурная система: быстрый PI контур управления током через ШИМ и медленный PI для управления скоростью через задание тока.

В итоге пришли к следующей архитектуре в составе модели Simulink :

Архитектура с двумя петлями управления скоростью вращения сервопривода в среде Simulink
Архитектура с двумя петлями управления скоростью вращения сервопривода в среде Simulink

Файл модели здесь

Модель включает мотор-редуктор с планетарной передачей и датчиками Холла, внешний редуктор, серво сенсор с моделью измерения скорости включая шумы измерения, 3-фазный инвертер с CMOS транзисторами, ШИМ модулятор и два контура управления: по скорости и по току. Цветами на модели выделяются участки работающие с разной частотой тактирования. Контур тока работает на частоте 16 Кгц, контур скорости на частоте 100 Гц.

Измерения частоты вращения ротора датчиками Холла

В модели показанной выше частота вращения ротора измеряется по датчикам Холла. Но в реальности оказалось, что измерение длительности интервалов между фронтами импульсов с датчиков плохо подходит для определения частоты вращения ротора. Из графика ниже следует, что при постоянной частоте разница в длительностях интервалов может составлять до 30%. Такая погрешность потом выразится в недостаточной точности или быстроте стабилизации частоты вращения.

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

Используется логика захвата таймеров GPT3211 и GPT3212 (согласно хидеру из SSP они имеет имена R_GPTB3 и R_GPTB4). Таймера имеют по два модуля захвата, поэтому для сигналов с трех датчиков нужно два таймера. Таймера тактируются абсолютно синхронно благодаря команде одновременного запуска (все таймера в Synergy можно запустить синхронно). 

Здесь фрагмент того как производится измерение в обработчике прерываний от АЦП:

  st = R_GPTB3->GTST;  // Читаем флаги состояния capture таймера  GPT3211

  if (st & BIT(0))     // Проверяем флаг TCFA. Input Capture/Compare Match Flag A
  {
    // Здесь если событие захвата произошло

    R_GPTB3->GTST = 0;      // Очищаем флаг захвата и остальные флаги.
                            // За другие флаги захвата не беспокоимся, поскольку они при штатной работе не могут быть взведены в этот момент.

    val = R_GPTB3->GTCCRA;  // Читаем регистр с захваченным состоянием счетчика

    // Здесь рассчитываем разницу по отношению к предыдущему захваченному значению на том же угле поворота ротора, т.е. для полюса с номером в переменной hall_u_cnt
    if (hall_u_prev_arr[hall_u_cnt] > 0)
    {
      if (val > hall_u_prev_arr[hall_u_cnt])
      {
        hall_u_capt_arr[hall_u_cnt] = val - hall_u_prev_arr[hall_u_cnt];
      }
      else
      {
        // Корректируем если было произошло переполнение счетчика между захватами
        hall_u_capt_arr[hall_u_cnt] =  0x7FFFFFFF -(hall_u_prev_arr[hall_u_cnt] - val -1);
      }

      hall_u_capt = hall_u_capt_arr[hall_u_cnt]; // Сохраняем разницу в промежуточную переменную. Эта переменная используется при отладке через FreeMaster
      // .......................................................
      one_turn_period = hall_u_capt;             // Записываем длительность полного оборота ротора в глобальную переменную для дальнейшего использования остальными задачами
      // .......................................................
    }
    hall_u_prev_arr[hall_u_cnt] = val;         // Сохраняем текущее захваченное значение в переменную предыдущего значения
    hall_u_cnt++;                              // Ведем счет полюсов. Для каждого полюса сохраняется своя измеренная величина
    if (hall_u_cnt >= 8) hall_u_cnt = 0;

    // Определяем направление вращения
    h = R_IOPORT5->PCNTR2 & 0x7;  // Читаем сигналы датчиков Холла здесь снова, несмотря на то что они уже были прочитаны в обработчике прерывания
                                  // Это нужно поскольку capture логика может сработать уже после того как в ISR были прочитаны состояния датчиков
    if ((h == 0b100) || (h == 0b011))      rotating_direction = 1; // Направление вращения определяем по паттернам сигнало с датчиков сразу после текущего фронта
    else if ((h == 0b010) || (h == 0b101)) rotating_direction = -1;

    R_GPTB5->GTCLR_b.CCLR13 = 1;  // Сброс счетчика отслеживающего остановку вращения
    R_GPTB5->GTST           = 0;  // Сброс флагов счетчика отслеживающего остановку вращения
  }

  if (st & BIT(1))
  {
    // Input capture/compare match of GTCCRB occurred
    R_GPTB3->GTST = 0;
    val = R_GPTB3->GTCCRB;
    if (hall_v_prev_arr[hall_v_cnt]>0)
    {
      if (val > hall_v_prev_arr[hall_v_cnt])
      {
        hall_v_capt_arr[hall_v_cnt] = val - hall_v_prev_arr[hall_v_cnt];
      }
      else
      {
        hall_v_capt_arr[hall_v_cnt] =  0x7FFFFFFF -(hall_v_prev_arr[hall_v_cnt] - val -1);
      }
      hall_v_capt = hall_v_capt_arr[hall_v_cnt];
      one_turn_period = hall_v_capt;
    }
    hall_v_prev_arr[hall_v_cnt] = val;
    hall_v_cnt++;
    if (hall_v_cnt >= 8) hall_v_cnt = 0;

    h = R_IOPORT5->PCNTR2 & 0x7;  // Читаем сигналы датчиков Холла здесь чтобы они были прочитаны не раньше чем стработает capture логика
    if ((h == 0b110) || (h == 0b001))      rotating_direction = 1;
    else if ((h == 0b011) || (h == 0b100)) rotating_direction = -1;

    R_GPTB5->GTCLR_b.CCLR13 = 1;  // Сброс счетчика отслеживающего остановку вращения
    R_GPTB5->GTST           = 0;
  }


  st = R_GPTB4->GTST;

  if (st & BIT(0))
  {
    // Input capture/compare match of GTCCRA occurred
    R_GPTB4->GTST = 0;
    val = R_GPTB4->GTCCRA;
    if (hall_w_prev_arr[hall_w_cnt]>0)
    {
      if (val > hall_w_prev_arr[hall_w_cnt])
      {
        hall_w_capt_arr[hall_w_cnt] = val - hall_w_prev_arr[hall_w_cnt];
      }
      else
      {
        hall_w_capt_arr[hall_w_cnt] =  0x7FFFFFFF -(hall_w_prev_arr[hall_w_cnt] - val -1);
      }
      hall_w_capt = hall_w_capt_arr[hall_w_cnt];
      one_turn_period = hall_w_capt;
    }
    hall_w_prev_arr[hall_w_cnt] = val;
    hall_w_cnt++;
    if (hall_w_cnt >= 8) hall_w_cnt = 0;

    h = R_IOPORT5->PCNTR2 & 0x7;  // Читаем сигналы датчиков Холла здесь чтобы они были прочитаны не раньше чем стработает capture логика
    if ((h == 0b101) || (h == 0b010))      rotating_direction = 1;
    else if ((h == 0b110) || (h == 0b001)) rotating_direction = -1;

    R_GPTB5->GTCLR_b.CCLR13 = 1;  // Сброс счетчика отслеживающего остановку вращения
    R_GPTB5->GTST           = 0;
  }

Здесь мы получаем значение частоты вращения c девиацией меньшей 0.3%, но с гораздо большей задержкой чем могли бы получить измеряя длительности между соседними фронтами. Но в последнем случае нам пришлось бы применить фильтр, который внёс бы ещё большую задержку.

Экстраполяция частоты вращения при внезапной остановке

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

  // Блок линейно понижающий скорость в случает отсутствия сигналов с датчиков Холла
  if (one_turn_period != 0)
  {
    if (R_GPTB5->GTST & BIT(6))  // Проверяем флаг TCFPO. Overflow Flag
    {
      // В случае переполения тамера сразу отмечаем скорость как нулевую.
      // Поскольку переполение таймера происходит с периодом в две секунды, то такую низкую скрость принимаем как нулевую
      one_turn_period = 0;
    }
    else
    {
      // За один оброт мотора мы имеем 8 полюсов * 3 датчика = 24 события сброса таймера R_GPTB5
      // И если таймер R_GPTB5 смог набрать 2/3 (16/24) длительности одного оборота и не был сброшен значит скорость упала
      // и можно начинать снижать оценку текущей скорости
      uint32_t no_edge_period = R_GPTB5->GTCNT*16;
      if (no_edge_period > one_turn_period)
      {
        one_turn_period = no_edge_period;
      }
    }
  }

Через некоторое время показатель частоты вращения упадёт до нуля и алгоритм управления скоростью примет решение об отключении тока.

Влияние способа коммутации силовых транзисторов на качество управления и энергопотребление

В прошлой статье о 6-шаговом управлении коммутацией были показаны несколько способов переключения транзисторов при ШИМ модуляции. В нашей программе оставлено два способа. Между ними можно выбирать с помощью настройки параметра Enable soft commutation.

На нашем мотор‑редукторе эти два способа показали только незначительные различия. При одинаковой частоте вращения ток потребления был идентичным. Но при мягкой коммутации вращение мотора начиналось при меньшем значении коэффициенте заполнения. Дело в том, что мотор из‑за трения не начинает крутиться до того момента как коэффициент заполнения ШИМ не превысит некий порог. Этот порог в случае жёсткой коммутации в нашем случае был в районе 55%, а в случае мягкой коммутации в районе 30%. т. е. мягкая коммутация даёт больший диапазон регулирования ШИМ — 70%, в противовес жёсткой — 45%. Это отражается потом и на качестве управления частотой вращения. При мягкой коммутации качество управления несколько лучше.

Способ коммутации выбирается параметром en_soft_commutation.

Регулировка тока затворов силовых транзисторов

На плате серво‑контроллера используется микросхема драйвера TMC6200. Микросхема получает сигналы от микроконтроллера и управляет затворами силовых транзисторов. Микроконтроллер не может напрямую управлять затворами поскольку там нужны высокие напряжения и довольно большие импульсные токи.

Типичная схема драйвера на TMC6200
Типичная схема драйвера на TMC6200

У этой микросхемы есть возможность через интерфейс SPI задавать уровень выходных токов на затворы. Есть 4 уровня: от самого слабого 400…600 мА до самого сильного 1200…1800 мА.

Как показали испытания всех возможных уровней, на ток потребления они практически не влияют. Измерения мощности при работе двигателя весьма зашумлены, и с учётом этого шума возможно допустить наличие некоторой разницы в пределах 100 мВт.

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

Управлять настройками драйвера TMC6200 можно с помощью ряда параметров. Настройки загружаются при старте основной задачи управления мотором.

Калибровка смещения токовых усилителей драйвера TMC6200

Микросхема TMC6200 имеет встроенные усилители напряжения с токовых шунтов расположенных в фазах серво драйвера. Наличие встроенных усилителей упрощает схему. Есть возможность менять коэффициент усиления через интерфейс SPI.

Однако недостатком этих усилителей является довольно большое напряжение смещения. Ещё хуже, то что оно сильно зависит от температуры микросхемы. Но мало того, оно разное для каждого из 3 усилителей и может меняться во времени даже при постоянной внешней температуре. В нашей программе используется самый чувствительный режим с усилением в 20 раз. При таком усилении смещение на выходе усилителей достигает 240 мВ и более.

Решение проблемы найдено в том что перед каждым включением мотора производится перекалибровка смещения нуля для каждого из 3 усилителей. Перекалибровка длится около 15 мсек и существенного влияния на быстродействие не оказывает.

Что делать с обратной ЭДС? 

Как только мотор начинают крутить внешние силы вместо нашего драйвера он превращается в генератор. В генератор мотор также превращается на короткие промежутки времени когда происходит торможение. Если частота вращения вызванная внешними силами превышает частоту вращения на номинальном напряжении при холостом ходе, то повышается напряжение на шине питания. Ниже на осциллограмме показаны скачки напряжения на шине питания когда сервопривод вращает груз на рычаге с постоянным значением ШИМ. В моменты опускания груза напряжение на шине питания повышается на 5.5 В. Это происходит потому что частота вращения ротора в эти моменты превышает максимальную частоту вращения на номинальном напряжении по даташиту.

Осциллограмма выброса обратной ЭДС на шине питания
Осциллограмма выброса обратной ЭДС на шине питания

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

Проблема определения скорости вращения серво-потенциометра

Если внимательно посмотреть на сигнал приходящий с серво‑потенциометра установленного на выходном валу, то можно заметить, что он содержит довольно существенный шум.
Чтобы найти скорость надо получить функцию дифференциала угла поворота от времени. При цифровом управлении это будет просто разница между текущим и предыдущим положением. Как видно из скриншота, после такой операции получается абсолютно зашумлённый сигнал. В этом случае применяем глубокую двухступенчатую фильтрацию. Фильтрацию выполняем с частотой ШИМ т.е. 16 КГц.

Вид сигнала с серво-потенциометра
Вид сигнала с серво-потенциометра
Модель Simulink для тестирования вариантов фильтрации сигнала скорости вращения выходного вала сервопривода.
Модель Simulink для тестирования вариантов фильтрации сигнала скорости вращения выходного вала сервопривода.

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

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

Модель для поиска критической фазовой задержки
Модель для поиска критической фазовой задержки

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

Результат работы модели
Результат работы модели

В точке времени 11.4 сек видим первую аномалию. Это и будет место первой инверсии фазы. Легко понять, что частота в этой точке будет равняться 1140 Гц. Следовательно после нашего фильтра мы не можем PID регулятором корректировать скоростью быстрее чем 1000 раз в секунду, иначе может возникнуть возбуждение в петле управления. Увеличение длины фильтра бегущего среднего в два раза также в два раза уменьшает предельную частоту управления.

Разработка приложений для MC50

Программный фреймворк серво контроллера MC50 выполнен на базе Azure RTOS. На данном этапе он содержит в себе практически весь состав промежуточного программного обеспечения Azure RTOS с драйверами периферии. Поверх слоя промежуточного ПО находится несколько прикладных задач: управления мотором, управления микросхемой 3-фазного драйвера мотора, измерения, GUI, управление выходами и другие.

Грубая структура программного обеспечения
Грубая структура программного обеспечения

В текущей версии софта реализовано управление рычагом на заданные углы с заданной скоростью. С помощью ручного энкодера производится калибровка углов, настройка скорости поворота и номер узла на шине CAN. Все остальные параметры можно настроить через терминал VT100 через USB VCOM или через интернет, как это было показано в предыдущих статьях. Команды сервоприводу могут подаваться с ручного энкодера, по шине CAN и через протокол FreeMaster через USB.

Разработка продолжается...

Комментарии (38)


  1. VelocidadAbsurda
    09.08.2023 07:37

    Правильно ли понял по измерению частоты вращения: измерение секторов (интервалов между соседними переключениями холлов) даёт погрешность до 30%, тогда как в целом тот же алгоритм, но с измерением целых эл.оборотов даёт точность 0.3%? Т.е. дело в настолько неточном размещении холлов относительно друг друга?


    1. Indemsys Автор
      09.08.2023 07:37
      +1

      Да, именно так. Кроме того что плата с датчиками неточно позиционируется, сами датчики выводные и могут быть отклонены под разными углами.


      1. RigidStyle
        09.08.2023 07:37

        Думаю использование сервпоривода без энкодера - узкоспециальная задача. Часто нужно начать вращение сразу и плавно. А не дергать мотор, пока система поймет где там ротор находиться по холлам.


        1. Goron_Dekar
          09.08.2023 07:37

          И в таких случаях датчик ставят после редуктора, чтобы ещё и люфты отработать корректно.

          Более того, иногда даже точности постановки энкодера на вал ШВП недостаточно, и обратную связь замыкают уже по фактическому перемещению детали, устанавливая линейный энкодер/интерфернеционный датчик. А для управляния мотором и 30% (10 после калибровки) хватит за глаза.


          1. Sabirman
            09.08.2023 07:37

            А на первой картинке же энкодер после редуктора стоит.


          1. RigidStyle
            09.08.2023 07:37

            Энкодер на моторе ставится не для точности перемещений. А для того, что бы мотор сразу плавно начал движение, что бы определить его положение вала. На редукторе или линейку на станке без энкодера на валу ставить чревато тем, что если есть люфт и нет одноименной нагрузки, его выбирающей, то удерживая положение мотор будет дергать туда сюда редуктор или ШВП, и тем самым разбивать его.

            Один энкодер после редуктора есть смысл ставить только тогда, когда его разрешающая способность меньше, чем люфт этого самого редуктора. Например, когда это какая то сервомашинка.


      1. Dynasaur
        09.08.2023 07:37

        Звучит диковато. Неужели такие двигатели считаются годными? Что мешает производителям точно ставить датчики под правильными углами?


  1. RV3EFE
    09.08.2023 07:37
    +3

    Статья отличная, как и предыдущие. Автору почтение!

    Понравился момент с измерением тока и перекалибровкой. Часто видел что просто забивают и что-то измеряют. Интересно, использование внешнего источника напряжения смещения не решит эту проблему, плывут сами ОУ или внутренний ref?

    Хотелось бы видео работы с пид наподобие того, где в примере на плече груз пролетал вниз.


    1. Indemsys Автор
      09.08.2023 07:37

      Будет позже.


  1. zatim
    09.08.2023 07:37
    +1

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

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

    В вашем случае параллельно шине питания можно мощный электронный стабилитрон на напряжение на 20-30% выше напряжения питания.


    1. Indemsys Автор
      09.08.2023 07:37
      -2

      Если действительно управляем скоростью, то выбросу ЭДС неоткуда взяться. Если мотор все же разогнался, значит скоростью уже не управляем, замыкаем обмотки и выброса не будет.

      В инвертерах ставят тормозные резисторы чтобы не перегревать торможением мотор и чтобы быстрее снижать ток.

      Здесь же сервопривод короткого действия и вполне можно тормозить рассеиванием тепла в моторе.


      1. zatim
        09.08.2023 07:37

        Если ваша цель - остановить мотор как можно быстрее, чтобы механизм колом встал, то да - замыкаем обмотки. Если вы хотите управлять скоростью то нужно куда то девать энергию от рекуперации. Или - или. Но никак не одновременно.


        1. Indemsys Автор
          09.08.2023 07:37
          -1

          Управление скоростью не устанавливает требование на то что является источником силы.

          Если крутит внешняя сила, то так же ШИМ-ом регулируем замыкание обмоток и управляем скоростью. Энергия, как уже сказал, рассеивается на моторе.


          1. zatim
            09.08.2023 07:37

            Мне кажется, вы немного не понимаете процесс. Если вы выдаете на выход 3 фазы ШИМом и пытаетесь регулировать скорость, а внешний механизм начинает раскручивать двигатель то автоматически получается режим рекуперации и механическая энергия от внешнего механизма превращается в электрическую и идет на повышение напряжения на шине постоянного тока. Режим торможения мотора путем замыкания обмоток - это ПРИНЦИПИАЛЬНО другой режим, возможно он даже аппаратно неосуществим в вашей схемотехнике. В этом режиме ШИМом надо СИНХРОННО замыкать или три верхних плеча или три нижних для замыкания вместе всех трех обмоток. Еще раз повторяюсь, это принципиально другой режим, в который нужно специально переключаться и переключать все механизмы регулирования и обратных связей. И то, при условии, если ваша микросхема драйвера так умеет (я подробно не вникал).


            1. Indemsys Автор
              09.08.2023 07:37
              -1

              Если бы вникли подробнее в открытые исходники, то увидели что там как раз так и сделано. При отрицательной ошибке в контуре скорости закорачиваются обмотки мотора.
              Современные микроконтроллеры могут как угодно управлять своими ШИМ модулями. Про принципы синхронизации было в предыдущей статье.


              1. zatim
                09.08.2023 07:37
                -1

                Если бы вникли подробнее в открытые исходники

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

                то увидели что там как раз так и сделано.

                Если это сделано, тогда откуда выбросы и зачем вы в статье заостряете внимание на этой проблеме без описания решения? Кстати, в статье написано что сделано другое:

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

                Я так понял, сделан только костыль при отсутствии питания.


                1. Polarisru
                  09.08.2023 07:37
                  +1

                  Я так понял, сделан только костыль при отсутствии питания.

                  Я тоже из текста статьи подумал именно так.


                  1. Indemsys Автор
                    09.08.2023 07:37

                    На модели торможение обмотками изображено достаточно явно -

                    Но это всего лишь один из множества вариантов управления. В дальнейшем все будет меняться.


        1. RigidStyle
          09.08.2023 07:37

          Ну замыкать обмотки то можно не одномоментно и навсегда, а тоже ШИМить, тем самым управляя усилием торможения.


  1. aspid-crazy
    09.08.2023 07:37

    А нельзя ли использовать вместо потенциометра магнитный энкодер? В идеале даже два - один непосредственно на валу, второй после всех редукторов. Тогда можно вообще добиться прецизионной точности, КМК.


    1. Goron_Dekar
      09.08.2023 07:37

      после редуктора можно и оптический, тыщ на 15 имп/об поставить.


      1. aspid-crazy
        09.08.2023 07:37

        К примеру абсолютный магнитный енкодер AS5600 стоит чуть более 100 рублей. Точность, конечно, пониже, но если ставить до редуктора - то в 0,1 градуса привода можно уложиться. т.е. это уже сопоставимо с дребезжанием самого редуктора.


        1. Goron_Dekar
          09.08.2023 07:37

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


          1. aspid-crazy
            09.08.2023 07:37

            Вы, вероятно, правы. Но объясню свою логику, которая привела меня к такому предположению:
            Допустим у нас редуктор 1 к 5, т.е. на 5 оборотов мотора, вал после редуктора совершает один.
            Точность магнитного энкодера заявляется подярка 0.1 градуса (12 бит), но в реальности, после сглаживания, думаю, можно рассчитывать на ~ 1 градус.
            Таким образом, если мы ставим энкодер до редуктора, то знаем положение детали с точностью до 1/5 градуса + люфт. Я не учитываю тут возможность ошибиться на целый оборот мотора, т.к. не ожидаю в задачах для сервоприводов таких скоростей вращения детали.
            Если же энкодер стоит после редуктора, то погрешность измерения положения составит целый градус, что кажется значительно менее точным.
            На самом деле, даже если мы знаем точное положение детали, то это не значит что мы можем им управлять так же точно. Наличие люфта не позволит.
            Именно поэтому, насколько я это себе представляю, в оптических стабилизаторах для видео/фото камер (двух- или трехосных) используют BLDC непосредственно на осях, без редукции.
            Но если бы реализовать редукцию без люфта, то кажется, энкодер на валу двигателя был бы значительно точнее. Так что это, кажется, вопрос отношения люфта к точности энкодера: при разных значениях, выгоднее может оказаться тот или иной вариант.


    1. Indemsys Автор
      09.08.2023 07:37
      +1

      Магнитные энкодеры имеют смещение от магнитного поля земли. Реагируют на магнитные поля от металла вокруг и на токи в проводах. Слишком много хлопот с ними.


      1. Polarisru
        09.08.2023 07:37

        Это вы какие-то не те сенсоры берете, да, в свое время столкнулись с этим у TLE от Infineon и у сенсоров от Allegro, а вот у AMS с этим вообще никаких проблем нет.


      1. ProLimit
        09.08.2023 07:37

        На самоме деле поле Земли слишком мало, там редкозмельный магнит в 1мм от датчика, поле достаточно сильное. Влияние железа также можно исключить, не используя ферромагнитный крепеж. Есть проблема нелинейности, но по датащитам 1-2%, для мотора вообще не критично


  1. Polarisru
    09.08.2023 07:37
    +1

    Что делать с обратной ЭДС? 

    так и не увидел в итоге, что делать с обратной ЭДС во включенном состоянии при опускании груза. Какая-то странная статья, особенно в свете бодания с дребезгом потенциометра, когда уже давно есть бесконтактные абсолютные магнитные сенсоры.


    1. aspid-crazy
      09.08.2023 07:37

      Справедливости ради, показания магнитного сенсора тоже шумят. Может меньше, но фильтровать тоже придется.


    1. Indemsys Автор
      09.08.2023 07:37

      Серво-потенциометр оказалось очень легко поставить и у него нет проблем с соосностью.
      Про чувствительность к магнитным полям выше ответил.


  1. devlev
    09.08.2023 07:37
    +1

    Сервопривод вовсе не означает что нужно использовать шаговый двигатель. В большинстве сервоприводов отлично справляются обычные коллекторные двигатели. В сочитании с хорошим энкодером устройство получается надежным и точным. Все зависит от поставленной задачи, но все равно если речь идет про умный дом то шаговые двигатели нужны не так уж и часто. Открытие ворот, запорные краны, жалюзи и прочие вещи можно легко обойтись без шаговых двигателей. Поэтому вопрос целесообразности использования шагового двигателя в конкретном случае для меня остается открытым.


    1. Indemsys Автор
      09.08.2023 07:37
      +1

      Ну так статья и открывает этот вопрос.

      Что удобнее в жалюзи - коллекторный дешёвый низковольтный, BLDC низковольтный, но более экономичный и надёжный, шаговый экзотический или асинхронный однофазный высоковольтный и шумный?
      Вот в чем вопрос!


      1. Dynasaur
        09.08.2023 07:37
        +1

        Что то у меня сомнения, что бесщёточный двигатель более экономичный, чем щёточный. Учитывая как греются контроллеры, по моему щётки так не греются.


  1. ValeriyS
    09.08.2023 07:37
    +1

    Классная статья, спасибо. Очень интересна часть с интеграцией Simulink модели и фреймворка на Azure RTOS.
    Возможность использования таких могучих инструментов как-бы предполагает использовать стандартный подход к построению систем с обратной связью:
    - измерить частотную характеристику
    - построить zero/pole модель (System Identification, функция ssest в Матлаб)
    - рассчитать контроллер как zero/pole cancelling для минимально-фазовой части (bi-quad sections, SOS объекты в Матлаб)
    - посмотреть и подстроить характеристики sensitivity и disturbance attenuation

    Для меня это прямо как чудо, что можно усилиями одного или нескольких человек поднять такую инфраструктуру серво-привода.


  1. Dynasaur
    09.08.2023 07:37

    Вращение ротора может внезапно прекратится при встрече сервопривода с препятствием. Но тогда блок расчёта частоты вращения по датчикам Холла просто не будет выполняться.

    Вот я не въехал. А зачем нужна частота вращения, если нет никакого вращения?

    Через некоторое время показатель частоты вращения упадёт до нуля и алгоритм управления скоростью примет решение об отключении тока.

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


    1. Indemsys Автор
      09.08.2023 07:37

      Есть сервоприводы управляющие скоростью, а есть управляющие положением.
      В тексте об этом говорится.

      Данный проект содержит приложение управляющее только скоростью вращения по траектории скорости от времени. Для летательных аппаратов конструкция не годится в том числе потому что китайский планетарный редуктор имеет сильные торсионные осцилляции из-за пластмассовых шестерёнок.


  1. retrodolivo
    09.08.2023 07:37

    Статья шикарная, спасибо! Вот бы научиться также в симулинке работать...


  1. NutsUnderline
    09.08.2023 07:37

    надо прочитать остальные статьи цикла чтоли: а то здесь как то красиво в симулинке а потом сразу резко "откуда то" взялся код.