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

Дело было вечером. Делать было нечего. Как ведет себя в такой ситуации среднестатистический добропорядочный гражданин? Или недобропорядочный? Или даже программист? Серфит интернет, смотрит телевизор, пьет пиво или что покрепче, играет с детьми… В конце концов — спит. Но не таков сотрудник компании JetRuby.

В один прекрасный момент нам подумалось: а почему бы не развлечься и не сделать что-нибудь действительно интересное? Так родилась идея стабилизации обратного маятника. Или по-простому: разработка балансирующего робота.

Немного математики

Изначально мы пытались построить робота под управлением PID регулятора без всякой математики. Но, увы, результата это не принесло. В итоге возникла такая модель:

image00

Где mp —  масса маятника, прикрепленного к концу стержня длины l. К противоположному концу стержня крепится двигатель, способный развить максимальный момент Mk и передать его колесу с радиусом r и массой mw.

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

Уравнения движения с описанием обратного маятника выглядят следующим образом:

image01

На первый взгляд они могут показаться достаточно сложными, однако сам робот не имеет о них ни малейшего представления. Что же касается управления, в нем используется линеаризованная модель. Она перед вами:

image03

Синтезирование управления

Для управления маятником был выбран линейно-квадратичный регулятор. Он отличается от PID-регулятора, потому что представляет собой произведение собственных коэффициентов на ошибки по координатам. Никаких интегралов или дискретных аналогов производной. Однако для вычисления линейно-квадратичного регулятора необходимы модель системы и Matlab.

Итак, получаем коэффициенты. Для этого необходимо выполнить в Matlab следующие команды:

Где А и В — соответствующе матрицы из линеаризированной модели.

Матрица Q определяет цену отклонения системы от начала координат.

Матрица R определяет цену расхода энергии на управление системой.

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

image02

Какой же робот без железа?

Каркас робота состоит из стеклотекстолита и металлических шпилек м5. Все это добро соединено обыкновенными гайками. Конструкцию робота можно назвать трехэтажной. Внизу располагается модуль управления двигателями и сами двигатели. На втором этаже — “мозг”, питание и BLE-модуль. На третьем — аккумулятор и IMU.

IMU — гироскоп и акселерометр — используется для определения угла и угловой скорости. Мы выбрали простой и популярный модуль на базе mpu6050. В чем особенность этого датчика? В том, что он поддерживает DMP. И мы этим воспользовались с превеликим удовольствием (отдельное и большое спасибо создателям библиотеки i2cdevlib).

aaa

Переходим к следующему сенсору. Речь идет о квадратурном энкодере на моторе. Он генерирует прямоугольные импульсы на каждом из двух выводов:

bbbbb

Их считают или прерываниями, или считыванием значений в цикле. На arduino playground размещена интересная статья с примерами кода. Теперь нам остается узнать угловую скорость колеса. Здесь на помощь спешит старая-добрая формула, хорошо известная каждому еще со школьных времен: пройденное расстояние/затраченное время.

EncPosRad переводит численность тиков энкодера в угол колеса. Наш энкодер выдает около 4800 тиков на оборот. Чтобы получить угол необходимо умножить тики на 2 Пи и разделить на их общее количество при полном обороте колеса.

Далее полученные нами показания сенсоров отправляются в LQR регулятор:

Коэффиценты для регулятора мы берем из проведенных расчетов. Все просто:)

Как управлять балансирующим роботом?

Наигравшись вдоволь, мы поняли, что стоять на месте неинтересно и бесперспективно. В результате было принято принципиальное решение запилить управление роботом с мобильного устройства.

Для этого прекрасно подошел Bluetooth Low Energy. Его поддерживает подавляющее большинство современных мобильных устройств. Добавить же этот модуль к системе управления роботом труда не составило.

В JetRuby процветает культ перфекционизма. Если мы ставим перед собой какую-либо задачу, она должна быть выполнена идеально. Ну или чуть лучше, чем идеально. Поэтому даже маленькое приложение для управления роботом просто обязано работать на все 100%.

ccccc

Оно должно быть удобным и приятным во всех смыслах — начиная внешним видом и заканчивая процессом использования.

Вернемся к программной части приложения. Для того чтобы оно могло подключаться и управлять роботом, необходимо разобраться с работой Bluetooth Low Energy (BLE). Нас интересует верхний уровень стека BLE, а именно — Generic Attribute Profile (GATT). GATT — это своего рода описание доступных атрибутов для чтения и записи в BLE устройстве. Он состоит из Services, которые, в свою очередь, включают в себя Characteristics. Characteristic — как раз тот атрибут, который мы использовали для чтения и написания данных.

Каждый Service и Characteristic имеет адрес, по которому его можно найти. Немного разобравшись с тем, как работает BLE, мы можем смело приступать к разработке приложения.

Apple предоставляет замечательное SDK для BLE перефирии. Оно хорошо тем, что берет на себя львиную часть работы. Для коммуникации с перифирией SDK предлагает использовать класс CBCentralManager.

Первое, что нам нужно сделать — это найти BLE устройство и подключиться к нему:

Здесь мы немного схитрили и ищем не все устройства, а только нужное нам с определенным адресом. Мы его подсмотрели и внесли значение в код.

После поиска и подключения, необходимо считать все Services, которые содержит наше устройство, выбрать нужный сервис и запросить его Characteristics:

Когда же мы “доберемся” до нужного Characteristic, можно сразу же делать подключение.

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

В качестве модуля BLE для робота был выбран популярный у ардуинщиков HM-10.

ард

При подключении этого модуля нужно учесть, то что он работает от 3v3. А “мозг” робота — от 5v. В остальном модуль предоставляет нам обычный RS-232, и его общение с роботом превращается в тривиальную задачу.

Ну и конечно же видео. Посмотрите на нашего робота 🙂

Будьте в курсе всех агро новостей

Ищи больше интересной информации в нашем TG-канале Подписаться

Заполните форму или свяжитесь
удобным для Вас способом

Контакты

г. Севастополь, ул. Руднева, д.41, 4 этаж технопарк ИТ-Крым +7 978 679-76-353 agro@crimeadigital.ru

Социальные сети

Нажимая на кнопку, вы даете согласие на обработку персональных данных и соглашаетесь c политикой конфиденциальности