В этот раз заглянем чуть глубже в реализацию некоторых ключевых методов библиотеки для ARDUINO (AVR), отвечающих за перемещение робота MIRO. Эта часть будет интересна всем, кто задавался вопросом о том, как управлять линейной и угловой скоростью робота на ARDUINO, оснащенного двигателями с самыми простыми энкодерами.
Оглавление: Часть 1, Часть 2, Часть 3, Часть 4, Часть 5.
Методы, отвечающие за движение с одометрией – это еще та боль с точки зрения объяснения как, что и почему. Первое, что нужно знать про управление движением робота, это простой и очевидный факт, что коллекторные двигатели робота без дополнительной корректировки никогда не вращаются с одинаковой скоростью. Разное сцепление, разные выходные характеристики каналов драйвера, немного разные электромоторы и смазка в редукторе.
Второй факт, который следует понимать и знать – это наличие инерции в двигателе даже с достаточно большим передаточным числом. Т.е. при снятии напряжения с клемм мотора, колесо, даже не нагруженное, совершает движение еще на несколько градусов. Величина этого дополнительного вращения зависит от нагружающего усилия на колесо, от скорости вращения перед снятием напряжения и от все тех же незримых факторов вроде типа и количества смазки в редукторе.
Перечисленные факты определяют реализацию группы методов, связанных с движением шасси, оснащенного датчиками одометрии (в случае MIRO – цифровыми энкодерами каждого колеса).
Как мы выяснили в четвертой части, в программной модели существует класс Chassis, в котором реализовано управление вращением отдельных двигателей шасси. Хочу подчеркнуть — не управление движением шасси, тележки, а именно управление двигателями тележки. Управление же непосредственно тележкой реализуется в классах Robot и Miro.
Начнем «сверху». Ниже приведен метод класса Miro, реализующий движение робота на определенное расстояние (dist, метров) с заданной линейной (lin_speed, м/с) и угловой (ang_speed, град/с) скоростями. На параметр en_break пока не обращаем внимание.
int Miro::moveDist(float lin_speed, float ang_speed, float dist, bool en_break)
{
float _wheelSetAngSpeed[WHEEL_COUNT];
_wheelSetAngSpeed[LEFT] = MIRO_PI2ANG * (lin_speed - (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS;
_wheelSetAngSpeed[RIGHT] = MIRO_PI2ANG * (lin_speed + (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS;
float _wheelSetAng[WHEEL_COUNT];
_wheelSetAng[RIGHT] = _wheelSetAngSpeed[RIGHT] * dist / lin_speed;
_wheelSetAng[LEFT] = _wheelSetAngSpeed[LEFT] * dist / lin_speed;
return this->chassis.wheelRotateAng(_wheelSetAngSpeed, _wheelSetAng, en_break);
}
В этом методе вначале рассчитываются ТРЕБУЕМЫЕ угловые скорости для левого и правого двигателей. По достаточно очевидным формулам, вывести которые не проблема. Нужно только иметь ввиду, что линейная скорость в методе задается в метрах в секунду, а угловая — в градусах в секунду (не в радианах). Поэтому, мы заранее рассчитываем константу MIRO_PI2ANG = 57.29 = 180/pi. ROBOT_DIAMETER — расстояние между левым и правым колесом робота (в метрах), WHEEL_RADIUS — радиус колеса (тоже в метрах). Всякие числовые константы для подобных случаев содержаться в файле defs.h, а настраиваемые параметры робота и шасси — в файле config.h.
После чего рассчитывается угол, на который необходимо повернуть каждое колесо, чтобы робот проехал расстояние dist (тоже в метрах).
Таким образом, на этом этапе мы получаем с какой скоростью и на какой угол нужно вращать каждое колесо шасси робота. И далее происходит вызов метода wheelRotateAng() объекта chassis.
Метод wheelRotateAng(float *speed, float *ang, bool en_break) служит для поворота колес робота с угловыми скоростями, задаваемыми массивом speed[] (в м/с), на углы, задаваемые массивом ang[] (в градусах). Последний параметр en_break (уже встреченный нами ранее) задает требование жесткого останова колес после совершения поворота, путем подачи на них кратковременного обратного напряжения. Это бывает необходимо, чтобы погасить инерцию робота, предотвратив его перемещение дальше необходимого расстояния уже после снятия управляющего напряжения с моторов. Для полного удовлетворения, конечно же есть метод wheelRotateAngRad(), аналогичный wheelRotateAng() с той разницей, что в качестве параметров принимает значения углов поворота и угловых скоростей в радианах и радианах в секунду.
Алгоритм работы метода wheelRotateAng() следующий.
1. Вначале проверяется соответствие значений из speed[] и ang[] некоторым граничным условиям. Очевидно, что у шасси есть физические ограничения как на максимальную угловую скорость вращения колес, так и на минимальную (минимальная скорость трогания). Также, углы в ang[] не могут быть меньше минимального фиксируемого угла поворота, определяемого точностью энекодеров.
2. Далее вычисляется направление вращения каждого колеса. Очевидно через знак произведения ang[i] * speed[i];
3. Вычисляется «дистанция поворота» Dw[i] для каждого колеса — количество отсчетов энкодера, которое необходимо сделать для поворота на заданный ang[i].
Это значение определяется по формуле:
Dw[i] = ang[i] * WHEEL_SEGMENTS / 360,
где WHEEL_SEGMENTS – количество сегментов колеса энкодера (полный оборот).
4. Регистрируется значение напряжения на драйвере двигателей.
Про напряжение на двигателях
* Для управления вращением двигателей применяется ШИМ, поэтому для того, чтобы знать напряжение, подаваемое на каждый двигатель, необходимо знать напряжение питания драйвера двигателей. В роботе MIRO драйвер подключается напрямую к цепи питания аккумулятора. Функция float getVoltage(); возвращает напряжение с делителя напряжения с коэффициентом VOLTAGE_DIVIDER. Опорное напряжение АЦП: 5В. В данный момент в роботе значение VOLTAGE_DIVIDER равно 2, а на вход АЦП (PIN_VBAT) подается напряжение с одной банки (1S) аккумулятора. Это не совсем корректно по причине того, что банки аккумулятора могут по-разному разряжаться и терять баланс, но, как показала практика, при постоянном заряде аккумулятора с балансировкой – решение вполне рабочее. В будущем планируем сделать нормальный делитель с двух банок аккумулятора.
5. По калибровочной таблице для каждого колеса определяется начальное значение ШИМ сигнала, обеспечивающего вращение колеса с требуемой скоростью speed[i]. Что за калибровочная таблица и откуда она взялась – разберем далее.
6. Производится запуск вращения двигателей согласно вычисленным значениям скорости и направлению вращения. В тексте реализации класса за это отвечает private-метод _wheel_rotate_sync().
Идем еще глубже. Метода _wheel_rotate_sync() работает по следующему алгоритму:
1. В бесконечном цикле происходит проверка достижения счетчика срабатываний энкодера дистанции поворота Dw[i] для каждого колеса. В случае достижения ЛЮБОГО из счетчиков Dw[i], происходит остановка всех колес и выход из цикла с последующим выходом из функции (шаг 5). Сделано это из следующих соображений. В силу дискретности измерения угла поворота, весьма частая ситуация, когда вычисленная дистанция Dw[i] одного колеса получена путем округления не целочисленного значения в меньшую сторону, а Dw[j] второго колеса – в большую. Это приводит к тому, что после остановки одного из колес, второе колесо продолжает совершать поворот. Для шасси с дифференциальным приводом (да и для многих других) это приводит к незапланированному «довороту» робота в конце задания. Поэтому, в случае организации пространственного перемещения всего шасси, останавливать надо все двигатели разом.
2. Если Dw[i] не достигнут, то в цикле проверяется факт очередного срабатывания энкодера (переменная _syncloop[w], обновляемая из прерывания энкодера и сбрасываемая в этом бесконечном цикле). При наступлении очередного пересечения, программа вычисляет модуль текущей угловую скорость каждого колеса (град/сек), по очевидной формуле:
W[i] = (360 * tau[i]) / WHEEL_SEGMENTS,
где:
tau[i] – усредненное значение времени между двумя последними срабатыванием энкодеров. «Глубина» фильтра усреднения определяется MEAN_DEPTH и по-умолчанию равна 8.
3. На основе вычисленных скоростей вращения колес, рассчитываются абсолютные ошибки как разницы между заданными и фактическими угловыми скоростями.
4. На основе вычисленных ошибок производится коррекция управляющего воздействия (значения ШИМ сигнала) на каждый двигатель.
5. После достижения Dw[i], в случае активного en_break, на двигатели подается обратное кратковременное напряжение. Длительность этого воздействия определяется из калибровочной таблицы (см. далее) и как правило составляет от 15 до 40 мс.
6. Происходит полное снятие напряжений с двигателей и выход из _wheel_rotate_sync().
Уже дважды упомянул некую калибровочную таблицу. Итак, в библиотеке существует специальная таблица значений, хранящаяся в EEPROM памяти робота и содержащая записи из трех связанных значений:
1. Напряжение на клеммах двигателя. Вычисляется путем перевода значения ШИМ сигнала в фактическое напряжение. Именно для этого, на шаге 4 метода wheelRotateAng() регистрируется фактическое напряжение на драйвере двигателей.
2. Угловую скорость вращения колеса (без нагрузки), соответствующую данному напряжению.
3. Длительность подачи сигнала жесткого останова, соответствующего этой угловой скорости.
По-умолчанию, размер калибровочной таблицы составляет 10 записей (определяется константой WHEEL_TABLE_SIZE в файле config.h) — 10 троек значений «напряжение — угловая скорость — длительность сигнала останова».
Для определения значений из 2 и 3 записей в этой таблице служит специальный метод — wheelCalibrate(byte wheel).
Заглянем немного в него. Этот метод реализует последовательность действий, позволяющих определить недостающие значения в таблице калибровки двигателя/колеса, а также узнать минимальную угловую скорость трогания и максимальную угловую скорость колеса.
Для выполнения калибровки, робот устанавливается на подставку, все вращения колес во время калибровки осуществляются без нагрузки.
1. Вначале необходимо определить минимальную скорость трогания. Делается это очень просто. В цикле на двигатель подается управляющее ШИМ, начиная с 0, с инкрементом 1. На каждом шаге программа ожидает в течение некоторого времени, определяемого константой WHEEL_TIME_MAX (обычный delay()). По прошествии времени ожидания, проверяется не совершено ли трогание (по изменению значения счетчика энкодера). Если трогание совершено, то вычисляется угловая скорость вращения колеса. Для большей уверенности, к значению ШИМ, соответствующего этой скорости трогания, прибавляется значение 10. Так получается первая пара значений «напряжение на двигателе» — «угловая скорость».
2. После того, как найдена скорость трогания, вычисляется шаг ШИМ для равномерного заполнения таблицы калибровки.
3. В цикле для каждого нового значения ШИМ производится вращение колеса на 2 полных оборота и измеряется угловая скорость по алгоритму, аналогичному в методе _wheel_rotate_sync(). В этом же цикле, также путем последовательного приближения, измеряется оптимальное значение длительности сигнала жесткого останова. Изначально берется некоторое заведомо большое значение. И затем тестируется в режиме «поворот-останов». В качестве оптимального выбирается максимальное значение длительности сигнала останова, при котором не происходит превышение установленной «дистанции поворота». Иными словами, такое значение длительности сигнала, при подаче которого на двигатель с одной стороны, происходит гашение инерции, а с другой – не происходит кратковременного обратного движения (что фиксируется все тем же энкодером).
4. После окончания калибровки, управляющее напряжение на калибруемый двигатель прекращает подаваться и производится запись калибровочной таблицы этого колеса в EEPROM.
Я опустил всякие мелочи реализации и попытался изложить самую суть. Можно обратить внимание, что методы wheelRotateAng() и wheelRotateAngRad() – блокирующие функции. Это цена за точность перемещения и достаточно простую интеграцию в скетчи пользователей. Можно было бы сделать небольшой диспетчер задач, с фиксированным таймингом, но это бы потребовало от пользователя встраивать свой функционал строго в отведенную квоту времени.
А для неблокирующего применения в API есть функция wheelRotate(float *speed). Она, как видно из списка параметров, просто выполняет вращение колес с установленными скоростями. А корректировка скорости вращения происходит в методе Sync() шасси робота, который вызывается в одноименном методе Sync() объекта класса Miro. И по требованиям к структуре скетча пользователя, этот метод должен вызываться каждую итерацию главного цикла loop() скетча ARDUINO.
На шаге 4 в описании метода _wheel_rotate_sync() я упомянул про «коррекцию управляющего воздействий» двигателя. Как вы догадались)? Это ПИД-регулятор). Ну точнее ПД-регулятор. Как известно (на самом деле – не всегда), лучший способ определения коэффициентов регулятора – подбор). В конфигурационном файле config.h есть одно определение:
#define DEBUG_WHEEL_PID
Если его его раскомментировать, то при вызове метода moveDist() класса Miro, в консоль робота будет выводится вот такой перевернутый график относительной ошибки управления угловой скоростью одного из колес робота (левого).
Ничего не напоминает)? Вниз — это время (каждая полоска — шаг цикла управления), а вправо отложена величина ошибки (с сохранением знака). Вот две пары графиков в одном масштабе с разными коэффициентами ПД-регулятора. «Горбы» — это как раз «волны» перерегулирования. Цифры на горизонтальных столбиках — относительная ошибка (с сохранением знака). Простая визуализация работы регулятора, помогающая вручную настроить коэффициенты. Со временем, я надеюсь, сделаем автоматическую настройку, но пока так.
Вот такой адок :-)
Ну и на последок давайте рассмотрим пример. Прям из библиотеки API_Miro_moveDist:
#include <Miro.h>
using namespace miro;
byte PWM_pins[2] = { 5, 6 };
byte DIR_pins[2] = { 4, 7 };
byte ENCODER_pins[2] = { 2, 3 };
Miro robot(PWM_pins, DIR_pins, ENCODER_pins);
int laps = 0;
void setup() {
Serial.begin(115200);
}
void loop() {
for (unsigned char i = 0; i < 4; i++)
{
robot.moveDist(robot.getOptLinSpeed(), 0, 1, true);
delay(500);
robot.rotateAng(0.5*robot.getOptAngSpeed(), -90, true);
delay(500);
}
Serial.print("Laps: ");
Serial.println(laps);
laps++;
}
Из текста программы все должно быть понятно. Как это работает — на видео.
Кафельная плитка размером 600 на 600 мм и зазоры между плитками по 5 мм. По идее, робот должен объезжать квадрат со стороной 1 метр. Конечно, траектория «уплывает». Но справедливости ради стоит сказать, что в оставшейся у меня для тестов версии робота стоят достаточно оборотистые двигатели, которые сложно заставить ехать медленно. А на большой скорости и пробуксовки имеют место быть, и с инерцией справиться не просто. Двигатели с бОльшим передаточным числом (такие есть даже в наших роботах MIRO, просто не оказалось на руках во время теста) должны вести себя несколько лучше.
Если есть непонятные моменты — с радостью готов уточнять, обсуждать и улучшать. Вообще интересна обратная связь.
