Дело было вечером. Делать было нечего. Как ведет себя в такой ситуации среднестатистический добропорядочный гражданин? Или недобропорядочный? Или даже программист? Серфит интернет, смотрит телевизор, пьет пиво или что покрепче, играет с детьми… В конце концов — спит. Но не таков сотрудник компании JetRuby.
В один прекрасный момент нам подумалось: а почему бы не развлечься и не сделать что-нибудь действительно интересное? Так родилась идея стабилизации обратного маятника. Или по-простому: разработка балансирующего робота.
Немного математики
Изначально мы пытались построить робота под управлением PID регулятора без всякой математики. Но, увы, результата это не принесло. В итоге возникла такая модель:
Где mp — масса маятника, прикрепленного к концу стержня длины l. К противоположному концу стержня крепится двигатель, способный развить максимальный момент Mk и передать его колесу с радиусом r и массой mw.
Наша главная цель — стабилизация маятника в вертикальном положении. Также его необходимо каждый раз возвращать в изначальное положение перед возмущением.
Уравнения движения с описанием обратного маятника выглядят следующим образом:
На первый взгляд они могут показаться достаточно сложными, однако сам робот не имеет о них ни малейшего представления. Что же касается управления, в нем используется линеаризованная модель. Она перед вами:
Синтезирование управления
Для управления маятником был выбран линейно-квадратичный регулятор. Он отличается от PID-регулятора, потому что представляет собой произведение собственных коэффициентов на ошибки по координатам. Никаких интегралов или дискретных аналогов производной. Однако для вычисления линейно-квадратичного регулятора необходимы модель системы и Matlab.
Итак, получаем коэффициенты. Для этого необходимо выполнить в Matlab следующие команды:
Где А и В — соответствующе матрицы из линеаризированной модели.
Матрица Q определяет цену отклонения системы от начала координат.
Матрица R определяет цену расхода энергии на управление системой.
После получения математической модели и вычисления параметров регулятора у нас есть все для того чтобы моделировать систему в свое удовольствие и наблюдать ее реакцию на различную длину и вес маятника.
Какой же робот без железа?
Каркас робота состоит из стеклотекстолита и металлических шпилек м5. Все это добро соединено обыкновенными гайками. Конструкцию робота можно назвать трехэтажной. Внизу располагается модуль управления двигателями и сами двигатели. На втором этаже — “мозг”, питание и BLE-модуль. На третьем — аккумулятор и IMU.
IMU — гироскоп и акселерометр — используется для определения угла и угловой скорости. Мы выбрали простой и популярный модуль на базе mpu6050. В чем особенность этого датчика? В том, что он поддерживает DMP. И мы этим воспользовались с превеликим удовольствием (отдельное и большое спасибо создателям библиотеки i2cdevlib).
Переходим к следующему сенсору. Речь идет о квадратурном энкодере на моторе. Он генерирует прямоугольные импульсы на каждом из двух выводов:
Их считают или прерываниями, или считыванием значений в цикле. На arduino playground размещена интересная статья с примерами кода. Теперь нам остается узнать угловую скорость колеса. Здесь на помощь спешит старая-добрая формула, хорошо известная каждому еще со школьных времен: пройденное расстояние/затраченное время.
EncPosRad переводит численность тиков энкодера в угол колеса. Наш энкодер выдает около 4800 тиков на оборот. Чтобы получить угол необходимо умножить тики на 2 Пи и разделить на их общее количество при полном обороте колеса.
Далее полученные нами показания сенсоров отправляются в LQR регулятор:
Коэффиценты для регулятора мы берем из проведенных расчетов. Все просто:)
Как управлять балансирующим роботом?
Наигравшись вдоволь, мы поняли, что стоять на месте неинтересно и бесперспективно. В результате было принято принципиальное решение запилить управление роботом с мобильного устройства.
Для этого прекрасно подошел Bluetooth Low Energy. Его поддерживает подавляющее большинство современных мобильных устройств. Добавить же этот модуль к системе управления роботом труда не составило.
В JetRuby процветает культ перфекционизма. Если мы ставим перед собой какую-либо задачу, она должна быть выполнена идеально. Ну или чуть лучше, чем идеально. Поэтому даже маленькое приложение для управления роботом просто обязано работать на все 100%.
Оно должно быть удобным и приятным во всех смыслах — начиная внешним видом и заканчивая процессом использования.
Вернемся к программной части приложения. Для того чтобы оно могло подключаться и управлять роботом, необходимо разобраться с работой 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, и его общение с роботом превращается в тривиальную задачу.
Ну и конечно же видео. Посмотрите на нашего робота 🙂
