Хабрахабр

MIRO — открытая платформа indoor-робота. Часть 5 — Программная составляющая: ARDUINO (AVR), лезем «под капот»

image

Эта часть будет интересна всем, кто задавался вопросом о том, как управлять линейной и угловой скоростью робота на ARDUINO, оснащенного двигателями с самыми простыми энкодерами.
Оглавление: Часть 1, Часть 2, Часть 3, Часть 4, Часть 5. В этот раз заглянем чуть глубже в реализацию некоторых ключевых методов библиотеки для ARDUINO (AVR), отвечающих за перемещение робота MIRO.

Первое, что нужно знать про управление движением робота, это простой и очевидный факт, что коллекторные двигатели робота без дополнительной корректировки никогда не вращаются с одинаковой скоростью. Методы, отвечающие за движение с одометрией – это еще та боль с точки зрения объяснения как, что и почему. Разное сцепление, разные выходные характеристики каналов драйвера, немного разные электромоторы и смазка в редукторе.

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

Перечисленные факты определяют реализацию группы методов, связанных с движением шасси, оснащенного датчиками одометрии (в случае 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. Таким образом, на этом этапе мы получаем с какой скоростью и на какой угол нужно вращать каждое колесо шасси робота.

Последний параметр en_break (уже встреченный нами ранее) задает требование жесткого останова колес после совершения поворота, путем подачи на них кратковременного обратного напряжения. Метод wheelRotateAng(float *speed, float *ang, bool en_break) служит для поворота колес робота с угловыми скоростями, задаваемыми массивом speed[] (в м/с), на углы, задаваемые массивом ang[] (в градусах). Для полного удовлетворения, конечно же есть метод wheelRotateAngRad(), аналогичный wheelRotateAng() с той разницей, что в качестве параметров принимает значения углов поворота и угловых скоростей в радианах и радианах в секунду. Это бывает необходимо, чтобы погасить инерцию робота, предотвратив его перемещение дальше необходимого расстояния уже после снятия управляющего напряжения с моторов.

Алгоритм работы метода wheelRotateAng() следующий.

Вначале проверяется соответствие значений из speed[] и ang[] некоторым граничным условиям. 1. Также, углы в ang[] не могут быть меньше минимального фиксируемого угла поворота, определяемого точностью энекодеров. Очевидно, что у шасси есть физические ограничения как на максимальную угловую скорость вращения колес, так и на минимальную (минимальная скорость трогания).

Далее вычисляется направление вращения каждого колеса. 2. Очевидно через знак произведения ang[i] * speed[i];

Вычисляется «дистанция поворота» Dw[i] для каждого колеса — количество отсчетов энкодера, которое необходимо сделать для поворота на заданный ang[i].
Это значение определяется по формуле: 3.

Dw[i] = ang[i] * WHEEL_SEGMENTS / 360,
где WHEEL_SEGMENTS – количество сегментов колеса энкодера (полный оборот).

Регистрируется значение напряжения на драйвере двигателей. 4.

Про напряжение на двигателях

В роботе MIRO драйвер подключается напрямую к цепи питания аккумулятора. * Для управления вращением двигателей применяется ШИМ, поэтому для того, чтобы знать напряжение, подаваемое на каждый двигатель, необходимо знать напряжение питания драйвера двигателей. Опорное напряжение АЦП: 5В. Функция float getVoltage(); возвращает напряжение с делителя напряжения с коэффициентом VOLTAGE_DIVIDER. Это не совсем корректно по причине того, что банки аккумулятора могут по-разному разряжаться и терять баланс, но, как показала практика, при постоянном заряде аккумулятора с балансировкой – решение вполне рабочее. В данный момент в роботе значение VOLTAGE_DIVIDER равно 2, а на вход АЦП (PIN_VBAT) подается напряжение с одной банки (1S) аккумулятора. В будущем планируем сделать нормальный делитель с двух банок аккумулятора.

5. По калибровочной таблице для каждого колеса определяется начальное значение ШИМ сигнала, обеспечивающего вращение колеса с требуемой скоростью speed[i]. Что за калибровочная таблица и откуда она взялась – разберем далее.

Производится запуск вращения двигателей согласно вычисленным значениям скорости и направлению вращения. 6. В тексте реализации класса за это отвечает private-метод _wheel_rotate_sync().

Метода _wheel_rotate_sync() работает по следующему алгоритму: Идем еще глубже.

В бесконечном цикле происходит проверка достижения счетчика срабатываний энкодера дистанции поворота Dw[i] для каждого колеса. 1. Сделано это из следующих соображений. В случае достижения ЛЮБОГО из счетчиков Dw[i], происходит остановка всех колес и выход из цикла с последующим выходом из функции (шаг 5). Это приводит к тому, что после остановки одного из колес, второе колесо продолжает совершать поворот. В силу дискретности измерения угла поворота, весьма частая ситуация, когда вычисленная дистанция Dw[i] одного колеса получена путем округления не целочисленного значения в меньшую сторону, а Dw[j] второго колеса – в большую. Поэтому, в случае организации пространственного перемещения всего шасси, останавливать надо все двигатели разом. Для шасси с дифференциальным приводом (да и для многих других) это приводит к незапланированному «довороту» робота в конце задания.

Если Dw[i] не достигнут, то в цикле проверяется факт очередного срабатывания энкодера (переменная _syncloop[w], обновляемая из прерывания энкодера и сбрасываемая в этом бесконечном цикле). 2. При наступлении очередного пересечения, программа вычисляет модуль текущей угловую скорость каждого колеса (град/сек), по очевидной формуле:

«Глубина» фильтра усреднения определяется MEAN_DEPTH и по-умолчанию равна 8. W[i] = (360 * tau[i]) / WHEEL_SEGMENTS,
где:
tau[i] – усредненное значение времени между двумя последними срабатыванием энкодеров.

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

На основе вычисленных ошибок производится коррекция управляющего воздействия (значения ШИМ сигнала) на каждый двигатель. 4.

После достижения Dw[i], в случае активного en_break, на двигатели подается обратное кратковременное напряжение. 5. далее) и как правило составляет от 15 до 40 мс. Длительность этого воздействия определяется из калибровочной таблицы (см.

Происходит полное снятие напряжений с двигателей и выход из _wheel_rotate_sync(). 6.

Итак, в библиотеке существует специальная таблица значений, хранящаяся в EEPROM памяти робота и содержащая записи из трех связанных значений: Уже дважды упомянул некую калибровочную таблицу.

Напряжение на клеммах двигателя. 1. Именно для этого, на шаге 4 метода wheelRotateAng() регистрируется фактическое напряжение на драйвере двигателей. Вычисляется путем перевода значения ШИМ сигнала в фактическое напряжение.

Угловую скорость вращения колеса (без нагрузки), соответствующую данному напряжению. 2.

Длительность подачи сигнала жесткого останова, соответствующего этой угловой скорости.
По-умолчанию, размер калибровочной таблицы составляет 10 записей (определяется константой WHEEL_TABLE_SIZE в файле config.h) — 10 троек значений «напряжение — угловая скорость — длительность сигнала останова». 3.

Для определения значений из 2 и 3 записей в этой таблице служит специальный метод — wheelCalibrate(byte wheel).

Этот метод реализует последовательность действий, позволяющих определить недостающие значения в таблице калибровки двигателя/колеса, а также узнать минимальную угловую скорость трогания и максимальную угловую скорость колеса. Заглянем немного в него.

Для выполнения калибровки, робот устанавливается на подставку, все вращения колес во время калибровки осуществляются без нагрузки.

Вначале необходимо определить минимальную скорость трогания. 1. В цикле на двигатель подается управляющее ШИМ, начиная с 0, с инкрементом 1. Делается это очень просто. По прошествии времени ожидания, проверяется не совершено ли трогание (по изменению значения счетчика энкодера). На каждом шаге программа ожидает в течение некоторого времени, определяемого константой WHEEL_TIME_MAX (обычный delay()). Для большей уверенности, к значению ШИМ, соответствующего этой скорости трогания, прибавляется значение 10. Если трогание совершено, то вычисляется угловая скорость вращения колеса. Так получается первая пара значений «напряжение на двигателе» — «угловая скорость».

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

В цикле для каждого нового значения ШИМ производится вращение колеса на 2 полных оборота и измеряется угловая скорость по алгоритму, аналогичному в методе _wheel_rotate_sync(). 3. Изначально берется некоторое заведомо большое значение. В этом же цикле, также путем последовательного приближения, измеряется оптимальное значение длительности сигнала жесткого останова. В качестве оптимального выбирается максимальное значение длительности сигнала останова, при котором не происходит превышение установленной «дистанции поворота». И затем тестируется в режиме «поворот-останов». Иными словами, такое значение длительности сигнала, при подаче которого на двигатель с одной стороны, происходит гашение инерции, а с другой – не происходит кратковременного обратного движения (что фиксируется все тем же энкодером).

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

Можно обратить внимание, что методы wheelRotateAng() и wheelRotateAngRad() – блокирующие функции. Я опустил всякие мелочи реализации и попытался изложить самую суть. Можно было бы сделать небольшой диспетчер задач, с фиксированным таймингом, но это бы потребовало от пользователя встраивать свой функционал строго в отведенную квоту времени. Это цена за точность перемещения и достаточно простую интеграцию в скетчи пользователей.

Она, как видно из списка параметров, просто выполняет вращение колес с установленными скоростями. А для неблокирующего применения в API есть функция wheelRotate(float *speed). И по требованиям к структуре скетча пользователя, этот метод должен вызываться каждую итерацию главного цикла loop() скетча ARDUINO. А корректировка скорости вращения происходит в методе Sync() шасси робота, который вызывается в одноименном методе Sync() объекта класса Miro.

Как вы догадались)? На шаге 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] = ;
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, просто не оказалось на руках во время теста) должны вести себя несколько лучше.

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

Теги
Показать больше

Похожие статьи

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *

Кнопка «Наверх»
Закрыть