Синхронизация моторов омни-платформы

Методист по олимпиадной робототехнике Университета Иннополис Алексей Овсянников рассказывает, как управлять робототехнической платформой на омни-колесах.

Продолжаю публиковать материалы для слушателей курсов повышения квалификации по робототехнике. Сегодня начинаем знакомство с нашим новым курсом, посвященным мобильным платформам всенаправленного движения. Это платформы, использующие омни-колеса и колеса Илона (mecanum-wheel).

Платформы могут быть с разной компоновкой (треугольные трехколесные, квадратные и прямоугольные четырехколесные с теми или иными типами колес), сейчас не будем на этом акцентироваться. Во всех этих вариантах может возникнуть неприятная ситуация: контроллер рассчитал скорость каждого колеса, подал управляющий сигнал на мотор, а он его отрабатывает некачественно, слишком быстро или слишком медленно.  Например, при вычислении скорость одного мотора могла оказаться близкой к нулю; при управлении по напряжению и без обратной связи мотор может просто не стартовать, его напряжение будет слишком мало для вращения или мотор будет работать в режиме прерывистых токов (вращаться рывками). Нужна обратная связь, которая нам покажет текущую скорость мотора или количество выполненных оборотов. Можно еще регулировать и управлять мотором по току, но это отдельная тема, в образовательной робототехнике так мало кто делает; вместо этого используются широко распространенные драйверы, управляющие напряжением. Они доступны, просты в настройке и управлении, дешевы. В прошлых статьях я рассказал, как использовать энкодер мотора. Так что постараемся применить эти знания в текущей теме.

Итак, контроллер рассчитал скорости, они были поданы на моторы, но в одном из них есть неучтенное трение, в другом напряжение слишком низкое для запуска. В результате робот будет двигаться неровно, по дуге или диагонали, вместо прямых проездов. Точно так же ведет себя робот на двух колесах (дифференциальная тележка), если их не синхронизировать. Вот только про то, как это сделать для двух моторов знают многие, это описано, например, в пособии «Управление моторами тележки с контроллером Трик на JavaScript«. Автор пособия Олег Киселев рассматривает несколько алгоритмов. Я адаптирую алгоритм регулировки по ведущему колесу для многоколесной платформы.

За ведущее колесо я буду выбирать колесо (или мотор, что в данном случае одно и то же) с самой большой скоростью. Логика алгоритма следующая:

1. Вычислить скорости всех моторов (Va, Vb, Vc…)
2. Найти мотор с максимальной скоростью (Vmax)
3. Вычислить масштабирующие коэффициенты, показывающие, во сколько раз каждое колесо медленнее самого быстрого (Ma=Va/Vmax, Mb=Vb/Vmax, Mc=Vc/Vmax…)
4. В цикле следить за разницей в показания энкодеров: любое колесо должно наматывать меньше оборотов, чем самое быстрое, причем ровно в Ma, Mb, Mc… раз. Вот эта разница будет ошибкой регулятора.
5. Настроить регулятор на каждый мотор. Если какой-то мотор начинает отставать или обгонять, то регулятор его ускоряет или замедляет.

Рассмотрим некоторые этапы подробнее. «Некоторые», потому что вычисление скоростей моторов зависит от конкретной механики робота. Например, для треугольной платформы на трех омни-колесах формулы могут быть следующие:

Va=velocity * cos(150-Alpha)

Vb=velocity * cos(30-Alpha)

Vc=velocity * cos(270-Alpha)

где

Velocity — скорость движения робота

Alpha — угол, определяющий направление движения робота.

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

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

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

Переходим к вычислению масштабирующих коэффициентов. Они показывают, во сколько раз каждое колесо медленнее самого быстрого. То есть, надо максимальную скорость разделить на скорость конкретного колеса. Или, наоборот, чтобы получить сразу нормированный коэффициент (его величина будет от 0 до 1, причем 0 — если текущий мотор стоит и 1 у самого быстрого мотора). Кроме того, надо проверять, не получилась ли максимальная или текущая скорость нулевой? Если проверку не делать, то произойдет деление на 0, что ни к чему хорошему не приведет (а к чему именно приведет — зависит от используемого контроллера и того, как он умеет обрабатывать ошибки). В псевдокоде этот этап может выглядеть так:

Коэффициенты получатся с разными знаками. Знак показывает, совпадает ли направление вращения у самого быстрого и текущего колеса / мотора. Теперь надо сравнить показания энкодеров моторов. Логика здесь следующая — если какой-то мотор крутится медленнее самого быстрого в M раз, то и его энкодер намотает в М раз меньше оборотов. Если проверки и сравнения делать регулярно, по 20-100 раз в секунду, то регулирование можно считать «в реальном времени» и возникающие на каждом шаге ошибки не успеют накопиться и увести систему в сторону. Робот будет двигаться с достаточной точностью (отклонение на несколько миллиметров за проезд в несколько метров).

Ошибка для каждого мотора вычисляется следующим образом (для примера используется мотор А):

Коэффициент Ma показывает, во сколько раз мотор А медленнее самого быстрого и во сколько раз меньше он намотал энкодером. Если мотор А крутился с правильной (рассчитанной ранее) скоростью, то он намотал maxEnc * Ma «тиков энкодера». Реально же он намотал encA «тиков энкодера». Если эти два числа совпадают, то ошибка errA будет нулевой.

Обратите внимание, что показания maxEnc могут быть отрицательными (если самый быстрый мотор крутится назад), а encA положительными (если мотор А крутится вперед). Если коэффициент Ma был посчитан верно, то он будет иметь отрицательный знак и «уравновесит» эти показания.

Такое вычисление ошибок надо сделать для каждого мотора. У самого быстрого ошибка всегда будет нулевой. Далее по этим ошибкам настраиваются ПИД-регуляторы на каждый мотор. Например, для того же мотора А вычисляется управляющее воздействие регулятора Ua и итоговая скорость получается их суммированием:

И так для каждого мотора.

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

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

Я тестировал этот алгоритм на роботе, собранном из Lego Mindstorms EV3. Программу писал на EV3Basic в clev3r IDE (возможно, я использовал несколько характерных именно для нее команд, поэтому перенос кода в обычный SmallBasic c надстройкой EV3 может не сработать). Для собственного удобства я собрал пару процедур синхронного движения на заданное время и количество «тиков энкодера».

Полный листинг программы:

Не пугайтесь строк 35-40, в них я перевожу привычные для понимания градусы в радианы (так как функция вычисляет косинус именно в них) и округляю до 4 знаков после запятой.

Для каждого мотора я описал только П-регуляторы, но вы можете развить до ПИД-регуляторов.

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

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

Теги: headline, Алексей Овсянников, Манипуляционные ИРС, робототехника, Университет Иннополис