Аппаратно-программный комплекс для дистанционного управления движением квадрокоптера

Шилин Антон Алексеевич

Аппаратно-программный комплекс для дистанционного управления движением квадрокоптера

В настоящей работе описывается разработка логики и структуры аппаратно-программного комплекса для дистанционного управления движением квадрокоптера. Рассматривается архитектура данного комплекса: аппаратная составляющая (организация взаимодействия бортовой части – летательного аппарата и наземной части – технического устройства, на котором непосредственно реализован пульт управления), а также программная составляющая (алгоритмы управления, используемые при работе, и передача бортовых данных). Разработаны алгоритмы аварийной защиты. Построена компьютерная модель квадрокоптера. В системе MATLAB-Simulink получены результаты имитационного моделирования динамики движения летательного аппарата с использованием синтезированных законов управления.

Общественные науки в целом

Дипломы

Вуз: Санкт-Петербургский государственный университет (СПбГУ)

ID: 587d363a5f1be77c40d589e9

UUID: 11d972b4-7d9d-42d1-87e2-4eeafa17330a

Язык: Русский

Опубликовано: больше 7 лет назад

Просмотры: 499

Шилин Антон Алексеевич

Источник: Санкт-Петербургский государственный университет

Комментировать 0

Рецензировать 0

Скачать - 1183218 bytes

Поделиться работой

САНКТ-ПЕТЕРБУРГСКИЙ ГОСУДАРСТВЕННЫЙ УНИВЕРСИТЕТ ФАКУЛЬТЕТ ПРИКЛАДНОЙ МАТЕМАТИКИ – ПРОЦЕССОВ УПРАВЛЕНИЯ КАФЕДРА КОМПЬЮТЕРНЫХ ТЕХНОЛОГИЙ И СИСТЕМ Шилин Антон Алексеевич Выпускная квалификационная работа бакалавра Аппаратно-программный комплекс для дистанционного управления движением квадрокоптера Направление 010400.62 Прикладная математика и информатика Научный руководитель, кандидат физ.-мат. наук, доцент Сотникова М. В. Санкт-Петербург 2016

Содержание Введение .................................................................................................... 3 Глава 1. Постановка задачи ....................................................................... 5 1.1. Математическая модель квадрокоптера ...................................... 5 1.2. Формулировка задачи ................................................................... 9 Глава 2. Разработка аппаратно-программного комплекса .................... 12 2.1. Аппаратная архитектура комплекса .......................................... 12 2.2. Программная архитектура комплекса ....................................... 17 2.3. Разработка законов управления ................................................. 24 Глава 3. Реализация комплекса ............................................................... 28 3.1. Имитационное моделирование динамики движения летательного аппарата ....................................................................... 28 3.2. Практическая реализация комплекса ........................................ 35 Выводы ..................................................................................................... 38 Список литературы ................................................................................. 39 Приложение .............................................................................................. 40 2

Введение Вопрос об управлении машинами и механизмами на расстоянии длительное время упоминание о оставался реализации нерешенным возможности для человечества. дистанционного Первое управления устройствами относится к 1898 году, когда Никола Тесла представил общественности радиоуправляемую лодку. Однако, меньше, чем через полвека в воздух поднялся первый беспилотный летательный аппарат, контролируемый пилотом с поверхности. С тех пор появились новые способы дистанционной связи и расширился спектр воздушных аппаратов, для которых актуальна проблема управления на расстоянии. Одним из классов подобных устройств являются многороторные вертолеты, в частности, квадрокоптеры (рис. 1). Рис. 1. Четырехроторный вертолет Ботезата, 1923. В связи с ростом микропроцессорных выполняемым вычислительных систем и летательными мощностей увеличением аппаратами, современных требований возникает к задачам, потребность в автоматизации части работы оператора беспилотного устройства. Для решения этой проблемы необходимо создание автоматизированных комплексов для дистанционного управления беспилотными летательными аппаратами, которые реализуют на борту определенные алгоритмы 3

управления, а также предоставляют оператору возможность обзора телеметрии, поступаещей с воздушного устройства. В зависимости от поставленных задач, у оператора должна быть возможность в выборе способов управления летательным аппаратом, например, задание определенного пути для следования. В данной работе раскрывается вопрос создания аппаратно- программного комплекса для дистанционного управления движением четырехроторного вертолета (квадрокоптера). Актуальность обуславливается развитием сфер применения класса мультироторных беспилотных летательных аппаратов (БПЛА). Кроме того, большинство реализаций подобных комплексов, находящихся в открытом доступе, имеют весьма упрощенные алгоритмы управления и предназначены для использования в развлекательных целях. 4

Глава 1. Постановка задачи 1.1. Математическая модель квадрокоптера Для описания динамики движения квадрокоптера введем две системы координат: инерциальную 𝑂𝑋𝑌𝑍, связанную с землей, и подвижную 𝑂𝐵 𝑋𝐵 𝑌𝐵 𝑍𝐵 , связанную с рамой летательного аппарата. Обе системы являются правыми. Их схема представлена на рис. 2. Рис. 2. Инерциальная и подвижная системы координат. Способ задания осей инерциальной системы следующий: 𝑂𝑋 направлена на север, 𝑂𝑌 — на запад, а 𝑂𝑍 — вверх, перпендикулярно земной поверхности. Точка отсчета 𝑂𝐵 подвижной системы находится в центре масс квадрокоптера. Будем считать, что он совпадает с точкой пересечения лучей рамы. Для подвижной системы оси зададим следующим образом: 𝑂𝐵 𝑋𝐵 5

направлена от точки пересечения лучей между первым и вторым моторами, 𝑂𝐵 𝑌𝐵 — между первым и четвертым моторами, а 𝑂𝐵 𝑍𝐵 — вверх, перпендикулярно плоскости лучей. Положение центра масс квадрокоптера в инерциальной системе отсчета описывается вектором 𝝃: Ориентация 𝑥 𝝃= 𝑦 . 𝑧 подвижной системы координат относительно неподвижной определяется с помощью углов Эйлера 𝜼: 𝜑 𝜼 = 𝜃 . 𝜓 Угол тангажа 𝜑 соответствует повороту квадрокоптера вокруг оси 𝑂𝑌, угол крена 𝜃 — вокруг оси 𝑂𝑋, а угол рысканья 𝜓 — вокруг оси 𝑂𝑍. В подвижной системе задаются вектор линейной скорости 𝑽 и вектор угловой скорости 𝝂: 𝑽 = 𝑣𝑥 𝑣𝑦 , 𝝂 = 𝑣𝑧 𝑝 𝑞 . 𝑟 Для перехода от подвижной системы к инерциальной используется следующая матрица преобразования координат 𝑹: 𝐶𝜓 𝐶𝜑 𝑹 = 𝑆𝜓 𝐶𝜑 −𝑆𝜑 𝐶𝜓 𝑆𝜑 𝑆𝜃 − 𝑆𝜓 𝐶𝜃 𝑆𝜓 𝑆𝜑 𝑆𝜃 + 𝐶𝜓 𝐶𝜃 𝐶𝜑 𝑆𝜃 𝐶𝜓 𝑆𝜑 𝐶𝜃 + 𝑆𝜓 𝑆𝜃 𝑆𝜓 𝑆𝜑 𝐶𝜃 − 𝐶𝜓 𝑆𝜃 , 𝐶𝜑 𝐶𝜃 где 𝑆𝑥 = sin 𝑥 , 𝐶𝑥 = cos 𝑥 . Связь между линейными скоростями 𝝃 и 𝑽 в неподвижной и связанной системах координат задаются выражением: 𝝃 = 𝑹𝑽, Аналогично, угловые скорости 𝜼 и 𝝂 связаны соотношением: 𝜼 = 𝑾−1 𝝂, 6

где матрица 𝑾−1 имеет вид [1]: 𝑾−1 𝐶𝜓 /𝐶𝜃 = −𝑆𝜓 𝐶𝜓 𝑇𝜃 𝑆𝝍 /𝐶𝜃 𝐶𝜓 𝑆𝜓 𝑇𝜃 0 0 , 1 𝑇𝑥 = tan 𝑥 . Будем считать квадрокоптер симметричным телом, у которого оси инерции совпадают с осями подвижной системы координат. В таком случае тензор инерции 𝑰 примет следующий вид: 𝐼𝑥 𝑰= 0 0 0 𝐼𝑦 0 0 0 . 𝐼𝑧 Введем следующие обозначения:  𝑚 — масса аппарата.  𝑔 — ускорение свободного падения.  𝜔𝑖 — угловая скорость вала мотора с индексом 𝑖.  𝑏 — коэффициент воздушного сопротивления винта в плоскости вращения.  𝑘 — коэффициент тяги винта.  𝑙 — расстояние от оси вала мотора до центра масс квадрокоптера.  𝑇 — суммарная тяга, создаваемая моторами летательного аппарата.  𝜏𝜑 , 𝜏𝜃 , 𝜏𝜓 — моменты по соответствующим осям. Управление квадрокоптером осуществляется путем задания определенных угловых скоростей валов моторов, что в свою очередь влияет на моменты и общую тягу: 𝜏𝜑 = −𝜔12 − 𝜔22 + 𝜔32 + 𝜔42 𝑙𝑘 𝜏𝜃 = 𝜔12 − 𝜔22 − 𝜔32 + 𝜔42 𝑙𝑘 𝜏𝜓 = 𝜔12 − 𝜔22 + 𝜔32 − 𝜔42 𝑏 (1.1) 𝑇 = 𝜔12 + 𝜔22 + 𝜔32 + 𝜔42 𝑘 Справедливы следующие уравнения динамики: 7

𝜏𝜑 𝐶𝜓 −𝑆𝜓 0 𝜼= 𝑆𝜓 𝐶𝜓 0 𝐼𝑦 𝜏𝜃 0 0 1 𝐼𝑥 𝜏𝜓 , 𝐼𝑧 𝑹𝑻 𝝃= 𝑚 0 − 𝑔 0 − 1 𝑨𝝃 𝑚 , где 𝑨 — матрица коэффициентов аэродинамического сопротивления, а 𝑻 — вектор тяги моторов: 𝐴𝑥 𝑨= 0 0 0 𝐴𝑦 0 0 0 0 ,𝑻 = 0 . 𝐴𝑧 𝑇 Для упрощения математической модели будем считать, что углы 𝜑 и 𝜃 достаточно малы. Тогда уравнения динамики принимают итоговый вид: 𝑥 = 𝐶𝜓 𝑣𝑥 − 𝑆𝜓 𝑣𝑦 𝑦 = 𝑆𝜓 𝑣𝑥 + 𝐶𝜓 𝑣𝑦 𝑧 = 𝑣𝑧 𝑥 = 𝐶𝜓 𝜑 𝑦 = 𝑆𝜓 𝜑 𝑧= 𝑇 𝑚 𝑇 𝑚 𝑇 𝑚 + 𝑆𝜓 𝜃 − 𝐶𝜓 𝜃 −𝑔− 𝑇 𝑚 𝑇 𝑚 𝐴𝑧 𝑚 − − 𝐴𝑥 𝑚 𝐴𝑦 𝑚 𝑥 𝑦 𝑧 𝜑 = 𝐶𝜓 𝑝 + 𝑆𝜓 𝑞 (1.2) 𝜃 = −𝑆𝜓 𝑝 + 𝐶𝜓 𝑞 𝜓= 𝑟 𝜑= 𝐶𝜓 𝜏 𝜑 𝜃= − + 𝐼𝑦 𝑆𝜓 𝜏 𝜑 𝐼𝑦 𝜓= 𝑆𝜓 𝜏 𝜃 + 𝜏𝜓 𝐼𝑥 𝐶𝜓 𝜏 𝜃 𝐼𝑥 𝐼𝑧 Далее под "положением летательного аппарата в пространстве" будет подразумеваться положение центра масс квадрокоптера в инерциальной системе отсчета. 8

1.2. Формулировка задачи Разработать комплекс для дистанционного управления движением квадрокоптера, который должен:  предоставлять возможность управления квадрокоптером при помощи мобильного устройства (смартфона/планшета) на дистанциях до 50 м от местонахождения пульта управления;  осуществлять непрерывную синхронизацию мобильного устройства и бортовой части (квадрокоптера) по каналу Wi-Fi;  предоставлять пользователю полный доступ к получаемой с датчиков летательного аппарата телеметрии;  предоставлять пользователю интуитивно-понятный интерфейс для управления квадрокоптером;  содержать на борту БПЛА алгоритмы управления для приведения квадрокоптера в определенную точку пространства с заданным углом рысканья;  содержать на борту БПЛА астатический регулятор по положению в пространстве для компенсации внешнего воздействия (ветер). Пусть 𝝃 = 𝑥, 𝑦, 𝑧 — координаты центра масс летательного аппарата в неподвижной инерциальной системе отсчета, 𝜼 = 𝜑, 𝜃, 𝜓 — ориентация БПЛА в этой системе; и имеется набор измерений 𝒀. Под задачей стабилизации квадрокоптера с определенными углами ориентации 𝜼𝒅 будем подразумевать поиск такого управления 𝑢 = 𝑢 𝒀, 𝜼𝒅 , чтобы 𝜼 → 𝜼𝑑 при 𝑡 → ∞. Пусть задана некоторая точка 𝑃𝑑 𝝃𝑑 и угол 𝜓𝑑 . В таком случае задача приведения квадрокоптера в определенную точку пространства с заданным углом рысканья состоит в следующем: найти такое управление 𝑢 = 𝑢 𝒀, 𝑃𝑑 , чтобы 𝝃 → 𝝃𝑑 и 𝜓 → 𝜓𝑑 при 𝑡 → ∞. Пусть 𝑃𝑠 𝝃𝑠 — координаты точки, в которой необходимо стабилизировать летательный аппарат. Под астатическим регулятором 9

такое управление 𝑢 = 𝑢 𝒀, 𝑃𝑠 , внешнего воздействия 𝐻 подразумевается постоянного на что при положение наличии центра масс квадрокоптера, выполняется 𝝃 → 𝝃𝑠 при 𝑡 → ∞. Исходя из изложенных требований были поставлены следующие задачи:  разработать логику и структуру работы аппаратно-программного комплекса для дистанционного управления движением квадрокоптера.  в рамках комплекса разработать бортовые алгоритмы управления для стабилизации летательного аппарата с определенными углами ориентации; для приведения летательного аппарата в заданную точку пространства с заданным углом рысканья; для компенсации постоянного внешнего воздействия на положение центра масс летательного аппарата в пространстве. Произвести имитационное моделирование динамики движения летательного аппарата с применением разработанных законов управления. В настоящее время уже ведутся разработки комплексов для управления движением мультикоптеров при помощи мобильных устройств, однако, большинство из них носит любительский характер. При рассмотрении отдельно бортовой части стоит отметить, что существуют готовые коммерческие решения плат управления полетом [2][3]. Среди реализованных возможностей представлены алгоритмы стабилизации по углам, а также стабилизация в точке на основе данных датчиков геолокации. Недостатком подобных решений является высокая стоимость и низкая точность определения положения в пространстве, ошибка которых достигает десятков метров. В свободном доступе представлено несколько алгоритмов стабилизации квадрокоптера по углам [4]. Также присутствуют методы 10

приведения летательного аппарата в определенную точку пространства [5], но они не учитывают внешнего возмущения. Таким образом, необходимо осуществить разработку астатического регулятора по положению в пространстве для компенсации воздействия на летательный аппарат внешних сил таких, как ветер. 11

Глава 2. Разработка аппаратно-программного комплекса 2.1. Аппаратная архитектура комплекса Комплекс можно условно разделить на две крупные части: базовую станцию, которая включает в себя пульт управления и коммуникационный блок, и бортовую часть. Рассмотрим подробнее каждую составляющую. Бортовая часть Компоненты бортовой части (рис. 3) можно разделить на 4 блока:  блок датчиков;  блок коммуникации;  блок исполнительных устройств;  вычислительный блок. Рис. 3. Структура бортовой части. 12

Блок датчиков содержит следующие компоненты:  трехосевой акселерометр;  трехосевой гироскоп;  барометр (датчик атмосферного давления);  ультразвуковой датчик расстояния (сонар);  датчик температуры окружающего воздуха;  вольтметр;  4 инфракрасных датчика обнаружения препятствий. Бортовой акселерометр возвращает оценку значений ускорений 𝑣𝑥 , 𝑣𝑦 , 𝑣𝑧 в связанной системе координат. При интегрировании этих данных находятся соответствующие оценки скоростей 𝑣𝑥 , 𝑣𝑦 , 𝑣𝑧 . Гироскоп возвращает оценку угловых скоростей 𝑝, 𝑞 , 𝑟 . Ультразвуковой датчик расстояния используется для нахождения высоты над поверхностью. Сонар находится на нижней стороне летательного аппарата и направлен вдоль отрицательного направления оси 𝑂𝐵 𝑍𝐵 . В качестве возвращаемого значения выступает время 𝑡𝑠 прохождения звуковым импульсом расстояния до ближайшего препятствия, находящегося в секторе работы датчика, и обратно. Данный сенсор имеет ограниченный рабочий диапазон. При удалении от препятствия дальше верхней допустимой границы значительно увеличивается погрешность получаемых данных. Оценка расстояния до поверхности осуществляется следующим образом: ℎ= 𝑡𝑠 𝑣𝑠 , 2 где 𝑣𝑠 — скорость звука в воздухе. Точность показаний подлежит увеличению путем учета текущей температуры воздуха при определении 𝑣𝑠 . Для этого используются данные 𝑇𝑎 температурного датчика (значение в Кельвинах). Вычисления осуществляются по следующей формуле [6]: 13

𝑣𝑠 = 𝛾𝑅𝑇𝑎 , 𝑀 где 𝛾 — показатель адиабаты, 𝑅 — универсальная газовая постоянная, 𝑀 — молярная масса воздуха. Барометр предоставляет данные о текущем атмосферном давлении 𝜌 на уровне квадрокоптера, которые позволяют произвести оценку высоты ℎ летательного аппарата в том случае, если расстояние до поверхности находится за рамками рабочего диапазона ультразвукового датчика. Для этого используется следующая формула [7]: 1 𝜌 5.255 ℎ = 44330 1 − . 𝜌0 Если возвышение над поверхностью позволяет задействовать ультразвуковой сенсор, то происходит калибровка датчика атмосферного давления, при которой его текущее возвращаемое значение 𝜌0 запоминается и соотносится с данными сонара, что позволяет увеличить точность оценки высоты. Вольтметр осуществляет измерение текущего уровня заряда аккумуляторных батарей летательного аппарата. Инфракрасные датчики обнаружения препятствий направлены в плоскости 𝑂𝐵 𝑋𝐵 𝑌𝐵 по положительным и отрицательным направлениям осей 𝑂𝐵 𝑋𝐵 и 𝑂𝐵 𝑌𝐵 . В качестве возвращаемого значения для каждого из датчиков выступает 1 при нахождении препятствия на оси действия датчика ближе определенной дистанции и 0 при его отсутствии. Блок коммуникации состоит из беспроводного интерфейса. Данный модуль организует точку доступа Wi-Fi и осуществляет прием и отправку данных на базовую станцию. Блок исполнительных устройств включает в себя 4 электронных регулятора хода (ESC) и 4 бесколлекторных двигателя. Регуляторы ESC служат для плавного варьирования оборотов электродвигателей летательного 14

аппарата. Вычислительный блок представляет собой плату микроконтроллера, на котором запускается главный цикл рабочей программы летательного аппарата, выполняющий обработку поступающей информации и синтезирующий управляющий сигнал для исполнительных устройств. Базовая станция В качестве пульта управления выступает программное приложение, реализованное на мобильном устройстве, имеющем сенсорный экран для ввода информации, с модулем беспроводной связи стандарта IEEE 802.11n Wi-Fi. В качестве предустановленной операционной системы была выбрана Android OS из-за обширной базы мобильных устройств, работающих на ее основе. Язык разработки является Java 8. Такой выбор был произведен в первую очередь из-за специализации Android-устройств для работы с приложениями, запущенными на Java Virtual Machine. Рис. 4. Структура базовой станции. Базовая станция (рис. 4), на которой установлено программное приложение комплекса, включает в себя три блока: 15

 блок графического интерфейса пользователя (GUI);  блок коммуникации;  блок обработки команд. Пользователь вводит команды с помощью графической реализации пульта управления типа "джойстик". В распоряжении оператора имеется два рычага управления: левый отвечает за вертикальное перемещение летательного аппарата по оси 𝑂𝑍 и за поворот вокруг этой оси (изменение угла рысканья); правый отвечает за перемещение в горизонтальной плоскости 𝑂𝑋𝑌. На уровне приложения введенные данные могут интерпретироваться в двух режимах: 1. режим задания определенных углов ориентации; 2. режим преследования мнимой точки. При работе в первом режиме приложение в качестве управляющей команды, заданной пользователем, передает бортовой части желаемые углы ориентации 𝜑𝑑 , 𝜃𝑑 , 𝜓𝑑 и желаемую вертикальную скорость 𝑧𝑑 . При переключении во второй режим приложение высчитывает координаты мнимой точки 𝐷(𝑥𝑑 , 𝑦𝑑 , 𝑧𝑑 ) (в подвижной системе координат). Под мнимой точкой подразумевается точка в пространстве, в которую необходимо привести центра масс квадрокоптера. В данном случае управляющая команда будет компоноваться на основе желаемого угла рысканья 𝜓𝑑 и координат 𝑥𝑑 , 𝑦𝑑 , 𝑧𝑑 . 2.2. Программная архитектура комплекса Как и в случае аппаратной архитектуры, программная составляющая делится на две основные части: бортовое программное обеспечение и приложение базовой станции. Бортовая часть Основная программа летательного аппарата выполняется в ходе 16

срабатывания специального таймера, который имеет определенную частоту, зависящую, в свою очередь, от частоты работы бортового микроконтроллера. Программа бортовой части должна выполнять следующие функции:  проводить инициализацию блока исполнительных устройств при запуске летательного аппарата;  обеспечивать постоянное взаимодействие с периферийными модулями такими, как датчики, беспроводной интерфейс;  осуществлять непрерывное дистанционное соединение и синхронизацию с пультом управления базовой станции;  поддерживать алгоритмы для аварийных ситуаций (потеря сигнала пульта управления, сильный разряд бортовых батарей, избежание столкновения с боковыми препятствиями на низких скоростях, необходимость экстренного отключения двигателей);  при необходимости осуществлять фильтрацию информации, получаемой с периферийных датчиков;  реализовывать алгоритмы управления, рассмотрение которых происходит в параграфе 2.3;  предоставлять возможность настройки подключенного оборудования; Далее рассмотрим программные решения для выполнения описанных выше функций. Инициализация блока исполнительных устройств при запуске летательного аппарата. При старте программы необходимо выполнить набор низкоуровневых команд для инициализации электронных регуляторов хода. Это необходимо для передачи информации о нижней и верхней границах последующих управляющих сигналов. Взаимодействие с периферийными модулями. Как правило, производители оборудования внешних компонентов предлагают вместе с устройством библиотеки драйверов для работы с ними. Таким образом, для 17

получения информации с соответствующего датчика или отправки данных через беспроводной интерфейс достаточно воспользоваться соответствующими функциями из прилагаемых библиотек. Обычно опрос внешних сенсоров происходит в параллельном потоке выполнения программы, который имеет меньшую частоту таймера. Это позволяет эффективнее использовать ресурсы микроконтроллера, не нагружая его дополнительными вычислениями. Непрерывное дистанционное соединение и синхронизация с пультом управления базовой станции осуществляется при помощи беспроводного интерфейса. Данный модуль позволяет создавать новую беспроводную сеть и выполняет роль точки доступа и сервера. Для создания канала связи между базовой станцией и летательным аппаратом необходимо: 1. начать работу основной программы бортовой части; 2. подождать запуска беспроводной точки доступа; 3. выполнить подключение базовой станции по Wi-Fi каналу к точке доступа в режиме клиента. Далее запускается дополнительный таймер в отдельном потоке, предназначенный для опроса беспроводного интерфейса. В ходе работы данной подпрограммы происходит считывание полученных от пульта управления командных сигналов из стека модуля и отправка накопленных данных телеметрии. В случае отсутствия команд от оператора посылается тривиальная команда для поддержки связи. После получения командных сигналов, необходимо произвести их интерпретацию. Структура полученного сообщения выглядит следующим образом: 𝑈 = 𝑛_𝑢𝑛𝑢𝑚 _𝑎𝑟𝑔𝑠, где 𝑛 — двухзначная длина сигнала в байтах , 𝑢𝑛𝑢𝑚 — трехзначный номер команды из списка интерпретатора, 𝑎𝑟𝑔𝑠 — список аргументов командного 18

сигнала. В качестве примера рассмотрим команду на изменение угловой скорости первого мотора: 𝑈 = 11_102_1_70 В приведенном сигнале 𝑛 = 11, 𝑢𝑛𝑢𝑚 = 102, 𝑎𝑟𝑔𝑠 = 1_70, где 1 — номер соответствующего электродвигателя, 70 — угловая скорость мотора в процентном соотношении от максимальной. Интерпретатор производит расшифровку полученного сигнала и передает необходимые команды в другие блоки программы. Алгоритмы для аварийных ситуаций. После получения команд с пульта управления производится анализ возможности их выполнения, который необходим в целях безопасности. Одним из проявлений работы данной функции выступает отказ движения в сторону препятствия, если оно в данный момент зафиксировано боковым инфракрасным датчиком. При генерации управления осуществляется схожая проверка, которая также включает в себя контроль безопасности по высоте. Он заключается в модификации желаемой координаты 𝑧𝑑 и выглядит следующим образом: 𝑧𝑑 = 𝑧𝑐 − ℎ, 𝑧𝑑 < 𝑧𝑐 − ℎ 𝑧𝑑 , 𝑧𝑑 > 𝑧𝑐 − ℎ В этом выражении 𝑧𝑐 — текущее положение по высоте в инерциальной системе отсчета, ℎ — высота над поверхностью. В аварийной ситуации, вызванной потерей сигнала с пульта управления, подается автоматическая команда на посадку летательного аппарата: 𝑥𝑑 = 𝑥𝑐 𝑦𝑑 = 𝑦𝑐 𝑧𝑑 = 0 где 𝑥𝑐 и 𝑦𝑐 — положение квадрокоптера в плоскости 𝑂𝑋𝑌 в момент потери связи, 𝑥𝑑 и 𝑦𝑑 — желаемые координаты. Особенностью этого способа решения внештатной ситуации является предположение, что под 19

летательным аппаратом находится земная поверхность, на которую возможна посадка. Схожим образом реализован алгоритм действий в ситуации сильного разряда бортовых батарей. Отличием выступает возможность оператора управлять движением квадрокоптера в горизонтальной плоскости 𝑂𝑋𝑌 (для пользователя блокируется управление по высоте): 𝑧𝑑 = 0. Необходимость экстренного отключения электродвигателей возникает при невозможности избежать столкновения с препятствием, находящимся на пути движения летательного аппарата, что призвано в первую очередь обезопасить живые существа, находящиеся поблизости от квадрокоптера. Данный механизм защиты может быть активирован либо по команде оператора, либо при обнаружении препятствия инфракрасными датчиками или ультразвуковым датчиком расстояния в опасной окрестности с условием, что скорость квадрокоптера превышает определенное значение. Фильтрация информации, получаемой с периферийных датчиков. Как правило, производители MEMS датчиков предоставляют встроенный функционал для фильтрации получаемых данных в своих устройствах такие, как комплементарный фильтр, DCM фильтр и другие. В настоящей работе рассматривается применение DCM фильтра [8] для оценки углов ориентации летательного аппарата. Блок-схема алгоритма его работы представлена на рис. 5. Как видно из схемы, фильтр может использовать показания магнитометра, который зачастую входит в состав инерциальных измерительных устройств. Достоинством данного метода оценки ориентации является компенсация дрейфа гироскопа с течением времени в сравнении с обычным комплементарным фильтром. 20

Рис. 5. Алгоритм работы DCM фильтра. Оценка текущих производится скоростей интегрированием в подвижной ускорений, системе полученных с координат помощью фильтрации данных акселерометра. Последнее необходимо для устранения шумов, присутствующих в показаниях датчика. Для выполнения этой задачи, проектируемый комплекс содержит модифицированный фильтр низких частот. Его работа описывается следующей формулой: 𝑅𝑛 = 𝑅𝑛 , 𝑅𝑛 − 𝑅𝑛−1 > 𝜀 𝑅𝑛−1 + 𝛼 𝑅𝑛 − 𝑅𝑛−1 , 𝑅𝑛 − 𝑅𝑛−1 ≤ 𝜀 , где 𝑅 — фильтрованная оценка по одной оси, 𝑅 — нефильтрованные данные сенсора, 𝛼 ∈ 0; 1 — коэффициент фильтрации, 𝜀 — пороговое значение для фильтрации. После фильтрации периферийных датчиков и предварительной под "компонентами обработки вектора данных с состояния" подразумеваются их оценки. Алгоритмы управления. Их программная реализация осуществляется в теле основного таймера, имеющего высший приоритет выполнения. Для данного блока программы предоставляются командные сигналы, передаваемые оператором, и информация с фильтров периферийных данных. Подробнее описание законов управления будет приведено в следующей 21

главе. Настройка подключенного оборудования. Некоторые периферийные датчики и электронные регуляторы хода предоставляют возможность программной настройки. Для удобства оператора в комплексе разрабатывается возможность удаленной отладки оборудования летательного аппарата при помощи консоли пульта управления. Базовая станция Как было упомянуто выше, пульт управления базовой станции комплекса представляет собой мобильное приложение. Рассмотрим выполняемые им функции:  непрерывное дистанционное соединение и синхронизация с бортовой частью комплекса после подключения;  предоставление оператору полной информации о получаемой телеметрии;  поддержка двух вариантов управления: режим задания определенных углов ориентации и режим преследования мнимой точки;  реализация команды на аварийное выключение электромоторов летательного аппарата;  консоль оператора для настройки комплекса. В целом алгоритм передачи данных имеет такую же структуру, как и бортовой блок коммуникации. Для начала работы комплекса необходимо, чтобы базовая станция была подключена к точке доступа бортового беспроводного интерфейса. После команды оператора на соединение приложение запускает блок подключения в режиме клиента в отдельном потоке-таймере. Частота данного таймера совпадает с аналогом, задействованным на борту летательного аппарата. Дальнейшее описание принципа работы интерпретатора не имеет смысла, так как он имеет 22

незначительные отличия от бортовой реализации. Вывод полученной телеметрии осуществляется на экран управления летательным аппаратом. Для значений текущих углов ориентации 𝜑, 𝜃, 𝜓, линейных скоростей 𝑥 , 𝑦, 𝑧 в неподвижной системе координат и высоты квадрокоптера ℎ относительно поверхности информация отображается в численных значениях. Кроме того, оператор получает доступ к информации о текущем уровне заряда бортовых батарей. В окне управления присутствует схематичная модель квадрокоптера, отображающая текущую ориентацию в инерциальной системе координат. При нажатии на эту область экрана происходит переключение между режимами управления. Отдельные кнопки служат для подключения (отключения) к летательному аппарату, вызова команды на аварийную остановку двигателей. Основные элементы на экране управления — это графическая реализация пульта управления типа "джойстик". Подобное интуитивно понятное решение призвано облегчить процесс управления квадрокоптером. При подаче команды оператором (например, изменение положение рычага управления) начинается компоновка командного сигнала. Она производится в полном соответствии со списком команд интерпретатора. Далее командный сигнал помещается в стек данных для отправки коммуникационного блока. Комплекс содержит консоль оператора в отдельном окне, которая позволяет осуществлять настройку оборудования и отправку информационных пакетов путем текстовых команд. Для удобства оператора разработана система справки для каждой команды. 2.3. Разработка законов управления Исходя из задачи приведения квадрокоптера в определенную точку пространства с заданным углом рысканья, необходимо разработать такой 23

закон управления, который переводил бы летательный аппарат из текущего положения в пространстве 𝑃𝑐 (𝑥𝑐 , 𝑦𝑐 , 𝑧𝑐 ) с углом рысканья 𝜓с в желаемое положение 𝑃𝑑 (𝑥𝑑 , 𝑦𝑑 , 𝑧𝑑 ) с углом рысканья 𝜓𝑑 . Математическая модель (1.2), полученная ранее, является нелинейной, что осложняет ее анализ. С другой стороны, это позволяет осуществлять построение закона управления рысканьем. Для синтеза закона управления используются следующие данные, полученные после фильтрации и предварительных вычислений:  скорости 𝑥 , 𝑦, 𝑧 ;  текущее положение 𝑥, 𝑦, 𝑧;  угловые скорости 𝜑, 𝜃 , 𝜓;  углы ориентации 𝜑, 𝜃, 𝜓. Управляющий сигнал для электромоторов подлежит ограничению из-за технических особенностей летательного аппарата. Угловая скорость для каждого исполнительного устройства лежит в диапазоне 0; 10000 об/мин. Стабилизация по углам наклона Для стабилизации по углам наклона и высоте летательного аппарата используются методы ПИД регулирования. Предполагается, что компоненты тензора инерции 𝑰 квадрокоптера приблизительно известны, тогда управляющие моменты по соответствующим углам вычисляются по следующим формулам: 𝜏𝜑 = 𝑘𝜑 ,𝑑 𝜑𝑑 − 𝜑 + 𝑘𝜑 ,𝑝 𝜑𝑑 − 𝜑 𝐼𝑦 𝜏𝜃 = 𝑘𝜃 ,𝑑 𝜃𝑑 − 𝜃 + 𝑘𝜃,𝑝 𝜃𝑑 − 𝜃 𝐼𝑥 , 𝜏𝜓 = 𝑘𝜓 ,𝑑 𝜓𝑑 − 𝜓 + 𝑘𝜓 ,𝑝 𝜓𝑑 − 𝜓 𝐼𝑧 (2.3.1) где 𝑘𝜑 ,𝑑 , 𝑘𝜑,𝑝 , 𝑘𝜃 ,𝑑 , 𝑘𝜃 ,𝑝 , 𝑘𝜓,𝑑 , 𝑘𝜓 ,𝑝 — постоянные вещественные числа, выбранные с учетом устойчивости положения равновесия замкнутой системы (1.2), (2.3.1). 24

В данном законе подразумевается, что общая тяга должна компенсировать силу притяжения: 𝑇 = 𝑚𝑔. Из уравнений (1.1) выводятся следующие выражения: 𝜔1 = 𝜔2 = 𝜔3 = 𝜔4 = 𝑇 4𝑘 𝑇 4𝑘 𝑇 4𝑘 𝑇 4𝑘 − − + + 𝜏𝜑 4𝑘𝑙 𝜏𝜑 4𝑘𝑙 𝜏𝜑 4𝑘𝑙 𝜏𝜑 4𝑘𝑙 + − − + 𝜏𝜃 4𝑘𝑙 𝜏𝜃 4𝑘𝑙 𝜏𝜃 4𝑘𝑙 𝜏𝜃 4𝑘𝑙 + − + − 𝜏𝜓 4𝑏 𝜏𝜓 4𝑏 (2.3.2) 𝜏𝜓 4𝑏 𝜏𝜓 4𝑏 Подставив вычисленные в (2.3.1) значения в (2.3.2), находятся необходимые угловые скорости для электромоторов летательного аппарата. Исходя из предположений математической модели (1.2) были установлены следующие ограничения: 𝜑𝑑 ∈ −0.5; 0.5 , 𝜃𝑑 ∈ −0.5; 0.5 (величины указаны в радианах). Достижение заданной точки пространства Закон управления для достижения квадрокоптером заданной точки 𝑃𝑑 (𝑥𝑑 , 𝑦𝑑 , 𝑧𝑑 ) с углом рысканья 𝜓𝑑 из текущего положения 𝑃𝑐 (𝑥𝑐 , 𝑦𝑐 , 𝑧𝑐 ) с углом рысканья 𝜓𝑐 базируется на следующем алгоритме: 1. вычисление желаемых ускорений 𝑥 , 𝑦, 𝑧 ; 2. вычисление желаемых углов ориентации 𝜑, 𝜃 , 𝜓 и общей тяги 𝑇; 3. использование закона стабилизации по углам наклона. Рассмотрим подробнее каждый шаг. Вычисление желаемых ускорений. На данном этапе происходит генерация таких ускорений 𝑥 , 𝑦, 𝑧, которые обеспечивают достижение необходимого положения 𝑃𝑑 в каждый момент времени 𝑡. Для их вычислений также применяется пропорционально-дифференциальный регулятор: 25

𝑥 = 𝑘𝑥,𝑑 𝑥𝑑 − 𝑥 + 𝑘𝑥,𝑝 𝑥𝑑 − 𝑥 𝑦 = 𝑘𝑦 ,𝑑 𝑦𝑑 − 𝑦 + 𝑘𝑦 ,𝑝 𝑦𝑑 − 𝑦 (2.3.3) 𝑧 = 𝑘𝑧,𝑑 𝑧𝑑 − 𝑧 + 𝑘𝑧,𝑝 𝑧𝑑 − 𝑧 где 𝑘𝑥,𝑑 , 𝑘𝑥,𝑝 , 𝑘𝑦,𝑑 , 𝑘𝑦,𝑝 , 𝑘𝑧,𝑑 , 𝑘𝑧,𝑝 — постоянные вещественные числа, выбранные с учетом устойчивости положения равновесия замкнутой системы (1.2), (2.3.3). Для ускорения достижения конечной точки и уменьшения перерегулирования (снижения времени на стабилизацию в заданном положении) приведенный метод нуждается в доработке из-за особенностей динамики летательного аппарата. Вычисление желаемых углов ориентации и общей тяги происходит на том условии, что при достижении летательным аппаратом полученных значений, будут обеспечены желаемые ускорения в данный момент времени. Из математической модели (1.2) следует: 𝐶𝜓 𝑑 𝑥 + 𝑆𝜓 𝑑 𝑦 𝑔 𝑆𝜓 𝑑 𝑥 − 𝐶𝜓 𝑑 𝑦 𝜃 = 𝑔 𝑇 = 𝑚 𝑧+𝑔 𝜑= Использование закона стабилизации по углам наклона выполняется для достижения найденных углов тангажа 𝜑 и крена 𝜃 , а также конечного угла рысканья 𝜓𝑑 . Разработанный алгоритм является решением задачи приведения квадрокоптера в заданную точку c определенным углом рысканья в каждый момент времени 𝑡. Астатический регулятор по положению в пространстве Для решения задачи подавления постоянного внешнего воздействия на 26

летательный аппарат такого, как ветер, была произведена модификация предыдущего закона управления. Уменьшение отклонения от заданного положения в пространстве достигается путем добавления в регулятор отвечающий (2.3.3), за вычисление желаемых ускорений, интегральной составляющей: x = kx,d xd − x + kx,p xd − x + kx,i xd − x y = ky,d yd − y + ky,p yd − y + ky,i yd − y z = kz,d zd − z + kz,p zd − z + kz,i zd − z (2.3.4) где 𝑘𝑥,𝑖 , 𝑘𝑦 ,𝑖 , 𝑘𝑧,𝑖 — постоянные вещественные числа, выбранные с учетом устойчивости положения равновесия замкнутой системы (1.2), (2.3.4). Подобное изменение позволяет с течением времени привести летательный аппарат в желаемое положение. Для стабилизации в текущей точке пространства достаточно задать ее, как необходимую для достижения. Однако, интегральная составляющая данного регулятора порождает проблему перерегулирования (наглядный пример будет продемонстрирован в следующей главе). Оптимизация сохранения полученного положительного решения воздействия выполняется интегральной при помощи составляющей регулятора (снижение отклонения от заданного положения с течением времени) и уменьшения ее недостатков. Рассмотрим работу алгоритма на примере управления движением по оси 𝑂𝑋: 1. при изменении коэффициент желаемого 𝑘𝑥,𝑖 интегральной суммы принимает положения 𝑥𝑑 значение 0, интегральный а вычисление 𝑥𝑑 − 𝑥 прекращается; 2. в момент достижения необходимого значения 𝑥 = 𝑥𝑑 значение 𝑖𝑛𝑖𝑡𝑖𝑎𝑙 параметра 𝑘𝑥,𝑖 меняется на исходное 𝑘𝑥,𝑖 , а интегральная сумма начинает вычисляться с текущего момента времени 𝑡. Подобный метод позволяет компенсировать воздействие внешних сил, действующих на квадрокоптер, с течением времени. 27

Глава 3. Реализация комплекса 3.1 Имитационное моделирование динамики движения летательного аппарата Для реализации способов управления летательным аппаратом, приведенных ранее (§ 2.1), используются разработанные законы управления? представленные в § 2.3. Процесс моделирования динамики движения квадрокоптера проводится на основе математической модели (1.2), рассмотренной выше, в среде MATLAB-Simulink. Схема Simulink-модели верхнего уровня приведена на рис. 6. Рис. 6. Схема Simulink-модели Блоки "1" и "2" реализуют алгоритмы управления (2.3.2), (2.3.3) и (2.3.4). В конкретный момент времени может работать только один из них 28

(зависит от текущего выбранного режима работы). Первый блок вычисляет управляющий сигнал для приведения квадрокоптера в определенную точку пространства (также содержит модификацию алгоритма: астатический регулятор по положению в пространстве). Второй блок использует закон управления для стабилизации по углам наклона для вычисления выходного сигнала. В третьем блоке содержится система дифференциальных уравнений, описывающая динамику летательного аппарата. Четвертый блок осуществляет вычисление измеряемой квадрокоптером информации. Пятый блок содержит константы, используемые в моделировании (табл. 1): Параметр: Значение: 𝑚 0.5 𝑔 9.81 𝐼𝑥 4.856 ∗ 10−3 𝐼𝑦 4.856 ∗ 10−3 𝐼𝑧 8.801 ∗ 10−3 𝐴𝑥 0.25 𝐴𝑦 0.25 𝐴𝑧 0.25 𝐾 3 ∗ 10−6 𝐿 0.25 𝐵 1.15 ∗ 10−7 Таблица 1. Параметры компьютерной модели. Время дискретизации компьютерного моделирования 𝑇𝑠𝑎𝑚𝑝𝑙𝑒 = 0.005с. Внешние воздействия на квадрокоптер задаются непосредственно в уравнениях динамики. Как видно из рис. 7 и 8, квадрокоптер является динамически неустойчивой системой при наличии даже незначительных внешних сил при отсутствии управления. Графики были получены при 29

моделировании движения квадрокоптера с постоянными оборотами моторов, заданными так, чтобы при отсутствии внешних возмущений компенсировать силу притяжения. Рис. 7. Динамика движения летательного аппарата без закона управления (положение центра масс). Рис. 8. Динамика движения летательного аппарата без закона управления (углы ориентации). Далее рассмотрим применение к данной модели регулятора для стабилизации по углам наклона (рис. 9) (2.3.2). В качестве желаемых значений углов приняты следующие: тангаж 𝜑𝑑 = 0, крен 𝜃𝑑 = 0, рысканье 𝜓𝑑 = 0. 30

Рис. 9. Углы ориентации летательного аппарата (регулятор по углам наклона). На графике видно, что при наличии внешнего воздействия регулятор обеспечивает положение равновесия замкнутой системы, отличное от желаемого. Отсутствие интегральной составляющей в данном регуляторе сказывается на небольшом отклонении от необходимого положения. Однако, без возможности устранить накопление ошибки, которое возникает из-за добавления интегрального слагаемого, применение последнего остается нежелательным. Далее рассмотрим графики, отражающие динамику движения летательного аппарата с применением закона управления для приведения квадрокоптера в определенную точку пространства (2.3.3) (рис. 10 и 11): 31

Рис. 10. Положение центра масс летательного аппарата с использованием регулятора для приведения в определенную точку пространства. Рис. 11. Углы ориентации летательного аппарата с использованием регулятора для приведения в определенную точку пространства. В качестве сигнала оператора выступает следующая последовательность команд: при времени симуляции 𝑡 = 1с задается координата 𝑧𝑑 = 3м, при 𝑡 = 2с задается 𝑥𝑑 = 10м, при 𝑡 = 3с задается 𝑦𝑑 = 10м. В конечном итоге квадрокоптер должен стабилизироваться в точке 𝑃𝑑 10, 10, 3 . Согласно графику положения центра масс летательного аппарата (рис. 10), регулятор выполняет возложенную на него функцию, и весь переходный процесс с учетом хронологических сдвигов занимает около 9 секунд. Для следующей симуляции увеличим воздействие внешнего возмущения. Пусть на квадрокоптер воздействует постоянный южный ветер с силой 𝐹𝑒 = 1.5Н. Также в момент времени 𝑡 = 4с задается желаемый угол рысканья 𝜓𝑑 = 0.3рад. Ниже приведены полученные графики (рис. 12 и 13): 32

Рис. 12. Положение центра масс летательного аппарата с использованием регулятора для приведения в определенную точку пространства (увеличенное внешнее возмущение). Рис. 13. Углы ориентации летательного аппарата с использованием регулятора для приведения в определенную точку пространства (увеличенное внешнее возмущение). Регулятор осуществляет стабилизацию по заданному углу рысканья и выводит центр масс летательного аппарата на заданную позицию, но из-за внешнего воздействия возникает сильное отклонение по координате 𝑥: 𝑥𝑒𝑟𝑟𝑜𝑟 = 0.6м. Не изменяя начальных условий, рассмотрим применение астатического регулятора (2.3.4), являющегося модификацией предыдущего 33

алгоритма. На рис. 14 и 15 представлены графики динамики движения летательного аппарата: Рис. 14. Положение центра масс летательного аппарата с использованием астатического регулятора (увеличенное внешнее возмущение). Рис. 15. Углы ориентации летательного аппарата с использованием астатического регулятора (увеличенное внешнее возмущение). Разработанный закон управления позволяет компенсировать отклонение 𝑥𝑒𝑟𝑟𝑜𝑟 с течением времени. Таким образом, для заданных параметров переходный процесс занимает около 15 секунд. 34

3.2 Практическая реализация комплекса Базовая станция может быть воспроизведена на любом мобильном устройстве, удовлетворяющем требованиям, описанным в §2.1, с установленным программным приложением пульта управления. Окно управления с графическим интерфейсом пользователя показано на рис. 16: Рис. 16. Графический интерфейс окна управления летательным аппаратом. Интерфейс содержит графическую реализацию двух рычагов управления. Вывод телеметрии осуществляется в текстовом виде, а также для этого используется 3d-модель, визуализирующая текущую ориентацию квадрокоптера (на рисунке представлена синей гранью трехмерной модели куба). Присутствуют кнопки дистанционного соединения с летательным аппаратом и аварийной остановки электромоторов. Переключение между режимами управления происходит при нажатии на индикатор уровня заряда батарей. Отладка комплекса производится при помощи консоли оператора, показанной на рис. 17. Для удобства пользователя реализована система 35

справки, содержащая сведения о доступных командах. Рис. 17. Консоль оператора. Сборка летательного аппарата производилась с учетом предъявляемых к комплексу требований. Основные компоненты бортовой части представлены в табл. 2. Вычислительный блок Плата микроконтроллера Stm32f407 Discovery Блок коммуникации Беспроводной интерфейс WIFI232-A Блок исполнительных устройств Электромотор 4х NTM 2830-1100KV Электронный регулятор хода 4х Turnigy Multistar 30 Amp Multirotor Brushless ESC 2-4S Блок датчиков Инерциальное измерительное MPU9255 устройство 36

Инфракрасный датчик препятствий LM393 Ультразвуковой датчик расстояния HC-SR04 Датчик атмосферного давления BMP180 (барометр) Таблица 2. Компоненты летательного аппарата. Бортовое программное обеспечение реализовано на языке программирования C++ 11 и содержит основной функционал, описанный в параграфе 2.2. В настоящий момент практическая реализация летательного аппарата находится на стадии предполетного тестирования. 37

Выводы В ходе настоящей научно-исследовательской работы были получены следующие результаты, которые выносятся на защиту:  Разработана логика и структура работы аппаратно-программного комплекса для дистанционного управления движением квадрокоптера.  В составе комплекса управления для определенными разработаны стабилизации углами бортовые летательного ориентации; алгоритмы аппарата для с приведения летательного аппарата в заданную точку пространства с заданным углом рысканья; для компенсации внешнего воздействия на положение центра масс летательного аппарата в пространстве. Произведено имитационное моделирование динамики движения летательного аппарата с применением разработанных законов управления. К преимуществам комплекса стоит отнести применение в управлении астатического регулятора и реализацию аварийных алгоритмов. Высокая производительность бортовой аппаратной части позволяет использовать данное решение в дальнейших исследованиях с применением фото и видео устройств. Планируется возможность применения других стандартов беспроводной связи для увеличения доступного радиуса удаления бортовой части от базовой станции, а также использование датчиков геолокации для задачи автоматизированного полета квадрокоптера вне зоны обнаружения базовой станции. 38

Список литературы 1. Balas C. Modelling and linear control of a quadrotor. MSc thesis. Cranfield University, 2007. 150 с. 2. ArduPilot. http://ardupilot.org/ardupilot/index.html 3. DJI Phantom 4. http://www.dji.com/product/phantom-4 4. Teppo Luukkonen. Modelling and control of quadcopter. Espoo, 2011. 26 с. 5. D. Mellinger. Trajectory Generation and Control for Quadrotors. University of Pennsylvania, 2012. 137 с. 6. Датчик давления BMP180 (BMP085). http://robotclass.ru/tutorials/arduino-pressure-sensor-bmp180-bmp085 7. Speed of sound. https://en.wikipedia.org/wiki/Speed_of_sound 8. R. Mahony, Sung Han Cha, T. Hamel. A coupled estimation and control analysis for attitude stabilization of mini aerial vehicles // Australasian Conference on Robotics and Automation. Australian National University 2006. 10 c. 39

Приложение Блок реализации алгоритма управления для приведения квадрокоптера в точку: Блок вычисления желаемых ускорений (верхний уровень): 40

Блок вычисления желаемых ускорений (нижний уровень): Код блока "X_Integral_Processing": function [ value ] = X_Integral_coef_processing( EMP ) global param_value_X_Integral; global last_des_X; global int_sum_X; Des_X = EMP(1); Cur_Err = EMP(2); if last_des_X ~= Des_X last_des_X = Des_X; param_value_X_Integral = sign(Cur_Err); coef = 0; else if param_value_X_Integral ~= 0 if param_value_X_Integral ~= sign(Cur_Err) coef = 0.5; param_value_X_Integral = 0; int_sum_X = 0; else coef = 0; end else coef = 0.5; end end int_sum_X = int_sum_X + Cur_Err * 0.005; value = coef * int_sum_X; end Пример алгоритма интерпретации команды на стороне базовой станции: String parse(String input){ String temp = ""; Vector<String> vector_input; if (input.contains("\n")) { String[] rows = input.split("\n"); 41

for (String i : rows) { vector_input = new Vector<>(); if (i.contains(" ")) { String[] parts = i.split(" "); Collections.addAll(vector_input, parts); } else vector_input.add(i); if (vector_input.size() != 0) { if (vector_input.get(0).equals("help")) { if (hmRunnable_table.containsKey(vector_input.get(1))) temp += !temp.equals("") ? "\n" + hmRunnable_table.get(vector_input.get(1)).run(vector_input) : hmRunnable_table.get(vector_input.get(1)).run(vector_input); else temp += !temp.equals("") ? "\n" + "Unknown command: " + vector_input.get(1) : "Unknown command: " + vector_input.get(1); } else { if (hmRunnable_table.containsKey(vector_input.get(0))) temp += !temp.equals("") ? "\n" + hmRunnable_table.get(vector_input.get(0)).run(vector_input) : hmRunnable_table.get(vector_input.get(0)).run(vector_input); else temp += !temp.equals("") ? "\n" + "Unknown command: " + vector_input.get(0) : "Unknown command: " + vector_input.get(0); } } } } else { vector_input = new Vector<>(); if (input.contains(" ")) { String[] parts = input.split(" "); Collections.addAll(vector_input, parts); } else { vector_input.add(input); } if (vector_input.size() != 0) { if (vector_input.get(0).equals("help")) { if (hmRunnable_table.containsKey(vector_input.get(1))) temp += !temp.equals("") ? "\n" + hmRunnable_table.get(vector_input.get(1)).run(vector_input) : hmRunnable_table.get(vector_input.get(1)).run(vector_input); else temp += !temp.equals("") ? "\n" + "Unknown command: " + vector_input.get(1) : "Unknown command: " + vector_input.get(1); } else { if (hmRunnable_table.containsKey(vector_input.get(0))) temp += !temp.equals("") ? "\n" + hmRunnable_table.get(vector_input.get(0)).run(vector_input) : hmRunnable_table.get(vector_input.get(0)).run(vector_input); else temp += !temp.equals("") ? "\n" + "Unknown command: " + vector_input.get(0) : "Unknown command: " + vector_input.get(0); } } } return temp; } 42

Рецензии:

Авторизуйтесь, чтобы добавить рецензию

- у работы пока нет рецензий -

Отзывы:

Авторизуйтесь, чтобы оставить отзыв