Проект балансирующего робота, который может передвигаться, избегая препятствий. Это робот шириной примерно 10см и высотой 10см, управляемый Arduino Pro Mini с использованием ультразвукового дальномера и модуля MPU6050 (акселерометр и гироскоп).
Список деталей:
Файлы проекта можно скачать по ссылке.
Самобалансирующийся робот похож на перевернутый маятник. В отличие от обычного маятника, который продолжает колебаться, после того как его толкают, этот перевернутый маятник не может оставаться сбалансированным сам по себе. Он просто упадет.
Рассмотрим балансировку карандаша на указательном пальце, который является классическим примером балансировки перевернутого маятника. Мы двигаем пальцем в направлении падения карандаша. Аналогично и с самобалансирующимся роботом, нужно вращать колеса в направлении, в котором он падает что бы удерживать центр тяжести робота над точкой поворота.
Для управления моторами нам понадобится информация о текущем состоянии робота. Нужно знать направление, в котором робот падает, угол наклона и скорость, с которой он падает. Вся эта информация может быть получена с помощью датчика MPU6050. Обработав данные с датчика, на драйверы моторов подаются соответствующие управляющие сигналы что бы поддерживать равновесие.
Робот собирается на трёх макетных платах, расположенных на расстоянии 25мм друг от друга с использованием нейлоновых стоек. На нижнем слое расположено два мотора и модуль двухканального драйвера моторов DRV8833. На среднем уровне размещается Arduino, модуль MPU6050 и два модуля 5В повышающих преобразователя. На верхнем уровне расположен выключатель, аккумулятор и ультразвуковой дальномер (будет установлен позже).
Соберите все детали на макетных платах, примерно, как показано на изображениях выше. После чего соедините платы нейлоновыми стойками.
Возможно, Вы заметили, что использовано два модуля повышающих преобразователей напряжения (один для Arduino, второй подключен к драйверу моторов), хотя для них обоих требуется напряжение 5В. В первой конструкции использовался один 5В повышающий регулятор для питания контроллера и двигателей, но программа периодически зависала. Это было связано с помехами от мотора, действующими на контроллер и MPU6050. Решением проблемы стало разделение питания и добавление конденсатора 10 мкФ.
MPU6050 содержит трёх-осевой акселерометр и трёх-осевой гироскоп. Акселерометр измеряет ускорение по трем осям, а гироскоп измеряет угловую скорость по трем осям. Чтобы измерить угол наклона робота, нам нужны значения ускорения по осям y и z. Функция atan2(y, z) возвращает угол в радианах между положительной осью z плоскости и точкой, заданной координатами (z, y) на этой плоскости. Функция возвращает положительное число, когда y> 0 и отрицательное, когда y <0. Для чтения данных из MPU6050 используется библиотека https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050. Загрузите скетч Sketch1.ino и посмотрите, как меняется угол наклона.
Попробуйте перемещать робота вперед, назад и наклоняя его под разными углами. Вы заметите, как выводимый в мониторе порта угол, внезапно меняется. Это связано с тем, что горизонтальная составляющая ускорения влияет на значения ускорения по осям y и z.
Гироскоп в MPU6050 измеряет угловую скорость (скорость вращения) вдоль трех осей. Для нашего самобалансирующегося робота угловая скорость вдоль оси x достаточна для измерения скорости падения робота.
Загрузите скетч Sketch2.ino. В нём считываются данные гироскопа по оси x, преобразуются в градусы в секунду, а затем умножаются на время цикла, чтобы получить изменение угла. Мы добавляем этот полученный угол к предыдущему углу, чтобы получить текущий угол.
Положение, в котором при запуске программы находится MPU6050 - это точка нулевого наклона. Угол наклона будет измеряться относительно этой точки.
Удерживайте робота под фиксированным углом и увидите, что значение угла не будет стабильным, он будет постоянно увеличиваться или уменьшаться. Это связано с дрейфом, который присущ гироскопу.
В приведенном выше коде время цикла вычисляется с помощью функции millis(), встроенной в Arduino IDE. Для более точных интервалов между считыванием данных, позже будет использоваться прерывание таймера. Этот интервал времени между считыванием данных также будет использоваться при в ПИД-регуляторе.
Один из наиболее популярных методов совмещения данных с акселерометра и гироскопа – это использование комплементарного фильтра. Данные с гироскопа и акселерометра содержат шумы, у гироскопа ещё есть так называемый дрейф нуля. Комплементарный фильтр компенсирует дрейф нуля гироскопа за счёт использования данных с акселерометра и является фильтром высоких частот для гироскопа и фильтром низких частот для акселерометра.
currentAngle = 0.9934 * (previousAngle + gyroAngle) + 0.0066 * (accAngle)
0.9934 и 0.0066 являются коэффициентами фильтра для интервала времени 0.75с. Фильтр нижних частот пропускает через него любой сигнал, длительность которого превышает эту длительность, а фильтр верхних частот пропускает любой сигнал, длина которого меньше этой длительности. Отклик фильтра можно настроить, выбрав правильную постоянную времени. Понижение постоянной времени позволит увеличить горизонтальное ускорение.
Устранение ошибок смещения акселерометра и гироскопа
Загрузите в Arduino скетч MPU6050_calibration.ino для калибровки смещений MPU6050. В скетче FullCode.ino в функции setup() есть следующие строчки:
mpu.setYAccelOffset(1593);
mpu.setZAccelOffset(963);
mpu.setXGyroOffset(40);
В этих строчках замените числа на полученные при калибровке.
Пропорционально-интегрально-дифференцирующий (ПИД) регулятор формирует управляющий сигнал, являющийся суммой трёх слагаемых: пропорционального, интегрального и дифференциального.
Пропорциональная составляющая противодействует отклонению регулируемой величины от заданного значения. Для нашей системы регулируемая величина - это угол наклона робота.
Интегральная составляющая вносит в управляющий сигнал ответ на накопленную ошибку. По сути, это сумма всех ошибок, умноженных на период выборки.
Дифференциальная составляющая пропорциональна темпу изменения отклонения регулируемой величины. Это разница между текущей ошибкой и предыдущей ошибкой, деленная на период выборки. Эта составляющая предназначена для противодействия отклонениям от целевого значения, которое прогнозируется в будущем.
Умножая каждое из этих составляющих на соответствующие им константы (Kp, Ki и Kd) и суммируя результат, мы генерируем выходной сигнал, который затем отправляется в качестве команды для управления двигателем.
Установите Ki и Kd равными нулю и постепенно увеличивайте Kp, чтобы робот начал колебаться около нулевой позиции;
Увеличьте Ki, чтобы робот быстрее реагировал, когда он выходит из равновесия. Ki должен быть достаточно большим, чтобы угол наклона не увеличивался. Робот должен вернуться в нулевое положение, если он наклонен;
Ультразвуковой дальномер, который я использовал, это US-020. Это аналог HC-SR05. Если у Вас нет US-020, используйте HC-SR05 или другой подобный дальномер.
Дальномер имеет четыре контакта: Vcc, Trig, Echo и Gnd. Напряжение питания 5В. Контакты триггера и эхо подключены к цифровым выводам Arduino 9 и 8 соответственно. Для получения данных о расстоянии используется библиотека NewPing. Расстояние будет считываться один раз каждые 100 миллисекунд. Если значение будет в диапазоне от 0 до 20 см, на моторы поступит команда вращения.
Потратив немного больше времени на настройку ПИД, можно получить результат ещё лучше. Небольшой размер робота также ограничивает уровень стабильности. Тем не менее, я полагаю, робот неплохо справляется с задачей балансировки на различных поверхностях.
