Oops... your message was not sent

Your message has been successfully sent

тематические истории, основанные на опыте компании JetRuby
Мобильная разработка

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

Дело было вечером. Делать было нечего. Как ведет себя в такой ситуации среднестатистический добропорядочный гражданин? Или недобропорядочный? Или даже программист? Серфит интернет, смотрит телевизор, пьет пиво или что покрепче, играет с детьми… В конце концов — спит. Но не таков сотрудник компании 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, и его общение с роботом превращается в тривиальную задачу.

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

department
Статью подготовил
Отдел мобильной разработки
Профессиональная разработка нативных приложений под Android и iOS, а также реализация гибридных кроссплатформенных решений и мобильных сайтов.
New Articles