Размер: px

Начинать показ со страницы:

Транскрипт

1 УДК Алгоритмы стабилизации и управления полетом квадрокоптера Гэн КэКэ, аспирант Россия, г. Москва, МГТУ им. Н.Э. Баумана, кафедра «Системы Автоматического Управления» Чулин Н.А., доцент Россия, г. Москва, МГТУ им. Н.Э. Баумана Квадрокоптер представляет собой беспилотный летательный аппарат (БПЛА, имеющий четыре двигателя с воздушными винтами (пропеллерами, создающими тягу. Оси винтов и углы лопастей зафиксированы и регулируются лишь скорости вращения, что существенно упрощает конструкцию. Вертикальное перемещение осуществляется синхронным изменением скоростей вращения всех винтов, для горизонтального перемещения нужно наклонять квадрокоптер, что достигается соответствующим изменением скоростей вращения разных винтов, создающих необходимые для наклона моменты. Противоположное направление вращения пар винтов обеспечивает компенсацию моментов сопротивления. В настоящее время подобные аппараты используются достаточно широко и разнообразно, но это использование ограничено, в основном, режимами «ручного» дистанционного управления с пульта оператора. Актуальной является задача разработки системы управления, позволяющей осуществлять автономный полёт квадрокоптера по заданному маршруту. В настоящей работе исследуется возможности использования для решения этой задачи алгоритмов стабилизации, построенных на традиционных принципах, в частности на основе ПИД-регуляторов. htt://sntbul.bmstu.ru/oc/ html

2 1.Математическая модель квадрокоптера При моделировании полёта квадрокоптера, дальность и продолжительность которого весьма ограничены, можно пренебречь движением Земли, т.е. считать земные системы координат инерциальными. На начальных этапах разработки можно также считать ненесущую часть аппарата твёрдым телом, а ветер учитывать только как внешнее возмущение. На рис. 1 показаны взаимное положение связанной (x b,y b,z b и нормальной земной (x e,y e,z e систем координат в режиме зависания и действующие на квадрокоптер силы и моменты. Рис. 1. Квадрокоптер в режиме зависания На рисунке Fi (i = 1, 2,3, 4 - силы тяги винтов, M i (i = 1,2,3,4 - моменты от сопротивления винтов. Матрицы преобразования двух систем координат R является: Силы тяги в связанной системе координат: T T Fb = Fxb, Fyb, F zb = [ 0,0, ] = F1 + F2 + F3 + F4 = b(w1 + w2 + w3 + w4 1 2 b = ρ CD Ai Ri 2 (1 Молодежный научно-технический вестник ФС, ISSN

3 где - суммарная тяга, b -коэффициент силы тяги, ρ -плотность воздуха, CD -коэффициент подъемной силы, Ri - радиус винта, Ai - площадь ометаемой лопастями винта поверхности, wi - угловая скорость вращения i-го винта. Сила тяги в нормальной земной системе координат: T Fe = Fxe, Fye, F ze = R Fb = sϕ sψ + cϕ cψ sθ, cϕ sψ sθ cψ sϕ, cϕ c θ T (2 где cψ cθ cψ sϕ sθ cϕ sψ sϕ sψ + cϕ cψ sθ R = cθ sψ cϕ cψ + sϕ sψ sθ cϕ sψ sθ cψ sϕ sθ cθ sϕ cϕ c θ - матрица перехода, (3 C x = cosx, S x = sinx, а ψ, θ, ϕ - углы рыскания, тангажа, крена. Сила сопротивления воздуха и сила тяжести: fe = f x, f y, f z Ge = [ 0,0, mg] T T (4 Уравнения динамики движения центра масс нормальной земной системе координат: (sϕ sψ + cϕ cψ sθ f x ɺɺ x = m (cϕ sψ sθ cψ sϕ f y y ɺɺ = m (c ϕcθ f z mg ɺɺ z = m (5 С учетом симметрии аппарата и считая, что центр масс расположен в начале координат связанной системы, уравнения динамики углового движения в связанной системе координат можно записать в виде: htt://sntbul.bmstu.ru/oc/ html

4 (I yy I zz L ɺ = r + I xx I (I zz I xx L ɺ = r + I yy I (I xx I yy L rɺ = + I zz I 2 xx 3 yy L2 = M + M + M L3 = M + M + M L4 = M z 4 zz x xm x y ym y (6 (7 где = w, = x, = wy r wz - проекции вектора угловой скорости аппарата; xx, Iyy, Izz I - осевые моменты инерции аппарата; M, M, M - моменты, создаваемые винтами, x y z M, M и M xm ym x y M, - гироскопические моменты двигателей и винтов. Если пренебречь инерционность винтов при изменении угловых скоростей их вращения, то указанные моменты можно выразить следующим образом: 2 2 M x = (F2 F4 l = bl(w2 w4 2 2 M y = (F3 F1 l = bl(w3 w M z = M1 + M 3 M 2 M 4 = Mi = (w1 + w3 w2 w4 1 2 = ρ CT Ai Ri 2 (8 M xm = Im (w1 + w3 w2 w4 M ym = Im (w2 + w4 w1 w3 M x = I (w1 + w3 w2 w4 M y = I (w2 + w4 w1 w3 (9 винта, где l - расстояние от центра масс до оси винта, I и m I - моменты инерции ротора и CT -коэффициент аэродинамического сопротивления винта. Изменения углов Эйлера определяются через проекции угловой скорости кинематическими уравнениями Эйлера: ɺ ϕ = + sinϕ tanθ + cosϕ tanθ r ɺ θ = cosϕ r sinϕ ψ ɺ = (sinϕ + r cos ϕ / cosθ (10 Молодежный научно-технический вестник ФС, ISSN

5 Математическое моделирование проводится в программных средах Matlab Simulink и Universal Mechanism . В Matlab Simulink удобно моделировать динамику, интегрируя уравнения движения, Universal Mechanism позволяет по параметрам конструкции определить инерционные характеристики. Совместное использование этих пакетов не только упрощает реализацию модели, но и позволяет получать параллельные результаты, сравнение которых может служить подтверждением их правильности. Модель в Matlab Simulink (см. рис.2 состоит из четырех блоков: входные напряжения, ограничители напряжений, модель двигателей с винтами, модель квадрокоптера. Входными сигналами модели являются напряжения: u = (u1, u2, u3, u4, выходными - координаты полета (x, y, z и углы (ϕ, θ, ψ. Управляющие напряжения на двигатели подаются через распределитель сигналов и ограничители напряжений. Для выбранного двигателя X2212 KV980 с линейной зависимостью скорости от управляющего напряжения скоростей входные напряжения ограничены значениями от 0 до 11,1 В. w = 102, 6 u в рабочем диапазоне 7 u1 x u1 Saturation y 7.1 u2 Saturation1 u2 z Scoe2 L2 L2 7.2 u3 Saturation3 u3 hi 7.15 u4 theta u4 Saturation2 L3 L3 si Scoe1 L4 L4 MOTOR Moel Рис. 2. Математическая модель квадрокоптера в Matlab Simulink B пакете Universal Mechanism (Универсальный Механизм квадрокоптер представлен как твердое дело, имеющее 6 степеней свободы, крестообразной формы с четырьмя двигателях и четырьмя винтами (Рис. 3. Инерционные характеристики такие же, как при задании в пакете Matlab (Рис. 4. htt://sntbul.bmstu.ru/oc/ html

6 Рис. 3 Структура квадрокоптера в пакете Universal Mechanism Рис. 4 Инерционные характеристики модели В программе Universal Mechanism (UM нельзя воспроизвести воздействие воздушной среды и работу пропеллеров, создающих подъемную силу. Поэтому вращение пропеллеров заменяется силами тяги (F 1, F 2, F 3, F 4, действующих на концах перекладин. Эти силы всегда перпендикулярны плоскости платформы квадрокоптера. Значение сил берется из блока, созданного в программе Matlab (Рис. 5. Оттуда же берутся действующие на квадрокоптер моменты M m, M P, M (Рис. 6. Рис. 5. Задание подъемной силы Рис. 6. Задание суммарных моментов Схема подключения и передачи сигналов во время моделирования между моделями Молодежный научно-технический вестник ФС, ISSN

7 в Matlab и UM показана на рисунке 7. Рис. 7. Схема подключения Matlab и UM Сравнение результатов моделирования в двух программных пакетах - в Matlab, где моделируется нелинейная система уравнений, и в UM, где траектория полета и другие параметры рассчитываются самой программой, приведено на рис. 8. Рис. 8. Координаты центра масса и углы поворота квадрокоптера Видно, что без регулятора система неустойчива, но результаты моделирования в Universal Mechanism почти совпадают с результатами моделирования в Matlab, что является признаком правильности математической модели. htt://sntbul.bmstu.ru/oc/ html

8 2 Алгоритмы стабилизации и управления на основе ПИД-регулятора ПИД-регулятор удобно использовать в качестве предварительного алгоритма управления из-за простоты его настройки и реализации. Общая схема системы управления квадрокоптером, включающей подсистему стабилизации его углового движения и подсистему приведения в заданные точки маршрута (координаты траекторного движения, показана на рис. 9. Рис. 9. Схема системы управления квадрокоптера. Блоки: 1,2,4-ПИД-регулятор, 3-преобразователь координат, 5-распределитель сигналов, 6-ограничитель напряжения,7-модель винтомоторный группы, 8-модель квадрокоптера Горизонтальное перемещение аппарата происходит под действием горизонтальной проекции суммарного вектора тяги, отклонённого от вертикали. В рассматриваемом варианте отклонение вектора тяги происходит за счёт изменении углов тангажа и крена при фиксированном положении угла рыскания. Изменение углового положения достигается путем дифференцированного управления скоростями вращения винтов, дающего соответствующие различия их сил тяги и моментов. Стабилизация и управление в вертикальном направлении обеспечивается изменением суммарной величины тяги. Подсистему, обеспечивающую необходимые значения угловых параметров и высоты за счёт изменения тяги винтов, будем считать системой стабилизации, а подсистему, осуществляющую приведение в заданные точки маршрута системой траекторного управления. Молодежный научно-технический вестник ФС, ISSN

9 2.1 Алгоритмы стабилизации ПИД-регуляторы углового положения и высоты имеют вид: U 2 = K ϕ (ϕ ϕ + Ki ϕ (ϕ ϕ t + K (ɺ ϕ ϕ ɺ ϕ U 3 = K θ (θ ϕ + Ki θ (θ θ t + K (ɺ θ θ ɺ θ U 4 = K ψ (ψ ψ + Ki ψ (ψ ψ t + K ψ (ψ ψ ɺ ɺ m U 1 = (K z (z z + Kiz (z z t + K z (z z g Cθ C ɺ ɺ ϕ (11 Схема моделирования в среде Matlab Simulink показана на рис. 10, реакции модели на ступенчатое воздействие (Рис. 11. z_ z_e hi U1 U1 u"1 Saturation u1 z theta U2 U2 u"2 Saturation1 u2 hi hi_ hi_e theta_e U3 U3 u"3 Saturation2 u3 L2 L2 theta theta_ si_ U4 U4 u"4 si_e Saturation3 PID Allocation forces u4 L3 L3 si L4 L4 moel of motors Moel of uarocoter Рис. 10. Схема модели регулирования углового положения и высоты Рис. 11. Реакции на ступенчатоевоздействие (z,φ,θ,ψ htt://sntbul.bmstu.ru/oc/ html

10 Работа алгоритма стабилизации проверена также в режимах отслеживания гладких входных сигналов вида: z = sin 0.5t ϕ = sin 0.5t θ = cos 0.5t ψ = sin(0.5 t + π / 5 (12 и при ветровых воздействиях, проекции которых по осям x, y и z показаны на рис. 12. Результаты моделирования, приведенные на рис. 13, свидетельствуют о приемлемой работоспособности алгоритма стабилизации. Рис.12 Модель ветровых воздействий по осям x,y,z Рис.13 Результаты моделирования по координатам (z,φ,θ,ψ 2.2 Алгоритмы точек траекторного управления Проекции горизонтальной силы без учёта сопротивления ненесущей части аппарата согласно уравнениям динамики (5 имеют вид: U x = L (s s + c c s ; = L (c s s c s, (13 1 ϕ ψ ϕ ψ θ U y 1 ϕ ψ θ ψ ϕ откуда можно определить углы крена и тангажа, при которых создаются требуемые воздействия при известной суммарной тяге L 1: ϕ U x sψ U ycψ U xcψ + U y sψ = arcsin ; θ = arcsin. (14 L L 1 1 Молодежный научно-технический вестник ФС, ISSN

11 Управляющие воздействия U x и U y, в свою очередь, можно получить как выходные сигналы ПИД-регулятора по отклонениям координат центра масс от требуемых: U U x y = K (x x + K (x x t + K (xɺ xɺ ; (15 x ix x = K (y y + K (y y t + K (yɺ yɺ (16 y iy y Значения коэффициентов ПИД-регуляторов по соответсвующим переменным приведены в таблице: K K i K x y z φ θ ψ Схема моделирования в среде Matlab Simulink показана на рис. 14. z_e U1 U1 u"1 u1 z x z_ f i_ hi Saturation hi y x_ x_e x_a x theta U2 U2 u"2 Saturation1 u2 theta z y _ trajectory y _e y _a PID out y theta_ si hi_e U3 theta_e U3 u"3 Saturation2 u3 L2 L2 si converter si_ U4 U4 u"4 si_e Saturation3 PID in Allocation forces u4 L3 L3 x L4 L4 y moel of motors Moel of uarocoter Рис. 14. Схема моделирования системы управления Работоспособность алгоритма проверялась моделированием перелёта в задаваемые точки, а также набора высоты, выхода на круговую траекторию и движения по ней. Переключение в следующую заданную точку происходило при попадании в окрестность htt://sntbul.bmstu.ru/oc/ html

12 очередной точки радиусом 0.1 м. На рис. 15 показаны изменения координат x, y, z при переключениях (моментам переключения соответствуют изломы на кривых; на рис. 16 получившиеся траектории. Результаты в целом удовлетворительны, хотя видны возможности улучшения: устранение перерегулирования по высоте и ускорение процессов. Рис. 15. Изменение координат при переключениях Рис. 16. Траектория отслеживания заданных точек(заданные точки обозначены звёздочками На рис. 17 показаны изменения координат x, y, z, на рис результаты моделирования при отслеживании траектории состоящей из частьях: AB - взлет; BC - равномерное прямолинейное движение; CD - равномерное ускоренное прямолинейное движение; DE - равномерное движение вокруг точки (15,5,10 по горизонтальному кругу радиусом 5 м.; EF - равномерное прямолинейное движение; FG - равномерное движение вокруг точки (10,-5,10 по горизонтальному кругу радиусом 10 м.; GH - посадка. Рис. 17. Изменение координат Рис. 18. Отслеживание траектории Молодежный научно-технический вестник ФС, ISSN

13 Заключение Разработана и проверена математическая модель квадрокоптера как объекта управления. Представлена разработка алгоритма управления на основе ПИД-регулятора, который позволяет стабилизировать высоту, угловое положение и координаты полета квадрокоптера. Результаты моделирования показывают работоспособность алгоритма и возможность его реализации. Список литературы 1. Зенкевич С.Л., Ющенко А.С. Основы управления манипуляционными роботами. М.: Изд-во МГТУ им. Н. Э. Баумана, c. 2. Samir Bouaballah, Rolan Siegwart Towars Intelligent Miniature Flying Robots // Fiel an Service Robotics, No. 25. P Голубев Ю.Ф. Основы тероретической механики. М.: Изд-во МГТУ им. Н. Э. Баумана, c. 4. Бесекерский В.А., Попов Е.П. Теория систем автоматического управления. 4-е изд. М.: Изд-во МГТУ им. Н. Э. Баумана, c. 5. Безщеточный мотор. Режим доступа: htt://bkso.baiu.com/view/ htm (дата обращения Программный комплекс «Универсальный механизм». Режим доступа: htt:// (дата обращения Егупов Н. Д., Воронов Е. М., Пилишкин В. Н. Синтез регуляторов систем автоматического управления. 2-е изд. М.: Изд-во МГТУ им. Н. Э. Баумана, c. htt://sntbul.bmstu.ru/oc/ html

УДК 62-523.8 Система стабилизации БЛА Квадрокоптера # 08, август 2012 А.С. Панов, С.П. Чашников Студенты, кафедра «Специальная робототехника и мехатроника» Научный руководитель: Ю.И. Рассадкин, к. т. н.,

Карцев Никита Владимирович магистрант Салыкова Ольга Сергеевна канд. техн. наук, заведующая кафедрой Костанайский государственный университет им. А. Байтурсынова г. Костанай, Республика Казахстан МАТЕМАТИЧЕСКОЕ

Тема 4. Уравнения движения самолета 1 Основные положения. Системы координат 1.1 Положение самолета Под положением самолета понимается положение его центра масс О. Положение центра масс самолета принято

УДК 621.865:4.896 ПОСТРОЕНИЕ РЕШЕНИЯ УРАВНЕНИЙ ДВИЖЕНИЯ МОБИЛЬНОГО МЕХАТРОННОГО КОМПЛЕКСА В НЕПРЯМОУГОЛЬНОЙ И СВЯЗНОЙ СИСТЕМАХ КООРДИНАТ асп. 1 Конон И.И., к.ф.-м.н. 1 Ширвель П.И., инженер 2 Трифанков

УДК 004.942 Создание летательного аппарата с цифровой дистанционной системой управления Коваль Д.О., ученик 11 класса Россия, 140236, Московская область, Воскресенский район, пос. Фосфоритный, МОУ СОШ

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

Краевая научно-практическая конференция учебно-исследовательских работ учащихся 6-11 классов «Прикладные и фундаментальные вопросы математики» Математическое моделирование Математическая модель полета

Санкт-петербургский политехнический университет петра великого ИНСТИТУТ КОМПЬЮТЕРНЫХ НАУК И ТЕХНОЛОГИЙ ВЫСШАЯ ШКОЛА КИБЕРФИЗИЧЕСКИХ СИСТЕМ И УПРАВЛЕНИЯ Работа допущена к защите Руководитель ООП Ефремов

УДК 681.511 АСАУ 10(30) 007 К.Ю. Мелкумян, C.В. Лапковский, В.А. Лемешко CОГЛАСОВАННОЕ УПРАВЛЕНИЕ РОБОТОМ-МАНИПУЛЯТОРОМ Основные положения Объектом управления (ОУ) будем называть неизменяемую часть системы,

Разработка системы автоматического управления беспилотным летательным аппаратом в режиме «зависание» Студент: Андрющенко Т. А. научный руководитель: к. т. н., н. с. ИАиЭ СО РАН Филиппов М. Н. Актуальность

УДК 62-83-529 Синтез мехатронных модулей системы управления квадрокоптера Калинин П.А., студент Россия, 105005, г. Москва, МГТУ им. Н. Э. Баумана, кафедры «Специальная робототехника и мехатроника» Научный

СБОРНИК НАУЧНЫХ ТРУДОВ НГТУ. 2009. 3(57). 33 40 УДК 519.24 РАЗРАБОТКА АЛГОРИТМА СТАБИЛИЗАЦИИ СИСТЕМЫ «ПОДВЕШЕННЫЙ ГРУЗ» Г.В. САБЛИНА, Д.И. ХОДАКОВА Предложен анализ задачи стабилизации груза, подвешенного

126 Теоретическая и прикладная механика ТРУДЫ МФТИ. 2013. Том 5, 2 УДК 531.38 А. А. Адуенко, Н. И. Амелькин Московский физико-технический институт (государственный университет) О предельных движениях волчка

УДК 62-523.8 Моделирование системы автоматического управления квадрокоптером Чжо Мьят Ту., аспирант Россия, 105005, г. Москва, МГТУ им. Н.Э. Баумана, кафедра «Системы автоматического управления» Научный

Введение При проектировании систем стабилизации и управления летательных аппаратов важным этапом является выявление динамических свойств летательного аппарата ЛА как объекта управления Имеется обширная

Атиенсия Вильягомес Х.М. Дивеев 2 А.И. Софронова Е.А. ФГБОУ Российский университет дружбы народов 2 ФГБУН Вычислительный центр им. А.А. Дородницына Российской академии наук СИНТЕЗ ЛОГИКО-ФУНКЦИОНАЛЬНОЙ

Управление высотой полета вертолета Рассмотрим задачу синтеза системы управления движением центра масс вертолета по высоте. Вертолет как объект автоматического управления представляет собой систему с несколькими

Математическая модель шагающего робота # 07 июль 2015 доцент к.т.н. Трудоношин В. А. 1* Чернышов Н. С.1 УДК: 621.865 681.3 1 Россия МГТУ им. Н.Э. Баумана Введение Моделирования поведения шагающего робота

УДК 621.865.8 ИСПОЛЬЗОВАНИЕ КВАТЕРНИОНОВ ПРИ МАТЕМАТИЧЕСКОМ МОДЕЛИРОВАНИИ МЕХАНИЗМОВ С ПАРАЛЛЕЛЬНЫМИ КИНЕМАТИЧЕСКИМИ ЦЕПЯМИ В.А. Смирнов, В.Б. Федоров На примере механизма с параллельными кинематическими

УДК 69.783 В. В. К о р о в и н, А. В. П о п о в, В. И. У с ю к и н КВАТЕРНИОННЫЕ ПАРАМЕТРЫ РОДРИГА ГАМИЛЬТОНА В МОДЕЛИ КОСМИЧЕСКОЙ ТРОСОВОЙ СВЯЗКИ Рассмотрена задача пространственного движения космической

ВВЕДЕНИЕ Условие каждого задания расчетно-графической работы сопровождается десятью рисунками и двумя таблицами числовых значений заданных величин. Выбор вариантов совершается согласно с шифром студента.

УДК 519-711 Разработка мобильного робота и визуалиация его движения Колосков С.С., студент Россия, 105005, г. Москва, МГТУ им. Н.Э. Баумана кафедра «Специальная робототехника и мехатроника» Научный руководитель:

УДК 62-503.55 Исследование взаимовлияния контуров управления автономного необитаемого подводного аппарата «Император» с учетом особенностей организации его движительно-рулевого комплекса Гладкова О.И.,

Задача С1. Определение реакции опор твердого тела. Найти реакции опор конструкции. Дано: P 15 кн, Q 50 кн, М 0 кн м, q 8 кн м, α 60, β 5 Найти: R, R? Решение Для нахождения реакции опор составим уравнения

Моделирование старта ракеты в программном комплексе EULER Цель данного примера показать основные особенности моделирования старта ракеты с учётом аэродинамических сил, помехи от бортового разъемного соединения

Управление пространственным движением схвата роботаманипулятора # 07, июль 015 Белов И. Р. 1, Ткачев С. Б. 1,* УДК: 519.71 1 Россия, МГТУ им. Н.Э. Баумана Введение Методы решения задачи управления движением

# 1, октябрь 216 УДК 531.553 Исследование влияния алгоритма расчета матрицы направляющих косинусов на результаты численного моделирования движения ЛА Веденичев И.В., студент Россия, 155, г. Москва, МГТУ

УДК 531.396 Инерция приводов в уравнениях движения манипуляционных систем роботов О.Н. Крахмалев Предприятие «Промбезопасность БГТУ», Брянск, 41035, Россия Представлены уравнения движения манипуляционных

Автоматическое управление четырехвинтовым вертолетом Пятая Традиционная всероссийская молодежная летняя школа Выполнила: Белинская Ю.С., студентка гр.фн12-121 Кафедра "Математическое моделирование" МГТУ

УДК 474 Управление группой квадрокоптеров для транспортировки симметричного груза Р.Т. Агишев Московский физико-технический институт (государственный университет) I. Математическая модель движения БПЛА

УДК 629.78 ТРЕХМЕРНАЯ ДИНАМИЧЕСКАЯ МОДЕЛЬ ДВИЖЕНИЯ В АТМОСФЕРЕ ИЗДЕЛИЯ С ПЕРЕМЕННЫМИ МАССО-ЦЕНТРОВОЧНЫМИ ХАРАКТЕРИСТИКАМИ А.А. Суслов В статье рассматривается трехмерная динамическая модель движения в

АВТОМЕТРИЯ.. Т., УДК 68..8 УПРАВЛЕНИЕ УГЛОВЫМ ПОЛОЖЕНИЕМ ЛЕТАТЕЛЬНОГО АППАРАТА Ю. Н. Золотухин, А. А. Нестеров Институт автоматики и электрометрии СО РАН, 69, г. Новосибирск, просп. Академика Коптюга,

УПРАВЛЕНИЕ И ОБРАБОТКА ИНФОРМАЦИИ В РОБОТОТЕХНИЧЕСКИХ СИСТЕМАХ УДК 681.5.3 А. А. ПЫРКИН, Т. А. МАЛЬЦЕВА, Д. В. ЛАБАДИН, М. О. СУРОВ, А. А. БОБЦОВ СИНТЕЗ СИСТЕМЫ УПРАВЛЕНИЯ КВАДРОКОПТЕРОМ С ИСПОЛЬЗОВАНИЕМ

ИЗУЧЕНИЕ ЗАКОНОМЕРНОСТЕЙ ДВИЖЕНИЯ ТЕЛА В ПОЛЕ СИЛЫ ТЯЖЕСТИ Цель работы - путем численного моделирования изучить основные закономерности движения тела вблизи поверхности Земли. Кинематическим законом движения

Электронный журнал «Труды МАИ». Выпуск 46 www.mi.ru/science/rud/ УДК 69.7.87 Решение задачи оптимизации управления пространственным движением легкого самолета на основе принципа минимума Понтрягина В.Н.Баранов,

Экстенсивное управление ориентацией околоземного спутника на основе нечеткой логики КБ Алексеев АА Малявин АВ Шадян Московский государственный индустриальный университет 115280 Москва ул Автозаводская

Глава II Построение модели системы управления Реальная система управления состоит из определенного числа взаимосвязанных приборов и устройств, включая, конечно, объект управления, обладающих различной

УДК: 62-529 СИСТЕМЫ АВТОМАТИЧЕСКОГО РЕГУЛИРОВАНИЯ С ПОСЛЕДОВАТЕЛЬНОЙ КОРРЕКЦИЕЙ Виталий Анатольевич Чигарев старший преподаватель Белорусского национального технического университета, [email protected]

336 УДК 6978:3518143 СИНТЕЗ УПРАВЛЕНИЙ ПОЛЕТОМ В АТМОСФЕРЕ ВОЗВРАЩАЕМОГО КОСМИЧЕСКОГО АППАРАТА ВА Афанасьев Казанский национальный исследовательский технический университет им АНТуполева КАИ Россия 456318

ДИНАМИКА МАТЕРИАЛЬНОЙ ТОЧКИ Уравнение движения материальной точки массой в некоторой инерциальной системе отсчета имеет вид () где радиус-вектор материальной точки; векторная сумма сил действующих на нее

УДК 621.446 АНАЛИЗ И ОЦЕНКА СУЩЕСТВУЮЩИХ МЕТОДОВ СТАБИЛИЗАЦИИ КОНСТРУКЦИИ В ВИДЕ ПЕРЕВЕРНУТОГО МАЯТНИКА Баранов Б. М., студент; Суков С. Ф., доц., к.т.н. (ГВУЗ «Донецкий национальный технический университет»,

УДК 69.113.1.5 Модель эластичного колеса для случая его возмущенного движения по траектории незначительной кривизны С.Д. Попов 1 1 МГТУ им. Н.Э. Баумана, Москва 155, Россия Описан подход к построению модели

Вестник Пензенского государственного университета 3, 013 УДК 53.084 О. В. Гаврина АНАЛИЗ РАБОТЫ ДАТЧИКА БИЕНИЙ ВАЛА С БЕГУЩИМ МАГНИТНЫМ ПОЛЕМ Аннотация: В статье рассмотрена конструкция датчика биений

УДК 6-75.4 Поведение роторного вибрационного гироскопа для вращающегося носителя при его колебаниях с частотами кратными частоте собственного вращения А.В. Кулешов В.В. Фатеев МГТУ им. Н.Э. Баумана Москва

Министерство образования Российской Федерации Государственное образовательное учреждение высшего профессионального образования «САМАРСКИЙ ГОСУДАРСТВЕННЫЙ ТЕХНИЧЕСКИЙ УНИВЕРСИТЕТ» Кафедра «МЕХАНИКА» ДИНАМИКА

Имитационная модель аксиального индукторного генератора с релейным регулятором напряжения 77-48211/631407 # 09, сентябрь 2013 Трунин Ю. В. УДК 621.313.323 Россия, МГТУ им. Н.Э. Баумана [email protected]

Электронный журнал «Труды МАИ». Выпуск 70 www.mai.ru/science/trudy/ УДК 004.89 Нейросетевая реализация автоматического управления безопасной посадкой беспилотного летательного аппарата Кузин А.В 1 *.,

Электронный журнал «Труды МАИ». Выпуск 49 www.mai.ru/science/trudy/ УДК- 69.7.017.1+519.85 Интегрированная система автоматического управления продольным движением летательного аппарата в строю при регулировании

Моделирование нелинейной системы управления мобильными объектами в среде Matlab Шарипбаев А.А. д.т.н., профессор, заведующий кафедрой "Вычислительная техника" ЕНУ им Л.Н.Гумилева, Атанов С.К., к.т.н, доцент

ТЕОРЕТИЧЕСКАЯ МЕХАНИКА СЕМЕСТР ЛЕКЦИЯ 13 ПРИВЕДЁННАЯ СИСТЕМА ПОТЕНЦИАЛ РАУСА СТАЦИОНАРНЫЕ ДВИЖЕНИЯ КОНСЕРВАТИВНОЙ СИСТЕМЫ С ЦИКЛИЧЕСКИМИ КООРДИНАТАМИ И ИХ УСТОЙЧИВОСТЬ ТЕОРЕМА РАУСА Лектор: Батяев Евгений

ОТКРЫТОЕ АКЦИОНЕРНОЕ ОБЩЕСТВО «ОПЫТНО КОНСТРУКТОРСКОЕ БЮРО ИМЕНИ А.С. ЯКОВЛЕВА» Ю.И. Янкевич, В.А. Подобедов, А.В. Матвеев, Е.Д. Икрянников, А.А. Махуков Моделирование движения беспилотного летательного

1 Направления подготовки: Авионика Аэронавигация Системная инженерия Бортовые системы управления Дисциплина: Курс, семестр, уч. год: 3, весенний, 2011/2012 Кафедра: 301 СУЛА Руководитель обучения: ассистент

МАТЕМАТИЧЕСКОЕ МОДЕЛИРОВАНИЕ ТЕХНОЛОГИЧЕСКИХ ПРОЦЕССОВ С ИСПОЛЬЗОВАНИЕМ МАЛЫХ ЛЕТАТЕЛЬНЫХ АППАРАТОВ д.т.н. Огородников П.И. Усик В.В. Оренбургский филиал ИЭ УрО РАН г. Оренбург, Россия Модернизация сельскохозяйственного

5.3. Законы Ньютона При рассмотрении движении материальной точки в рамках динамики решаются две основные задачи. Первая или прямая задача динамики заключается в определении системы действующих сил по заданным

ФГБОУ ВПО «Омский государственный технический университет» РАЗДЕЛ II НЕПРЕРЫВНЫЕ ЛИНЕЙНЫЕ СИСТЕМЫ АВТОМАТИЧЕСКОГО УПРАВЛЕНИЯ Лекция 5 СОСТАВЛЕНИЕ ИСХОДНЫХ ДИФФЕРЕНЦИАЛЬНЫХ УРАВНЕНИЙ СИСТЕМ АВТОМАТИЧЕСКОГО

00-0 уч. год., кл. Физика. Основные законы механики.. Динамика В динамике механическое движение изучается в связи с причинами, вызывающими тот или иной его характер. В инерциальных системах отсчёта этими

УДК 61.1 Ф.А. ДОРОНИН, В.С. ДОЕВ ИССЛЕДОВАНИЕ ДВИЖЕНИЯ ПЛОСКОГО МЕХАНИЗМА С ПОМОЩЬЮ ПАКЕТА MATHCAD В статье излагается алгоритм составления и решения системы дифференциальных уравнений, описывающих движение

Лабораторная работа 1.1 ИЗУЧЕНИЕ ЗАКОНОМЕРНОСТЕЙ ДВИЖЕНИЯ ТЕЛА В ПОЛЕ СИЛЫ ТЯЖЕСТИ ЗЕМЛИ: ДВИЖЕНИЯ ТЕЛА, БРОШЕННОГО ПОД УГЛОМ К ГОРИЗОНТУ. Цель работы: путем численного моделирования выяснить основные

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

ТЕОРЕТИЧЕСКАЯ МЕХАНИКА.3. Динамика. Динамика это часть теоретической механики, в которой рассматривается движение материальной точки или тела под действием приложенных сил, а также устанавливается связь

УДК 004 ПРОЕКТИРОВАНИЕ СИСТЕМЫ АВТОМАТИЧЕСКОГО УПРАВЛЕНИЯ ПРИВОДАМИ МАЛОГАБАРИТНОГО КВАДРОКОПТЕРА Хохлов А.В., Лушников Б.В. Юго-Западный государственный университет В статье представлены результаты проектирования

УДК 62-112.9 Разработка кинематической схемы манипулятора типа «хобот» Лихтенберг С.Р., студент Россия, 105005, г. Москва, МГТУ им. Н.Э. Баумана, кафедра «Робототехнические системы» Научный руководитель:

1 Электронный журнал «Труды МАИ». Выпуск 46 www.mai.ru/science/trudy/ УДК 629.7.05 Программно-автоматическое управление боковым движением беспилотного летательного аппарата по ликвидации угла сноса В.Д.Елисеев,

В.Д. Суслов, Д.В. Козис УДК 621.396.988.6: 629.19 МОДЕЛИРОВАНИЕ ТРАЕКТОРИИ ПОЛЕТА В НАВИГАЦИОННЫХ КОМПЛЕКСАХ ЛЕТАТЕЛЬНЫХ АППАРАТОВ В ГОРИЗОНТАЛЬНОЙ ПЛОСКОСТИ В.Д. Суслов, Д.В. Козис Рассматривается подход

БАНК ЗАДАНИЙ ПО НАПРАВЛЕНИЮ «МЕХАТРОНИКА И РОБОТОТЕХНИКА» ЗАДАНИЯ 6 65 ПРИМЕР Исследуемая система Манипулятор, схема которого приведена на рисунке, работает в вертикальной плоскости Стрела манипулятора

Примеры решения задач Пример 1 Через вращающийся вокруг горизонтальной оси блок (рис1а) перекинута невесомая нерастяжимая нить к концам которой привязаны грузы 1 и Найдите силу давления X N F блока на

1. ТЕОРЕТИЧЕСКАЯ МЕХАНИКА 1.. Кинематика. Кинематика это часть теоретической механики, в которой изучается механическое движение материальных точек и твердых тел. Механическое движение это перемещение

ДИНАМИКА ВРАЩАТЕЛЬНОГО ДВИЖЕНИЯ ТВЕРДОГО ТЕЛА ВОКРУГ НЕПОДВИЖНОЙ ОСИ Основные формулы Момент силы F действующей на тело относительно оси вращения M = F l где F проекция силы F на плоскость перпендикулярную

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

1. Неподвижная система координат. Начало находиться в точке O, в которой необходимо стабилизировать квадрокоптер. Оси OX и OY расположены в горизонтальной плоскости, а ось OZ направлена вверх.

2. Подвижная система координат. Начало находиться в центре масс квадрокоптера, в точке. Оси этой системы координат: , и сонаправлены с осями координат неподвижной СК. Таким образом, подвижная СК получается из неподвижной СК параллельным переносом на радиус вектор центра масс квадрокоптера в неподвижной СК.

Рис. 1.2.

Момент инерции

Пусть квадрокоптер лежит в плоскости OXY, его центр масс находиться в точке O, а балки AC и BD, на которых расположены роторы, лежат вдоль осей OX и OY. Момент инерции квадрокоптера одинаков вокруг любой оси MN, лежащей в плоскости квадрокоптера OXY. Обозначим массу квадрокоптера как. Пусть масса каждого ротора сосредоточена в точках A, B, C и D и равна, а вся масса корпуса равномерно распределена на отрезках АС и BD и равна.

Рис. 1.3.

Тогда момент инерции квадрокоптера относительно любой оси MN составляющей с осью BD угол б равен:

Момент инерции относительно оси OZ, перпендикулярной плоскости OXY и проходящей через точку O, вычисляется по формуле:

Уравнения движения

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

Уравнения движения центра масс

Вектор силы тяжести приложен к центру масс квадрокоптера и имеет вид: , где - ускорение свободного падения.

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

Вектор суммарной силы тяги всех роторов также приложен к центру масс и имеет вид: , где, и - силы тяг первого, второго, третьего и четвертого роторов соответственно

Вектор некоторой посторонней силы обозначим как. В случае когда сила вызвана ветром, так как сила, с которой действует ветер на квадрокоптер фактически является силой сопротивления воздуха.

Таким образом, векторное уравнение описывающее движение цента масс квадрокоптера в неподвижной системе координат имеет вид:

Скорость можно получить выразив ускорение и проинтегрировав его по времени:

Проинтегрировав скорость, получается радиус вектор до центра масс квадрокоптера, т.е. координаты положения центра масс квадрокоптера:

Уравнение моментов:

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

И - радиус векторы роторов в подвижной системе координат. Длины этих векторов равны между собой и равны.

Рис. 1.4.

В данном случае удобней разбить это уравнение на два: первое уравнение будет описывать вращение вокруг оси симметрии квадрокоптера, второе уравнение будет описывать вращение вокруг оси лежащей в плоскости квадрокоптера. Тогда первое уравнение имеет вид:

Выразив угловое ускорение из этого уравнения и проинтегрировав его по времени, можно получить угловую скорость:

где, и в - компоненты вектора по осям координат.

Проинтегрировав по времени угловую скорость, можно получить углы поворота квадрокоптера вокруг осей подвижной СК:

где компоненты, и вектора представляют собой углы поворота вокруг осей нормальной системы координат OX, OY и OZ соответственно.

Рассмотрим вращение квадрокоптера вокруг оси перпендикулярной плоскости. Обозначим реактивный момент винтов как. Вектор этого зависит только от величины и направлен вдоль нормали к плоскости квадрокоптера. Кроме реактивного момента существует еще сонаправленый ему момент сил, вызванный гироскопическим эффектом из-за изменения гироскопических моментов роторов, который тоже может вращать корпус квадрокоптера вокруг этой оси, но так как роторы вращаются в разных направлениях его можно не рассматривать. Тогда модуль углового ускорения выражается формулой:

Где - некоторая функция зависящая от величины.

Проинтегрировав это выражение можно получить модуль угловой скорости, при этом полученная угловая скорость всегда будет направлена перпендикулярно плоскости квадрокоптера. Обозначим единичный вектор направленный перпендикулярно плоскости квадрокоптера как. Таким образом, получаем:

А проинтегрировав это уравнение получается углы поворота квадрокоптера вокруг осей подвижной СК.

Таким образом, суммарные значение угловой скорости и углов поворота будут равны:

Данная статья скорее логическое продолжение моей статьи о балансере: «Создание робота балансера на arduino» .
В ней будут очень кратко освещены: простая модель угловой стабилизации квадрокоптера с использованием кватернионов, линеаризация, построение управления для объекта и проверка его в Matlab Simulink, а так же проверка на реальном объекте. В качестве подопытного будет выступать Crazyflie 1.0.

Сейчас оно летает так (на момент съемок я не очень правильно выставил управление):

Построение динамической системы

Вращение тела удобнее представлять, используя кватернионы, в связи с меньшим количеством необходимых вычислений. О них написано много статей, в том числе и на хабре. Я рекомендую к прочтению книгу «Бранец В.Н., Шмыглевский И.П. Применение кватернионов в задачах ориентации», спасибо Slovak из центра компетенций MathWorks за подсказку.

Воспользуемся основным законом динамики вращательного движения:

Где
- моменты, действующие на тело,
I - тензор инерции, а
- угловые скорости по главным осям(в связанной системе координат).
Таким образом:
.

В силу теоремы о приведении тензора инерции к главным осям, тензор инерции представим в виде: .

Внешние моменты определим через управления: , где

Таким образом, уравнения угловых скоростей в связанной системе координат:

Замечу, что если бы мы учитывали положение коптера, можно было бы не вводить отдельные функции управлений, а сразу использовать в качестве них силы тяги, что удобнее и быстрее при расчетах. В данном случае система стабилизации не имеет никаких данных о необходимой сумме сил тяги, поэтому необходимо использовать именно такие управления…

Сила тяги пропеллера может быть примерно описана как . Тогда уравнения можно записать через угловые частоты пропеллеров, если вы сможете управлять напрямую частотой моторов и знаете конкретное b:
где
- углы эйлера
Замечу, что подбор коэффициента b у меня произведен вручную, простым подбором.

Также необходимо выписать уравнение для кватерниона вращения. Из свойств кватернионов следует, что
, где являются угловыми скоростями в связанной с ЛА системе координат, в ней гироскопы измеряют угловую скорость .

Попробуем стабилизировать только углы и угловые скорости:

Или подробнее

Введем вектор пространства состояний:
.
Необходимо заметить, что если в вектор пространства входит компонента система перестает быть управляемой. Однако мы можем считать, что и убрать ее из вектора состояний, тем самым уменьшив количество координат .

Вектор управлений:
,

Система представима в стандартном виде

В нашем случае

, а

Линеаризация и построение управления

,

Как и в прошлый раз используем линейно-квадратичный регулятор. Напомню команду Matlab для его расчета:
=lqr(A,B,Q,R)
Матрицы Q и R являются весовыми матрицами. Q штрафует за отклонение от нуля, а R за расход энергии управлением.
В результате получили матрицу K. В моей матрице коэффициентов все недиагональные элементы были очень малы (порядка 10^-4) и я не стал учитывать их.
Напомню, что для получения управления необходимо умножить матрицу K на вектор X. Конечно, в коде можно не вводить понятие матрицы и просто умножить каждую координату на некоторый коэффициент для быстродействия.

Проверка модели

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

Код

Необходимо добавить код получения кватерниона в sensfusion6.c:

Void sensfusion6GetQuaternion(float* rq0,float* rq1,float* rq2,float* rq3){ *rq0=q0; *rq1=q1; *rq2=q2; *rq3=q3; }

Я не стал добавлять отдельный модуль для LQR регулятора, вместо этого я изменил stabilizer.c. Да, возможно, это и не самый интеллигентный способ, однако для проверки модели он подойдет.

Начать стоит с добавления переменных текущего и желаемого положения аппарата, а так же управлений:

Static float q0Actual; static float q1Actual; static float q2Actual; static float q3Actual; static float q1Desired; static float q2Desired; static float q3Desired; int16_t actuatorU1; int16_t actuatorU2; int16_t actuatorU3;

Желаемое положение по q0 не указываем в силу того, что нам не нужно его стабилизировать.

Произведем изменения в код получения команд. Коптер получает угол в градусах, математически правильнее сделать так:

СommanderGetRPY(&q1Desired, &q2Desired, &q3Desired); q1Desired=cos((-q1Desired/2+90)*0.01745);//*3.14/180/2; q2Desired=cos((q2Desired/2+90)*0.01745); q3Desired=cos((q3Desired/2+90)*0.01745);

Изменим «быстрый» цикл (250Гц) стабилизатора:

Sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT); sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); sensfusion6GetQuaternion(&q0Actual, &q1Actual,&q2Actual,&q3Actual); sensfusion6UpdateP(FUSION_UPDATE_DT); sensfusion6UpdateV(acc.x, acc.y, acc.z, FUSION_UPDATE_DT); actuatorU1=50*(1*(-gyro.x)+245*(q1Actual-q1Desired)); actuatorU2=50*(1*(gyro.y)-200*(q2Actual-q2Desired)); actuatorU3=50*(1.5*(gyro.z)+0*(q3Actual-q3Desired));
Подбор коэффициентов произведен опытным путем, так как не было возможности узнать зависимость между посылаемой на моторы командой и силой, которую выдает мотоустановка.

Также я изменил функцию распределения мощностей моторов:
static void distributePower(const uint16_t thrust, const int16_t u2, const int16_t u3, const int16_t u4) { motorPowerM1=limitThrust((thrust/4+u3/2+u4/4)*5); motorPowerM2=limitThrust((thrust/4-u2/2-u4/4)*5); motorPowerM3=limitThrust((thrust/4-u3/2+u4/4)*5); motorPowerM4=limitThrust((thrust/4+u2/2-u4/4)*5); motorsSetRatio(MOTOR_M1, motorPowerM1); motorsSetRatio(MOTOR_M2, motorPowerM2); motorsSetRatio(MOTOR_M3, motorPowerM3); motorsSetRatio(MOTOR_M4, motorPowerM4); }

Заключение