+7-960-0655211 (Билайн)
+7-987-4207734 (МТС)

интернет-магазин
доставка по России и СНГ
работаем с 2010 года

Балансирующий робот на Arduino

Балансирующий робот на Arduino

01.jpg

19.jpg

Проект балансирующего робота, который может передвигаться, избегая препятствий. Это робот шириной примерно 10см и высотой 10см, управляемый Arduino Pro Mini с использованием ультразвукового дальномера и модуля MPU6050 (акселерометр и гироскоп).

Список деталей:

1x Arduino Pro Mini
1x Модуль GY-521 с MPU-6050
1x Модуль драйвера мотора DRV8833
2x 5В повышающий преобразователь
1x Ультразвуковой дальномер US-020 (аналог HC-SR05)
1x 18650 li-ion аккумулятор
1x Держатель для аккумулятора 18650
2x Мотор-редуктор
2x Кронштейн для мотор-редуктора
2x Колесо 42x19мм
3x Двусторонняя макетная платы 4см х 6см
8x Нейлоновые стойки 25мм
4x Нейлоновые гайки

Помимо вышеперечисленного, Вам понадобиться немного проводов, pbs разъемы и выключатель.

Репозиторий с исходным кодом: Balancer.

Шаг 1: немного теории

Самобалансирующийся робот похож на перевернутый маятник. В отличие от обычного маятника, который продолжает колебаться, после того как его толкают, этот перевернутый маятник не может оставаться сбалансированным сам по себе. Он просто упадет.
Рассмотрим балансировку карандаша на указательном пальце, который является классическим примером балансировки перевернутого маятника. Мы двигаем пальцем в направлении падения карандаша. Аналогично и с самобалансирующимся роботом, нужно вращать колеса в направлении, в котором он падает что бы удерживать центр тяжести робота над точкой поворота.
Для управления моторами нам понадобится информация о текущем состоянии робота. Нужно знать направление, в котором робот падает, угол наклона и скорость, с которой он падает. Вся эта информация может быть получена с помощью датчика MPU6050. Обработав данные с датчика, на драйверы моторов подаются соответствующие управляющие сигналы что бы поддерживать равновесие.

Шаг 2: приступаем к сборке

02.jpg03.jpg

04.jpg05.jpg

06.jpg07.jpg

08.jpg09.jpg

10.jpg13.jpg

11.jpg12.jpg

14.jpg15.jpg

20.jpg

Робот собирается на трёх макетных платах, расположенных на расстоянии 25мм друг от друга с использованием нейлоновых стоек. На нижнем слое расположено два мотора и модуль двухканального драйвера моторов DRV8833. На среднем уровне размещается Arduino, модуль MPU6050 и два модуля 5В повышающих преобразователя. На верхнем уровне расположен выключатель, аккумулятор и ультразвуковой дальномер (будет установлен позже).

Соберите все детали на макетных платах, примерно, как показано на изображениях выше. После чего соедините платы нейлоновыми стойками.

Возможно, Вы заметили, что использовано два модуля повышающих преобразователей напряжения (один для Arduino, второй подключен к драйверу моторов), хотя для них обоих требуется напряжение 5В. В первой конструкции использовался один 5В повышающий регулятор для питания контроллера и двигателей, но программа периодически зависала. Это было связано с помехами от мотора, действующими на контроллер и MPU6050. Решением проблемы стало разделение питания и добавление конденсатора 10 мкФ.

Шаг 3: измерение угла наклона с использованием акселерометра

16.png

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.

Шаг 4: измерение угла наклона с помощью гироскопа

Гироскоп в MPU6050 измеряет угловую скорость (скорость вращения) вдоль трех осей. Для нашего самобалансирующегося робота угловая скорость вдоль оси x достаточна для измерения скорости падения робота.
Загрузите скетч Sketch2.ino. В нём считываются данные гироскопа по оси x, преобразуются в градусы в секунду, а затем умножаются на время цикла, чтобы получить изменение угла. Мы добавляем этот полученный угол к предыдущему углу, чтобы получить текущий угол.
Положение, в котором при запуске программы находится MPU6050 - это точка нулевого наклона. Угол наклона будет измеряться относительно этой точки.
Удерживайте робота под фиксированным углом и увидите, что значение угла не будет стабильным, он будет постоянно увеличиваться или уменьшаться. Это связано с дрейфом, который присущ гироскопу.
В приведенном выше коде время цикла вычисляется с помощью функции millis(), встроенной в Arduino IDE. Для более точных интервалов между считыванием данных, позже будет использоваться прерывание таймера. Этот интервал времени между считыванием данных также будет использоваться при в ПИД-регуляторе.

Шаг 5: комбинирование и фильтрация данных

17.png

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

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);

В этих строчках замените числа на полученные при калибровке.

Шаг 6: ПИД-регулятор

18.png

Пропорционально-интегрально-дифференцирующий (ПИД) регулятор формирует управляющий сигнал, являющийся суммой трёх слагаемых: пропорционального, интегрального и дифференциального.
Пропорциональная составляющая противодействует отклонению регулируемой величины от заданного значения. Для нашей системы регулируемая величина - это угол наклона робота.
Интегральная составляющая вносит в управляющий сигнал ответ на накопленную ошибку. По сути, это сумма всех ошибок, умноженных на период выборки.
Дифференциальная составляющая пропорциональна темпу изменения отклонения регулируемой величины. Это разница между текущей ошибкой и предыдущей ошибкой, деленная на период выборки. Эта составляющая предназначена для противодействия отклонениям от целевого значения, которое прогнозируется в будущем.
Умножая каждое из этих составляющих на соответствующие им константы (Kp, Ki и Kd) и суммируя результат, мы генерируем выходной сигнал, который затем отправляется в качестве команды для управления двигателем.

Шаг 7: настройка ПИД

  1. Установите Ki и Kd равными нулю и постепенно увеличивайте Kp, чтобы робот начал колебаться около нулевой позиции;

  2. Увеличьте Ki, чтобы робот быстрее реагировал, когда он выходит из равновесия. Ki должен быть достаточно большим, чтобы угол наклона не увеличивался. Робот должен вернуться в нулевое положение, если он наклонен;

  3. Увеличьте Kd, чтобы уменьшить колебания;
  4. Повторяйте описанные выше шаги, выполняя точную настройку каждого параметра для достижения наилучшего результата.

Шаг 8: дальномер

Ультразвуковой дальномер, который я использовал, это US-020. Это аналог HC-SR05. Если у Вас нет US-020, используйте HC-SR05 или другой подобный дальномер.

Дальномер имеет четыре контакта: Vcc, Trig, Echo и Gnd. Напряжение питания 5В. Контакты триггера и эхо подключены к цифровым выводам Arduino 9 и 8 соответственно. Для получения данных о расстоянии используется библиотека NewPing. Расстояние будет считываться один раз каждые 100 миллисекунд. Если значение будет в диапазоне от 0 до 20 см, на моторы поступит команда вращения.

Заключение


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

 

"Arduino Self-Balancing Robot", midhun_s

Перевод и адаптация для RobotoTehnika.ru: Swarg