Открытый проект серво‑контроллера MC50 продолжает развиваться. На этот раз поговорим о создании сервопривода.
Сервоприводы в умном доме нужны повсеместно: в запорных кранах, в электрических замках, в моторизированных кронштейнах, столах, жалюзи, в автоматических дверях, калитках, окнах, маркизах, воротах, поворотных видеокамерах, регулируемых креслах, электро‑пандусах и проч. Поэтому технология сервоприводов весьма востребована и тут есть где развернуться творчеству.
Предыдущие статьи по теме проекта MC50
В прошлой статье было показано как заставить BLDC мотор крутиться с помощью периферии микроконтроллера S5D9 семейства Renesas Synergy™
Теперь это вращение надо заставить делать полезную работу. Сторонние наблюдатели могут сказать, что работа уже сделана если мотор вращается, цель достигнута. Ставь редуктор и в прод. Но на самом деле проблемы только начинаются.
Конструкция
Конструкция состоит из мотор‑редуктора, дополнительного редуктора 29/40, простого узла с серво‑потенциометром для контроля за скоростью выходного вала и термистором для контроля за температурой мотора. Как видно, внешний редуктор даёт удобно разместить серво‑потенциометр. Дополнительно он увеличивает момент силы 1.3 раза Такая конструкция в разных масштабах встречается во множестве сервоприводов.
Схема подключения
Плата серво‑контроллера рассчитана на питание от внешнего источника напряжением от 20 до 30 В. Но силовой драйвер продолжает функционировать при понижении напряжения до 7 В. При отсутствии внешнего источника напряжения плата может питаться от USB, но силовой драйвер при этом функционировать не будет.
О частоте вращения BLDC мотора
Частота вращения BLDC двигателя определяется приложенным к нему напряжением при условии постоянной нагрузки на валу (считаем что 6-шаговая коммутация в это время работает безукоризненно, напряжение меняем ШИМ‑ом). Эта частота записывается в даташите на мотор. Если момент силы нагрузки будет больше или меньше, то частота будет разная при неизменном напряжении. Но мы не знаем точно момент силы нагрузки и он скорее всего будет переменным.
Для идеального двигателя без нагрузки, без трения и без сопротивления обмоток частоту вращения при определенном напряжении можно узнать вращая мотор внешним приводом до достижения заданного напряжения. Частота вращения в этот момент и будет искомой частотой. Но тут важна форма напряжения. Применив 6-шаговую коммутацию к мотору с синусоидальной обратной ЭДС, не получится получить ту же частоту.
Идеальный BLDC двигатель будет крутиться с бесконечной частотой при подаче на него бесконечного напряжения. У реального же BLDC двигателя в какой‑то момент сначала наступит насыщение магнитопровода, резко повысится ток через обмотки, обмотки раскалятся и спустя некоторое время мотор загорится. Поэтому частота вращения и допустимый момент силы ещё ограничиваются температурой мотора.
Разработка архитектуры управления.
Если для для управления голым двигателем архитектура достаточно ясна, то для управления сервоприводом с неизвестной динамикой нагрузки все несколько сложнее.
Для начала отметим, что частота вращения ротора не равна частоте вращения сервопривода, а в динамике с реальной механикой с люфтами и упругими реакциями даже не пропорциональна.
Для управления частотой вращения самое лёгкое решение — это просто менять скважность широтно‑импульсной модуляции (ШИМ).
При постоянном моменте силы нагрузки на мотор или ручном управлении все довольно хорошо получается. Мы линейно меняем скважность и добиваемся нужной частоты вращения. Коммутация автоматическая с помощью датчиков Холла находящихся в моторе. Метод отлично работает в самокатах.
Но как только момент силы начнёт меняться частота вращения начнёт пульсировать, как показано на видео ниже.
Самое очевидное решение — это применить PID управление с обратной связью по частоте вращения. К сожалению все не так просто. Серво‑приводы могут предназначаться для управления угловой скоростью или для управления положением, в обоих случаях нужны разные алгоритмы. И в обоих случаях простой одноконтурный PID очень плохо работает. В случае со стабилизацией положения нужен регулятор с переходной характеристикой не менее третьего порядка. А PID имеет второй порядок. т. е. оставаясь в рамках PID нужно две петли управления, вложенные одна в другую. Нам нужна хорошая, быстрая, оптимизируемая, масштабируемая реализация PID алгоритма.
Такая реализация есть в среде MATLAB Simulink в виде модели конвертируемой в исходники на C. Ниже представлена модель в виде открытой расширяемой подсистемы.
Модель повторяет функциональность встроенного PID компонента Simulink, но при этом выводит все коэффициенты PID как входные аргументы в одной шине. Аргументы можно менять и в стандартном компоненте используя символьные обозначения переменных из рабочего пространства, но это влечёт за собой неявные зависимости приводящие к ошибкам. А шина обеспечивает генерацию аргумента как передаваемой структуры. Это уменьшает объем кода при интеграции модели в embedded проект и упрощает рефакторинг моделей.
Для предотвращения бесконечного нарастания интегральной составляющей (anti‑windup) применён метод clamping.
Кроме того, в нашей модели реализован ограничитель скорости нарастания выходного сигнала. Это вносит дополнительную защиту в модель от предельных осцилляций в случае использования некорректных начальных коэффициентов во время тюнинга.
Сгенерированные из модели исходники находятся в директории MATLAB_PID.
Выбор одноконтурного или двухконтурного управления скоростью вращения серво-привода.
Чтобы сделать выбор архитектуры петли управления проведём эксперимент. Сделаем в среде FreeMaster две записи сигналов в разных режимах управления мотором. Записывать будем одновременно четыре сигнала:
сигнала включения,
тока протекающего через мотор,
состояния датчиков Холла, отражающих скорость вращения ротора,
скорость выходного вала привода.
Первая запись ниже сделана во время скачкообразного включения вращения мотора в режиме 6-step с максимальным напряжением.
Вторая запись ниже сделана во время скачкообразного включения вращения мотора в режиме 6-step с удержанием постоянного тока через мотор с помощью петли управления током.
Если посмотреть на реакцию скорости выходного вала на скачок воздействия, то на первой записи видна крайне нелинейная кривая скорости. На второй записи скорость достаточно линейно нарастает, но нарастание начинается с некоторой задержкой.
Очевидно в первом случае практически трудно будет проводить регулировку скорости регулируя алгоритмом PID скважность ШИМ. Во всяком случае с интервалом регулирования меньшим 0.1 сек. Поскольку скорость откликается совершенно не похожим на линейную систему образом. А классический PID хорош только для линейных систем.
Во втором случае отклик гораздо более линейный и PID здесь подойдёт. Вернее тут подойдёт простой PI контур, поскольку отклик демонстрирует инерциальную нагрузку без упругости (осцилляций). Но надо помнить — тут уже применён высокоскоростной контур поддержки заданного тока. т. е. в данном случае у нас получится 2-контурная система: быстрый PI контур управления током через ШИМ и медленный PI для управления скоростью через задание тока.
В итоге пришли к следующей архитектуре в составе модели 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. Микросхема получает сигналы от микроконтроллера и управляет затворами силовых транзисторов. Микроконтроллер не может напрямую управлять затворами поскольку там нужны высокие напряжения и довольно большие импульсные токи.
У этой микросхемы есть возможность через интерфейс SPI задавать уровень выходных токов на затворы. Есть 4 уровня: от самого слабого 400…600 мА до самого сильного 1200…1800 мА.
Как показали испытания всех возможных уровней, на ток потребления они практически не влияют. Измерения мощности при работе двигателя весьма зашумлены, и с учётом этого шума возможно допустить наличие некоторой разницы в пределах 100 мВт.
Силовые транзисторы на схеме подобраны оптимально по отношению к драйверу, поэтому регулировка его силы не оказывает заметного влияния. Стоит ожидать в общем случае, что чем больше ток драйвера тем больше в нем будет потерь. В настройках программы можно свободно менять уровни токов, а также уровни детекции и защиты от коротких замыканий.
Управлять настройками драйвера TMC6200 можно с помощью ряда параметров. Настройки загружаются при старте основной задачи управления мотором.
Калибровка смещения токовых усилителей драйвера TMC6200
Микросхема TMC6200 имеет встроенные усилители напряжения с токовых шунтов расположенных в фазах серво драйвера. Наличие встроенных усилителей упрощает схему. Есть возможность менять коэффициент усиления через интерфейс SPI.
Однако недостатком этих усилителей является довольно большое напряжение смещения. Ещё хуже, то что оно сильно зависит от температуры микросхемы. Но мало того, оно разное для каждого из 3 усилителей и может меняться во времени даже при постоянной внешней температуре. В нашей программе используется самый чувствительный режим с усилением в 20 раз. При таком усилении смещение на выходе усилителей достигает 240 мВ и более.
Решение проблемы найдено в том что перед каждым включением мотора производится перекалибровка смещения нуля для каждого из 3 усилителей. Перекалибровка длится около 15 мсек и существенного влияния на быстродействие не оказывает.
Что делать с обратной ЭДС?
Как только мотор начинают крутить внешние силы вместо нашего драйвера он превращается в генератор. В генератор мотор также превращается на короткие промежутки времени когда происходит торможение. Если частота вращения вызванная внешними силами превышает частоту вращения на номинальном напряжении при холостом ходе, то повышается напряжение на шине питания. Ниже на осциллограмме показаны скачки напряжения на шине питания когда сервопривод вращает груз на рычаге с постоянным значением ШИМ. В моменты опускания груза напряжение на шине питания повышается на 5.5 В. Это происходит потому что частота вращения ротора в эти моменты превышает максимальную частоту вращения на номинальном напряжении по даташиту.
Если бы мы управляли скоростью и не давали разгоняться сервоприводу, то таких выбросов напряжения не было бы. Однако скачки напряжения могут возникнуть и при выключенном состоянии сервопривода. Тогда стратегия такая. Когда напряжение достигнет уровня достаточного для включения процессора, он сразу подаёт сигнал включения на все нижние транзисторы. Транзисторы закорачивают обмотки мотора и напряжение спадает.
Проблема определения скорости вращения серво-потенциометра
Если внимательно посмотреть на сигнал приходящий с серво‑потенциометра установленного на выходном валу, то можно заметить, что он содержит довольно существенный шум.
Чтобы найти скорость надо получить функцию дифференциала угла поворота от времени. При цифровом управлении это будет просто разница между текущим и предыдущим положением. Как видно из скриншота, после такой операции получается абсолютно зашумлённый сигнал. В этом случае применяем глубокую двухступенчатую фильтрацию. Фильтрацию выполняем с частотой ШИМ т.е. 16 КГц.
Поскольку фильтр вносит фазовую задержку мы не можем на основе такого отфильтрованного сигнала управлять как угодно быстро и точно.
Для того чтобы узнать задержку в результате фильтрации измерим её на модели
Посмотрим разницу во времени между сигналами Serv.speed и Meas.serv.speed. Это два разных значения скорости вращения выходного вала в градусах в секунду. Первая величина реальная, снятая непосредственно с вала, а вторая оценочная, после фильтрации дифференциала сигнала позиции вала.
При коэффициенте экспоненциального фильтра (на модели обозначен как Speed exp.filter1) равного 1-1/2048 (0.99951) получаем задержку 83 мс.
При коэффициенте экспоненциального фильтра равного 1-1/256 (0.9961) получаем задержку 16 мс.
Задержки существенные. Они ограничивают такт регулятора скорости величиной не более 10...50 Гц.
Как видно, простая комбинация FIR и IIR фильтров не слишком хороший способ получить чистое значение скорости. Но пока останавливаемся на этом.
К вопросу о применении магнитных сенсоров вместо серво-потенциометра
Можно слышать мнение о лучших свойствах магнитных сенсоров по сравнению с серво-потенциометром. Был проведён эксперимент с GMR сенсорами угла поворота TLI5012B.
Была изготовлена отладочная плата с этим сенсором, написан софт для обмена с FreeMaster. Непосредственно над сенсором закрепили диаметрально поляризованный магнит. Магнит повернут так чтобы сенсор показывал угол близкий к нулю. И были проведены замеры показаний при смене ориентации платы относительно магнитного поля земли
Вот результат изменения показаний сенсора при повороте платы в поле земли:
Как видно присутствие рядом мощного магнита не спасло сенсор от чувствительности к полю земли. Изменение показаний достигают 10%.
Шум серво-потенциометра достигает 5 сэмплов на любом угле поворота при 12-битном АЦП. На середине диапазона это будет равно 5/2048 = 0.24%. У GMR сенсора TLI5012 шум составляет 0.025 %
Однако деформации платы с сенсором на доли миллиметра приводили к искажениям показаний на 0.06 %. Сенсор очень чувствителен к изменению расстояния до магнита и очень чувствителен к вибрациям.
Изменение дистанции между осями сенсора и магнита на 1 мм приводит к изменению значения угла на 1 градус.
Появление на расстоянии 50 см магнита аналогичного приложенного к сенсору также вызывает флуктуации приблизительно в 1 градус.
Всё говорит о том, что в целом шумы от магнитного сенсора будут незначительно меньше шумов от серво-потенциометра, но при этом понадобиться гораздо тщательней продумывать механическое крепление сенсора.
Разработка приложений для MC50
Программный фреймворк серво контроллера MC50 выполнен на базе Azure RTOS. На данном этапе он содержит в себе практически весь состав промежуточного программного обеспечения Azure RTOS с драйверами периферии. Поверх слоя промежуточного ПО находится несколько прикладных задач: управления мотором, управления микросхемой 3-фазного драйвера мотора, измерения, GUI, управление выходами и другие.
В текущей версии софта реализовано управление рычагом на заданные углы с заданной скоростью. С помощью ручного энкодера производится калибровка углов, настройка скорости поворота и номер узла на шине CAN. Все остальные параметры можно настроить через терминал VT100 через USB VCOM или через интернет, как это было показано в предыдущих статьях. Команды сервоприводу могут подаваться с ручного энкодера, по шине CAN и через протокол FreeMaster через USB.
Разработка продолжается...
