Управление траекторным движением квадрокоптера при транспортировке груза на подвесе: эксперимент

УДК 681.532.8

DOI 10.25205/1818-7900-2019-17-4-5-46-56

Управление траекторным движением квадрокоптера при транспортировке груза на подвесе: эксперимент

А. С. Димова, К. Ю. Котов, А. С. Мальцев, Е. Д. Семенюк, М. А. Соболев

Институт автоматики и электрометрии СО РАН Новосибирск, Россия

Аннотация

Рассматривается задача управления движением квадрокоптера при транспортировке груза на подвесе. В данной работе представлены результаты экспериментальной проверки разработанного ранее нелинейного регулятора подавления колебаний подвеса относительно вертикали. Приведено описание созданного экспериментального стенда по управлению полетом малогабаритного квадрокоптера в помещении. Ключевые слова

квадрокоптер, управление траекторным движением, транспортировка груза на подвесе Благодарности

Работа выполнена в рамках комплексной программы фундаментальных исследований Сибирского отделения Российской академии наук «Междисциплинарные интеграционные исследования» на 2018-2020 гг. по проекту «Разработка цифровых технологий раннего обнаружения и локализации поражений посевов сельскохозяйственных культур» Для цитирования

Димова А. С., Котов К. Ю., Мальцев А. С., СеменюкЕ. Д., СоболевМ. А. Управление траекторным движением квадрокоптера при транспортировке груза на подвесе: эксперимент // Вестник НГУ. Серия: Информационные технологии. 2019. Т. 17, № 4. С. 46-56. DOI 10.25205/1818-7900-2019-17-4-46-56

Control of the Trajectory Movement of a Quadrocopter during Transportation of Cargo on a Suspension:

Experiment

A. S. Dimova, K. Yu. Kotov, A. S. Maltsev, E. D. Semenyuk, M. A. Sobolev

Institute of Automation and Electrometry SB RAS Novosibirsk, Russian Federation

Abstract

The problem of controlling the movement of a quadrocopter during transportation of cargo on a suspension is considered. This paper presents the results of an experimental verification of a previously developed nonlinear regulator for suppressing suspension vibrations relative to the vertical. The description of the created experimental stand for flight control of a small-sized quadrocopter in the room is given. Keywords

quadrotor, trajectory control, suspension cargo transportation Acknowledgements

This work was carried out as part of a comprehensive program of basic research of the Siberian Branch of the Russian Academy of Sciences "Interdisciplinary integration studies" for 2018-2020 on the project "Development of digital technologies for early detection and localization of lesions in crops"

© А. С. Димова, К. Ю. Котов, А. С. Мальцев, Е. Д. Семенюк, М. А. Соболев, 2019

For citation

Dimova A. S., Kotov K. Yu., Maltsev A. S., Semenyuk E. D., Sobolev M. A. Control of the Trajectory Movement of a Quadrocopter during Transportation of Cargo on a Suspension: Experiment. Vestnik NSU. Series: Information Technologies, 2019, vol. 17, no. 4, p. 46-56. (in Russ.) DOI 10.25205/1818-7900-2019-17-4-46-56

На сегодняшний день беспилотные летательные аппараты успешно применяются в решении задач аэрофотосъемки, создании цифровых карт местности. Аппараты мультироторной конструкции широко используются при решении этих задач ввиду своей управляемости, надежности и простоты конструкции. Перспективными областями применения мультиротор-ных летательных аппаратов являются доставка грузов, аэрогеофизические работы, точное земледелие [1-4].

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

В работах [6; 7] авторами предложен алгоритм управления квадрокоптером, обеспечивающий перемещение груза вдоль предписанной траектории с одновременным подавлением колебаний подвеса относительно вертикали. Решению данной задачи посвящено относительно небольшое количество исследований, кроме того существуют только единичные работы, в которых представлены экспериментальные результаты [2].

Целью работы является создание экспериментального стенда и получение результатов экспериментов по управлению полетом квадрокоптера с грузом на подвесе. Приведено описание аппаратной и программной части реализованной системы управления. Результаты экспериментов сравниваются с результатами численного моделирования.

Рассмотрим на плоскости (, z) систему «квадрокоптер - груз», в которой центр масс квадрокоптера M1 соединен невесомым стержнем длины l с грузом M2 (рис. 1). Поворот квадрокоптера вокруг поперечной оси вращения соответствует углу тангажа 0. Угол отклонения шарнирно закрепленного подвеса от вертикали обозначим у.

Введение

Уравнения движения и описание алгоритма управления

о

.Г] Х2

Xref

Рис. 1. Схематическое изображение системы «квадрокоптер - груз» Fig. 1. Schematic illustration of quadrotor-load system

Координаты центра масс аппарата x1, z1 и угол у связаны с координатами x2 , z2 центра масс груза соотношениями

В работе [7] на основе уравнений Лагранжа получены уравнения динамики движения центра масс квадрокоптера с учетом присоединенного груза:

r(mí + т2)х1 = их sinö — m2l(y cosy — у2 siny), (m1 + m2)z1 = и1 sinö — m2l(y siny—у2 cosy) — (т1 + т2)д,

SÄ-

т11у = — щ sin(ö + у) .

Здесь mi и m2 - масса квадрокоптера Mi и груза M2, соответственно; g - ускорение силы тяжести; Iyy - момент инерции относительно поперечной оси вращения, проходящей через центр масс квадрокоптера.

Приведем основные соотношения из [7] для подъемной силы ui и вращающего момента u3, которые обеспечивают сведение к нулю рассогласования между текущими x2, z2 и целевыми xref, zref координатами груза согласно требуемым дифференциальным уравнениям следующего вида:

Гх2 + (кх + ах)х2 + ахкх(х2 _хге/) = 0,

z2 +

{?2 + (kY + ar)y + aYkYy = 0. Определим вспомогательные переменные:

rAx =

= {ky + aY)y + aYkYy.

Сходимость переменных x2, z2 к значениям xref, zref обеспечивается выбором управляющих воздействий в виде

' и1 ~ V ^хх

arctan

^"з =1хх (-(«0 + кв)в -авкв(в - бгв/)).

При положительных значениях коэффициентов а и k в уравнениях (3) и (6), полученными управляющими воздействиями, обеспечиваются устойчивые переходные процессы, а при а = k эти процессы будут иметь монотонный характер.

Входными данными для алгоритма управления являются зашумленные реализации координат центра масс груза и угла отклонения подвеса от вертикали. Дополнительно требуется наличие оценок первых производных этих переменных. Соотношения (2) могут быть использованы для оценки переменных состояния объекта в расширенном фильтре Калмана. Для этого с учетом (1) запишем (2) в виде:

<

' CD

Xmi +m2)x2 =и1 sin в — т2(у cos у — у2 sin у), (m1 + m2)z2 =u1 sin0 — т2(у siny—y2 cosy) — (m1 +т2)д,/ Iyy@ =Щ,

<m1ly = — щ sin(0 + у) .

Дополним модель процесса (7) моделью наблюдений и запишем полученную нелинейную динамическую систему в компактном виде:

{X = f(X,U) + W, Z = h(X) + V,

где Х= [x2,z2,x2,z2,6,y,6,y] , U = [и1,и3]Т, f - нелинейная вектор-функция, соответствующая правой части уравнений (7), W - входной шум, V - шум измерений.

В расширенном фильтре Калмана нелинейная система (8) линеаризуется посредством разложения в ряд Тейлора. Дальнейшая оценка вектора состояния X состоит из последовательных этапов экстраполяции и коррекции [10].

В качестве измеряемых компонент вектора X использовались координаты x2 cam, z2 cam AR маркера, закрепленного в центре масс груза, угол тангажа аппарата 0bim, вычисленный бортовой инерциальной системой, и угол отклонения подвеса от вертикали, рассчитанный следующим образом:

7cam = arctanyam-yam.

zl_cam z2_cam

Абсолютные координаты x1cam, z1cam AR маркера, закрепленного в центре масс аппарата, также определялись с помощью внешней видеосистемы. Компоненты линейных и угловых скоростей вычислялись посредством фильтрации сигналов x2cam, z2_cam, вЫш, ycam. Результирующий вектор измерения имеет вид:

т

Z = [х2_

cam, cam, Х2_ cam, cam,@bins,Ycam,@bins,Ycam\ .

Результаты численного моделирования

Для возможности проверки предложенных алгоритмов управления и достоверности получаемых экспериментальных данных удобно воспользоваться численным представлением модели процесса (8). Рассмотрим свободное движение системы (7), когда u1 = (m1 + m2)g, из = 0, y(0) = 0,1 рад.

Если приращение в ряде Тейлора при экстраполяции решения Xk уравнения модели процесса взято достаточно малым, сумма двух первых членов ряда может обеспечить достаточную точность. В этом случае

Xk+1=Xk+f

Здесь At - величина k-го шага дискретизации по времени.

Следует отметить, что в случае наличия колебаний в системе (7) точность вычислений по формуле (10) значительно падает. Наблюдается накопление ошибки по координате z2 (рис. 2, а), при этом величина ошибки зависит от шага интегрирования. Так, для шага At = {0.01, 0.005, 0.001} секунд отклонение по координате z2 в момент времени t = 200 секунд составляет {0.370, 0.101, 0.008} м соответственно. Накопление ошибки усиливается наличием нелинейностей в уравнениях (7).

Для более точного представления физической системы «квадрокоптер - груз» моделью процесса (8) необходимо применять другие методы экстраполяции. Повысить точность экстраполяции решения можно при использовании так называемого улучшенного метода Эйлера. Первое значение Xk+1 вычисляется по формуле

,

определяется соответствующее ему значение ДХк+1, £4+1) из дифференциального уравнения (7). Затем значение Хк+1 уточняется по формуле

Результаты движения системы (7), вычисленного по формулам (11), (12) при таких же начальных условиях, приведены на рис. 2, б. Метод обеспечивает экстраполяцию решения на большем интервале и с большей точностью. Так, для шага дискретизации А( = 0,01 секунды отклонение по координате ^ в момент времени ^ = 200 секунд составляет 0,001 м. На большем интервале времени не происходит накопления ошибки, например, в момент времени ^ = 2000 секунд отклонение по координате ^ составляет 0,0003 м. Частота колебаний по координате х2 уменьшается более чем в 2 раза, что связано с уточнением расчета угловой скорости

б

Рис. 2. Изменение координат x2, z2 во времени: a - интегрирование уравнений (7) по формуле (10); б - интегрирование уравнений (7) по формулам (11), (12) - сплошная линия - координата x2, пунктирная линия - координата z2 Fig. 2. Change of coordinates x2, z2 in time: a - integration of equations (7) according to formula (10); б - integration of equations (7) by formulas (11), (12) - solid line - coordinate x2, dashed line - coordinate z2

a

Результаты моделирования движения замкнутой системы управления, с регулятором на основе соотношений (6), представлены на рис. 5, б, 6, б.

Описание программной и аппаратной части системы управления

Программное обеспечение (ПО) созданного стендового комплекса имеет модульную архитектуру и использует механизм межпроцессного взаимодействия ROS (Robot Operation System) [8]. Авторами используются пакеты программ ROS, предназначенные для решения задач коммуникации с аппаратом, распознавания визуальных AR (Augmented Reality) маркеров, сохранения данных. Разработанные авторами модули управления и оценки состояния в расширенном фильтре Калмана, написаны на языке Python с применением асинхронной обработки на основе ROS Timer и интегрированы в систему ROS.

В качестве управляющего контроллера используется автопилот Pixhawk с открытым исходным кодом. Оценка углового положения аппарата выполняется в модуле комплементарного фильтра ПО PX4 [9]. Установление заданной ориентации выполняется встроенным

в ПО PX4 модулем управления на основе линейных регуляторов. На вход автопилота по протоколу mavlink подается управляющая команда set_attitude, содержащая требуемые значения углов Эйлера и суммарной тяги двигателей (рис. 3). Изображения AR-маркеров, закрепленных в центре масс квадрокоптера и груза, поступали с двух совместно откалиброванных видеокамер с частотой 30 кадров в секунду.

Рис. 3. Структурная схема системы управления Fig. 3. Control system block diagram

Разработанная система управления полетом квадрокоптера включает набор независимых программ, взаимодействующих между собой посредством обмена сообщениями в системе ROS. Ее структура показана на рис. 4. В состав системы входят следующие программы:

• получения изображения с камеры (camera);

• распознавания AR-маркеров (ar_track_alvar);

• определения пространственного положения маркеров (vision_pose);

• реализация алгоритма стабилизации подвеса и оценки вектора состояния в расширенном

• фильтре Калмана (controller);

• протокол коммуникации с полетным контроллером PX4 (mavros).

Абсолютные координаты летательного аппарата в пространстве и его углы ориентации относительно неподвижной камеры определяются с помощью программ ar_track_alvar и vision_pose и передаются в расширенный фильтр Калмана. На основе этой информации и данных об угловом положении полученных от PX4 вычисляется положение и ориентация аппарата и оценка его линейных скоростей. Эти данные о координатах и скорости используются программой controller, которая вычисляет управляющие команды для углов ориентации квадрокоптера и тяги его двигателей в соответствии с разработанным алгоритмом стабилизации подвеса. Затем эти команды через модуль mavros поступают для исполнения в полетный контроллер PX4.

Результаты экспериментов и сравнение с моделированием

Для исследования вопросов устойчивости и качества управления в предложенной системе проведен ряд экспериментов по управлению полетом квадрокоптера в помещении.

Рис. 4. Структурная схема программной части системы управления Fig. 4. Software block diagram of the control system

Квадрокоптер собран на базе 250 мм рамы. Груз закреплен на подвесе из текстолитовой пластины длиной I = 0,55 м, масса груза и квадрокоптера составила т2 = 0,18 кг и Ш\ = 0,6 кг, соответственно. Рассчитанный момент инерции 1уу = 0,046 кгм2. Установлены следующие параметры системы управления: кх = ах = 2, кг = аг = 2, к7 = ау = 1.

На рис. 5, 6 приведены результаты экспериментов по удержанию системы «квадрокоптер - груз» в неподвижной точке с координатами хге/ = 0,5 м, 2ге/ = 0,6 м. Рассмотрены два режима работы регулятора (6). В первом случае в выражениях (4), (5) не учитываются составляющие, связанные с углом отклонения подвеса у. Регулятор выполняет стабилизацию положения без учета маятниковых колебаний подвеса (рис. 5).

Оценка отклонения подвеса от вертикали, полученная из численного моделирования уравнений (11), (12) для таких же параметров объекта и системы управления, согласуется с экспериментом (рис. 5, б, 6, б). В модель (7) добавлены аддитивные гауссовские шумы по переменным состояния х2, z2, у, оценка линейных и угловых скоростей 2, 2 получена с помощью фильтрации данных сигналов. Во втором случае, включение в систему управления регулятора по углу отклонения подвеса от вертикали позволяет значительно уменьшить прецессионные колебания подвеса (рис. 6, б).

Рассчитанное значение среднеквадратичного отклонения (СКО) от среднего переменных х2, z2 и у уменьшается с 0,07 до 0,03 м и с 0,03 до 0,02 рад при использовании предложенного регулятора. Расчет СКО выполнен на интервале времени от 30 до 90 секунд, с отбрасыванием значений, превышающих по модулю 3,5 первоначально вычисленного СКО.

Колебания подвеса можно подавить полностью, если учесть колебания в плоскости (у, ¿). Синтез такого регулятора является предметом дальнейших исследований.

..........il ..

li ¡1 iii —Hill! ii! ¡<1

m t s, il y s Vif M 1! • '< v i ' ! I >I|L yïyivw > Ii! Щ'Щ 1

! i!

a

time, s б

Рис. 5. Движение без учёта угла у: a - траектория движения груза в плоскости (x, z); б - угол отклонения груза от вертикали y(i) -штриховой линией показана экспериментальная зависимость, сплошной - полученная при численном моделировании Fig. 5. Motion without taking the angle у into account: a - cargo trajectory in the plane (x, z); б - the deviation angle of the load from the vertical y(i) -the dashed line corresponds to the experimental dependence, the solid line - to numerical simulation

a б

Рис. 6. Движение с учетом отклонения по углу у: a - траектория движения груза в плоскости (x, z); б - угол отклонения груза от вертикали y(i) -штриховой линией показана экспериментальная зависимость,

сплошной - полученная при численном моделировании Fig. 6. Motion with taking the oscillation of angle у into account: a - cargo trajectory in the plane (x, z); б - the deviation angle of the load from the vertical y(i) -the dashed line corresponds to the experimental dependence, the solid line - to numerical simulation

Заключение

В работе представлены результаты предварительных экспериментов по управлению системой «квадрокоптер - груз» с шарнирно закрепленным подвесом. Эксперимент по удержанию системы в неподвижной точке показал эффективность предложенного регулятора на основе требуемых дифференциальных уравнений изменения регулируемых параметров, в том числе угла отклонения подвеса от вертикали. Дальнейшая работа будет направлена

на получение данных траекторного движения и разработку регулятора подавления колебаний

по продольной и поперечной плоскостям движения.

Список литературы

1. Cwiakala P., Kocierz R., Puniach E., Nedzka M., Mamczarz K., Niewiem W., Wiacek P.

Assessment of the Possibility of Using Unmanned Aerial Vehicles (UAVs) for the Documentation of Hiking Trails in Alpine Areas. Sensors, 2018, vol. 18, no. 1, p. 81.

2. Goodarzi F. A., Lee D., Lee T. Geometric control of a quadrotor UAV transporting a payload connected via flexible cable. International Journal of Control, Automation and Systems, 2015, vol. 13, no. 6, p. 1486-1498.

3. Kotaru P., Wu G., Sreenath K. Dynamics and control of a quadrotor with a payload suspended through an elastic cable. In: ACC, IEEE, 2017, p. 3906-3913.

4. Duggal V., Sukhwani M., Bipin K., Reddy G. S., Madhava K. K. Plantation monitoring and yield estimation using autonomous quadcopter for precision agriculture. In: ICRA, IEEE, 2016, p. 5121-5127.

5. Palunko and Fierro R., Cruz P. Trajectory generation for swing-free maneuvers of a quadrotor with suspended payload: A dynamic programming approach. In: IEEE International Conference on Robotics and Automation, ICRA. Saint Paul, MN, USA, 2012, p. 2691-2697.

6. Димова А. С. Управление траекторным движением квадрокоптера при транспортировке груза на подвесе // Материалы XIV Международной научной конференции «Устойчивость и колебания нелинейных систем управления» (конференция Пятницкого). М.: ИПУ РАН, 2018. С. 134-137.

7. Димова А. С., Котов К. Ю., Мальцев А. С., Нестеров А. А., Филиппов М. Н. Управление квадрокоптером в задаче транспортировки груза на подвесе // Автометрия. 2018. Т. 54, № 5. С. 116-121. DOI 10.15372/AUT20180515

8. Quigley M., Conley K., Gerkey B. P. et al. ROS: an opensource robot operating system. In: ICRA Workshop on Open Source Software, IEEE, 2009, vol. 3, p. 5.

9. Meier L., Honegger D., Pollefeys M. PX4: A node-based multithreaded open source robotics framework for deeply embedded platforms. In: IEEE International Conference on Robotics and Automation (ICRA), 2015, p. 6235-6240.

10. Белоконь С. А., Золотухин Ю. Н., Котов К. Ю., Мальцев А. С., Нестеров А. А., Пив-кин В. Я., Соболев М. А., Филиппов М. Н., Ян А. П. Использование фильтра Калмана в системе управления траекторным движением квадрокоптера // Автометрия. 2013. Т. 49, № 6. С. 14-24.

References

1. Cwiakala P., Kocierz R., Puniach E., Nedzka M., Mamczarz K., Niewiem W., Wiacek P.

Assessment of the Possibility of Using Unmanned Aerial Vehicles (UAVs) for the Documentation of Hiking Trails in Alpine Areas. Sensors, 2018, vol. 18, no. 1, p. 81.

2. Goodarzi F. A., Lee D., Lee T. Geometric control of a quadrotor UAV transporting a payload connected via flexible cable. International Journal of Control, Automation and Systems, 2015, vol. 13, no. 6, p. 1486-1498.

3. Kotaru P., Wu G., Sreenath K. Dynamics and control of a quadrotor with a payload suspended through an elastic cable. In: ACC, IEEE, 2017, p. 3906-3913.

4. Duggal V., Sukhwani M., Bipin K., Reddy G. S., Madhava K. K. Plantation monitoring and yield estimation using autonomous quadcopter for precision agriculture. In: ICRA, IEEE, 2016, p. 5121-5127.

5. Palunko and Fierro R., Cruz P. Trajectory generation for swing-free maneuvers of a quadrotor with suspended payload: A dynamic programming approach. In: IEEE International Conference on Robotics and Automation, ICRA. Saint Paul, MN, USA, 2012, p. 2691-2697.

6. Dimova A. S. Upravlenie traektornym dvizheniem kvadrokoptera pri transportirovke gruza na podvese // Materialy XIV Mezhdunarodnoj nauchnoj konferencii "Ustojchivost' i kolebanija nelinejnyh sistem upravlenija"

7. Dimova A. S., Kotov K. Y., Maltsev A. S. et al. Quadrotor Control in Payload Transportation on Suspension. Optoelectronics, Instrumentation and Data Processing, 2018, vol. 54, no. 5, p. 520-524. DOI 10.3103/S8756699018050151

8. Quigley M., Conley K., Gerkey B. P. et al. ROS: an opensource robot operating system. In: ICRA Workshop on Open Source Software, IEEE, 2009, vol. 3, p. 5.

9. Meier L., Honegger D., Pollefeys M. PX4: A node-based multithreaded open source robotics framework for deeply embedded platforms. In: IEEE International Conference on Robotics and Automation (ICRA), 2015, p. 6235-6240.

10. Belokon S. A., Zolotukhin Y. N., Kotov K. Y. et al. Using the Kalman filter in the quadrotor vehicle trajectory tracking system. Optoelectronics, Instrumentation and Data Processing, 2013, vol. 49, no. 6, p. 536-545. DOI 10.3103/S8756699013060022

Материал поступил в редколлегию Received 10.10.2019

Сведения об авторах

Александра Сергеевна Димова, студент, инженер-программист, Институт автоматики и электрометрии СО РАН (пр. Академика Коптюга, 1, Новосибирск, 630090, Россия) salat77@mail.ru

Константин Юрьевич Котов, кандидат технических наук, зав. лаб. нечетких технологий, Институт автоматики и электрометрии СО РАН (пр. Академика Коптюга, 1, Новосибирск, 630090, Россия) kotov@idisys.iae.nsk.su

Александр Сергеевич Мальцев, кандитат технических наук, научный сотрудник, Институт автоматики и электрометрии СО РАН

alexandr@idisys.iae.nsk.su

Евгений Денисович Семенюк, аспирант, инженер-исследователь, Институт автоматики и электрометрии СО РАН (пр. Академика Коптюга, 1, Новосибирск, 630090, Россия) sedzoom@gmail.com

Максим Андреевич Соболев, младший научный сотрудник, Институт автоматики и электрометрии СО РАН (пр. Академика Коптюга, 1, Новосибирск, 630090, Россия) max@ idisys.iae.nsk.su

Information about Authors

Aleksandra S. Dimova, student, software engineer, Institute of Automation and Electrometry SB RAS (1 Academician Koptyug Ave., Novosibirsk, 630090, Russia) salat77@mail.ru

Konstantin Yu. Kotov, Candidate of Science (Techniques), Head of the Fuzzy Technologies Laboratory, Institute of Automation and Electrometry SB RAS (1 Academician Koptyug Ave., Novosibirsk, 630090, Russia) kotov@idisys.iae.nsk.su

Aleksandr S. Maltsev, Candidate of Science (Techniques), Researcher, Institute of Automation and Electrometry SB RAS (1 Academician Koptyug Ave., Novosibirsk, 630090, Russia) alexandr@idisys.iae.nsk.su

Evgeny D. Semenyuk, graduate student, research engineer, Institute of Automation and Electrometry SB RAS (1 Academician Koptyug Ave., Novosibirsk, 630090, Russia) sedzoom@gmail.com

Maksim A. Sobolev, Junior Researcher, Institute of Automation and Electrometry SB RAS (1 Academician Koptyug Ave., Novosibirsk, 630090, Russia) max@ idisys.iae.nsk.su