57д. FOC своими руками: SimpleFOC и ODrive — векторное управление бесколлекторным мотором

В разделе 57а векторное управление уместилось в пять строк: измерить токи, Кларк, Парк, два ПИ-регулятора, обратно в ШИМ. В 57б — как измерить ток в нужный момент периода. В 57в двигателем командовали по CAN, не заглядывая внутрь контроллера. Здесь заглядываем: как устроен весь цикл в двух открытых проектах, которые люди ставят на свои платы.

SimpleFOC (Arduino-FOC) — библиотека под любой микроконтроллер, в том числе STM32: цикл loopFOC() + move() в loop() программы. ODrive — контроллер двух двигателей на STM32F405: весь регулятор в прерывании таймера на жёсткой частоте. На их коде разобраны пространственно-векторная модуляция и откуда берутся лишние 15,47 % напряжения, такты ODrive, контур тока и его предельная полоса, контуры скорости и положения, оценка скорости по энкодеру и измерение сопротивления и индуктивности фазы.

Опоры: исходники ODrive 0.5.6 (Firmware/MotorControl/foc.cpp, utils.cpp, motor.cpp, motor.hpp, encoder.cpp, controller.hpp, Board/v3/Inc/main.h, board.h, Board/v3/Src/tim.c) и SimpleFOC 2.4.0 (src/common/foc_utils.cpp, pid.cpp, defaults.h, src/BLDCMotor.cpp, src/common/base_classes/FOCMotor.cpp, CurrentSense.cpp, Sensor.h). Код не пересказан на глаз: настоящие foc.cpp, SVM из utils.cpp, законы измерения R и L из motor.cpp (вырезаны дословно), our_arm_sin_f32/our_arm_cos_f32, foc_utils.cpp и ветка модуляции из BLDCMotor::setPhaseVoltage собраны g++ на компьютере и работают на модели двигателя. Двигатель модели — допущение, а не паспорт реального мотора: R = 0,05 Ом, L = 20 мкГн на фазу, Kv = 270 об/мин/В, 7 пар полюсов, момент инерции с нагрузкой 3·10⁻⁴ кг·м², шина 24 В. Прогон — _verify/foc57d/foc57d.py, запись — _verify/foc57d/foc57d_host.txt. Лицензия: ODrive — MIT (© ODrive Robotics, файл LICENSE.md; прошивка — MIT, © Oskar Weigl, Firmware/LICENSE), arm_sin_f32.c/arm_cos_f32.c в ODrive — Apache-2.0 (© ARM, в заголовке файла); SimpleFOC — MIT (© Antun Skuric, файл LICENSE).

Как это называется в документации

Оригинал	Перевод
field oriented control (FOC)	векторное управление
Clarke / Park transform, inverse Park	преобразование Кларк / Парка, обратное Парка
d-axis, q-axis current (Id, Iq)	ток по продольной оси (поле) и по поперечной (момент)
space vector modulation (SVM, SVPWM)	пространственно-векторная модуляция
sine PWM	синусный ШИМ
midpoint clamp, zero sequence	сдвиг «по середине», нулевая последовательность
modulation index, mod_to_V	индекс модуляции, перевод модуляции в вольты
current control bandwidth	полоса контура тока
pole-zero cancellation	сокращение полюса нулём регулятора
back-EMF, feedforward	противо-ЭДС, упреждение
PLL (phase-locked loop)	ФАПЧ — следящая оценка угла и скорости
phase resistance / inductance calibration	измерение сопротивления и индуктивности фазы
voltage mode, foc_current	управление напряжением, управление током d/q

57д.1 Один такт FOC и два проекта

Цепочка одна и та же, что в 57а.7. Токи фаз → Кларк (α, β) → Парк на угол ротора θ (d, q) → два ПИ-регулятора дают напряжения Vd, Vq → обратный Парк (Vα, Vβ) → модулятор раскладывает вектор на три скважности. Снаружи — контур скорости, который задаёт Iq, и контур положения, который задаёт скорость.

Кларк в проектах записан по-разному. SimpleFOC (CurrentSense::getABCurrents) берёт β = (ia + 2·ib)/√3, как в 57а; ODrive (foc.cpp) — β = (ib − ic)/√3. При ia + ib + ic = 0 это одно и то же: на круге через 1° расхождение вместе с Парком не больше 7,2·10⁻¹⁶. Если измерены все три тока, SimpleFOC сначала вычитает из каждого треть суммы — погрешность датчиков делится поровну.

Устроены проекты по-разному. В SimpleFOC loopFOC() читает датчик, считает электрический угол, по режиму момента получает Uq и Ud и вызывает setPhaseVoltage(); move() — внешние контуры (скорость, угол) и может вызываться реже (motion_downsample). Оба зовут из loop(): частота — сколько успеет процессор. В ODrive всё сидит в прерывании: измерение тока, оценка угла, контуры, SVM — строго раз в такт 8 кГц (57д.3).

Синус и косинус для Парка — таблицей. В SimpleFOC _sin — 65 значений четверти волны в 16-битных целых и линейная интерполяция по 8-битной дроби; на 3217 точках [0, 2π) СКО ошибки 5,74·10⁻⁵ (в комментарии кода — 0,00006480), наибольшая на 10⁶ точках — 1,59·10⁻⁴. ODrive берёт arm_sin_f32/arm_cos_f32 из CMSIS: таблица на 512 отрезков, ошибка не больше 1,9·10⁻⁵. На G474 то же считает CORDIC (раздел 46а).

57д.2 SVM против синуса: откуда 15,47 %

Каждая фаза моста — полумост: напряжение на выходе от 0 до Vbus. Двигатель в звезде чувствует только разности фаз, общую для трёх фаз добавку он не видит. Синусный ШИМ даёт каждой фазе 0,5 + m·sin(...) — амплитуда фазного напряжения не больше Vbus/2. Но линейное напряжение — разность двух таких синусов, и его амплитуда в √3 раз больше фазной; если добавить ко всем трём фазам одну и ту же добавку, которая «прижимает» вектор к краям, предел фазной амплитуды станет Vbus/√3 (линейное — сама шина). Отношение:

(Vbus/√3) / (Vbus/2) = 2/√3 = 1,154701 — на 15,47 % больше.

Это и есть SVM. В SimpleFOC она записана как сдвиг «по середине» (BLDCMotor.cpp, режим SpaceVectorPWM): из трёх фазных напряжений вычитают полусумму наибольшего и наименьшего.

          float Umin = min(Ua, min(Ub, Uc));
          float Umax = max(Ua, max(Ub, Uc));
          center -= (Umax+Umin) / 2;

ODrive (utils.cpp, функция SVM) делает то же через шесть секторов: находит сектор вектора, время двух соседних базовых векторов и три момента переключения. Сектор 1:

            float t1 = alpha - one_by_sqrt3 * beta;
            float t2 = two_by_sqrt3 * beta;

            // PWM timings
            tA = (1.0f - t1 - t2) * 0.5f;

Вход — вектор в долях «модуляции», где 1 = ⅔·Vbus (mod_to_V = (2.0f / 3.0f) * vbus_voltage в foc.cpp). Выход — момент включения фазы в долях полупериода: скважность фазы 1 − t. Настоящая SVM, SimpleFOC и независимый расчёт на Python дают одно и то же:

SimpleFOC со своим табличным синусом расходится с SVM в пятом знаке — у неё на входе Uq и угол, а не α, β.

Предел проверен перебором: по 3600 направлениям наибольший допустимый модуль SVM — от 0,866025 (√3/2, между базовыми векторами) до 1,000000 (вдоль них). Круг радиусом √3/2 модуляции — это 0,577350·Vbus амплитуды фазы. SimpleFOC при 24 В: синусный режим держит Uq до 12,0000 В (0,5·Vbus), SVPWM — до 13,8564 В (0,577350·Vbus).

ODrive весь круг не берёт: mod_scalefactor = 0.80f * sqrt3_by_2 * ... — не больше 0,692820 модуляции, то есть 0,461880·Vbus, меньше даже предела синусного ШИМ. Зато скважность любой фазы не выше 0,9: нижний ключ открыт хотя бы 10 % периода — окно для шунта в нижнем плече (57б.5).

57д.3 Такты ODrive: ШИМ 24 кГц, регулятор 8 кГц

Числа — из Board/v3/Inc/main.h: TIM_1_8_CLOCK_HZ = 168 000 000, TIM_1_8_PERIOD_CLOCKS = 3500, TIM_1_8_RCR = 2, TIM_1_8_DEADTIME_CLOCKS = 20. TIM1 и TIM8 считают по центру (TIM_COUNTERMODE_CENTERALIGNED3 в tim.c), период ШИМ — 2·3500 тактов: 24 000 Гц. Счётчик повторений 2 даёт событие обновления раз в три полупериода, регулятор работает на каждом втором из них: CURRENT_MEAS_HZ = 168 МГц / (2·3500·3) = 8000 Гц, такт 125 мкс. Защитный интервал 20 тактов — 119 нс.

На событии, когда счётчик идёт вверх, АЦП только что взял ток в «нулевом» векторе, где открыты нижние ключи (57б.2); на встречном событии — отсчёт при нулевом токе для калибровки смещения. Так один таймер делает и ШИМ, и выборку в нужный момент, как в 57б.3.

Выход считается с упреждением. В board.cpp метка выхода — timestamp + 3 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1): 31 500 тактов = 187,5 мкс = 1,5 такта регулятора — середина промежутка, когда новые скважности будут действовать. foc.cpp доворачивает угол обратного Парка на phase_vel × это время. В модели так и сделано: ток взят в момент tₖ, скважности, посчитанные по нему, работают от tₖ₊₁ до tₖ₊₂.

57д.4 Контур тока: ПИ с сокращением полюса

Обмотка по оси d или q — последовательные R и L: звено первого порядка с полюсом R/L. ODrive ставит ПИ так, чтобы ноль регулятора сократил этот полюс (motor.cpp, update_current_controller_gains):

    float p_gain = config_.current_control_bandwidth * config_.phase_inductance;
    float plant_pole = config_.phase_resistance / config_.phase_inductance;
    current_control_.pi_gains_ = {p_gain, plant_pole * p_gain};

Замкнутый контур — снова звено первого порядка с постоянной 1/bw. По умолчанию current_control_bandwidth = 1000 рад/с; для нашего двигателя p_gain = 0,02 В/А, i_gain = 50 В/(А·с).

Ступенька Iq с 0 на 10 А, настоящий FieldOrientedController:

• два первых такта ток не меняется — это задержка 1,5 такта из 57д.3;
• 63,2 % — за 1,000 мс, ровно 1/bw; 90 % — за 2,125 мс;
• |Id| не больше 0,0268 А: оси развязаны;
• пик Iq за 10 мс — 9,6004 А, а не 10.

Недостающие 0,4 А — противо-ЭДС. Ротор разгоняется (1,4215 об/с через 10 мс), ЭДС растёт линейно, а ПИ с нулём ошибки держит только постоянную помеху. Расчёт: ускорение 1021,0 рад/с², наклон ЭДС 20,85 В/с, отставание = наклон / i_gain = 0,417 А. Лечится упреждением: bEMF_FF_enable и R_wL_FF_enable в motor.hpp (по умолчанию выключены) добавляют к Vq и Vd расчётные ЭДС и падения.

Полосу нельзя поднимать без конца: задержка 1,5 такта съедает запас устойчивости. Та же ступенька при bw = 5000 рад/с даёт пик 13,837 А (перерегулирование 38,4 %), при 10 000 рад/с контур раскачивается — пик 69,764 А за 10 мс.

SimpleFOC выбирает режим момента полем torque_controller. В режиме voltage ток вообще не измеряют: voltage.q = current_sp, то есть задание — прямо Uq в вольтах. Для высокоомных моторов это удобно (в ODrive такой тип так и назван — MOTOR_TYPE_GIMBAL, и «ток» там — вольты). Для нашего 0,05 Ом — опасно: Uq = 0,5 В на стоящем роторе даёт пик Iq 9,488 А (Uq/R = 10 А), дальше ток падает по мере роста ЭДС; через 1 с с нагрузкой 0,01 Нм — 3,7662 об/с, Iq 0,3264 А, и Id 0,1313 А, которым никто не управляет. Режим foc_current — полноценные два ПИ по d и q с фильтром: по умолчанию DEF_PID_CURR_P 3, DEF_PID_CURR_I 300, DEF_CURR_BANDWIDTH 300 (defaults.h, ветка для STM32); ПИ в pid.cpp — трапециями (Тастин) с ограничением выхода.

57д.5 Скорость, положение и датчик угла

Внешний контур должен быть заметно медленнее внутреннего — тогда для него контур тока «мгновенный». В ODrive по умолчанию (controller.hpp): pos_gain = 20 (об/с)/об, vel_gain = 1/6 Нм/(об/с), vel_integrator_gain = 2/6 Нм/(об/с·с). Полоса контура скорости — vel_gain / (2π·J) = 88,42 рад/с при нашем J: в 11,31 раза ниже контура тока, а контур положения (20 1/с) ещё в 4,42 раза ниже. Коэффициент момента — 8.27/Kv (motor.hpp); 8,27 = 60·√3/(4π), при Kv = 270 это 0,03063 Нм/А.

Разгон 0 → 20 об/с за 0,2 с с этими коэффициентами и настоящим контуром тока: Iq на разгоне в среднем 6,2708 А (расчёт J·a/Kt — 6,1540 А), ошибка скорости не больше 1,1016 об/с, через 0,4 с — 20,2759 об/с, перелёт 0,276 об/с медленно съедает интегратор. Модуляция не выше 0,1736 при пределе 0,6928, скважности 0,4…0,6: ЭДС на 20 об/с всего 2,566 В.

Датчик. Угол и скорость ODrive получает ФАПЧ (encoder.cpp): предсказание по скорости, ошибка «отсчёт энкодера − оценка», поправка с pll_kp = 2·bw и pll_ki = 0.25·kp² — два совпадающих полюса, без колебаний. При bandwidth = 1000 рад/с и 8 кГц kp·T = 0,250 (проверка < 1 есть в коде). Зачем: энкодер на 8192 отсчёта на 3 об/с даёт 3,072 отсчёта за такт, и «разность отсчётов за такт» скачет между 2,9297 и 3,9062 об/с; ФАПЧ на тех же отсчётах — 2,9907…3,0060 об/с. Скорость ниже 0,5·T·ki = 62,5 отсчёта/с ODrive обнуляет, чтобы стоящий вал не дрожал. Датчики Холла идут через ту же ФАПЧ со своей моделью (hall_model); без датчика — наблюдатель потока в sensorless_estimator.cpp и ещё одна ФАПЧ с той же формулой.

SimpleFOC делает датчик классом: наследник Sensor реализует одну getSensorAngle() (0…2π), а обороты, полный угол и скорость база считает сама в update()/getVelocity(); скорость — разностью, не чаще раза в min_elapsed_time = 100 мкс, и потом фильтр DEF_VEL_FILTER_Tf = 5 мс. Угол перед _sin приводит _normalizeAngle в [0, 2π): −0,5 → 5,783185, 7,0 → 0,716815.

57д.6 Параметры двигателя: как ODrive меряет R и L

Коэффициенты контура тока нужны из R и L, и ODrive меряет их сам при калибровке, ротор стоит. Законы — в motor.cpp, в модели работают дословно.

Сопротивление. Вдоль оси α подаётся напряжение, которое интегратор подгоняет под ток calibration_current = 10 А: test_voltage_ += (kI * current_meas_period) * (target_current_ - actual_current_), kI = 1 (В/с)/А. Длится 3000 циклов по 1 мс, напряжение не выше resistance_calib_max_voltage = 2 В. Итог — test_voltage_ / target_current_. В модели ток через 50 мс — 6,335 А, через 0,5 с — 9,9996 А; измерено 0,049999 Ом при 0,05 (−0,0012 %). Заодно фильтруется Iβ: больше 0,2 тока — ошибка ERROR_UNBALANCED_PHASES (обрыв или плохой контакт фазы).

Индуктивность. Напряжение ±2 В меняет знак каждый такт; за 1250 мс копится сумма приращений тока, L = |V| / (ΔI/dt). Размах тока в модели — 12,3993 А, измерено 20,1658 мкГн при 20 (+0,829 %). Ошибка — от R, которое метод не учитывает: для прямоугольника с периодом смены T поправка x/th(x) − 1 при x = T·R/(2L) даёт 0,812 %. Допустимый результат — от 2 до 4000 мкГн, иначе ошибка.

57д.7 Грабли

Ток при разгоне меньше заданного, хотя регулятор «с интегралом». ЭДС растёт линейно, ПИ оставляет ошибку наклон/i_gain: 0,417 А при ускорении 1021 рад/с². Включить упреждение ЭДС или поднять полосу.

Подняли полосу тока — двигатель запищал и ушёл в ошибку. Задержка 1,5 такта: при bw = 5000 рад/с перерегулирование 38,4 %, при 10 000 рад/с контур неустойчив. На 8 кГц разумный предел — единицы тысяч рад/с.

Не хватает оборотов на полном газе. Синусный ШИМ даёт 0,5·Vbus амплитуды фазы, SVM — 0,577·Vbus. В SimpleFOC по умолчанию foc_modulation = FOCModulationType::SinePWM — SVM включают сами. ODrive к тому же режет модуляцию до 0,8 круга (0,462·Vbus); при насыщении интеграторы тока затухают множителем 0,99 за такт.

SimpleFOC в режиме voltage сжигает мосфеты. Задание — вольты, ток никто не ограничивает: 0,5 В на 0,05 Ом — почти 10 А на старте. Для низкоомных моторов — foc_current и датчик тока.

_sin получил отрицательный угол. Индекс таблицы считается приведением к unsigned int; на Cortex-M4 это команда VCVT.U32.F32. SimpleFOC сама приводит угол _normalizeAngle в electricalAngle(), свой код должен делать так же.

Скорость с энкодера «пилой». Разность отсчётов за такт на 8 кГц квантуется шагом 0,977 об/с при 8192 отсчётах. Нужна ФАПЧ или фильтр.

Калибровка R не проходит. 10 А при пределе 2 В не набираются, если R > 0,2 Ом, — поднять resistance_calib_max_voltage или снизить ток.

57д.8 Итоги

• FOC: Кларк, Парк, два ПИ по d и q, обратный Парк, модулятор; Кларк SimpleFOC и ODrive записан по-разному, результат совпадает до 7,2·10⁻¹⁶.
• SVM даёт фазе до Vbus/√3 против Vbus/2 у синуса — в 2/√3 = 1,1547 раза больше; настоящая SVM ODrive и сдвиг «по середине» SimpleFOC совпадают; ODrive берёт 0,8 круга, скважность не выше 0,9.
• ODrive v3: ШИМ 24 кГц по центру, регулятор 8 кГц (такт 125 мкс), выход с упреждением 1,5 такта (187,5 мкс).
• Контур тока: kp = L·bw, ki = R·bw; при 1000 рад/с 63,2 % за 1,000 мс; 5000 рад/с — перерегулирование 38,4 %; без упреждения ЭДС ток отстаёт.
• Контур скорости 88 рад/с (в 11,3 раза медленнее тока), положения 20 1/с; ФАПЧ сглаживает скорость до 2,99…3,01 об/с против 2,93…3,91 у разности отсчётов.
• Калибровка ODrive: R — интегратором на 10 А (−0,0012 %), L — знакопеременным 2 В (+0,829 % из-за R).
• На Cortex-M4F (armclang -O2): get_alpha_beta_output — 760 байт, SVM — 468 байт без вызовов, _sin SimpleFOC — 248 байт.

Дальше: раздел 57е «Ток в обмотке шагового двигателя: ШИМ-чоппер и режимы спада» — от бесколлекторного мотора обратно к шаговому: как драйвер держит ток в обмотке, медленный, быстрый и смешанный спад.