Адаптивная система управления квадрокоптером на основе кватернионной модели вращений
УДК 681.52 ББКЖ30
АДАПТИВНАЯ СИСТЕМА УПРАВЛЕНИЯ
КВАДРОКОПТЕРОМ НА ОСНОВЕ КВАТЕРНИОННОЙ МОДЕЛИ ВРАЩЕНИЙ1
Никитин Д. А.2
(Санкт-Петербургский государственный университет, Санкт-Петербург)
Предложен синтез системы стабилизации квадрокоптера, основанный на кватернионной модели вращений. Рассматривается адаптивная система идентификации коэффициентов тяги квадрокоптера на основе метода скоростного градиента. Приводятся доказательства устойчивости, а также примеры моделирования системы.
Ключевые слова: БПЛА, квадрокоптер, адаптивное управление, метод скоростного градиента, кватернионы.
Введение
В последнее время в обществе всё большую роль начинают играть беспилотные летательные аппараты, в особенности самые доступные из них – квадрокоптеры. Их популярность в последние годы обеспечена простотой конструкции и появлением большого количества различных систем управления, многие из которых являются программами open-source.
Существует множество задач, которые можно решать в связи с квадрокоптерами, в том числе задач научных. К примеру, работы [10, 11, 24] демонстрируют обучение роботов эффективным траекториям полета, работы [7, 23] – кооперативному поведению, [25, 26, 28] – навигации при помощи камер и RGBD-сенсоров,
1 Работа была выполнена в ИПМаш РАН при поддержке гранта РНФ 14-29-00142.
2Денис Александрович Никитин, магистрант ([email protected]).
а [2, 22] – отслеживанию траектории нелинейными регуляторами. Оценка ориентации подобных машин обычно осуществляется модификациями комплементарного фильтра [17, 18] или же расширенным фильтром Калмана [14], а обзор применяемых математических моделей, используемых для описания квадрокопте-ра, приведен в [8].
Для управления реальными квадрокоптерами обзор основных существующих решений можно найти в статье [16]. Однако можно выделить два общих свойства, присущие этим системам: они все работают на PID-регуляторах, и они все используют углы Эйлера (иначе называемые углами Крылова) в качестве переменных состояния.
Проблема использования углов Эйлера заключается в том, что этот способ имеет сингулярность, а значит, в некоторых областях пространства ориентаций не будет работать. К примеру, ни одна из общедоступных систем не способна сделать полностью контролируемый flip (сальто, переворот), не отключая при этом основной регулятор. В отличие от углов Эйлера, кватернионы не имеют подобных проблем, хотя их редко используют для создания систем управления квадрокоптерами. Примером системы, полностью основанной на кватернионах, может являться [9].
Проблема PID-регулятора в том, что для каждого конкретного робота нужно заново подбирать коэффициенты. К программам open-source прилагаются специальные инструкции о том, как это делать, однако всё равно такой подход снижает эффективность системы, так как не позволяет достичь оптимальных параметров. Примеры синтеза PID-регулятора для квадрокоптера можно найти в работах [13, 19, 21].
В связи с этими проблемами была создана своя система стабилизации, использующая для синтеза регулятора кватернионы в качестве переменной состояния и идею, похожую на линеаризацию обратной связью. В результате получился регулятор, большинство параметров которого являются в точности физическими параметрами конкретного квадрокоптера, а оставшиеся коэффициенты не зависят от робота и могут быть подобраны единожды
для всего класса машин.
Некоторые из физических параметров конкретного робота часто бывает сложно измерить, или же они могут меняться со временем прямо в полете. Поэтому особенно большую ценность приобретает система, позволяющяя идентифицировать неизвестные параметры и использовать их в системе стабилизации. Существующие же адаптивные системы, к примеру [1, 20, 29], полагаются на углы Эйлера и, в основном, стандартные РГО-регуляторы.
Данная работа состоит из четырёх частей. В первой определяется используемая математическая модель. Вторая часть посвящена системе стабилизации и доказательству её устойчивости. В третьей части приводится система адаптации и идентификации коэффициентов тяги квадрокоптера, ключевых для устойчивого полета. Последняя часть описывает проведенное моделирование общей системы.
I I
а) Ь)
Рис. 1. Тестовые самодельные квадрокоптеры: а) робот Квадракон (FlyMaple); б) робот Бонд (ТРИК)
Надо также сказать, что система стабилизации была проверена на реальных, самодельных квадрокоптерах (рис. 1), один из которых работал на контроллере Б1уМар1е, второй – на контроллере ТРИК (подробнее об этой российской разработке см. www.trikset.com, [27]).
1. Математическая модель квадрокоптера
1.1. ОБОЗНАЧЕНИЯ
Пусть q – кватернион. Обозначим через qw скалярную часть q, а через qv – векторную. Тогда q = (qw, qv) = (qw,qx, qy, qz).
Пусть г и s – два вектора в R3. Их скалярное произведение обозначается как (г, s), а векторное – как г х s. Произведение кватернионов a = (aw,av) и b = (bw,bv) может быть записано как
a * b = (awbw – (av ,bv), awbv bwav av х bv).
Под умножением кватерниона a на вектор г будем понимать ква-тернионное произведение a * (0,г).
Кватернион, сопряженный к q, будем обозначать как q*. Если
IMI = 1, то q * q* = q* * q = (1, 0, 0, 0).
1.2. КВАТЕРНИОН ОРИЕНТАЦИИ КВАДРОКОПТЕРА Пусть у нас есть абсолютная (земная) система координат
XYZ (где XY – горизонтальная плоскость, а ось Z направлена наверх, против силы тяжести), тогда для системы X’Y’ Z’, связанной с квадрокоптером, существует единственная ось и и единственный угол ф £ [0, ж) такие, что, если повернуть штрихованную систему вокруг оси и на угол ф против часовой стрелки, получится исходная система. Пусть (их,иу,uz) – единичный вектор в системе XYZ, являющийся напрявляющей оси и. Тогда кватернионом, описывающим поворот X’Y’Z’ в положение XYZ, называется q = ^cos | ,их sin |, иу sin | ,uz sin | j. Заметим, что ||g|| = 1 для любых и и ф. Пусть теперь нам задан вектор v1 в системе X’Y’Z’. Для его представления v в системе XYZ будет выполнено равенство v = q * v’ * q*. Если система X’Y’Z’ вращается относительно вектора угловой скорости ш, то закон изменения кватерниона ориентации q будет следующим:
(1) q = 1 q * ш.
В данном уравнении используются координаты вектора ш в системе, связанной с квадрокоптером. Таким образом, д и ш являются частью вектора состояния динамической системы, описывающей квадрокоптер. Кроме того, мы можем измерять ш при помощи гироскопов, установленных на робота, и использовать её для оценки д. Вывод уравнения (1), а также дополнительные сведения о кватернионной арифметике можно посмотреть в [4].
1.3. МОДЕЛЬ
В дополнение к уравнению (1) введем еще два уравнения:
/ ВД –
(2) 1й = I ¿(^э – I – и х 1ш,
– ^4 –
(3) н = ^ (1 – 2^2 – 2^) № ^2 ^ – д.
Уравнение (2) – уравнение Ньютона-Эйлера для вращающегося тела (см. [3]). Здесь ^ – сила тяги (Н), создаваемая г-м винтом, I – матрица моментов инерции (кг м2), предполагаемая диагональной, Ь – расстояние от центра масс до винтов (м), а £ -коэффициент связи силы тяги винта с реактивным моментом (м). Соответствие номеров винтов с их положениями и направлениями вращений можно увидеть на рис. 2.
‘
Я
Рис. 2. Схематичное изображение квадрокоптера
Уравнение (3) описывает динамику квадрокоптера по вертикальной оси 2, направленной против силы тяжести (движение в 80
горизонтальной плоскости ХУ опущено для упрощения, так как в данной работе мы не наблюдаем и не пытаемся управлять позицией робота в этой плоскости). Здесь Н – координата робота по оси 2 (м), т – масса робота (кг), д – ускорение свободного падения (м/с2). Множитель (1 — 2д2 — 2^) равен косинусу угла между осями 2 и 21 и определяет, насколько суммарная тяга квадрокоптера воздействует на его ускорение по высоте. Данный множитель в дальнейшем для удобства будет обозначаться через кто(1.
Сила тяги винта ^ предполагается зависящей квадратично от скорости вращения винта (см. [15]), которую в свою очередь можно считать линейно зависящей от напряжения, подаваемого на г-й мотор. Представим, что управление щ – напряжение на г-м моторе, при этом задаваемое по шкале от 0 до 1, где 0 – отсутствие напряжения, а 1 – максимальное напряжение на мотор. Тогда можно ввести коэффициент пропорциональности квадрата напряжения и силы тяги Кг, который также будет иметь смысл максимального значения тяги г-го винта. Если ^ = К^2, то мы также можем заменить переменную управления, введя Щ = и2. Тогда зависимость между тягой и управлением принимается линейной с коэффициентом пропорциональности Кг. Оценке Кг и посвящен раздел 4. В разделе 3 подразумевается, что значения Кг известны, и, более того, для простоты изложения равны единому коэффициенту К.
2. Система стабилизации
2.1. СТАБИЛИЗАЦИЯ ОРИЕНТАЦИИ
Основная идея закона управления лежит в создании двух последовательных регуляторов: сначала для кватерниона ориентации, потом для угловой скорости. Если задача, которая стоит перед роботом – совпадение систем отсчета ХУ2 и Х’У’ 21 (т.е. висение на месте), то выполнение цели означает равенство кватерниона д значению (1, 0, 0, 0). Определим тогда целевой кватернион который будет целью управления. В простейшем случае его значение будет (1, 0, 0, 0), однако можно использовать и лю-
81
бые другие значения для реализации более сложных движений, вплоть до переворотов.
Алгоритм 1.
1) Определим желаемую производную кватерниона: та = кР(да – {^а,я) д), где кр > 0 – коэффициент усиления, а {Чй, я} – скалярное произведение кватернионов как векторов в М4.
2) Инверсией уравнения (1) получаем целевую угловую скорость: ша = 2 д* * та.
3) Определим желаемое угловое ускорение: ра = ка(ша – со), где ка > 0 – коэффициент усиления.
4) Инверсией уравнения (2) получаем целевой момент: М^ =
1ра ш х 1ш. В итоге Со = ра.
5) Момент М^ дает нам 3 линейных уравнения на управления и г. Определив любым способом четвертое управление (например, задав суммарную тягу четырех винтов), мы можем полностью рассчитать управление и^.
Скалярное произведение в шаге 1 необходимо для того, чтобы выполнялось условие ортогональности т,а и д в М4, которое в свою очередь есть следствие условия ||д|| = 1. Оказывается, этого достаточно, чтобы при кватернионном умножении в шаге 2 скалярная часть результирующего кватерниона обращалась в 0 и
была бы вектором.
Заметим, что единственными параметрами регулятора являются коэффициенты кр и ка, которые не зависят от физических параметров системы. То есть однажды найденные коэффициенты смогут стабилизировать любой квадрокоптер (при условии отсутствия погрешностей в определении физических параметров системы).
2.2. СТАБИЛИЗАЦИЯ ВЫСОТЫ
Момент сил М^ определяет 3 уравнения на Щ. Один из вариантов полностью определить управления – добавить регулятор высоты. Пусть задана целевая высота Нкоторую необходимо выдержать квадрокоптеру. Применим РБ-регулятор, задав целевое ускорение:
(4) У = АР(Щ -Н) – АйН,
где Ар и А^ положительные. Из уравнения (3) получаем четвертое уравнение на управляющие воздействия:
(5)
г=1
™ (У д)
К ктоа
2.3. ОБЩИЙ ЗАКОН УПРАВЛЕНИЯ
Введем матрицу С, связывающую целевые моменты и ускорения с управляющими воздействиями и с
С =
/ 0 КЬ 0 -кь -1 / 0
-КЬ 0 КЬ 0 1 2КЬ
щ -ка 0
V К т К т К т – / т / – 1 2КЬ
__^
2КЬ 4КЁ 4К
_1
2 КЬ 0 1
2КЬ 0
1
4ке 1
” 4К£
4Щ 1
4К т 4К т 4К
__^
4К( 4К/
И тогда управления рассчитываются просто из матричного умножения:
(6)
(иЛ
и2 и3
(
0 1
2КЬ 0
1
1
2КЬ 0 1
2КЬ
2КЬ 0
1
4К£
4Щ
4Щ 1
4Щ
т
4К т 4К т 4К т
4К /
( м4х мАу
У д
ктоа )
2.4. УСТОЙЧИВОСТЬ ЗАКОНА УПРАВЛЕНИЯ
Устойчивость высоты при использовании регулятора (4) очевидна. Докажем теперь, что алгоритм 1 обеспечивает цель управления ориентацией для почти всех начальных состояний. Пусть дл = (1, 0, 0, 0). Это предположение неограничивающее, так как в противном случае мы можем ввести модифицированный кватернион ориентации д’ = д* * д в качестве нового состояния. При
таком выборе да уравнения замкнутой системы для ориентации сводятся к
(7)
Я
1
2<? *ш,
ш = -2крк^ – каш. Теорема 1. Пусть кр,ка > 0. Тогда точка Ь
[(1,0,0,0), (0, 0,0)] является асимптотически устойчивой точкой равновесия системы (7), Ь2 = [(-1,0,0, 0), (0,0,0)] -неустойчивой точкой равновесия, и областью притяжения точки Ь является область §4 х М3{£2}.
Доказательство. В качестве кандидата на функцию Ляпунова рассмотрим функцию
(8) V(д,ш) = Ни ||дь ||2 2Н^<ду, ш) Н22 ||ш||2 2v(1 – дш). Очевидно, что V(Ь) = 0 и V(Ь2) = . Необходимо найти такие коэффициенты и V, чтобы производная V в силу системы была отрицательна всюду, кроме точек Ь и Ь2, а сама V была положительно определена. Для удобства обозначим кд = 2крка.
Производную функции V в силу системы можно записать
как
(9) V = дц(ды) ||^||2 2д 12(дт)<,ш) ^) ||ш|2 ,
где коэффициенты д^ удовлетворяют равенству
(10)
Г9и( Чи,) 912 (Чи, А /0 2
V V 2 0
=
912 (Чы ) 922 (Ч-ш ),
(Ни ЬЛ2( 0 /0 -кЛ (кп Н1Л
^12 Н22У —кд -кл) ^^ -кл) ЬЛ2 Ь^) ‘
Иной способ записи уравнения (9),
У = шт) [С(дш) ® /з](Щ;)
показывает, что отрицательная определенность V равносильна отрицательной определенности матрицы О. Также очевидно, что (д,и ,ш) = (0, 0) только в точках Ь и Ь2.
Теперь покажем, что существуют такие коэффициенты и V, что матрица О будет отрицательно определена. Задача осложняется тем, что по уравнению (10) коэффициенты матрицы, в общем случае, зависят от . Для решения этой проблемы воспользуемся частотной теоремой (см. [6, 12]).
Пусть
А =
( 0 М В = (Л Н = (ки М
-кд -кл) ‘ Я ‘ Я к2 к22)
»=(0 9 • *=
Введем линейную динамическую систему
х = Ах Ви.
Условие отрицательной определенности матрицы О тогда запишется как
(11) Пусть
(Х! Х2 Щ^Х2)
НА А1 Н НВ
пТ тт
И
Ы |ж2| =0.
N 0′
‘ X!
Х2 ) < 0,
и = -2-Х2, | < 1
(Ж2 2и) (Х2 – 2и) > 0 ^
0 0 0
‘х 1’
(х! Х2 и) 10 1 0 ) I Ж2 ) > 0. 0 -4/ и
Определим
0 0 0
М = 10 1 0
0 —4)
и 5 = М
(о 0)
‘0 2 2 2
0 0
0 0 -4,
Тогда выполнение условия (11) будет следовать из матричного неравенства
(12) В я 0
„ (НА АТН НВ п 3 ( ВТН 0 )< 0.
Частотная теорема утверждает, что существование матрицы Н = Нт такой, что неравенство (12) выполнено, равносильно выполнению неравенства
(13)
(х* и*0
при всех (х, и) € Мш для каждого ш € М и {те}, где множества Мх при Л € С определены как
М)
( х, (0,
и) | х € С2{0}, и € С, Лх = Ах Ви, при |Л| < те, (0, и) | и € С1{0}, при |Л| = те.
В случае, когда гш не является собственным числом А, условие (13) может быть записано в терминах частотной характеристики:
(14)
(гш1 – А) 1
1
-1В
Б
(г ш I -А)-1 В
Выражая частотную характеристику
< 0.
(Ш -А)-1 В = (1Ш „ -1 (0)
1
-ш2 гшка -к, и подставляя её в (14), получаем условие
(гш кЛ _ I -ш
I т, = кд(ш гкл)
V – к9 / ш(ш2 к2) )
( ± кд (ш-i кл) ^ ш(ш2 к2)
‘0 2
2 2 00
0 0
—А)
/ – ш
кд (ш г кл) ш(ш2 к2)
V 1 )
Щ -»кяка _ 4 < 0
ш2(ш2 к2)
для выполнения которого достаточно положить
> 0.
(15)
кл
Если же %ш – собственное число А, то условие (13) надо проверять для всех векторов вида (хш х3,и) таких, что хш € кег(гш I – А), х3 ± кег(гш1 – А), Ах3 – шх3 Ви = 0. В 86
1
1
нашем случае ш = 0, и для него хш = {ка -кд)Т. Более того,
х3 = (0 1)Т ± В, а значит, (х3,и) = (0, 0). Тогда частотное условие сводится к неравенству
– )(01)(_%) <
которое в свою очередь равносильно (15). Таким образом, доказано существование матрицы Н и числа V таких, что производная V в силу системы отрицательно определена.
Осталось показать, что функция V положительно определена. Из отрицательной определенности матрицы С и равенства (10), выбирая дш = 1, получаем:
-2 кд¡112 = 911 < 0, -2кЛк22 ¡12 = 922 < 0,
откуда сразу следует положительность коэффициента ¡22. Следовательно, так как ||д,и|| ^ 1 и |дш| ^ 1, функция V ^ те при ||ш|| ^ те. Значит, существует такое К > 0, что для любого д € 84 из ||ш|| > К следует V > 1. Рассмотрим множество Е = §4 х В3(К), содержащее все пары кватернионов ориентации д и угловых скоростей и> таких, что ||-ш|| ^ К. Множество Е компактно, значит, существует точка Ь-3, в которой V(Ь3) = ттжеЕ V(х). Так как V(Ь1) =0 и Ь1 € Е, значение V(Ьз) ^ 0. Таким образом, Ь3 не может находиться на границе множества Е, а значит, существует открытое множество и С Е, содержащее Ь3. Обозначим за (£) решение системы (7), выпущенное из точки Ьз. Тогда будет существовать такое > 0, что фь3 (т) € и для всех т € [0, Д£]. Пусть теперь У(Ь3) < 0. Выбрав Д£ достаточно маленьким, мы получим, что V(фь3(т)) убывает, что противоречит минимальности V(Ь3). Таким образом, У(Ь3) не может быть меньше нуля. Однако таких точек всего две – это Ь1 и Ь2. V(Ь1) < V(Ь2), значит, Ь3 = Ь1. То есть функция V не принимает отрицательных значений, а нуль достигается только в точке Ь1. Значит, функция V положительно определена.
Итак, мы нашли положительно определенную функцию V, производная которой в силу системы отрицательно определена всюду, кроме двух точек равновесия, Ь и Ь2. По теореме Ляпунова об асимптотической устойчивости точка Ь является асимптотически устойчивой, при этом её областью притяжения является всё пространство, кроме точки Ь2.
3. Система адаптации
В реальной машине, к сожалению, из-за небольших погрешностей в изготовлении винтов или двигателей, а также из-за возможных повреждений, коэффициенты К для каждого винта будут немного отличаться. И эта разница оказывает существенное влияние, в первую очередь, на угловую стабилизацию, так как робота начинает кренить в одну из сторон, и не выполняется условие на кватернионы д ^ q¿,. Далее рассматривается случай, когда у каждого двигателя есть свой коэффициент тяги К±, а также своя оценка этого коэффициента К^. Рассмотрим замкнутую систему, которая получится в таком случае.
Уравнения модели можно записать так:
(16)
/ / 0 ьк2 0 -ЬКЛ
1ш ш х 1ш -ьк1 0 ьк3 0
= екх -ек2 К -ек4
Й д К1 К2 К К±
кто<С т т т т ‘
/иД
и2 и
а уравнения регулятора (6) – так:
/
/иА 1 2 К1Ь 0 1 4К^ 4К1 т
(17) и2 и = 2 К2Ь 0 4К2 т
и4) 1 2К3Ь 0 4КзС 4К3 т
2 К4Ь 4К4^ 4 К4/
I Мах МЛу МЛг Т д
^ кто<< }
1
1
т
0
Обозначим -1 через Ф^. Подставив (17) в (16) и выразив М^
Кг
по алгоритму 1, получим /
(18)
1ш ш х 1ш
Н д
кто(1 ‘
(
= В
– 1к^ш – ш х 1ш У д
ктос1
V
/
где коэффициенты матрицы В имеют следующий вид:
В[1 : 2, •] =
( 1( К2Ф2 К4 Ф4) 20 – 2с (К2Ф2 – К4Ф4) (К2Ф2 -К4Ф4)
0
1 (К1Ф1 К3Ф3) – ^(К1Ф1 -К3Ф3) – 2Ш ( К1Ф1 – К3Ф3)У
В[3, •] =
( – (К2Ф2 – К4Ф4)
– ^ (К1Ф1-К3Ф3)
1 4?(К1Ф1
4 (К1Ф1 К2Ф2 К3Ф3 К4Ф4) Уз^(К1Ф1 – К2Ф2 К3Ф3 – К4Ф4)
/
В[4, •] =
^Р(К2Ф2 -К4Ф4) – ^р (К1Ф1 -К3Ф3) ^(К1Ф1 – К2Ф2 К3Ф3 – К4Ф4) V 4 (К1Ф1 К2Ф2 К3Ф3 К4Ф4)}
Как можно было ожидать, в случае точного совпадения оценок коэффициентов с их реальными значениями, т.е. в случае К^Ф^ = 1 матрица В совпадает с единичной.
В (18) перейдем к линейной системе
I !хшх ( – 1хк4шх – 1хкдЧх
1укс1шу 1укдЧу – 1хкйшх – 1хкдЧх
Ар(На -Н) – АЛН д) Теперь введем новые обозначения, чтобы перейти к полностью матричной записи задачи. Определим новый вектор состояния X = (ХТ,ХТ)Т, где
(19)
1у ш у
ш г
Н д)
= В
Х1 = (21хдх, 21уду, 21гдг, Н – Нл) ‘
Х2
(
и обозначим через вектор д общее воздействие гравитации на систему:
¿г = (0, 0, 0, д)Т . Тогда система (19) может быть переписана как
(20) ^ = ^
X2 = В(-ТХг -Т2Х2 £) – д, где матрицы Т и Т2 – матрицы коэффициентов регуляторов:
Т = diag , у , у, Т2 = diag (кл, кл, кл, Ал).
Фактически они диагональны, но ниже предполагается лишь их симметричность, положительная определенность и, ради удобства вычислений, перестановочность между собой.
Теперь для оценки Ф^ мы можем применить метод скоростного градиента в дифференциальной форме (см. [5]). Данный метод позволяет синтезировать управление, обеспечивающее стремление заданной целевой функции ((X) к нулю. Для этого функция лишь должна удовлетворять четырем условиям:
1) условию регулярности: ((X) и (((X, Ф) непрерывны,
2) условию роста: ((X) ^ 0 и ((X) ^ при |Х| ^
3) условию выпуклости производной в силу системы: ((X, Ф) – ((X, Ф’) ^ (Ф – Ф’)ТУ*^, Ф’),
4) условию достижимости цели: ЗФ* £ М4 и р > 0 :
((X, ф*) < -рЯ^) VX.
В качестве целевой функции для метода скоростного градиента рассмотрим функцию
((X) = 1121X1 22X2^ XТТ12X1 =
(21) = ^Т X2Т2 Т1Т2fX1
= № X2 ) [тТ Т2 ) ^ ,
Благодаря положительной определенности матриц Т и Т2, ((X) также является положительно определенной квадратичной формой, а значит, её стремление к нулю гарантирует стремление к 90
нулю X, кроме того, очевидно выполнены условия роста и регулярности.
Возьмем производную в силу системы от функции Q(X):
((Х,В) =
(22) = 2 (хТ -ХТтВ -ХТТ2ВТ дТ(ВТ – /)) ■
/ 2Т2 /Х^
■ T1T2 т2 ) Х2/ Условие выпуклости функции (( выполнено, так как она линейна по матрице В, которая в свою очередь линейна по оценкам Ф .
Введем вспомогательную функцию 2(X,3) такую, что 2(X, 3) = -Уф4((Х, В), где 3 = Ц:
2(Х,3) = (ХТ Х2Т £Т) ■
2Т1Т23Т1 Т1Т23Т2 Т13ТТ22 -3ТТ^Х нХ1е
Т2 3ТТ1Т2 Т22 3Т1 2Т22 3Т2 – 3ТТ22 I I Х2
-Т1ТЗ -Т223 0
Теорема 2. Пусть Т22 > Т1. Тогда алгоритм
(23) * = (хЦ) .
обеспечивает устойчивость линеаризованной системы и стремление Кг ^ Кг.
Доказательство. Мы уже показали, что для функции ((X)
выполнены условия регулярности, роста и выпуклости метода скоростного градиента. Осталось проверить последнее условие -условие достижимости цели. Для этого подставим в функцию
(22) В = I, соответствующее идеально найденным оценкам Ф^. Получим
(1(Х) = – (Х1Т ХЛ (2Т1ТТ21-Т22Т12 ^Т3- 2Т{Т2) (Х2) =
= -X ТКХ.
Достаточно показать, что матрица этой квадратичной формы К положительно определена. Попробуем найти неособое пре-
91
образование координат, превращающее эту матрицу в блочно-диагональную. Одним из таких преобразований может быть
= / 2Т?Т2 2Ш2 – 2Т2 = К 2TiTg – 2Т2 2Т23 – 2ТТ2)
Т ТгТ-1 (2Т1Т-1 0 ^ ^ Т1
0 I М 0 2Т23 – 2Т1Т2) Т1Т-1 I
откуда следует критерий положительной определенности матрицы К:
Т1Т-1 > 0, Т23 – Т{Т2 > 0,
который, учитывая положительность матриц Т1 и Т2, преобразуется в условие
(24) Т22 > Т1.
Итак, если для матриц коэффициентов выполнено условие (24), то адаптация коэффициентов Ф^ по методу скоростного градиента приведет к пределу Q(X, В) ^ 0 и, следовательно, X ^ 0, что доказывает утверждение об устойчивости линеаризованной системы. Означает ли это, что оценки Ф^ сойдутся к своим истинным значениям ^ ? Для доказательства этого факта рассмотрим систему (20). Так как точка X = 0 является устойчивой точкой равновесия для адаптивной системы, в пределе должно быть выполнено равенство
9 = Вд,
которое означает, что матрица В в пределе имеет собственный вектор (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,…, 4}.
Адаптация по методу скоростного градиента может быть записана так:
Фг
– Кг =
К2 г V
д Фг)
Неизвестные, но постоянные значения настоящих коэффициентов тяги Кг входят в соответствующую частную производную линейно, и потому могут быть вынесены из функции 2 в
множитель 7. Выразив отсюда КГг, получаем (23).
Для полноты алгоритма приведем конкретные значения используемых производных , считая при этом коэффициенты Кг убранными в множитель 7:
/0 0 0
(25)
дВ дФ1
дВ
дФ2
дВ дф
дВ
дФ4
0 0 0
( 2
0
– 0 1
I
2Ь 4
__1
2 Ьт 4§т
0
Ьт 4
1
4 !
0
__Ь Ьт
4£ 4
0 0 0
I
4
— –
4£ т 4 /
0
2 Ьт 0 0 0 0
/ 2 0
2Ь 0
__0__
2 Ьт 0 4§ т
I
4
0 0 0
1 Ь Ьт
2 4? 4
? 1
2 Ь 4
1 1 1
2 Ьт 4£ т 4 )
0
Ь _Ьт
4? 4
0 0 0
I £т
4 4
1 1
4. Моделирование
Проверка алгоритма, представленного в предыдущем разделе, была проведена при помощи моделирования полета квадро-коптера в системе МАТЬАБ Я2023а. Для этого была реализована модель квадрокоптера, система управления и система адаптации. Физические параметры для моделирования были приближенно измерены у одного из реальных изготовленных квадрокоптеров (робот Квадракон), и они перечислены в таблице 1.
Таблица 1. Значения параметров при моделировании
Физические параметры Параметры регуляторов Коэффициенты тяги
m = 1,3 kg L = 0,22 m £ = 0,017 m Ix = 0,12 kg*m2 Iy = 0,12 kg*m2 Iz = 0,22 kg*m2 g = 9,8 m/sec2 kp = 2,6 kd = 10,0 Ap = 2,0 Ad = 5,0 Hd = 3 m Ki = 12,6 Н K2 = 18,6 Н K3 = 4,6 Н K4 = 7,0 Н
Здесь же указаны примененные коэффициенты регуляторов, которые удовлетворяют условию (24), и выбранные для моделирования «истинные» коэффициенты тяги, неизвестные для алгоритма управления. В начальном положении робот считается покоящимся на горизонтальной поверхности, его координата по высоте принята за 0.
Для проверки алгоритма было проведено 3 эксперимента, в первом из них адаптации не было (7 = 0), во втором 7 = 0,0005, в третьем 7 = 0,005. Начальные оценки коэффициентов Ki равны 8,6 Н, время симуляции – 10 с.
В первом случае (рис. 3) в отсутствие адаптации робот входит в устойчивое движение, но не достигает цели управления, в частности, у него есть наклон по крену и тангажу, что в реальной системе привело бы к появлению ускорения в плоскости XY.
Yaw Pitch Roll ■
4 6
seconds
4 6
seconds
а) Ь)
Рис. 3. Отсутствие адаптации, 7 = 0: а) ориентация, углы Эйлера; б) высота Н
4
2
(и 0 о>
(U.2 ■о *
-4
4 6
seconds
a)
-Estimate K. • Estimate K ■
5-
0I-,-,-,-,-,
0 2 4 6 8 10
seconds
r-
4 6
seconds
b)
0 2 4 6 8
seconds
с) а)
Рис. 4. Медленная адаптация, 7 = 0,0005: а) ориентация, Эйлера; б) высота Н; в) оценки коэффициентов Кг; г) величины КгФг
углы
2
10
0
2
10
2
10
0
2
10
K .Ф
“1 “1
K. Ф.
2 2
K. Ф.
K Ф
10
В случае медленной адаптации (рис. 4) состояние робота асимптотически достигает своих целевых значений, как и коэффициенты тяги. Также можно заметить, что при отсутствии адаптации ошибка по крену и тангажу составила 10-15 градусов, тогда как при её наличии она не превысила 5, а через 10 секунд составляла уже 2 градуса. Ошибки такого порядка уже могут быть приемлемы в реальной системе.
При быстрой адаптации (рис. 5) коэффициенты сходятся к
4 6
seconds
a)
Yaw Pitch Roll
Л _ -Estimate K .-Estimate K2
Estimate K3 .-Estimate K4
У у*-
0 2 4 6 8 10
seconds
4 6
seconds
b)
0 2 4 6 8 10
seconds
c) d)
Рис. 5. Быстрая адаптация, 7 = 0,005: а) ориентация, углы Эйлера; б) высота H; в) оценки коэффициентов Ki; г) величины
2
10
0
2
10
к .ф
1 1
к. Ф.
<_Ф_
К.Ф.
4″4
своим истинным значениям гораздо быстрее, а ошибка по крену и тангажу не превышает 1 градус. Однако можно заметить, что в начальный период коэффициенты, а вместе с ними и ориентация испытывают сильные колебания. Дальнейшее увеличение коэффициента приводит к тому, что вычисления становятся неустойчивыми.
5. Заключение
В данной работе был описан основанный на кватернионах параметрический регулятор для стабилизации квадрокоптера, а также система идентификации коэффициентов тяги на каждом винте. Также было проведено численное моделирование, подтверждающее работоспособность предложенных алгоритмов.
Литература
1. БЕЛЯВСКИЙ А.О., ТОМАШЕВИЧ С.И. Синтез адаптивной системы управления квадрокоптером методом пассификации // Управление большими системами. -2023.-Вып. 63.-С. 155-181.
2. КАНАТНИКОВ А.Н., АКОПЯН К.Р. Управление плоским движением квадрокоптера // Математика и математическое моделирование. МГТУ им. Н.Э. Баумана. – 2023. -№2. – С. 23-36.
3. ЛАНДАУ Л.Д., ЛИФШИЦ Е.М. Теоретическая физика, том 1. Механика. 5-е издание. – М.:ФИЗМАТЛИТ, 2023. -224 с.
4. МАТВЕЕВ В.В., РАСПОПОВ В.Я. Основы построения бесплатформенных инерциальных навигационных систем. – ЦНИИ «Электроприбор», 2009. – 278 с.
5. МИРОШНИК И.В., НИКИФОРОВ В.О., ФРАДКОВ А.Л. Нелинейное и адаптивное управление сложными динамическими системами. – СПб.: Наука, 2000. – 549 с.
6. ЯКУБОВИЧ В.А. Частотная теорема в теории управления // Сибирский математический журнал. – 1973. -Т. 14. – №2. – C. 484-420.
7. AUGUGLIARO F., SCHOELLIG A.P., D’ANDREA R. Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach // IEEE/RSJ Int. Conference on Intelligent Robots and Systems (IROS), October 7-11, 2023, Vilamoura-Algarve, Portugal. -P. 1917-1922.
8. chovancova A., FICO T., CHOVANEC L’., HUBINSK P. Mathematical Modelling and Parameter Identification of Quadrotor (a survey) // Procedia Engineering. – 2023. -Vol. 96. – P. 172-181.
9. FRESK E., NIKOLAKOPOULOS G. Full Quaternion Based Attitude Control for a Quadrotor // European Control Conference (ECC’2023), July 17-19, 2023, Zurich, Switzerland. – P. 3864-3869.
10. HEHN M., D’ANDREA R. A frequency domain iterative learning algorithm for high-performance, periodic quadrocopter maneuvers // Mechatronics. – 2023. – Vol. 24, Iss. 8. – P. 954-965.
11. HEHN M., D’ANDREA R. Quadrocopter trajectory generation and control // IFAC World Congress, August 28 -September 2, 2023, Milano, Italy. – Vol. 18, Iss. 1. – P. 14851491.
12. KALMAN R.E. Lyapunov functions for the problem of Lur ‘e in automatic control // Proc. Natl. Acad. Sci. USA. – 1963, February. – No. 49(2) – P. 201-5.
13. KHATOON S., SHAHID M., CHAUDHARY H. Dynamic modeling and stabilization of quadrotor using PID controller // IEEE Int. Conference on Advances in Computing, Communications and Informatics (ICACCI), September 24-27, 2023, Delhi, India. – P. 746-750.
14. LEFFERTS E.J., MARKLEY F.L., SHUSTER M.D. Kalman filtering for spacecraft attitude estimation // J. of Guidance, Control, and Dynamics. – 1982. – Vol. 5, Iss. 5. – P. 417-429.
15. LEISHMAN J.G. Principles of Helicopter Aerodynamics, Second Edition. – Cambridge Aerospace Series, 2006.
16. LIM H., PARK J., LEE D., KIM H.J. Build your own quadrotor. Open-Source Projects on Unmanned Aerial Vehicles // IEEE Robotics and Automation Magazine. -Septermer 2023. – P. 33-45.
17. MADGWICK S.O.H. An efficient orientation filter for inertial and inertial/magnetic sensor arrays // Internal Report. – 2023.
18. MAHONY R., HAMEL T., PFLIMLIN J.M. Nonlinear complementary filters on the special orthogonal group // IEEE Transactions on Automatic Control. – Vol. 53, Iss. 5. – 2008. -P. 1203-1218.
19. MAHONY R., KUMAR V., CORKE P. Multirotor aerial vehicles. Modelling, Estimation, and Control of Quadrotor // IEEE Robotics and Automation Magazine. – Septermer, 2023. – P. 20-32.
20. PEREZ I.C., FLORES-ARAIZA D., FORTOUL-DIAZ J.A., MAXIMO R., GONZALEZ-HERNANDEZ H.G. Identification and PID control for a quadrocopter // IEEE Int. Conference on Electronics, Communications and Computers (CONIELECOMP), Febuary 26-28, 2023, Puebla, Mexico. -P. 77-82.
21. POUNDS P., MAHONY R., CORKE P. Modelling and Control of a Quad-Rotor Robot // Proc. Australasian Conference on Robotics and Automation’2006, December 68, 2006, Auckland, New Zealand. – P. 1-10.
22. RAFFLO G.V., ORTEGA M.G., RUBIO F.R. MPC with Nonlinear H^ Control for Path Tracking of a Quad-Rotor Helicopter // Proc. of the 17th IFAC World Congress, July 6-11, 2008, Seoul, Korea. – P. 8564-8569.
23. RITZ R., MULLER M.W., HEHN M., D’ANDREA R. Cooperative quadrocopter ball throwing and catching // IEEE/RSJ Int. Conference on Intelligent Robots and Systems (IROS), October 7-11, 2023, Vilamoura-Algarve, Portugal. -P. 4972-4978.
24. SCHOELLIG A.P., WILTSCHE C., D’ANDREA R. Feedforward parameter identification for precise periodic quadrocopter motions // IEEE American Control Conference, June 27-29, 2023, Montreal, Canada. – P. 4313-4318.
25. SHEN S., MULGAONKAR Y., MICHAEL N., KUMAR V. Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor // Robotics: Science and Systems, June 24-28, 2023, Berlin, Germany. -Vol. 1. – P. 32.
26. STEGAGNO P., BASILE M., BULTHOFF H.H., FRANCHI A. 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. – 2023. – P. 87-92.
27. TEREKHOV A.N., LUCHIN R.M., FILIPPOV S.A. Educational Cybernetical Construction Set for schools and universities // Proc. of the 9th IFAC Symposium Advances in Control Education, June 19-21, 2023, Nizhny Novgorod, Russia. – P. 430-435.
28. TOURNIER G.P., VALENTI M., HOW J.P. Estimation and control of a quadrotor vehicle using monocular vision and moire patterns // AIAA Guidance, Navigation and Control Conference and Exhibit. – 2006. – P. 21-24.
29. ZHU J., LIU E., GUO S., XU C. A gradient optimization based PID tuning approach on quadrotor // IEEE 27th Chinese Control and Decision Conference (CCDC), May 2325, 2023, Qingdao, China. – P. 1588-1593.
ADAPTIVE QUATERNION-BASED QUADROTOR CONTROL SYSTEM
Denis Nikitin, Saint-Petersburg State University, master ([email protected]).
Abstract: In this paper we propose the quaternion-based control system for quadrotor. Adaptive scheme for thrust coefficients identification, based on speed-gradient method, is designed. Proofs of stability are provided, as well the results of numerical simulations. In existing theoretical works, Euler angles are often used as coordinates for for describing quadrotor’s coordinates. Equations using those coordinates, however, have a singularity, which prevents their use near certain points. We use quaternions instead, which have no such restrictions. The process of discovering PID-regulator coefficients is known to be tedious, error-prone and specific for each quadcopter. We propose a control scheme in which most of the parameters are physical values, and the rest do not depend on the quadcopter and can be found once for the whole class of the flying machines. An identification algorithm for obtaining physical parameters is also described. MATLAB modelling is used to test and confirm the performance of the proposed scheme.
Keywords: UAV, quadrotor, adaptive control, speed-gradient method, quaternions.
Статья представлена к публикации членом редакционной коллегии Б.Р. Андриевским.
Поступила в редакцию 07.04.2023. Дата опубликования 30.09.2023.
