УДК 519.71 Жабко Н.А., Лепихин Т.А. Санкт-Петербургский государственный университет, г. Санкт-Петербург, Россия СИНТЕЗ АСТАТИЧЕСКОГО РЕГУЛЯТОРА ДЛЯ УПРАВЛЕНИЯ ДВИЖЕНИЕМ КВАДРОКОПТЕРА* Аннотация В статье представлен алгоритм управления движением квадрокоптера под воздействием внешних возмущений, описываемых ступенчатыми функциями. Целью работы является разработка закона управления движением квадрокоптера, который обеспечивает стабилизацию и астатизм по контролируемым переменным на основе так называемого скоростного регулятора. Скоростной регулятор предложено использовать в работах Веремея Е.И как один из элементов регулятора специальной многоцелевой структуры, представленного в виде линейной стационарной системы, который позволяет обеспечить стабилизацию по контролируемым переменным, а также требуемые динамические свойства при наличии внешних возмущений. Регуляторы такого типа могут эффективно применяться в ряде задач управления динамическими объектами, в частности, они успешно используются в задачах управления морскими подвижными объектами. В работе приводится математическая модель динамики квадрокоптера, для которой формируется управление контролируемыми переменными на основе нескольких упрощенных линейных моделей. Для каждой из упрощенных моделей формируется скоростной регулятор, который за счет структурных особенностей подавляет в замкнутой системе воздействие ступенчатых возмущений на контролируемые переменные. Это свойство, как правило, обеспечивается путем введения в регулятор дополнительных интегральных составляющих по контролируемым переменным. Для демонстрации работоспособности алгоритма в работе приводятся графические иллюстрации переходных процессов по высоте, курсу, крену и тангажу, полученные в результате имитационного моделирования в среде MATLAB- Simulink. Ключевые слова Управление квадрокоптером; скоростной закон управления; MБПЛА; астатический регулятор. Zhabko N.A., Lepikhin T.A. Saint-Petersburg State University, Saint-Petersburg, Russia ASTATIC CONTROLLER SYNTHESIS FOR QUADCOPTER MOTION CONTROL Abstract The paper presents control algorithm of quadcopter dynamics under the influence of perturbations, described by step functions. The aim of this work is to develop the control law of quadcopter dynamics, providing stabilization and astaticism of regulated variables on the base of the so-called speed controller. Speed controller was proposed in the papers of Veremey E.I. as one of the elements of controller with a special multi-purpose structure, which is described by linear time-invariant system and allows having stability of regulated variables and also the required dynamic properties in the presence of external perturbations. Such controllers can be effectively used in a number of control tasks for dynamic objects, in particular, they are actively used for marine mobile objects control. The mathematical model of quadcopter is described, for which the regulated variables control is constructed on the base of simplified linear models. For each of simplified models the speed controller is designed, which due to its structural features can suppress the action of the step disturbances. Usually, this property is provided by introducing the integral components for regulated variables in * Труды II Международной научной конференции «Конвергентные когнитивно- информационные технологии» (Convergent’2017), Москва, 24-26 ноября, 2017 Proceedings of the II International scientific conference "Convergent cognitive information technologies" (Convergent’2017), Moscow, Russia, November 24-26, 2017 255 the control law. To demonstrate the efficiency of the algorithm, some graphical illustrations of transient processes in height, heading, roll and pitch, obtained as a result of simulation in MATLAB- Simulink, are given in the work. Keywords Quadcopter control; speed control law; UAV; astatic controller. Введение В настоящее время сфера применения беспилотных летательных аппаратов мультироторного типа исключительно широка, в том числе они применяются наряду с другими мобильными роботами в учебных лабораториях, как описано, например, в [1]. Это обуславливает интерес и актуальность развития соответствующих алгоритмов управления для таких устройств. Несмотря на достаточно большое количество существующих решений в задачах управления движением беспилотных летательных аппаратов мультироторного типа, основанных в частности на применении методов LQR-оптимизации, H∞-оптимизации, использовании ПИД – регулятора и различных методов нелинейного управления: скользящего управления, бэкстеппинг управления, управления с использованием нейронных сетей, нечеткой логики, а также различных комбинаций указанных подходов (некоторые решения приведены в [2-4]), поиск решений, обладающих лучшими свойствами с точки зрения его качества или простоты, обеспечения необходимых свойств управляемых процессов, является актуальным. Одним из свойств управляемой системы, которое следует учитывать при построении регулятора, является обеспечение требуемой динамики в условиях действия внешних возмущений. В данной работе рассматривается алгоритм управления квадрокоптером, обеспечивающий подавление аддитивных ступенчатых возмущений, порождаемых воздействием от ветра, то есть астатизм по контролируемым переменным, который обеспечивается за счет использования так называемого скоростного регулятора. Такая форма представления закона управления введена в работах Веремея Е.И., в которых в ряде ситуаций подобный скоростной регулятор используется как базовая часть регулятора специальной многоцелевой структуры. Описание общих принципов построения таких регуляторов и особенностей применения в задачах управления движением морских судов, можно найти, например, в работах [5-8]. Цель исследования Целью исследования является разработка закона управления контролируемыми переменными квадрокоптера, обеспечивающего стабилизацию и астатизм по этим переменным на основе скоростного регулятора. Такой регулятор составляет базовую часть регулятора специальной многоцелевой структуры, которая показала эффективность в применении к задачам управления морскими судами и другими динамическими объектами, позволяя обеспечить желаемое качество стабилизации по контролируемым переменным, а также, дополнительно, требуемую динамику при воздействии внешних возмущений. Такая структура регулятора введена в работах Веремея Е.И., в которых проводится обоснование его использования и обсуждение отдельных вопросов его построения и применения, преимущественно при управлении морскими судами, в частности, в статьях [5:8]. Рис. 3. Общая схема элементов квадрокоптера 256 Математическая модель объекта В качестве базовой математической модели МБПЛА рассматриваемого типа принимается система обыкновенных нелинейных дифференциальных уравнений 12-го порядка, которая описывает движение объекта, как управляемого твердого тела, совершающего продольные перемещения вдоль осей координат и вращательные движения по углам Эйлера. Структурная схема объекта управления для понимания формирования математической модели показана на рис.1. Система дифференциальных уравнений, описывающих продольные перемещения квадрокоптера, представлена выражениями: x  M1  Fi cos  cos  sin   sin  sin    d1 , 4 1  Fi cos  sin  sin   sin  cos    d 2 4 y  1 M 1  Fi cos  cos   Mg  d3 , 4 z  1 M (1) 1   J1 F3  F4 L  d 4 ,     1 F  F L  d , J 1 2 5   J1 F1  F2  F3  F4   d 6 ,   где x, y, z  – вектор координат объекта, , ,  – угол крена, тангажа и курса соответственно, M – масса объекта управления, g – ускорение свободного падения, Fi , i  1, 4 – силы тяги двигателей квадрокоптера,   J  , J  , J  – моменты инерции по крену, тангажу и курсу, L – расстояние от центра масс до двигателя, дополнительно введенные слагаемые в правых частях d1 , d 2 , d3 , d 4 , d5 , d 6 задают внешние возмущающие силы, порождаемые порывами ветра, и могут быть представлены в виде ступенчатых функций d j  A j  1t  A j – некоторые постоянные числа, j  1, 6 . Тяга, производимая каждым пропеллером, описывается уравнением в следующем виде:  Fi  K ui , (2) s где  – частота привода, K – положительный множитель, i  1, 4 . Введем новые управляющие сигналы, связь которых с силами тяги представима в виде (3):  F1  F2  F3  F4   4u z     2u   F3  F4   K    , (3)  F1  F2  s    2u       F1  F2  F3  F4  4u   также введем переменные, задающие состояние приводов  j  u j , j  z, , ,  , тогда в пространстве s состояний уравнения динамики приводов могут быть представлены в виде:  j   j  u j , j  z, , , . (4) Теперь из уравнения (3) с учетом (4) можно выразить силу тяги каждого двигателя в виде зависимости от динамики приводов:   F1  K  z     , F  K      , 2 z   F  K      , . 3 z   F  K      . 4 z   Стоит также учесть, что реальная динамика приводов в виду конструктивных особенностей ограничена. Вводятся ограничения на величину силы тяги Fi  Mg , i  1,4 , и ограничение на управляющее воздействие ui  15 . Уравнения динамики приводов (4) учитываются при формировании законов управления и моделировании динамических процессов. 257 Структура скоростного закона управления Приведем описание структуры скоростного регулятора [5:8]. С этой целью предположим, что линейная математическая модель, задающая динамику объекта управления, представляется в следующем виде x  Ax  B  H wf w (t ), (5) y  Cx, где матрицы A, B, Hw , C имеют постоянные компоненты. Здесь x  E n – вектор состояния объекта,   Em – вектор отклонения управляющих органов, y  E k – вектор используемых для синтеза регулятора измеряемых переменных, именно они же принимаются в качестве контролируемых, f w  E l – вектор внешних возмущений, в данном случае представляющих ветровые возмущения. Наряду с уравнениями объекта, введем в рассмотрение линейные уравнения гидравлического привода исполнительных органов в виде   u , (6) где u  E – векторный управляющий сигнал. m Структура математической модели закона управления для представленной линейной модели объекта составляется из следующих элементов: z  Az  B  H(y  Cz) – уравнение наблюдающего устройства; (7) u  z   y  – уравнение скоростного управляющего сигнала. (8) Здесь матрицы H, ,  подлежат поиску, исходя из требований по динамике объекта в условиях действия внешних возмущений. Матрицы H, ,  ищутся последовательно в соответствии со следующей схемой. Сначала строится закон управления в виде обратной связи по состоянию при нулевом командном сигнале и при отсутствии внешнего возмущения u  K x x  K  , (9) где K x и K  – постоянные матрицы искомых коэффициентов, выбор которых естественно выполнять исходя из требования обеспечения необходимого быстродействия в замкнутой нелинейной системе при обеспечении асимптотической устойчивости замкнутой линейной системы (5), (6), (9). Далее на основе найденных матриц K x и K  однозначно определяются матрицы ,  в скоростном законе (8) путем фиксации части контролируемых переменных в уравнениях (9) и представлении остальных в (9) в соответствии с уравнениями объекта (5) в зависимости от производных переменных состояния: u  x   y . (10) Переход к такой форме представления обусловлен тем, что помимо стандартных требований асимптотической устойчивости при необходимом качестве динамических процессов, такое управление в силу структурных особенностей позволяет обеспечить свойство астатизма по контролируемым переменным y . Классическим способом обеспечения такого свойства является введение в регулятор интегральной составляющей по контролируемым переменным, и во многих случаях обеспечивается путем использования ПИД – управления, однако такой способ управления, как правило, не обладает достаточной универсальностью и гибкостью, в том числе с точки зрения его построения. Следующим этапом является формирование асимптотического наблюдателя (7), который помимо построения оценок тех переменных состояния и их производных, которые не измеряются или измеряются с помехами, позволяет улучшить динамику в условиях действия возмущений. Матрицу H в наблюдателе необходимо выбрать так, чтобы она обеспечивала гурвицевость характеристического полинома матрицы A  HC , после чего осуществляется переход к уравнению скоростного закона управления вида (8). При этом для МБПЛА с учетом присутствующих внешних ветровых воздействий имеет смысл потребовать, чтобы на этом этапе управление, обладая свойством астатизма, обеспечивало допустимые отклонения контролируемых переменных от желаемых значений при действии ступенчатых внешних возмущений вида d (t )  d 0  1(t ) . Уравнения (7) – (8) могут быть преобразованы к эквивалентному виду, итоговому для реализации z  Az  B  H(y  Cz), (11)   Kz  K    y, 0 0 где K  (A  HC) , K 0  B , 0  H   . Основная часть 258 В соответствии с описанной выше структурой закона управления перейдем к описанию системы управления контролируемыми переменными для квадрокоптера с учетом требования астатизма по ним. Для этого выполним линеаризацию уравнений (1) в окрестности нулевого положения равновесия (положения зависания), и, учитывая динамику приводов, получим представления линейной модели, причем они могут быть разделены на группы независимых уравнений, описывающих движение отдельно по высоте и трем углам вращения, которые удобно использовать для синтеза управления отдельно каждой контролируемой переменной (высоте и углам), что существенно облегчает исследование динамики объекта управления. Вводя новые переменные z1  z , z2  z , 1   , 2   , 1   , 2   , 1   ,  2    , в качестве исходных моделей для формирования базовых законов управления по контролируемым переменным, примем линейные модели по высоте, по крену, по тангажу и по курсу, каждая из которых дополняется соответствующим уравнением привода двигателя из (4): по высоте z1  z2 , 4K z2  z  g, (12) M  z   z  u z , по крену  1  2 , 2 LK  2   , . (13) J        u по тангажу  1  2 , 2 LK  2   , . (14) J      u    по курсу  1   2 , 4 K  2   , . (15) J        u где u z , u , u , u  E1 – скалярные управляющие сигналы, подаваемые на приводы. На основе рассмотренного варианта управления высотой и углами аппарата может быть осуществлен переход к управлению непосредственно координатами при заданном угле курса. Базовые законы для каждой из полученных систем представляются в виде линейной обратной связи по вектору состояния в виде: u z  K1z  z  z z   K 2 z z  K 0 z  z , u  K1    z   K 2   K 0  , (16) u  K1    z   K 2   K 0  , u  K1    z   K 2    K 0   , где в качестве заданных командных сигналов при моделировании приняты значения zz  40 м, z  16, z  4,  z  14 , совпадающие с желаемыми конечными значениями по высоте, крену, тангажу и курсу соответственно. Коэффициенты регуляторов задают матрицы     K z  K1z K 2 z , K   K1 K 2 , K   K1 K 2 , K   K1 K 2 . Поскольку в рассматриваемой ситуации регуляторы вида (16) непосредственной реализации не подлежат, а используется для синтеза скоростных регуляторов, которые обеспечивают астатизм в замкнутой системе, входящая в уравнения (12) постоянная составляющая не компенсируется непосредственно в уравнении в (16). 259 Базовые законы управления сформируем на основе решения задачи синтеза линейного квадратичного регулятора (LQR) следующего вида:    J  J (K vj )   xTj Q j x j  uTj R ju j dt  vmin , 0 K j  где K  K j v j   K0 j ,  – область асимптотической устойчивости соответствующей замкнутой системы в пространстве параметров. Для каждой системы (12) при управлении высотой, (13) при управлении креном, (14) при управлении тангажом и (15) при управлении курсом соответствующая матрица Q j выбирается диагональной с неотрицательными весовыми коэффициентами на диагонали, и, совместно с соответствующим скалярным положительным значением R j , определяет компромисс между точностью стабилизации и интенсивностью управления по каждой из переменных, позволяя добиться допустимого быстродействия при заданных ограничениях на управляющие переменные. Конкретные числовые значения параметров базовых регуляторов по всем исследуемым переменным, полученные при численном моделировании, представлены ниже: 100 0 0  300.5 0 0  Q z   0 100 0  , Rz  8000 , Q    0 200 0  , R  1000 (17)  0 0 10  5  0 0 10000 1500 0 0  1000 0 0   Q   0 10  5  0  , R  10 , Q    0 2000 0  , R  106 (18)  0 0 1000  0 0 10000 Коэффициенты регуляторов при этом принимают следующие значения: K z   0.1118  0.1239, K z 0  3.3777, K    0.5482  0.4551, K 0  9.3966, K    0.1225  0.0215, K θ 0  1.3656, K    0.0316  0.0451, K ψ 0  15.9948. Для систем (12) – (15), принимая в качестве входов переменные состояния приводов, формируются асимптотические наблюдатели следующего вида: по высоте zˆ1  zˆ 2  h1z  z1  zˆ1 , (19) zˆ  b   h  z  zˆ , 2 z z 2z 1 1 по крену ˆ 1  ˆ 2  h1 1  ˆ 1 , (20) ˆ 2  b    h2  1  ˆ 1 , по тангажу ˆ   1  ˆ 2  h1 1  ˆ 1 ,  (21) ˆ   2  b    h2 1  ˆ 1 ,  по курсу ˆ 2  h1 1   ˆ 1    ˆ 1 , (22) ˆ  b   h     ˆ 1 . 2   2 1 В векторном виде оценки переменных состояния в системах (19) – (22) задаются переменными zˆ   zˆ zˆ  ,  1 2 T 1 2  ˆ  ˆ ˆ T , ˆ  ˆ ˆ 1 2 T  ˆ   , ˆ T . Выбор коэффициентов h , h , j  z, , ,  , равно как и ˆ  1 2 1j 2j окончательный выбор коэффициентов в матрицах в (17) – (18), осуществляется в замкнутой нелинейной системе со всеми управлениями, включенными в контур обратной связи, в процессе имитационного моделирования в тестовом режиме, характеризуемым наличием возмущений рассматриваемого типа. Скоростной астатический регулятор по каждой контролируемой переменной формируется на основе полученных значений коэффициентов в уравнениях наблюдателей. Такие регуляторы с учетом того, что уравнения динамики приводов (4) в нашем случае отличаются от общего вида (6), примут вид: 260 u z   z zˆ   z  z1  z z    z , u   ˆ         ,    1 z  (23) ˆ u      1   z    , u    ˆ         ,   1 z  где коэффициенты представляются формулами:  K3z   K 3     z   K 2 z ,  z  K1z , bz  4K ,     K 2 ,    K1 , b  2 LK ,     K 2  K 3 ,    K1 , b  2 LK ,    b   bz  M  b  J  J  K 3      K 2 ,    K1 , b  4 K ,  b  J  с учетом введенных обозначений K3 z  K0 z  1 , K 3  K 0   1 , K3  K0  1 , K 3  K 0   1 . Теперь, окончательно, подставляя управляющие сигналы в форме (23) в уравнения (12) – (15) и далее используя (19) – (22), перейдем для каждой системы к форме записи вида (11). Например, для контролируемой переменной курса, полученная система будет иметь вид: ˆ  ( A  H C ) ˆ  B  H,       ~ ( A  H C ) ~ H ~ ~ B   ( ˆ   ),            где  0   h1  ~  0 1   A     , B    4 K , H     ,     , ~      .  0 0   J   h2     Ниже на рис.2 и рис.3 приведены иллюстрации работы построенных регуляторов при переходе контролируемых переменных из начальных значений в заданные конечные при действии постоянных возмущений, имитационное моделирование выполнено в среде MATLAB-Simulink. Из графиков видно, что синтезированные регуляторы обеспечивают достаточно хорошее быстродействие и отрабатывают постоянные возмущения по всем переменным, то есть обладают свойством астатизма. высота 40 35 30 град 25 20 15 0 5 10 15 20 25 30 35 40 45 50 сек Рис. 2. Переходный процесс для изменения по высоте с 20 м. на 40 м Заключение В данной работе представлен алгоритм управления движением квадрокоптера, обеспечивающий астатизм по контролируемым переменным – высоте, курсу, крену и тангажу за счет использования специальных скоростных регуляторов. Для подтверждения работоспособности представленного алгоритма, в среде MATLAB-Simulink при различных условиях проведено имитационное моделирование, в результате чего можно сделать вывод от том, что рассмотренный алгоритм обеспечивает достаточно хорошее качество переходных процессов и астатизм по контролируемым переменным. 261 Угол крена 20 15 град 10 5 0 0 10 20 30 40 50 сек Угол тангажа 5 4 град 3 2 0 10 20 30 40 50 сек Угол курса 15 10 град 5 0 0 10 20 30 40 50 сек Рис. 3. Переходные процессы по заданным углам z  16, z  4, z  14 . Литература 1. Sotnikova M., Zhabko N., Lepikhin T. Control System Analysis and Design Labs with Educational Plants. IFAC Proceedings Volumes (IFAC- PapersOnline), 2012. Vol. 9, No 1. pp. 212-217. 2. Besnard L., Shtessel Y.B., Landrum B. Control of a Quadrotor Vehicle Using Sliding Mode Disturbance Observer. Proceedings of the 2007 American Control Conference. July 11-13, 2007. New York City, USA. pp. 5230-5235. 3. Changlong L., Pan J., Chang Y. PID and LQR Trajectory Tracking Control for An Unmanned Quadrotor Helicopter: Experimental Stu dies. Proceedings of the 35th Chinese Control Conference. July 27-29, 2016, Chengdu, China. pp. 10845-10850. 4. Lepikhin T.A. The analysis of Quadrocopter and Hexacopter dynamics. AIP Conference Proceedings 1738, 160005 (2016). doi: 10.1063/1.4951938 5. Veremey E.I., Sotnikova M.V., Eremeev V.V., Korovkin M.V. Modal Parametric Optimization of Control Laws with Special Structure. 14 International Conference on Control, Automation and Systems (ICCAS, 2014). 22-25 Oct. 2014. 2014. Korea. pp. 1278-1283. 6. Veremey E.I., Korovkin M.V., Sotnikova M.V. Ships' Steering in Accurate Regime Using Autopilots with Special Structure of Control Law. Proceedings of the 10th IFAC Conference on Manoeuvring and Control of Marine Craft (IFAC MCMC 2015). August 24 -26, 2015.Copenhagen, Denmark. IFAC-PapersOnline, Volume 48, Issue 16, pp. 7–12. 7. Veremey E.I. Dynamical Correction of Control Laws for Marine Ship's Accurate Steering. Journal of Marine Science and Application. 2014. Vol. 13, No 2. pp. 127-133. 8. Веремей Е.И., Сотникова М.В. Многоцелевая структура законов управления морскими подвижными объектами. Труды XII Всероссийского совещания по проблемам управления.2011. References 1. Sotnikova M., Zhabko N., Lepikhin T. Control System Analysis and Design Labs with Educational Plants. IFAC Proceedings Volumes (IFAC- PapersOnline), 2012. Vol. 9, No 1. pp. 212-217. 2. Besnard L., Shtessel Y.B., Landrum B. Control of a Quadrotor Vehicle Using Sliding Mode Disturbance Observer. Proceedings of the 2007 American Control Conference. July 11-13, 2007. New York City, USA. pp. 5230-5235. 3. Changlong L., Pan J., Chang Y. PID and LQR Trajectory Tracking Control for An Unmanned Quadrotor Helicopter: Experimental Studies. Proceedings of the 35th Chinese Control Conference. July 27-29, 2016, Chengdu, China. pp. 10845-10850. 4. Lepikhin T.A. The analysis of Quadrocopter and Hexacopter dynamics. AIP Conference Proceedings 1738, 160005 (2016). doi: 10.1063/1.4951938 5. Veremey E.I., Sotnikova M.V., Eremeev V.V., Korovkin M.V. Modal Parametric Optimization of Control Laws with Special Structure. 14 International Conference on Control, Automation and Systems (ICCAS, 2014). 22-25 Oct. 2014. 2014. Korea. pp. 1278-1283. 6. Veremey E.I., Korovkin M.V., Sotnikova M.V. Ships' Steering in Accurate Regime Using Autopilots with Special Structure of Con trol Law. Proceedings of the 10th IFAC Conference on Manoeuvring and Control of Marine Craft (IFAC MCMC 2015). August 24-26, 2015.Copenhagen, Denmark. IFAC-PapersOnline, Volume 48, Issue 16, pp. 7–12. 262 7. Veremey E.I. Dynamical Correction of Control Laws for Marine Ship's Accurate Steering. Journal of Marine Science and Application. 2014. Vol. 13, No 2. pp. 127-133. 8. Veremey E.I., Sotnikova M.V. Mnogotselevaya struktura zakonov upravleniya morskimi podvizhnymi ob'ektami. Trudy XII Vserossiyskogo soveshthaniya po problemam upravleniya. 2011. Об авторах: Жабко Наталия Алексеевна, кандидат физико-математических наук, доцент, кафедра компьютерных технологий и систем, Санкт-Петербургский государственный университет, n.zhabko@spbu.ru Лепихин Тимур Андреевич, кандидат физико-математических наук, главный специалист, Санкт- Петербургский государственный университет, t.lepihin@spbu.ru Note of the authors: Zhabko Nataliia A., Candidate of Physics-Mathematical Sciences, Associate Professor, Department of Computer Applications and Systems, Saint-Petersburg State University, n.zhabko@spbu.ru Lepikhin Timur A., Candidate of Physics-Mathematical Sciences, Chief Specialist, Saint-Petersburg State University, t.lepihin@spbu.ru 263