CC BY
205
УДК 681.5
К. Е. Шилов
Московский физико-технический институт (государственный университет)
Разработка системы автоматического управления беспилотным летательным аппаратом мультироторного
типа
В последнее время разработка беспилотных летательных аппаратов (БПЛА) является динамично развивающимся направлением. Особый интерес представляют небольшие БПЛА мультироторного типа благодаря ряду присущим им особенностям и областям применения. Основной областью применения аппаратов данного типа является проведение аэрофотосъемки и аэровидеосъемки, а также мониторинг различных параметров окружающей среды с использованием дополнительного оборудования на борту. Имея небольшой размер, БПЛА мультироторного типа могут выполнять полет в условиях ограниченного пространства, а также в местах, нахождение в которых представляет угрозу для жизни человека.
Основным направлением при разработке подобных БПЛА является повышение автономности полета, что в свою очередь увеличивает надежность аппарата, обеспечивает простоту в использовании, не требуя высокого уровня квалификации оператора, а также снижает общие затраты на выполнение задания.
Ключевые слова: система автоматического управления, беспилотный летательный аппарат, инерциальная-навигационная система, спутниковая навигационная система, автономный полёт, комплементарный фильтр, фильтр Калмана, ПИД-регулятор, наземная станция управления и контроля.
1. Введение
Благодаря развитию электроники в последние годы стало возможным создание миниатюрной системы автоматического управления для БПЛА мультироторного типа. В данной работе отражены основные концепции, этапы создания и результаты, которые были достигнуты при разработке подобной системы. Следует отметить, что опять же благодаря темпам развития современной электроники устройства такого рода являются быстро устаревающими. Одной из ключевых особенностей системы управления, разработка которой описана в данной работе, является применение новейших электронных компонентов, доступных на сегодняшний день.
2. Компоненты БПЛА мультироторного типа
На структурной схеме (рис. 1) приведены основные компоненты комплекса БПЛА мультироторного типа (на примере квадрокоптера - мультикоптер с четыремя двигателями) и их взаимная связь.
Наряду с основным объектом комплекса — системой автоматического управления, которая и обеспечивает автономность полета летательного аппарата, как уже было отмечено выше, в бортовую систему летательного аппарата также входят:
• приемник радиосигнала (2.4 ГГц), принимающий сигнал передатчика для осуществления ручного управления аппаратом;
• контроллеры двигателей, которые принимают входной сигнал широтно-импульсной модуляции (ШИМ, англ. PWM - Pulse-Width Modulation) от системы управления и устанавливают заданный режим работы каждого из двигателей;
Рис. 1. Структурная схема комплекса БПЛА мультироторного типа
• литий-полимерный (LIPO) аккумулятор напряжением 11.1В для питания двигателей и DC-DC преобразователей (11.1 В в 5 В), встроенных в контроллеры двигателей для питания системы управления и приемника радиосигнала;
• ноутбук в качестве наземной станции управления и отладки.
3. Компоненты системы управления
Остановимся подробнее на рассмотрении компонентов системы автоматического управления, которая является основной частью всего комплекса. Структурная схема приведена ниже (рис. 2).
Рис. 2. Структурная схема компонентов системы управления
Как видно из схемы выше, основным компонентом является микроконтроллер. В данной разработке было решено использовать микроконтроллер STM32F103RCT6 [1] от компании STMicroelectronics на базе ядра ARM Cortex M3 с тактовой частотой 72 МГц и 256 КБ флэш-памяти, производительности которого достаточно для выполнения необходимых задач. Программный код для микроконтроллера может быть разделен на три составные части:
• периферия STM32 - набор библиотек для работы с периферией микроконтроллера (I2C, SPI, USB, PWM и т.д.);
• драйвер - собственный набор библиотек, специально созданный для передачи данных между микроконтроллером и другими необходимыми устройствами, входящими в систему управления: акселерометры, гироскопы, магнитометр, барометр, GPS приемник, модуль беспроводной телеметрии, microSD карта памяти;
• логика управления полетом представляет собой набор высокоуровневых функций, которые обеспечивают стабилизацию и навигацию летательного аппарата.
Наиболее важным сенсором является МЭМС (микро электро-механические система, англ. MEMS — Micro Electro-Mechanical System) инерциально-измерительная система (ИИС), которая включает в себя 3-осевой акселерометр и 3-осевой гироскоп в единой интегральной схеме (ИС) [2], а также 3-осевой магнитометр на отдельной интегральной схеме. Следует отметить, что данная система управления является одной из первых в мире, в которой применен сенсор MPU6050 [3] от компании InvenSense с использованием технологии DMP (Digital Motion Processing), что подразумевает обработку угловых скоростей и ускорений с применением фильтрации Калмана непосредственно в самом процессоре датчика и отправку информации об ориентации в пространстве в цифровом формате в виде кватернионов или углов Эйлера с частотой обновления 200 Гц. Цифровые барометр для определения высоты, магнитометр [4] для определения курса и ИИС подключены к процессору по шине I2C.
В системе управления также присутствует слот для microSD карты памяти, которая используется для хранения конфигурационных файлов и записи всех параметров полета для их последующего анализа.
В качестве внешних портов используются входы и выходы ШИМ для чтения сигналов с приемника и управления контроллерами двигателей, а также входы АЦП для подключения ультразвукового сонара измерения высоты и дифференциального датчика давления измерения скорости, а также мониторинга напряжения и тока батареи. Кроме того, используются два UART порта для подключения модулей GPS и беспроводной телеметрии, также USB интерфейс, настроенный в режиме виртуального COM порта для отладки и тестирования программного обеспечения.
4. Разработка аппаратной части
После выбора элементной базы, необходимой для применения в системе, была разработана принципиальная электрическая схема. Разводка была выполнена на четырехслойной печатной плате с использование пакета EagleCAD [5]. Размер платы составляет 60 х 40 мм с двухсторонним размещением электронных компонентов. На рис. 3 приведена разводка платы по слоям.
б
Рис. 3. а — слой 2, корпус; б — слой 1 и 4, сигналы; в — слой 3, 3.3 В и 5 В
Перед началом изготовления печатных плат была сгенерирована ЭБ-модель платы (рис. 4).
После изготовления плат на них были установлены электронные компоненты. На рис. 5а, б приведены фотографии готовых собранных плат, вид сверху и снизу соответственно.
Рис. 4. а — 3Б-модель плат, вид сверху; б — 3Б-модель плат, вид снизу
Рис. 5. а — печатная плата с элементами, вид сверху; б — печатная плата с элементами, вид снизу
5. Основные принципы обработки информации датчиков
Как уже было отмечено выше, программное обеспечение подразделяется на три группы: периферия процессора, драйверы, логика управления полетом.
5.1. Периферия процессора
В первую очередь был написан код для инициализации первостепенно необходимой периферии микроконтроллера. Следующим этапом было написание кода для инициализации USB интерфейса [6] и настройки его работы в режиме виртуального COM порта, что в свою очередь позволило осуществлять «общение» компьютера с системой управления по средствам терминального клиента. Иначе говоря, это дает возможность вводить команды для системы управления с компьютера и выводить на экран необходимую информацию. Обмен информации по интерфейсам осуществляется либо по прерываниям (Interrupt Mode), либо в режиме прямого доступа в память (DMA - Direct Memory Access). Подробности этого этапа разработки в данной статье опущены.
5.2. Драйвер
Более подробно остановимся на описании процесса разработки драйвера, поскольку именно в данной части программного обеспечения происходит получение физических данных с сенсоров, а также управление актюаторами (в данном случае контроллерами двигателей).
Инерциально-змерительная система
Одним из наиболее важных сенсоров является инерциально-измерительная система (ИИС), которая включает в себя цифровые 3-осевой МЭМС акселерометр и 3-осевой МЭМС гироскоп, что позволяет определить текущую ориентацию летательного аппарата в пространстве, тем самым обеспечивая возможность последующей стабилизации.
Интегральная схема ИИС МРи-6050 от компании ¡пуепБепве представлена в разрезе на рис. 6. Размер корпуса составляет 4 х 4 х 0.9 мм. В данной микросхеме впервые были интегрированы в одном корпусе 3-осевой акселерометр и 3-осевой гироскоп с процессором обработки движения.
На рис. 7 показано расположение и оси ИИС на печатной плате системы управления, а также расположение печатной платы с привязкой к осям летательного аппарата, где ах, ау, ах — измеряемые ускорения в направлении соответствующих осей в системе отсчета, связанной с летательным аппаратом, 0,х, 0,у, — измеряемые угловые скорости вращения вокруг соответствующих осей в системе отсчета, связанной с летательным аппаратом.
Рис. 7. а — расположение осей относительно печатной платы; б — расположение осей относительно аппарата
Таким образом, для вычисления угла поворота а^ вокруг {-й оси используется следующее соотношение, где ¿1 и ¿2 - времена начала и окончания измерения:
Характерное время ¿2 — ¿1 составляет 5 мс, что соответствует частоте измерения 200 Гц. Акселерометры предназначены для компенсации увода гироскопов и определения начальных углов отсчета. Если К — вектор кажущегося ускорения, измеренного акселерометром, то пусть Кх, Ку, Дг — проекции этого вектора на оси X, У, Z. Тогда угол наклона вокруг г-й оси летательного аппарата, измеренный акселерометром, равен:
Рис. 6. Интегральная схема ИИС
(1)
Внешний магнитометр используется для компенсации увода угла рысканья, измеряемого осью Z гироскопа, так как, находясь в положении с близкими к нулю креном и тангажом, данная величина не может быть компенсирована акселерометром.
Данные о текущих углах, полученные с акселерометра, гироскопа и магнитометра, пройдя процессы слияния и фильтрации, дают информацию об углах крена, тангажа и рысканья.
Данный принцип измерений и обработки используется во внутреннем процессоре интегральной схемы ИИС, который известен как DMP [7].
Специально для тестирования и отладки ИИС было разработано приложение c использованием Processing© [8], которое отображает параллелепипед, ориентация которого соответствует ориентации платы в пространстве. На рис. 8 представлена фотография системы управления на фоне монитора с запущенным приложением.
Рис. 8. Приложение для отладки ИИС
Датчик давления и сонар для определения высоты
Сенсорами для определения высоты служат цифровой МЭМС-датчик давления, а также ультразвуковой сонар.
Зависимость высоты от атмосферного давления может быть представлена следующей формулой и графически приведена на рис. 9:
Н = 44330
/ 1 \ Л ( р\ 5.255
V14 ^ !
(3)
где Р — давление на высоте Н, Ро — давление над уровнем моря (101325 Па).
Согласно заявленным характеристикам точность измерения используемого датчика давления (Bosch BMP085) [9], который на данный момент является одним из лучших в своем классе, составляет 0.03 гПа, что соответствует погрешности в 0.25 м. При тестировании оказалось, что датчик воспринимает высокочастотный шум колебаний воздуха, и из-за этого разброс измерений составляет около 2 м, что недопустимо для выполняемых летательным аппаратом задач (например, автономный полет на высоте порядка 2 м и ниже). Усредняющий фильтр давал сильное запаздывание, что также недопустимо. В связи с этим было решено применить фильтр Калмана [10], который использует метод экстраполяции данных с последующей корректировкой отклонения.
Рис. 9. Зависимость изменения высоты от давления
4700 4800 4900 5 000 5 100 5200 5300 5 400 5 500 5600 5700 5 500 5 &00 6000 6100
Номер измерения, Г—10 Гц
Рис. 10. Результаты измерений и фильтрации барометрических данных высоты
На рис. 10 представлены результаты измерений при нахождении на постоянной высоте, где красная линия соответствует исходным данным с барометра, а синяя отображает ту же величину, но после прохождения фильтра Калмана. Из графика видно, что разброс высоты после фильтрации составляет не более 0.8 м.
Г >
Рис. 11. Ультразвуковой сонар ХЬ-МахБопаг EZ-0
Для повышения точности измерения высоты на высотах до 6 метров используется ультразвуковой сонар, разрешение которого составляет 1 см. Для данной системы управления используется сонар ХЬ-МахБопаг EZ-0 (рис. 11), работающий на частоте 42 кГц, имею-
щий область чувствительности, представленную на рис. 12, и подключенный к входу АЦП микроконтроллера.
Рис. 12. Область чувствительности сонара
Высота Н (см), измеренная ультразвуковым сонаром, вычисляется по следующей формуле:
ив
Н = , (4)
0.0049' v 7
где Uв — выходное напряжение (В).
GPS-приемник для определения положения в пространстве
Для определения положения летательного аппарата в пространстве используется GPS-приемник, подключенный к системе управления через UART порт. GPS-приемник посылает данные по протоколу NMEA [11] (от англ. National Marine Electronics Association) с частотой 10 Гц, который содержит в себе информацию о текущей широте, долготе, высоте, скорости, курсе, времени и т.д.
Система автоматического управления осуществляет получение и разборку NMEA строк в соответствующие переменные. По результатам тестов точность GPS позиционирования составляет не более 2-3 метров, а также сильно зависит от окружающей электромагнитной обстановки. Из-за относительно слабого сигнала навигация по GPS возможна только при наличии открытого неба.
Датчик воздушной скорости для определения скорости полета
Также в системе управления предусмотрена возможность подключения аналогового дифференциального датчика давления для измерения текущей скорости полета. Согласно уравнению Бернулли:
рУ 2
р = Рс + Р-1Г, (5)
где Р — полное давление, Рс — статическое давление, р — плотность воздуха, V — скорость набегающего потока.
Отсюда скорость набегающего потока
*»
где величина Р — Рс и есть разница давлений, измеряемая датчиком.
Чтение данных с приемника и управление контроллерами
Чтение данных с приемника радиоуправления и управление оборотами двигателей осуществляется с помощью сигнала ШИМ. Импульс длительностью 1 мс соответствует минимальному состоянию, 2 мс - максимальному. Частота обновления состояния сигнала для управления оборотами двигателей составляет 400 Гц.
MicroSD карта памяти для чтения и записи полетной информации
Кроме всего прочего, как было отмечено ранее, в системе управления присутствует слот для microSD карты, что позволяет вести запись информации и ее послеполетный анализ. Данные, приведенные ниже, были получены таким способом.
После завершения этапа разработки и тестирования программного обеспечения для получения данных с датчиков и их последующей обработки последовал следующий этап -разработка алгоритмов и программирование логики управления полетом.
6. Алгоритмы логики управления полетом
Как уже отмечалось ранее, управление полетом летательного аппарата может осуществляться как в ручном режиме, так и в автоматическом. Кроме того, присутствует иерархическая структура режимов полета, при которой каждый следующий режим с более высоким уровнем автономности включает в себя алгоритмы предыдущего режима, у которого уровень автономности ниже.
Таким образом, данная система управления включает в себя следующее режимы полета, разработка большинства из которых будет описана далее.
• Ручной режим. Управление производится полностью оператором.
• Режим стабилизации. Управление производится также оператором, но летательный аппарат автоматически поддерживает нулевой крен, тангаж и стабилизирует угол курса при отсутствии действий со стороны оператора, тем самым удерживаясь в воздухе.
• Режим удержания высоты. При переходе в данный режим аппарат автоматически выдерживает высоту, которая была на момент включения данного режима, используя данные сонара и датчика давления.
• Режим удержания позиции по GPS. При переходе в данный режим аппарат автоматически выдерживает позицию, которая была на момент включения данного режима, используя данные с GPS-приемника.
• Режим возврата на точку старта. При включении данного режима аппарат независимо от его текущего положения автоматически возвращается на точку взлета и переходит в режим удержания позиции.
• Режим автоматического полета по точкам. В данном режиме полета аппарат автоматически следует по заранее заданному маршруту.
• Режим автоматического взлета и посадки
6.1. Ручной режим
В данном режиме полета происходит линейная конвертация величины отклонения джойстиков крена, тангажа и рысканья пульта управления (рис. 13) в разницу числа оборотов соответствующих двигателей (рис. 14), среднее значение которых соответствует текущему положению джойстика газа.
6.2. Режим стабилизации
В качестве алгоритма автоматической стабилизации углов летательного аппарата используется пропорционально-интегральный Р1-каскад, примененный к каждой из осей вращения (крен, тангаж, рысканье), схема которого представлена на рис. 15.
Рис. 15. Схема И-каскада управления стабилизацией угла
аась — текущий угол, поступающий с ИИС в градусах; а^е8 — желаемый угол в градусах;
^асг — текущая угловая скорость вращения вокруг оси а в градусах/с;
— желаемая угловая скорость вращения вокруг оси а в градусах/с; аегг = а^е8 — ааС1 — текущая ошибка угла а в градусах;
&егг = — — текущая ошибка угловой скорости вращения вокруг оси а в гра-
дусах/с;
Out — выходной сигнал ШИМ (мс), отправляемый на микшер сигналов в соответствии с конфигурацией летательного аппарата (число двигателей, например, 3, 4 или 6).
Далее вычисляется желаемая угловая скорость вращения для данной ошибки угла по формуле
^des — kaQe
(7)
где ка — пропорциональный коэффициент усиления по углу. Желаемая угловая скорость сравнивается с текущей и вычисляется ошибка угловой скорости:
^егг — ^des ^acty
(8
где кг — пропорциональный коэффициент усиления по угловой скорости. Окончательно, получается
Out — к% [kpaaerr - ^act] + К / (ka,aerr - Qact)dt + кга aerrdt,
(9)
где кгг — интегральный коэффициент усиления по угловой скорости;
ка — интегральный коэффициент усиления по углу.
Для обеспечения максимальной эффективности коэффициенты усиления к1^, кг, кгг, кга подбираются экспериментально, так как сильно зависят от конкретного летательного аппарата, а именно: его массы, моментов инерции, характеристик двигателей и контроллеров скорости и т.д.
Реакция на изменение желаемого угла представлена на графике (рис. 16).
Рис. 16. Реакция системы на изменение желаемого угла
6.3. Режим удержания высоты
В данном режиме также используется пропорционально-интегральный Р1-каскад, за исключением того, что в качестве а теперь выступает высота Н, а в качестве О выступает вертикальная скорость ^.
Рис. 17. Реакция системы на изменение желаемой высоты
График реакции на изменение желаемой высоты представлен на рис. 17.
6.4. Режим удержания позиции
Как уже было отмечено ранее, для работы данного режима требуются данные с GPS-приемника. По протоколу NMEA широта и долгота отображаются в формате: широта: DDMM.MMMM;
долгота: DDDMM.MMMM, то есть градусы, минуты и доля минуты. Работать с таким форматом для дальнейших вычислений неудобно, поэтому выполняется пересчет в следующий формат:
широта: DD.DDDDDD;
долгота: DD.DDDDDD, то есть градусы и доля градуса.
Перевод осуществляется по следующим формулам, где LAT — широта (от англ. Latitude), LON — долгота (от англ. Longitude). Индекс D означает систему измерения в долях градусах, M — систему измерения в долях минуты. Floor() — функция округления до целого в меньшую сторону.
LATd — floor
(LATM\
V 100 )
+
LATM - 100floor(
^ — +
60
LONM - 100floor
(LONM\
V 100 )
60
Поскольку стабилизация позиции по GPS использует аналогичный PI-каскад, описанный выше, то в качестве входных величин удобнее использовать расстояние, измеряемое в метрах.
Для этого необходимо знать число метров в одном градусе широты и долготы. Для меридиональных направлений эта величина постоянна, а вот для параллельных изменяется в зависимости от текущей широты по закону
Sx —
2kRe cos(LAT ) 360 !
(10)
где Бх — число метров в одном градусе долготы, Ке — радиус Земли. Тогда положение в X, У метрах может быть определено как
X — SxLONd , Y — Sy LATd .
(11)
6.5. Режим автоматического возврата на точку старта
X, M
Рис. 18. Траектория автоматического возврата на точку старта
При переключении в данный режим летательный аппарат устанавливает точку взлета как желаемую на данный момент и начинает полет к ней. С незначительными изменениями алгоритмы навигации в этом режиме основываются на режиме удержания позиции.
На рис. 18 представлена траектория полета при возврате аппарата на точку старта. Стрелками отмечены места активации режима возврата и направление полета.
6.6. Режим автоматического полета по точкам
Автоматический режим полета по точкам основывается на режиме полета, который был рассмотрен ранее. Доработка алгоритма заключается в том, что при приближении к контрольной точке на заранее заданное расстояние, которое называется 'радиус контрольной точки, происходит установка следующей точки маршрута в качестве желаемой. На рис. 19 представлен трек, записанный во время полета на встроенную шюгоВБ карту памяти и отображенный в программе Google.Earth.
Рис. 19. Трек полета, отображенный в Google.Earth
На момент написания статьи алгоритм автоматического полета по точкам проходит тестирование и оптимизацию, поэтому результат, представленный на рис. 19, не является окончательным.
7. Заключение
В заключении хотелось бы отметить, что работа по исследованиям и разработкам в данной области началась в октябре 2011 года и продолжается по сегодняшний день. Объем разработанного программного обеспечения в настоящий момент составляет около 10 000 строк.
Летом 2012 года автор принял участие в конференции 1МЛУ 2012, г. Брауншвейг, Германия. Автором был представлен доклад [12] о принципах построения данной системы управления, а летательный аппарат с разработанным автопилотом принял участие в соревнованиях. В сентябре 2013 года разработка была продемонстрирована на конференции 1МЛУ 2013, г. Тулуза, Франция. Новым направлением разработки в ближайшее время станет система компьютерного зрения и адаптивные алгоритмы управления, которые дополнят данную систему управления и повысят уровень автономности летательного аппарата в условиях ограниченного пространства.
Литература
1. High-density performance line ARM-based 32-bit MCU, STMicroelectronics, http://st.com/
2. Seeger J., Lim M. and Nasiri S. Development of Hight-Perfomance, High-Voulume Consumer MEMS Gyroscopes, InvenSense, http://invensense.com/
3. MPU-6000 and MPU-6050 Product Specification Revision 3.2, InvenSense, http://invensense.com/
4. 3-Axis Digital Compass IC HMC5883, Honeywell, http://honeywell.com/
5. EAGLE — Easily Applicable Graphical Layout Editor, Manual, Version 5. — 2010. CadSoft Computer.
6. Агуров П. Интерфейс USB. Практика использования и программирования — Спб.: БХВ-Петербург, 2006.
7. MPU-6000 and MPU-6050 Register Map and Descriptions Revision 3.2, InvenSense http://invensense.com/
8. Shiffman D. Learning Processing: A Beginner's Guide to Programming Images, Animation, and Interaction Burlington, MA 01803, USA. — 2008. Elsevier Inc.
9. BMP085, Digital barometric pressure sensor, BOSCH, http:// boschsensortec.com/
10. Introduction to Estimation and the Kalman Filter Hugh Durrant-Whyte Australian Centre for Field Robotics The University of Sydney. — NSW 2006. — Australia, 2001.
11. NMEA Data and Protocol Description, http://www.gpsinformation.org/dale/nmea.htm
12. Shilov K., Lazurin G. MAV Autopilot For Commercial and Research Braunschweig. — Germany. — 2012.
Поступила в редакцию 02.10.2013.
