Идентификация параметров квадрокоптера

Никитин Денис Александрович

Идентификация параметров квадрокоптера

Целью данной работы является представление нового, базирующегося на кватернионах, параметрического регулятора для управления летательным аппаратом -- квадрокоптером, и системы адаптации для идентификации параметров квадрокоптера. Предложен алгоритм, основанный на методе скоростного градиента, позволяющий во время полета определить коэффициенты силы тяги каждого винта робота. Полученные результаты проверены с помощью моделирования в среде Matlab.

Математика

Дипломы

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

ID: 587d365b5f1be77c40d58d94

UUID: 5fb14404-808e-4572-8b33-d7ea1dece9bb

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

Опубликовано: почти 8 лет назад

Просмотры: 62

Никитин Денис Александрович

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

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

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

Скачать - 2012803 bytes

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

Санкт-Петербургский государственный университет Математико-механический факультет Кафедра теоретической кибернетики Никитин Денис Александрович Идентификация параметров квадрокоптера Бакалаврская работа Научный руководитель: д. т. н., проф. А. Л. Фрадков Рецензент: д. ф.-м. н., проф. А. С. Матвеев Санкт-Петербург 2016

SAINT-PETERSBURG STATE UNIVERCITY Mathematics and Mechanics Theoretical Cybernetics Denis Nikitin Identification of Quadrotor Parameters Bachelor’s Thesis Scientific supervisor: Dr. Sc. Eng., Prof. Аlexander L. Fradkov Reviewer: Dr. Sc. Phys.Math., Prof. Аlexey S. Matveev Saint-Petersburg 2016

Оглавление Введение . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1. Вывод модели 1.1. Ориентация квадрокоптера . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.2. Модель динамики квадрокоптера . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.3. Упрощение модели . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2. Система стабилизации . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.1. Контроллер угловой стабилизации . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.2. Линеаризация и доказательство устойчивости . . . . . . . . . . . . . . . . . . . . . 11 2.3. Контроллер стабилизации высоты . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.4. Финальные формулы контроллера . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 3. Система адаптации 3.1. Оценка коэффициента тяги 𝐾 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 𝐾 на разных моторах . . . . . . . . . . . . . . . . . 15 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3.2. Случай разных коэффициентов 4. Моделирование Заключение Список литературы Приложение Код программы моделирования . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 28

Введение В последнее время в обществе всё большую роль начинают играть беспилотные летатель ные аппараты, в особенности самые доступные из них — квадрокоптеры. Их популярность в последние годы обеспечена простотой конструк ции и появлением большого количества различ ных систем управления, многие из которых яв ляются open-source программами. Существует множество задач, которые можно решать с помощью квадрокоптеров, в том числе задач научных. К примеру, работы [1], [2], [3] демонстрируют обучение роботов эффек тивным траекториям полета, работы [4] и [5] — кооперативному поведению, а [6], [7] и [8] — навигации при помощи камер и RGBD-сенсоров. Оценка ориентации подобных машин обычно осуществляется модификациями комплементарного фильтра ([9] и [10]) или же расширенным фильтром Калмана ([11]). Для управления квадрокоптером обзор основных существующих решений можно найти в статье [14]. Однако, можно выделить два общих свойства, присущие этим системам: они все работают на PID-регуляторах, и они все используют углы Эйлера в качестве ошибки, которую стремится минимизировать регулятор. Проблема использования углов Эйлера заключается в том, что этот способ имеет сингуляр ность, а значит, в некоторых областях пространства ориентаций не будет работать. К примеру, ни одна из общедоступных систем не способна сделать полностью контролируемый flip (сальто, переворот), не отключая при этом основной регулятор. В отличие от углов Эйлера, кватерни оны не имеют подобных проблем, хотя их редко используют для создания систем управления квадрокоптерами. Примером системы, полностью основанной на кватернионах, может являть ся [15]. Проблема же PID-регулятора в том, что для каждого конкретного робота нужно заново подбирать коэффициенты. К open-source продуктам прилагаются специальные инструкции о том, как это делать, однако всё равно, такой подход снижает эффективность системы, так как не позволяет достичь оптимальных параметров. Примеры синтеза PID-регулятора для квадро коптера можно найти в работах [16], [17], [18]. 4

В связи с этими проблемами была создана своя система стабилизации, использующая ква тернионы в качестве ошибки регулятора, и идею, похожую на линеаризацию обратной связью, для синтеза самого регулятора. В результате получился регулятор, большинство параметров которого являются в точности физическими параметрами конкретного квадрокоптера, а остав шиеся коэффициенты не зависят от робота и могут быть подобраны единожды для всего класса машин. Также, некоторые из физических параметров конкретного робота часто бывает сложно измерить, или же они могут меняться со временем прямо в полете. Поэтому особенно большую ценность приобретает система, позволяющяя идентифицировать неизвестные параметры и ис пользовать их в системе стабилизации. Существующие же системы идентификации, к примеру [12] и [13], полагаются на стандартные PID-регуляторы и углы Эйлера. Данная работа состоит из четырёх частей. В первой описан вывод уравнений динамики квадрокоптера. Вторая часть посвящена системе стабилизации и доказательству её устойчи вости в малом. В третьей части приводится система адаптации и идентификации некоторых параметров квадрокоптера, ключевых для устойчивого полета. Последняя часть описывает про веденное моделирование общей системы. Робот Бонд, контроллер ТРИК Робот Квадракон, контроллер FlyMaple Рис. 1. Тестовые самодельные квадрокоптеры Надо также сказать, что система стабилизации была проверена на двух настоящих, само дельных коптерах, один из которых работал на контроллере FlyMaple, второй — на контроллере ТРИК. Разработанная в данной работе система адаптации призвана решить некоторые пробле мы, имевшиеся у данных роботов. 5

1. Вывод модели 1.1. Ориентация квадрокоптера В трехмерном пространстве ориентация может быть описана несколькими способами, но одним из самых удобных является запись через кватернионы. Пусть у нас есть абсолютная система координат единственная ось му вокруг оси поворот 𝑢 𝑋 ′𝑌 ′𝑍 ′ 𝑋𝑌 𝑍 , 𝑢 тогда для системы и единственный угол на угол 𝜑, 𝜑 𝑋 ′𝑌 ′𝑍 ′, связанной с квадрокоптером, существует такие, что, если повернуть штрихованную систе получится исходная система. Тогда кватернионом, описывающим относительно 𝑋𝑌 𝑍 , называется (︂ )︂ 𝜑 𝜑 𝜑 𝜑 𝑞 = cos , 𝑢𝑥 sin , 𝑢𝑦 sin , 𝑢𝑧 sin . 2 2 2 2 Заметим, что ‖𝑞‖ = 1 для любых Для его представления 𝑣 𝑢 в системе 𝜑. и 𝑋𝑌 𝑍 Пусть теперь нам задан вектор 𝑣′ в системе 𝑋 ′𝑌 ′𝑍 ′. будет выполнено равенство 𝑉 = 𝑞 * 𝑉 ′ * 𝑞*, где 𝑉 и 𝑉 ′ – кватернионные представления векторов 𝑣 и 𝑣 ′ , звездочка обозначает кватернионное произведение, а 𝑞* Если система – сопряженный к 𝑋 ′𝑌 ′𝑍 ′ 𝑞 кватернион. вращается относительно нения кватерниона поворота 𝑞 𝑋𝑌 𝑍 с угловой скоростью будет следующим: 1 𝑞˙ = 𝑞 * Ω, 2 где Ω 𝜔 , то закон изме – кватернионное представление 𝜔. В данном уравнении (1) 𝜔 скорости в системе, связанной с квадрокоптером. Таким образом, – это координаты угловой 𝑞 и 𝜔 могут стать частью вектора состояния динамической системы, описывающей квадрокоптер. Кроме того, именно эту угловую скорость мы можем измерять при помощи гироскопов, установленных на робота, и использовать её для оценки 𝑞. Вывод уравнения (1), а также дополнительные сведения о кватернионной арифметике можно посмотреть в [19]. 6

1.2. Модель динамики квадрокоптера Через уравнение Ньютона-Эйлера можно описать динамику вращательного движения квад рокоптера в его собственной системе отсчета. Уравнение в общем виде выглядит так: 𝐼 𝜔˙ = 𝑀 − 𝜔 × 𝐼𝜔, где 𝐼 𝑀 – тензор моментов инерции, а – момент сил. Вывод уравнения можно посмотреть в [20]. 𝑌′ 𝑋′ 𝐹2 𝐹1 𝐹3 𝐹4 Перейдем к описанию моментов сил, действующих на квадрокоптер. Предположим, что робот имеет форму креста, оси 𝑋′ и 𝑌′ от центра масс до винтов обозначим за 𝐿(𝐹2 − 𝐹4 ) и 𝐿(𝐹3 − 𝐹1 ) направлены вдоль лучей, а ось 𝐿. Тогда моменты сил вдоль осей соответственно. Здесь 𝐹𝑖 𝐶. Тогда 𝐹𝑖 = 𝐶𝑣𝑖2 . Вращающий момент скорости винта, а значит, пропорционален силе тяги ности 𝜉. 𝜉=𝑘 𝜌 𝑋′ 𝐹𝑖 . 𝑄𝑖 𝑣𝑖 . и 𝑌′ 𝑖-м будут равны винтом. Сила Обозначим коэффициент про также пропорционален квадрату Назовем коэффициент пропорциональ Он может быть приближенно вычислен по формуле √︃ где – наверх. Расстояние – сила тяги, создаваемая тяги винта квадратично зависит от скорости вращения винта порциональности 𝑍′ – плотность воздуха, ности”, приблизительно 𝐴 𝐶 = 2𝜌𝐴 √︃ 𝐹 𝑚𝑎𝑥 𝑘 , 2𝜌𝐴 𝑣 𝑚𝑎𝑥 – площадь диска вращения винта, 𝑘 ≈ 1.15, 𝐹 𝑚𝑎𝑥 и 𝑣 𝑚𝑎𝑥 𝑘 – коэффициент ”потерь мощ – максимальная тяга и максимальная скорость вращения винта. Данная формула носит приближенный характер, и полученный коэффициент требует уточнения. Для более детального изучения аэродинамики винтов см. [21]. Теперь мы можем записать полный момент сил, действующий на квадрокоптер. Согласно рис.1, винты 1 и 3 вращаются по часовой стрелке, винты 2 и 4 – против, поэтому соответству 7

ющие вращающие моменты 𝜉𝐹𝑖 будут входить в итоговый момент сил вдоль оси знаками: ⎛ ⎜ ⎜ 𝑀 =⎜ ⎝ 𝑍′ с разными ⎞ 𝐿(𝐹2 − 𝐹4 ) ⎟ ⎟ ⎟ 𝐿(𝐹3 − 𝐹1 ) ⎠ 𝜉(𝐹1 + 𝐹3 − 𝐹4 − 𝐹2 ) Запишем далее уравнения динамики поступательного движения робота. На него действу 4 ∑︀ ют две силы – сила тяжести 𝑚𝑔 , направленная вниз против оси 𝑍 , и суммарная сила тяги 𝐹𝑖 , 𝑖=1 ′ направленная вдоль оси 𝑍 . Определим вектор 𝑃 положения робота в абсолютной системе от счета. Тогда после поворота суммарной силы тяги при помощи кватерниона 𝑞, второй закон Ньютона дает уравнение ⎛ ⎞ ⎛ ⎞ 0 0 ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ * ⎜ ⎟ 𝑃¨ = 𝑞 * ⎜ ⎟ * 𝑞 − ⎜0⎟ 0 ⎝ ⎠ ⎝ ⎠ 1 (𝐹1 + 𝐹2 + 𝐹3 + 𝐹4 ) 𝑔 𝑚 Осталось еще написать уравнения динамики двигателей. Пусть управляющее напряжение на моторе держки – 𝑖 𝑘𝑡 . равно 𝑢𝑖 , коэффициент связи напряжения и скорости – 𝑘𝑣 , а характеристика за Тогда уравнение, описывающее разгон двигателя, выглядит так: 𝑣˙ 𝑖 = 𝑘𝑣 𝑢𝑖 − 𝑣𝑖 𝑘𝑡 Этим описание модели квадрокоптера завершено, мы можем выписать полную систему уравнений: 1 𝑞˙ = 𝑞 * 𝜔 2 ⎛ ⎜ ⎜ 𝐼 𝜔˙ = ⎜ ⎝ 𝐿𝐶(𝑣22 − 𝑣42 ) ⎞ 𝐿𝐶(𝑣32 ⎟ ⎟ ⎟ − 𝜔 × 𝐼𝜔 ⎠ − 𝑣12 ) 𝜉𝐶(𝑣12 + 𝑣32 − 𝑣42 − 𝑣22 ) ⎛ ⎞ ⎛ ⎞ 0 0 ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ * ⎜ ⎟ 𝑃¨ = 𝑞 * ⎜ ⎟ * 𝑞 − ⎜0⎟ 0 ⎝ ⎠ ⎝ ⎠ 𝐶 2 2 2 2 (𝑣 + 𝑣 + 𝑣 + 𝑣 ) 𝑔 2 3 4 𝑚 1 𝑣˙ 𝑖 = 𝑘𝑣 𝑢𝑖 − 𝑣𝑖 , 𝑘𝑡 𝑖 ∈ {1..4} 8

1.3. Упрощение модели Полученная модель существенно нелинейна, поэтому далее будет сделано несколько упро щающих предположений. В первую очередь, скорость реакции моторов обычно гораздо выше, чем скорость движения самого робота. Поэтому динамикой разгона моторов можно пренебречь. Это влечет за собой равенство 𝑣𝑖 = 𝑘𝑣 𝑢𝑖 . Подставив 𝑣𝑖 Назовем новый коэффициент пропорциональности в выражение для 𝐾 = 𝐶𝑘𝑣2 . входят во все выражения только с квадратом, сделаем замену 𝐹𝑖 , получим 𝐹𝑖 = 𝐶𝑘𝑣2 𝑢2𝑖 . Замечая, далее, что управления 𝑈𝑖 = 𝑢2𝑖 . Таким образом, 𝐹𝑖 = 𝐾𝑈𝑖 . Второе упрощение заключается в том, что мы не наблюдаем и не пытаемся управлять находящимися в горизонтальной плоскости компонентами вектора 𝑃 . Обозначим 𝐻 = 𝑃𝑧 . Если расписать кватернионное произведение в уравнении динамики поступательного движения, то видно, что 𝑍 -компонента повернутого вектора суммарной тяги, имеющего в свою очередь един ственную ненулевую компоненту вдоль оси тяги на коэффициент 𝑍 ′, может быть вычислена умножением суммарной 𝑘𝑚𝑜𝑑 = 1 − 2𝑞𝑥2 − 2𝑞𝑦2 . Упрощенная модель тогда есть 1 𝑞˙ = 𝑞 * 𝜔 2 ⎛ ⎜ ⎜ 𝐼 𝜔˙ = ⎜ ⎝ 𝐿𝐾(𝑈2 − 𝑈4 ) 𝐿𝐾(𝑈3 − 𝑈1 ) ⎞ ⎟ ⎟ ⎟ − 𝜔 × 𝐼𝜔 ⎠ 𝜉𝐾(𝑈1 + 𝑈3 − 𝑈4 − 𝑈2 ) (︀ )︀ ¨ = 𝐾 1 − 2𝑞 2 − 2𝑞 2 (𝑈1 + 𝑈2 + 𝑈3 + 𝑈4 ) − 𝑔 𝐻 𝑥 𝑦 𝑚 9 (2)

2. Система стабилизации 2.1. Контроллер угловой стабилизации Основная идея закона управления лежит в инверсии уравнений модели. Висение робота на месте означает совпадение систем отсчета терниона 𝑞, равному (1, 0, 0, 0). 𝑋𝑌 𝑍 и 𝑋 ′𝑌 ′𝑍 ′, что соответствует значению ква Определим тогда целевой кватернион управления. Для висения его значение будет 𝑞𝑑 , который будет целью (1, 0, 0, 0), также его можно использовать для дви жения в горизонтальной плоскости. Далее, определим желаемую производную кватерниона обеспечивать цель 𝑞 как 𝜏𝑑 . Это значение должно 𝑞 → 𝑞𝑑 . Естественным было бы определить его как 𝜏𝑑 = 𝑘𝑃 (𝑞𝑑 − 𝑞), где 𝑘𝑃 > 0 – коэффициент пропорционального регулятора. Однако, значение реальной производной ква терниона обязано быть ортогонально кватерниону, так как тот подчиняется закону Поэтому доопределим как проекцию: 𝜏𝑑 = 𝑘𝑃 (𝑞𝑑 − < 𝑞𝑑 , 𝑞 > 𝑞), R4 . Теперь из уравнения 𝜔𝑑 = 2 𝜏𝑑 * 𝑞 * . Благодаря тому, что ное произведение в скорость 𝜏𝑑 𝜏𝑑 = 21 𝑞 * 𝜔𝑑 𝜏𝑑 где < ·, · > альному регулятору, а 𝜌𝑑 = 𝑘𝐷 (𝜔𝑑 − 𝜔). ортогонален 𝑞, полученный кватернион будет как целевую 𝑘𝐷 > 0. 𝐼 , 𝐿, 𝜉 воздействие момента 𝜌𝑑 Данное уравнение соответствует дифференци Теперь мы можем рассчитать целевой момент сил значения констант означает скаляр можно определить целевую угловую иметь нулевую скалярную часть, то есть быть вектором. Теперь определим производную угловой скорости: ‖𝑞‖ = 1. 𝑀𝑑 и 𝐾, 𝑀𝑑 = 𝐼𝜌𝑑 +𝜔×𝐼𝜔 . Если у нас есть точные то правильной установкой команд 𝑈𝑖 на робота. Это даст нам точное совпадение мы можем обеспечить 𝜔˙ ≡ 𝜌𝑑 . Таким образом, система управления выглядит так: 𝜏𝑑 = 𝑘𝑃 (𝑞𝑑 − < 𝑞𝑑 , 𝑞 > 𝑞) 𝜔 𝑑 = 2 𝜏𝑑 * 𝑞 * 𝜌𝑑 = 𝑘𝐷 (𝜔𝑑 − 𝜔) (3) 𝑀𝑑 = 𝐼𝜌𝑑 + 𝜔 × 𝐼𝜔 𝜔˙ ≡ 𝜌𝑑 Заметим, что единственными параметрами регулятора тогда являются коэффициенты 𝑘𝑃 и 𝑘𝐷 , которые не зависят от физических параметров системы. То есть однажды найденные коэффи циенты смогут стабилизировать любой квадрокоптер. 10

Данный алгоритм дает нам 3 уравнения на управляющие воздействия 𝑈𝑖 , и нужно четвер тое уравнение, чтобы полностью определить управления. 2.2. Линеаризация и доказательство устойчивости Для простоты проведем линеаризацию системы только вокруг точки висения, 𝑞𝑑 = (1, 0, 0, 0). Аналогичные уравнения можно получить и для любой другой точки. Кватернион поворота ся к виду 𝑞 ≈ (1, 𝑞𝑥 , 𝑞𝑦 , 𝑞𝑧 ). )︀ (︀ 𝑞 = cos 𝜑2 , 𝑢𝑥 sin 𝜑2 , 𝑢𝑦 sin 𝜑2 , 𝑢𝑧 sin 𝜑2 после линеаризации приводит Далее: 1 𝑞˙ ≈ (0, 𝜔𝑥 , 𝜔𝑦 , 𝜔𝑧 ) 2 𝜏𝑑 ≈ (0, −𝑘𝑃 𝑞𝑥 , −𝑘𝑃 𝑞𝑦 , −𝑘𝑃 𝑞𝑧 ) 𝜔𝑑 ≈ (−2𝑘𝑃 𝑞𝑥 , −2𝑘𝑃 𝑞𝑦 , −2𝑘𝑃 𝑞𝑧 ) 𝜌𝑑 ≈ (−𝑘𝐷 (𝜔𝑥 + 2𝑘𝑃 𝑞𝑥 ), −𝑘𝐷 (𝜔𝑦 + 2𝑘𝑃 𝑞𝑦 ), −𝑘𝐷 (𝜔𝑧 + 2𝑘𝑃 𝑞𝑧 )) 𝜔˙ ≡ 𝜌𝑑 𝑞𝑖 и интересует. Кроме того, заметим, что, к примеру, 𝑞˙𝑥 Оставим из всей системы только переменные от 𝜔𝑥 и 𝑞𝑥 . 𝜔𝑖 , так как именно их устойчивость нас зависит только от 𝜔𝑥 , а 𝜔˙ 𝑥 зависит только То же самое верно и для других координат. Таким образом, можно сосредоточиться только на одной координате: ⎛ ⎝ 𝑞˙𝑥 𝜔˙ 𝑥 ⎞ ⎛ ⎠=⎝ 1 2 0 −2𝑘𝑃 𝑘𝐷 −𝑘𝐷 ⎞⎛ ⎠⎝ 𝑞𝑥 𝜔𝑥 ⎞ ⎠ Собственные числа матрицы этой системы выражаются таким образом: 𝜆1,2 = −𝑘𝐷 ± √ 2 −4𝑘 𝑘 𝑘𝐷 𝑃 𝐷 . 2 Тогда достаточным условием устойчивости здесь будет положительность коэффициентов 𝑘𝐷 , что, по предположению, выполнено. Этим доказана устойчивость в малом. 11 𝑘𝑃 и

2.3. Контроллер стабилизации высоты Пусть задана целевая высота 𝐻𝑑 , которую необходимо выдержать коптеру. Тогда применим PD-регулятор для стабилизации высоты, задав целевое ускорение: ˙ Υ𝑑 = 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻, где 𝐴𝑃 и 𝐴𝐷 (4) положительные. Из системы (2) получаем четвертое уравнение на управляющие воздействия: 4 ∑︁ 𝑈𝑖 = 𝑖=1 𝑚 (Υ𝑑 + 𝑔) 𝐾 𝑘𝑚𝑜𝑑 (5) 2.4. Финальные формулы контроллера Из (3) мы получаем целевой момент 𝑀𝑑 , определяющий три уравнения в контроллере. Четвертое уравнение определяется регулятором высоты (4), и целью там является обеспечение ускорения в системе коптера, равного ϒ𝑑 +𝑔 . Введем матрицу 𝑘𝑚𝑜𝑑 и ускорения с управляющими воздействиями 𝐺, связывающую целевые моменты 𝑈𝑖 : ⎞−1 ⎛ 1 1 0 − 2𝐾𝐿 −𝐾𝐿 4𝐾𝜉 ⎜ ⎟ ⎜ ⎜ ⎟ ⎜ 1 1 ⎜−𝐾𝐿 ⎜ 2𝐾𝐿 0 𝐾𝐿 0 ⎟ 0 − 4𝐾𝜉 ⎜ ⎟ ⎜ 𝐺=⎜ ⎟ =⎜ 1 1 ⎜ 𝐾𝜉 −𝐾𝜉 𝐾𝜉 −𝐾𝜉 ⎟ ⎜ 0 2𝐾𝐿 4𝐾𝜉 ⎝ ⎠ ⎝ 𝐾 𝐾 𝐾 𝐾 1 1 − 2𝐾𝐿 0 − 4𝐾𝜉 𝑚 𝑚 𝑚 𝑚 ⎛ 0 𝐾𝐿 0 ⎞ 𝑚 4𝐾 ⎟ 𝑚 ⎟ ⎟ 4𝐾 ⎟ 𝑚 ⎟ ⎟ 4𝐾 ⎠ 𝑚 4𝐾 И тогда управления рассчитываются просто из матричного умножения: ⎛ ⎞ ⎛ 1 1 𝑈 0 − 2𝐾𝐿 4𝐾𝜉 ⎜ 1⎟ ⎜ ⎜ ⎟ ⎜ 1 1 ⎜𝑈2 ⎟ ⎜ 2𝐾𝐿 0 − 4𝐾𝜉 ⎜ ⎟=⎜ ⎜ ⎟ ⎜ 1 1 ⎜𝑈3 ⎟ ⎜ 0 2𝐾𝐿 4𝐾𝜉 ⎝ ⎠ ⎝ 1 1 − 2𝐾𝐿 0 − 4𝐾𝜉 𝑈4 12 ⎞ ⎞⎛ 𝑚 𝑀𝑑𝑥 ⎟ 4𝐾 ⎟ ⎜ ⎜ ⎟ 𝑚 ⎟⎜ 𝑀 ⎟ ⎟ 𝑑𝑦 ⎟ 4𝐾 ⎟ ⎜ ⎜ ⎟ 𝑚 ⎟ ⎜ 𝑀𝑑𝑧 ⎟ ⎟ ⎜ ⎟ 4𝐾 ⎠ ⎝ Υ𝑑 + 𝑔 ⎠ 𝑚 4𝐾 𝑘𝑚𝑜𝑑 (6)

3. Система адаптации 3.1. Оценка коэффициента тяги 𝐾 В уравнениях модели неизвестными параметрами, зависящими от физической модели ко птера, являются расстояние от центра масс до винтов 𝐼𝑥 , 𝐼𝑦 и 𝐼𝑧 𝐿, масса коптера 𝑚, три момента инерции (мы считаем, что коптер собран так, что оси винтов совпадают с главными осями симметрии, поэтому тензор инерции имеет диагональный вид), коэффициент связи тяги и вра щательного момента 𝜉, коэффициент связи тяги и управления 𝐾. Заметим, что независимое 𝐿 определение всех параметров невозможно: мы можем, к примеру, увеличить в 2 раза затем уменьшить в 2 раза 𝑚 и 𝐾, и 𝜉, и динамика коптера не изменится. Два из неизвестных параметров, 𝑚 и 𝐿, очень легко могут быть достаточно точно изме рены на физической модели. Поэтому, чтобы избавиться от сложностей переопределенности, зафиксируем 𝑚 и 𝐿 и в дальнейшем будем считать их известными. Из оставшихся параметров особое внимание следует обратить на коэффициент 𝐾 , так как различие его значений в реальной модели и в системе стабилизации приведет, как дальше будет показано, к невыполнению цели управления 𝐻 → 𝐻𝑑 , тогда как незнание точных значений других параметров приведет нас лишь к уменьшению эффективности регулятора. Поэтому сосредоточимся на задаче определения коэффициента 𝐾 . В системе уравнений регулятора есть уравнение, зависящее только от него, и не зависящее от 𝐼 и 𝜉. Это уравнение (5). Пусть в каждый момент времени у нас будет оценка коэффициента значим за ˆ. 𝐾 𝐾, которую мы обо Тогда управление высоты определяется двумя уравнениями: 4 ∑︁ 𝑖=1 (︁ ˙ 𝑚 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 + 𝑔 𝑈𝑖 = ˆ 𝑘𝑚𝑜𝑑 𝐾 ¨ = 𝑘𝑚𝑜𝑑 𝐾 𝐻 𝑚 4 ∑︁ )︁ (7) 𝑈𝑖 − 𝑔 (8) 𝑖=1 Подставив (7) в (8) и обозначив новую переменную Ψ= 1 ^ , получим 𝐾 (︁ )︁ (︁ )︁ ¨ ˙ ˙ 𝐻 = 𝐾Ψ 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 + 𝑔 − 𝑔 = 𝐾Ψ 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 + 𝑔 (𝐾Ψ − 1) Видно, что при любом фиксированном положительном 13 ˆ 𝐾 уравнение устойчиво, но при (9) ˆ ̸= 𝐾 𝐾

не выполнено 𝐻 → 𝐻𝑑 . Вместо этого появляется добавка: 𝐻 → 𝐻𝑑 + 𝑔 (1 − 𝐾Ψ) 𝐴𝑃 Теперь можно сформулировать и доказать теорему о законе адаптации для коэффициента тяги 𝐾. Теорема. Пусть выполнено соотношение положительных коэффициентов регулятора вы соты 𝐴2𝐷 > 𝐴𝑃 . (10) ˆ˙ = −𝛾 𝐾 ˆ 2 Υ𝑑 (Υ𝑑 + 𝑔) , 𝐾 (11) Тогда, при помощи закона адаптации где Υ𝑑 определена по (4), оценка коэффициента тяги координата по высоте квадрокоптера — к цели ˆ 𝐾 сходится к истинному значению, а 𝐻𝑑 . Доказательство. Рассмотрим функцию )︁2 1 1 (︁ ˙ 𝑄(𝐻) = 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 + ∆ (𝐻 − 𝐻𝑑 )2 , 2 2 где ∆ = 2𝐴𝑃 𝐴2𝐷 − 𝐴2𝑃 . в 0 только при (12) По условию (10) эта функция положительно определена и обращается 𝐻 ≡ 𝐻𝑑 и 𝐻˙ ≡ 0. будет равносильно стремлению ˆ 𝐾 Кроме того, из (9) очевидно, что её стремление к нулю к истинному значению 𝐾 . Производная этой функции в силу системы равна (︁ )︁ (︁ )︁ ˙ ˙ ˙ ¨ 𝑄(𝐻) = 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 −𝐴𝑃 𝐻 − 𝐴𝐷 𝐻 + ∆(𝐻 − 𝐻𝑑 )𝐻˙ Подставив сюда выражение для ¨ 𝐻 из (9), получим (︁ )︁2 (︁ )︁ (︁ )︁ ˙ 𝑄(𝐻, Ψ) = −𝐴𝐷 𝐾Ψ 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻˙ − 𝐴𝑃 𝐻˙ + 𝑔𝐴𝐷 (𝐾Ψ − 1) 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻˙ + Δ(𝐻 − 𝐻𝑑 )𝐻˙ (13) Теперь для оценки Ψ мы можем применить метод скоростного градиента в дифференциальной форме (см. [22]). Для этого достаточно лишь проверить условия. Условие регулярности, очевид но, выполнено, как и условие роста так как функция ˙ 𝑄(𝐻, Ψ) 𝑄(𝐻) → +∞ линейна по Ψ. при 𝐻 → ∞. Условие выпуклости выполнено, Условие достижимости цели следует из того, что при 14

Ψ≡ 1 , то есть при идеально подобранной оценке тяги выполнено 𝐾 (︁ )︁2 (︁ )︁ ˙ ˙ ˙ ˙ 𝑄(𝐻) = −𝐴𝐷 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 − 𝐴𝑃 𝐻 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 + ∆(𝐻 − 𝐻𝑑 )𝐻˙ Раскроем скобки и подставим ∆, получим ˙ 𝑄(𝐻) = −𝐴𝐷 𝐴2𝑃 (𝐻𝑑 − 𝐻)2 − (𝐴3𝐷 − 𝐴𝑃 𝐴𝐷 )𝐻˙ 2 Из положительности коэффициентов регулятора высоты и условия (10) следует отрицательная определенность ˙ 𝑄(𝐻) , откуда, для квадратичных форм, следует ˙ 𝑄(𝐻) 6 −𝜌𝑄(𝐻) для некоторого 𝜌, что и является условием достижимости цели. В итоге, беря градиент в (13), мы можем применить метод скоростного градиента для адаптации Ψ: (︂ (︁ )︁2 (︁ )︁)︂ ˆ˙ 𝐾 ˙ ˙ = 𝛾 𝐴𝐷 𝐾 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 + 𝑔𝐴𝐷 𝐾 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 Ψ̇ = − ˆ2 𝐾 Вспомним, что выражение 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻˙ Также, уберем в коэффициент адаптации для 𝛾 в системе управления (4) обозначалось как постоянные величины 𝐴𝐷 и Υ𝑑 . 𝐾 . Тогда, в итоге, получаем закон ˆ: 𝐾 ˆ˙ = −𝛾 𝐾 ˆ 2 Υ𝑑 (Υ𝑑 + 𝑔) , 𝐾 3.2. Случай разных коэффициентов 𝐾 на разных моторах В реальной машине, к сожалению, из-за небольших погрешностей в изготовлении винтов или двигателей, а также из-за возможных повреждений, коэффициенты 𝐾 для каждого винта будут немного отличаться. И эта разница оказывает существенное влияние, в первую очередь, на угловую стабилизацию, так как робота начинает кренить в одну из сторон, и не выполняется условие на кватернионы 𝑞 → 𝑞𝑑 . есть свой коэффициент тяги 𝐾𝑖 , Далее рассматривается случай, когда у каждого двигателя а также своя оценка этого коэффициента, замкнутую систему, которая получится в таком случае. 15 ˆ 𝑖. 𝐾 Рассмотрим

Уравнения модели можно записать так: ⎛ ⎞ ⎛ 0 𝐿𝐾2 0 −𝐿𝐾4 ⎞⎛ ⎞ 𝑈 ⎜ ⎟ ⎜ ⎟ ⎜ 1⎟ ⎜ ⎟ ⎜ ⎟ ⎟ ⎜ ⎜𝐼 𝜔˙ + 𝜔 × 𝐼𝜔 ⎟ ⎜−𝐿𝐾1 0 𝐿𝐾3 0 ⎟ ⎜𝑈2 ⎟ ⎜ ⎟ ⎜ ⎟⎜ ⎟, ⎜ ⎟=⎜ ⎜ ⎟ ⎜ ⎟ ⎜ 𝜉𝐾1 −𝜉𝐾2 𝜉𝐾3 −𝜉𝐾4 ⎟ ⎟ ⎜𝑈3 ⎟ ⎜ ⎟ ⎝ ⎠ ⎝ ⎠ ⎝ 𝐻 ¨ +𝑔 ⎠ 𝐾1 𝐾2 𝐾3 𝐾4 𝑈4 𝑚 𝑚 𝑚 𝑚 𝑘𝑚𝑜𝑑 (14) а уравнения регулятора (6) — так: ⎛ ⎛ ⎞ − 0 ⎜ 𝑈 ⎜ 1⎟ ⎜ 1 ⎜ ⎟ ⎜ ⎜𝑈2 ⎟ ⎜ ˆ 2𝐿 2𝐾 ⎜ ⎟=⎜ ⎜ ⎟ ⎜ ⎜𝑈3 ⎟ ⎜ 0 ⎝ ⎠ ⎜ ⎜ ⎝ 1 𝑈4 − ˆ 4𝐿 2𝐾 1 ˆ 1𝐿 2𝐾 0 1 ˆ 3𝐿 2𝐾 0 1 ˆ 1𝜉 4𝐾 1 − 4̂𝐾2 𝜉 1 ˆ 3𝜉 4𝐾 1 − ˆ 4𝜉 4𝐾 𝑚 ⎞⎛ ⎞ ˆ1 ⎟ 4𝐾 ⎜ 𝑀𝑑𝑥 ⎟ 𝑚 ⎟ ⎟⎜ ⎟ ⎟ ⎟ ⎜ 𝑀 𝑑𝑦 ˆ2 ⎟ ⎜ ⎟ 4𝐾 ⎟ ⎜ 𝑚 ⎟ ⎟ ⎜ 𝑀𝑑𝑧 ⎟ ⎟ ⎜ ˆ3 ⎟ ⎝Υ + 𝑔 ⎟ 4𝐾 ⎠ 𝑑 𝑚 ⎠ 𝑘𝑚𝑜𝑑 ˆ4 4𝐾 Как и в предыдущем параграфе, для удобства обозначим (14) и выразив 𝑀𝑑 1 ˆ 𝐾𝑖 через Ψ𝑖 . (15) Подставив (15) в по алгоритму (3), получим ⎛ ⎛ ⎞ ⎞ ⎜ ⎜ ⎟ ⎟ ⎜ ⎜ ⎟ ⎟ ⎜𝐼 𝜔˙ + 𝜔 × 𝐼𝜔 ⎟ ⎜𝐼𝜌𝑑 + 𝜔 × 𝐼𝜔 ⎟ ⎜ ⎜ ⎟ ⎟ ⎜ ⎟=𝐵⎜ ⎟ ⎜ ⎜ ⎟ ⎟ ⎜ ⎜ ⎟ ⎟ ⎝ 𝐻 ⎝ Υ𝑑 + 𝑔 ⎠ ¨ +𝑔 ⎠ 𝑘𝑚𝑜𝑑 𝑘𝑚𝑜𝑑 где матрица ⎛ 1 (𝐾2 Ψ2 2 𝐵 определена как + 𝐾4 Ψ4 ) ⎜ 0 ⎜ 𝜉 ⎝− 2𝐿 (𝐾2 Ψ2 − 𝐾4 Ψ4 ) 1 (𝐾2 Ψ2 2𝐿𝑚 (16) − 𝐾4 Ψ4 ) ⎞ 0 𝐿 − 4𝜉 (𝐾2 Ψ2 − 𝐾4 Ψ4 ) 1 (𝐾1 Ψ1 + 𝐾3 Ψ3 ) 2 𝜉 − 2𝐿 (𝐾1 Ψ1 − 𝐾3 Ψ3 ) 1 − 2𝐿𝑚 (𝐾1 Ψ1 − 𝐾3 Ψ3 ) 𝐿 − 4𝜉 (𝐾1 Ψ1 − 𝐾3 Ψ3 ) 𝐿𝑚 (𝐾2 Ψ2 − 𝐾4 Ψ4 ) 4 − 𝐿𝑚 (𝐾1 Ψ1 − 𝐾3 Ψ3 ) 4 1 (𝐾1 Ψ1 + 𝐾2 Ψ2 + 𝐾3 Ψ3 + 𝐾4 Ψ4 ) 4 1 (𝐾1 Ψ1 − 𝐾2 Ψ2 + 𝐾3 Ψ3 − 𝐾4 Ψ4 ) 4𝜉𝑚 𝜉𝑚 (𝐾1 Ψ1 − 𝐾2 Ψ2 + 𝐾3 Ψ3 − 𝐾4 Ψ4 ) 4 1 (𝐾 1 Ψ1 + 𝐾2 Ψ2 + 𝐾3 Ψ3 + 𝐾4 Ψ4 ) 4 ⎟ ⎟ ⎠ Как можно было ожидать, в случае точного совпадения оценок коэффициентов с их реальными значениями, то есть в случае 𝐾𝑖 Ψ𝑖 ≡ 1, матрица 𝐵 совпадает с единичной. В (16) перейдем к линейной системе, отбросив члены старших порядков, расписав 16 𝜌𝑑 так,

как это было сделано в параграфе про линеаризацию, и раскрыв ⎛ ⎞ ⎛ Υ𝑑 : ⎞ 𝐼 𝜔˙ −𝐼𝑥 𝑘𝐷 𝜔𝑥 − 2𝐼𝑥 𝑘𝐷 𝑘𝑃 𝑞𝑥 ⎜ 𝑥 𝑥⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ 𝐼𝑦 𝜔˙ 𝑦 ⎟ ⎜ −𝐼𝑦 𝑘𝐷 𝜔𝑦 − 2𝐼𝑦 𝑘𝐷 𝑘𝑃 𝑞𝑦 ⎟ ⎜ ⎟=𝐵⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ 𝐼𝑧 𝜔˙ 𝑧 ⎟ ⎜ −𝐼𝑧 𝑘𝐷 𝜔𝑧 − 2𝐼𝑧 𝑘𝐷 𝑘𝑃 𝑞𝑧 ⎟ ⎠ ⎝ ⎠ ⎝ ¨ ˙ 𝐴𝑃 (𝐻𝑑 − 𝐻) − 𝐴𝐷 𝐻 + 𝑔 𝐻 +𝑔 (17) Теперь введем новые обозначения, чтобы перейти к полностью матричной записи задачи. Определим новый вектор состояния )︀𝑇 (︀ 𝑋 = 𝑋1𝑇 , 𝑋2𝑇 , 𝑇 𝑋1 = (2𝐼𝑥 𝑞𝑥 , 2𝐼𝑦 𝑞𝑦 , 2𝐼𝑧 𝑞𝑧 , 𝐻 − 𝐻𝑑 ) , и еще обозначим через вектор 𝑔˜ где (︁ 𝑋2 = 𝐼𝑥 𝜔𝑥 , 𝐼𝑦 𝜔𝑦 , 𝐼𝑧 𝜔𝑧 , 𝐻˙ )︁𝑇 , общее воздействие гравитации на систему: 𝑔˜ = (0, 0, 0, 𝑔)𝑇 Тогда система (17) может быть переписана как 𝑋˙ 1 = 𝑋2 (18) 𝑋˙ 2 = 𝐵(−𝑇1 𝑋1 − 𝑇2 𝑋2 + 𝑔˜) − 𝑔˜, где матрицы 𝑇1 и 𝑇2 — матрицы коэффициентов регуляторов: ⎛ 𝑘𝐷 𝑘𝑃 ⎜ ⎜ ⎜ 0 𝑇1 = ⎜ ⎜ ⎜ 0 ⎝ 0 0 0 𝑘𝐷 𝑘𝑃 0 0 𝑘𝐷 𝑘𝑃 0 0 0 ⎞ ⎟ ⎟ 0 ⎟ ⎟, ⎟ 0 ⎟ ⎠ 𝐴𝑃 ⎛ 𝑘𝐷 ⎜ ⎜ ⎜0 𝑇2 = ⎜ ⎜ ⎜0 ⎝ 0 0 0 𝑘𝐷 0 0 𝑘𝐷 0 0 0 ⎞ ⎟ ⎟ 0 ⎟ ⎟ ⎟ 0 ⎟ ⎠ 𝐴𝐷 Фактически, они диагональны, но ниже предполагается лишь их симметричность, положитель ная определенность и, ради удобства вычислений, перестановочность между собой. Теперь, чтобы применить метод скоростного градиента, определим функцию ˜ 𝑄(𝑋) = ‖𝑇1 𝑋1 + 𝑇2 𝑋2 ‖2 . (19) Фактически, значение этой функции равно норме вектора управления. Её стремление к нулю 17

равносильно стремлению 𝑋2 → −𝑇2−1 𝑇1 𝑋1 , что, в свою очередь, благодаря первому уравне нию системы (18) и положительной определенности матриц 𝑇1 и 𝑇2 , равносильно устойчивости системы (18). К сожалению, функция (19) сама по себе не подходит для применения метода скоростного градиента, так как не обладает свойством 𝑋2 = −𝑇2−1 𝑇1 𝑋1 , при котором ˜≡0 𝑄 ˜ → ∞ при 𝑋 → ∞: достаточно рассмотреть случай 𝑄 при любом 𝑋1 . Чтобы функция (19) удовлетворяла условию роста, её можно модифицировать добавлени ем еще одного квадратичного члена: ⎛ (︁ 𝑄(𝑋) = ‖𝑇1 𝑋1 + 𝑇2 𝑋2 ‖2 + 𝑋1𝑇 Γ𝑋1 = 𝑋1𝑇 𝑋2𝑇 где Γ )︁ ⎞⎛ ⎞ + Γ 𝑇1 𝑇2 𝑋 ⎝ ⎠ ⎝ 1⎠ , 𝑇1 𝑇2 𝑇22 𝑋2 𝑇12 (20) — некоторая симметричная положительно определенная матрица. Очевидно, что условие регулярности также выполнено. Возьмем производную в силу системы от функции (︁ ˙ 𝑄(𝑋, 𝐵) = 2 𝑋2𝑇 ⎞⎛ ⎞ ⎛ )︁ 𝑇 2 + Γ 𝑇1 𝑇2 𝑋 1 ⎠ ⎝ 1⎠ −𝑋1𝑇 𝑇1 𝐵 𝑇 − 𝑋2𝑇 𝑇2 𝐵 𝑇 + 𝑔 𝑇 (𝐵 𝑇 − 𝐼) ⎝ 𝑋2 𝑇1 𝑇2 𝑇22 Условие выпуклости функции очередь линейна по оценкам для этого введем матрицы 𝑊𝑔𝑖 𝑄(𝑋): (21) 𝑄˙ выполнено, так как она линейна по матрице 𝐵 , которая в свою Ψ𝑖 . 𝑊𝑖𝑗 Теперь для удобства разложим на слагаемые уравнение (21), — матрицы квадратичной формы между — матрицы квадратичной формы между 𝑔˜ и 𝑋𝑖 и 𝑋𝑗 , и матрицы 𝑋𝑖 : 𝑊11 = −2𝑇1 𝑇2 𝐵𝑇1 , 𝑊12 = 2𝑇12 + 2Γ − 2𝑇1 𝑇2 𝐵𝑇2 − 2𝑇1 𝐵 𝑇 𝑇22 , 𝑊22 = 2𝑇1 𝑇2 − 2𝑇22 𝐵𝑇2 , (22) 𝑊𝑔1 = 2(𝐵 𝑇 − 𝐼)𝑇1 𝑇2 , 𝑊𝑔2 = 2(𝐵 𝑇 − 𝐼)𝑇22 . Посмотрев на эти матрицы, можно заметить, что выбор вспомогательной матрицы не повлияет на финальный алгоритм, так как нет членов, в которых матрицы Γ никак 𝐵 и Γ встречались бы вместе. Осталось проверить последнее условие — условие достижимости цели. Для этого подставим 18

в функцию (21) и в уравнения (22) 𝐵 ≡ 𝐼, соответствующее идеально найденным оценкам Ψ𝑖 . Получим ⎛ (︁ ⎞⎛ ⎞ 𝑋 −Γ − ⎠ ⎝ 1 ⎠ = −𝑋 𝑇 𝑅𝑋 𝑋2 2𝑇23 − 2𝑇1 𝑇2 2𝑇1 𝑇22 2𝑇12 𝑇2 )︁ 𝑄˙ 𝐼 (𝑋) = − 𝑋1𝑇 𝑋2𝑇 ⎝ 2𝑇1 𝑇22 − 𝑇12 − Γ 𝑇12 Достаточно показать, что матрица этой квадратичной формы лена хоть для какого-либо значения Γ. Определим Γ = 𝑇12 𝑅 положительно опреде и попробуем найти неособое пре образование координат, превращающее эту матрицу в блочно-диагональную. Одним из таких преобразований может быть ⎛ 𝑅=⎝ 2𝑇12 𝑇2 2𝑇1 𝑇22 − 2𝑇12 ⎞⎛ ⎞ ⎛ ⎞⎛ −1 −1 𝑇 0 − 𝑇 𝑇𝑇 2𝑇 𝑇 0 ⎠⎝ 1 ⎠ ⎠ = ⎝ 1 1 2 ⎠⎝ 1 2 𝑇1 𝑇2−1 𝐼 0 2𝑇23 − 2𝑇1 𝑇2 2𝑇23 − 2𝑇1 𝑇2 0 𝐼 2𝑇1 𝑇22 2𝑇12 ⎞ откуда следует критерий положительной определенности матрицы 𝑅: 𝑇1 𝑇2−1 > 0, 𝑇23 − 𝑇1 𝑇2 > 0, который, учитывая положительность матриц 𝑇1 и 𝑇2 , преобразуется в условие 𝑇22 > 𝑇1 . (23) Итак, если для матриц коэффициентов выполнено условие (23), то адаптация коэффици ентов Ψ𝑖 𝑋 → 0. по методу скоростного градиента приведет к пределу Означает ли это, что оценки Ψ𝑖 𝑄(𝑋, 𝐵) → 0 и, следовательно, сойдутся к своим истинным значениям зательства этого факта рассмотрим систему (18). Так как точка 𝑋 =0 1 ? Для дока 𝐾𝑖 является устойчивой точкой равновесия для этой системы, должно быть выполнено равенство 𝑔˜ = 𝐵˜ 𝑔, которое означает, что матрица 𝐵 имеет собственный вектор 19 (0, 0, 0, 1)𝑇 с собственным числом 1.

Раскрыв это равенство как систему уравнений, получим 𝐾2 Ψ2 − 𝐾4 Ψ4 = 0, 𝐾1 Ψ1 − 𝐾3 Ψ3 = 0, 𝐾1 Ψ1 − 𝐾2 Ψ2 + 𝐾3 Ψ3 − 𝐾4 Ψ4 = 0, 𝐾1 Ψ1 + 𝐾2 Ψ2 + 𝐾3 Ψ3 + 𝐾4 Ψ4 = 4, откуда немедленно следует набор равенств 𝐾𝑖 Ψ𝑖 = 1 и, соответственно, Ψ𝑖 = 1 для всех 𝐾𝑖 𝑖 ∈ {1..4}. Введем вспомогательную функцию ции 𝑄˙ при 𝐽= 𝑍(𝑋, 𝐽), которая будет описывать производную функ 𝜕𝐵 , взятую с обратным знаком: 𝜕Ψ𝑖 ⎛ 𝑇1 𝑇2 𝐽𝑇2 + 𝑇1 𝐽 𝑇 𝑇22 −𝐽 𝑇 𝑇1 𝑇2 2𝑇1 𝑇2 𝐽𝑇1 (︁ )︁ ⎜ ⎜ 𝑍(𝑋, 𝐽) = 𝑋1𝑇 𝑋2𝑇 𝑔˜𝑇 ⎜𝑇2 𝐽 𝑇 𝑇1 𝑇2 + 𝑇22 𝐽𝑇1 ⎝ −𝑇1 𝑇2 𝐽 2𝑇22 𝐽𝑇2 −𝐽 −𝑇22 𝐽 𝑇 𝑇22 0 ⎞⎛ 𝑋1 ⎞ ⎟⎜ ⎟ ⎟⎜ ⎟ ⎟ ⎜𝑋2 ⎟ ⎠⎝ ⎠ 𝑔˜ (24) Адаптация по методу скоростного градиента может быть тогда записана так: (︂ )︂ ˆ˙ 𝑖 𝜕𝐵 𝐾 , Ψ̇𝑖 = − 2 = 𝛾𝑍 𝑋, ˆ 𝜕Ψ𝑖 𝐾 𝑖 Неизвестные, но постоянные значения настоящих коэффициентов тяги ствующую частную производную множитель 𝐾𝑖 входят в соответ 𝜕𝐵 линейно, и потому могут быть вынесены из функции 𝜕Ψ𝑖 𝑍 в 𝛾. В итоге, запишем финальный закон адаптации для коэффициентов тяги ˆ 𝑖: 𝐾 (︂ )︂ 𝜕𝐵 ˙ˆ 2 ˆ 𝑖 𝑍 𝑋, . 𝐾𝑖 = −𝛾 𝐾 𝜕Ψ𝑖 (25) Для полноты алгоритма приведем конкретные значения используемых производных 𝜕𝐵 , 𝜕Ψ𝑖 20

считая при этом коэффициенты ⎛ 𝐾𝑖 убранными в множитель ⎞ ⎛ 1 2 𝛾. 𝐿 − 4𝜉 𝐿𝑚 4 ⎞ 0 ⎟ ⎜ ⎟ ⎜ ⎜ 0 0 0 0 ⎟ 𝜕𝐵 ⎟ =⎜ 𝜉 𝜉𝑚 ⎟ 1 𝜕Ψ2 ⎜ ⎜− 2𝐿 0 − 4 ⎟ 4 ⎠ ⎝ 1 1 1 0 − 4𝜉𝑚 2𝐿𝑚 4 ⎛ ⎞ 1 𝐿 𝐿𝑚 0 − 4𝜉 4 ⎟ ⎜ 2 ⎜ ⎟ ⎜ 0 0 0 0 ⎟ 𝜕𝐵 ⎜ ⎟ = 𝜉𝑚 ⎟ 𝜉 1 𝜕Ψ4 ⎜ ⎜ 2𝐿 0 − 4 ⎟ 4 ⎝ ⎠ 1 1 1 − 2𝐿𝑚 0 − 4𝜉𝑚 4 0 0 0 0 ⎟ ⎜ ⎜ 1 𝐿 𝐿𝑚 ⎟ ⎜0 − 4𝜉 − 4 ⎟ 𝜕𝐵 2 ⎟ =⎜ 𝜉 𝜉𝑚 ⎟ 1 𝜕Ψ1 ⎜ ⎟ ⎜0 − 2𝐿 4 4 ⎠ ⎝ 1 1 1 0 − 2𝐿𝑚 4𝜉𝑚 4 ⎛ ⎞ 0 0 0 0 ⎜ ⎟ ⎜ 1 𝐿 𝐿𝑚 ⎟ ⎜0 2 ⎟ 𝜕𝐵 4𝜉 4 ⎟ =⎜ ⎜ 𝜉𝑚 ⎟ 1 𝜕Ψ3 ⎜0 𝜉 ⎟ 2𝐿 4 4 ⎠ ⎝ 1 1 1 0 2𝐿𝑚 4𝜉𝑚 4 21 (26)

4. Моделирование Моделирование алгоритма, представленного в прошлом параграфе, было проведено в си стеме MATLAB R2016a. Для этого была реализована модель коптера, система управления им и система адаптации. Физические параметры были взяты от одного из реальных изготовленных роботов, и они перечислены в следующей таблице: Физические параметры Параметры регуляторов Коэффициенты тяги 𝑚 = 1.3 kg 𝐿 = 0.22 m 𝜉 = 0.017 m 𝐼𝑥 = 0.12 kg*m2 𝐼𝑦 = 0.12 kg*m2 𝐼𝑧 = 0.22 kg*m2 𝑔 = 9.8 m/sec2 𝐾𝑃 = 2.6 𝐾𝐷 = 10.0 𝐴𝑃 = 2.0 𝐴𝐷 = 5.0 𝐻𝑑 = 3 m 𝐾1 𝐾2 𝐾3 𝐾4 = 12.6 Н = 18.6 Н = 4.6 Н = 7.0 Н Здесь же указаны примененные коэффициенты регуляторов, которые удовлетворяют усло вию (23), и истинные коэффициенты тяги, являющиеся физическими, но неизвестными нам параметрами коптера. В начальном положении робот считается покоящимся на горизонталь ной поверхности, его координата по высоте принята за 0. Для проверки алгоритма было проведено 3 эксперимента, в первом из них адаптации не было (𝛾 = 0), во втором 𝛾 = 0.0005, в третьем 𝛾 = 0.005. Начальные оценки коэффициентов ˆ𝑖 𝐾 равны 8.6 Н, время симуляции — 10 секунд. Ориентация, углы Эйлера Координата по высоте Рис. 2. Отсутствие адаптации, 𝐻 𝛾=0 В первом случае (Рис. 2), в отсутствие адаптации, робот входит в устойчивое движение, но не достигает цели управления, в частности, у него есть наклон по крену и тангажу, что в реальной системе привело бы к появлению ускорения в плоскости 22 𝑋𝑌 .

Ориентация, углы Эйлера Оценки коэффициентов Координата по высоте ˆ𝑖 𝐾 Величины Рис. 3. Медленная адаптация, 𝐻 𝐾𝑖 Ψ𝑖 𝛾 = 0.0005 В случае медленной адаптации (Рис. 3) состояние робота асимптотически достигает своих целевых значений, как и коэффициенты тяги. Также можно заметить, что при отсутствии адаптации, ошибка по крену и тангажу составила 10-15 градусов, тогда как при её наличии она не превысила 5, а через 10 секунд составляла уже 2 градуса. Ошибки такого порядка уже могут быть приемлемы в реальной системе. При быстрой адаптации (Рис. 4) коэффициенты сходятся к своим истинным значениям го раздо быстрее, а ошибка по крену и тангажу не превышает 1 градус. Однако можно заметить, что в начальный период коэффициенты, а вместе с ними и ориентация испытывают сильные ко лебания. Дальнейшее увеличение коэффициента неустойчивыми. 23 𝛾 приводит к тому, что вычисления становятся

Ориентация, углы Эйлера Оценки коэффициентов Координата по высоте ˆ𝑖 𝐾 Величины Рис. 4. Быстрая адаптация, 24 𝛾 = 0.005 𝐾𝑖 Ψ𝑖 𝐻

Заключение В данной работе был описан основанный на кватернионах параметрический регулятор для стабилизации квадрокоптера, а также система идентификации коэффициентов тяги на каждом винте. Также было проведено численное моделирование, подтверждающее работоспособность предложенных алгоритмов. Направлением дальнейшей работы, в первую очередь, является оценка остальных парамет ров модели, в особенности моментов инерций 𝐼𝑥 , 𝐼𝑦 и 𝐼𝑧 и коэффициента 𝜉. Также необходимо изучить, как будут вести себя предложенные алгоритмы при дискретизации. 25

Список литературы 1. Markus Hehn, Raffaello D’Andrea. Quadrocopter trajectory generation and control, IFAC world congress, Vol. 18. No. 1. 2011. 2. Markus Hehn, Raffaello D’Andrea. A frequency domain iterative learning algorithm for high performance, periodic quadrocopter maneuvers, in Mechatronics 24.8 (2014): 954-965. 3. Angela P. Schoellig, Clemens Wiltsche, and Raffaello D’Andrea. Feed-forward parameter identification for precise periodic quadrocopter motions. American Control Conference (ACC), 2012. IEEE, 2012. 4. Federico Augugliaro, Angela P. Schoellig, and Raffaello D’Andrea. Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach. Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on. IEEE, 2012. 5. Robin Ritz, Mark W. Muller, Markus Hehn, and Raffaello D’Andrea. Cooperative quadrocopter ball throwing and catching. Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on. IEEE, 2012. 6. Glenn P. Tournier, Mario Valenti, and Jonathan P. How. Estimation and control of a quadrotor vehicle using monocular vision and moire patterns. AIAA Guidance, Navigation and Control Conference and Exhibit. 2006. 7. Shaojie Shen, Yash Mulgaonkar, Nathan Michael, Vijay Kumar. Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor. Robotics: Science and Systems. Vol. 1. 2013. 8. Paolo Stegagno, Massimo Basile, Heinrich H. Bulthoff, Antonio Franchi. Vision-based Autonomous Control of a Quadrotor UAV using an Onboard RGB-D Camera and its Application to Haptic Teleoperation. 2nd IFAC Work. on Research, Education and Development of Unmanned Aerial Systems, Compiegne, France (2013). 9. Robert Mahony, Tarek Hamel, and Jean-Michel Pflimlin. Nonlinear complementary filters on the special orthogonal group. Automatic Control, IEEE Transactions on 53.5 (2008): 1203-1218. 10. Sebastian O.H. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Report x-io and University of Bristol (UK) (2010). 11. Ern J. Lefferts, F. Landis Markley, and Malcolm D. Shuster. Kalman filtering for spacecraft attitude estimation. Journal of Guidance, Control, and Dynamics 5.5 (1982): 417-429. 12. I. Carro Perez, D. Flores-Araiza: J. A. Fortoul-D?az, R. Maximo and H. G. Gonzalez-Hernandez. 26

Identification and PID control for a quadrocopter. Electronics, Communications and Computers (CONIELECOMP), 2014 International Conference on. IEEE, 2014. 13. Jiangcheng Zhu, Endong Liu, Shan Guo, Chao Xu. A gradient optimization based PID tuning approach on quadrotor. Control and Decision Conference (CCDC), 2015 27th Chinese. IEEE, 2015. 14. Hyon Lim, Jaemann Park, Daewon Lee, and H.J. Kim. Build your own quadrotor. Open-Source Projects on Unmanned Aerial Vehicles, in IEEE Robotics and Automation Magazine, Septermer 2012. 15. Emil Fresk and George Nikolakopoulos. Full Quaternion Based Attitude Control for a Quadrotor, in 2013 European Control Conference (ECC), July 17-19, 2013, Zurich, Switzerland. 16. Paul Pounds, Robert Mahony, Peter Corke. Modelling and Control of a Quad-Rotor Robot, Proceedings Australasian Conference on Robotics and Automation 2006. Australian Robotics and Automation Association Inc., 2006. 17. Robert Mahony, Vijay Kumar, and Peter Corke. Multirotor aerial vehicles. Modelling, Estimation, and Control of Quadrotor, in IEEE Robotics and Automation Magazine, Septermer 2012. 18. Shahida Khatoon, Muhammad Shahid, and Himanshu Chaudhary. Dynamic modeling and stabilization of quadrotor using PID controller. Advances in Computing, Communications and Informatics (ICACCI, 2014 International Conference on. IEEE, 2014. 19. В.В. Матвеев, В.Я. Распопов. Основы построения бесплатформенных инерциальных нави гационных систем.: ЦНИИ ”Электроприбор”, 2009. — 278 с. 20. Л.Д. Ландау, Е.М. Лифшиц. Теоретическая физика, том 1. Механика. 5-е издание. М.:ФИЗМАТЛИТ, 2013. — 224 с. 21. J.G.Leishman. Principles of Helicopter Aerodynamics, Second Edition. 2006. Cambridge Aerospace Series. 22. Мирошник И.В., Никифоров В.О., Фрадков А.Л. Нелинейное и адаптивное управление сложными динамическими системами. СПб.: Наука, 2000. — 549 с. 27

Приложение Код программы моделирования %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %−−−−COPTER−−CONTROL−−FUNCTION−−−−% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [ control ] = copter_control ( state , InertiaMatrix , H_desired , L, m, xi , . . . K1, K2, K3, K4, K_P, K_D, A_P, A_D ) ; g = 9.8; q_des = [1 0 0 0 ] ; %state = [qw, qx , qy , qz , wx, wy, wz , h , dot_h , hK1, hK2, hK3, hK4] ControlMatrix = [ . . . 0 − 1/(2 *L*K1) 1/(4 * xi *K1) m/(4 * K1) ; . . . 1/(2 * L*K2) 0 − 1/(4 * xi *K2) m/(4 * K2) ; . . . 0 1/(2 * L*K3) 1/(4 * xi *K3) m/(4 * K3) ; . . . − 1/(2 * L *K4) 0 − 1/(4 * xi *K4) m/(4 * K4 ) ] ; q = [ state (1) state (2) state (3) state ( 4 ) ] ; w = [ state (5) state (6) state ( 7 ) ] ; tau_des = K_P * (q_des − (q_des * q ’) * q ) ; w_des = 2 * quatmultiply ( tau_des , quatconj (q ) ) ; w_des = w_des ( 2 : 4 ) ; rho_des = K_D * (w_des − w) ; M_des = InertiaMatrix * rho_des ’ + cross (w’ , InertiaMatrix Yota_des = A_P * ( H_desired − state (8)) − A_D * state ( 9 ) ; control = ControlMatrix * [M_des; (Yota_des+g )/(1 − q(2)^2 * w’ ) ; − q (3)^2)]; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %−−−−COPTER−−MODEL−−FUNCTION−−−−% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [ dot_state ] = copter_model ( state , ModelMatrix , InertiaMatrix , L, m, xi ) g = 9.8; H_desired = 3; K_P = 2 . 6 ; K_D = 10; A_P = 2; A_D = 5; gamma = 0.005; %state = [qw, qx , qy , qz , wx, wy, wz , h , dot_h , hK1, hK2, hK3, hK4] 28

%%%−−−−−−−−CONTROL−−−−−−−−−−−−−−−−− hK1 = state (10); hK2 = state (11); hK3 = state (12); hK4 = state (13); control = copter_control ( state , InertiaMatrix , H_desired , L, m, xi , . . . hK1, hK2, hK3, hK4, K_P, K_D, A_P, A_D) ; %%%−−−−−−−−MODEL−−−−−−−−−−−−−−−−− q = [ state (1) state (2) state (3) state ( 4 ) ] ; w = [ state (5) state (6) state ( 7 ) ] ; dq = 0.5 * quatmultiply (q , [0 w] ) − ( norm (q) − 1) * q ; res_vector = ModelMatrix * control ; dw = InertiaMatrix \ ( res_vector (1:3) − cross (w’ , InertiaMatrix * w’ ) ) ; ddot_H = (1 − q(2)^2 − q(3)^2) * res_vector (4) − g ; %%%−−−−−−−−ADAPTATION−−−−−−−−−−−− T1 = [K_D*K_P 0 0 0; 0 K_D*K_P 0 0; 0 0 K_D*K_P 0; 0 0 0 A_P] ; T2 = [K_D 0 0 0; 0 K_D 0 0; 0 0 K_D 0; 0 0 0 A_D] ; X = [ q ( 2 : 4 ) ’ ; state (8) − H_desired ; w’ ; state ( 9 ) ; 0; 0; 0; g ] ; Z = @(J) X’ * [2 * T1*T2* J *T1, T1*T2* J *T2+T1* J ’ * T2^2, −J ’ * T1*T2; . . . T2* J ’ * T1*T2+T2^2* J *T1, 2 * T2^2* J *T2, −J ’ * T2^2; . . . −T1 * T2 * J , −T2^2 * J , zeros ( 4 ) ] * X; dB_dPsi1 = [0 0 0 0; 0 0.5 −L/(4 * xi ) −L*m/4; . . . 0 − xi /(2 * L) 0.25 xi *m/4; 0 − 1/(2 *L*m) 1/(4 * xi *m) 0 . 2 5 ] ; dB_dPsi2 = [ 0 . 5 0 −L/(4 * xi ) L*m/4; 0 0 0 0; . . . − xi /(2 * L) 0 0.25 − xi *m/4; 1/(2 * L *m) 0 − 1/(4 * xi *m) 0 . 2 5 ] ; dB_dPsi3 = [0 0 0 0; 0 0.5 L/(4 * xi ) L*m/4; . . . 0 xi /(2 * L) 0.25 xi *m/4; 0 1/(2 * L*m) 1/(4 * xi *m) 0 . 2 5 ] ; dB_dPsi4 = [ 0 . 5 0 L/(4 * xi ) −L*m/4; 0 0 0 0; . . . xi /(2 * L) 0 0.25 − xi *m/4; − 1/(2 *L*m) 0 − 1/(4 * xi *m) 0 . 2 5 ] ; dhK1 = −gamma * hK1^2 * Z(dB_dPsi1 ) ; dhK2 = −gamma * hK2^2 * Z(dB_dPsi2 ) ; dhK3 = −gamma * hK3^2 * Z(dB_dPsi3 ) ; dhK4 = −gamma * hK4^2 * Z(dB_dPsi4 ) ; %%%−−−−−−−−RESULT−−−−−−−−−−−−−−−−− dot_state = [ dq ’ ; dw; state ( 9 ) ; ddot_H ; dhK1; dhK2; dhK3; dhK4 ] ; end 29

%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%% %−−−−MAIN−−SCRIPT−−−−% %%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%% clear a l l ; close a l l ; L = 0.22; m = 1.3; xi = 0.017; Ix = 0.12; Iy = 0.12; Iz = 0.22; InertiaMatrix = [ Ix 0 0; 0 Iy 0; 0 0 Iz ] ; K1 = 12.6; K2 = 18.6; K3 = 4 . 6 ; K4 = 7; ModelMatrix = [ . . . 0 L*K2 0 −L*K4; . . . −L *K1 0 L *K3 0; . . . xi *K1 − xi *K2 xi *K3 − xi *K4; . . . K1/m K2/m K3/m K4/m] ; H_desired = 3; %state = [qw, qx , qy , qz , wx, wy, wz , h , dot_h , hK1, hK2, hK3, hK4] f = @( t , state ) ( copter_model ( state , ModelMatrix , InertiaMatrix , L, m, xi ) ) ; state_0 = [1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 8.6 , 8.6 , 8.6 , 8 . 6 ] ; [ t , state ] = ode45 ( f , [0 10] , state_0 ) ; ang = quat2eul ( state ( : , 1 : 4 ) , ’ZYX ’ ) * 180 / pi ; figure ; plot ( t , ang (: ,1) , ’o-’ ) ; hold on ; plot ( t , ang (: ,2) , ’o-’ ) ; plot ( t , ang (: ,3) , ’o-’ ) ; plot ( t , zeros ( s i z e ( t )) , ’k--’ ) ; xlabel ( ’seconds ’ ) ; ylabel ( ’degrees ’ ) ; legend ( ’Yaw ’ , ’Pitch ’ , ’Roll ’ ) ; figure ; plot ( t , state (: ,8) , ’go -’ ) ; hold on ; plot ( t , H_desired * ones ( s i z e ( t )) , ’k--’ ) ; ylim ([0 3 . 5 ] ) ; xlabel ( ’seconds ’ ) ; ylabel ( ’meters ’ ) ; legend ( ’Height ’ ) ; 30

figure ; plot ( t , state (: ,10) , ’o-’ ) ; hold on ; plot ( t , state (: ,11) , ’o-’ ) ; plot ( t , state (: ,12) , ’o-’ ) ; plot ( t , state (: ,13) , ’o-’ ) ; legend ( ’Estimate K_1 ’ , ’Estimate K_2 ’ , ’Estimate K_3 ’ , ’Estimate K_4 ’ ) ; plot ( t , K1 * ones ( s i z e ( t )) , ’b--’ ) ; plot ( t , K2 * ones ( s i z e ( t )) , ’r--’ ) ; plot ( t , K3 * ones ( s i z e ( t )) , ’g--’ ) ; plot ( t , K4 * ones ( s i z e ( t )) , ’m--’ ) ; xlabel ( ’seconds ’ ) ; figure ; plot ( t , K1 ./ state (: ,10) , ’*-’ ) ; hold on ; plot ( t , K2 ./ state (: ,11) , ’*-’ ) ; plot ( t , K3 ./ state (: ,12) , ’*-’ ) ; plot ( t , K4 ./ state (: ,13) , ’*-’ ) ; legend ( ’K_1\Psi_1 ’ , ’K_2\Psi_2 ’ , ’K_3\Psi_3 ’ , ’K_4\Psi_4 ’ ) ; plot ( t , ones ( s i z e ( t )) , ’k--’ ) ; xlabel ( ’seconds ’ ) ; 31

Рецензии:

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

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

Отзывы:

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