Вы находитесь на странице: 1из 15

УДК 681.

А. С. Мартинез Леон*, Ян Найнг Со*


аспиранты кафедры механики, мехатроники и робототехники
С. Ф. Яцун*
доктор технических наук, профессор – научный руководитель
Юго-западный государственный университет (ЮЗГУ)
*

УПРАВЛЕНИЕ ЭЛЕКТРОПРИВОДАМИ МУЛЬТИРОТОРНОЙ


СИСТЕМЫ ПРИ ПОМОЩИ МОДУЛЯ МPU-6050

Аннотация. В данной статье рассматривается вопрос о управлении


электроприводами мультироторной системы, выявлены достоинства и
недостатки существующие на сегодняшний день методы управления БПЛА,
разработан цифровой комплементарный фильтр для обработки сигналов от
инерциального модуля MPU-6050, предложен алгоритм управления
электроприводами беспилотного летательного аппарата (БПЛА) типа
квадрокоптер с использованием ПИД-регулятора, написан код программы в
среде Arduino IDE на базе микроконтроллера Arduino Nano.
Ключевые слова: Беспилотные летательные аппараты (БПЛА), управление
электроприводами БПЛА, инерциальный модуль MPU-6050, ПИД-регулятор
для БПЛА.

A. S. Martinez Leon, Yan Naing Soe


Ph.D Students, Department of Mechanics, Mechatronics and Robotics
S. F. Jatsun
Doctor of Technical Sciences, professor – Advisor
*
Southwest State University (SWSU)
UAV MOTOR CONTROL SYSTEM USING MPU6050 SENSOR

Abstract. This paper discusses the issue of electric drives controlling of a multi-rotor
system, reveals the advantages and disadvantages of existing UAV control methods.
A digital complementary filter for processing signals from the inertial module MPU-
6050 has been developed, an algorithm for controlling electric drives of an
unmanned aerial vehicle (UAV) of the type quadcopter using a PID controller has
been proposed, and also a program code in the Arduino IDE based on the Arduino
Nano microcontroller has been written.

Keywords: Unnamed Aerial Vehicle (UAV), UAV motor control, inertial module
MPU-6050, PID control for UAV.

Введение
Разработка беспилотных летательных аппаратов (БПЛА) является
динамично развивающимся направлением. Особый интерес представляют
небольшие БПЛА мультироторного типа благодаря особенностям и областям
применения. Основной целью при разработке подобных БПЛА является
повышение автономности полета, что увеличивает надежность аппарата,
обеспечивает простоту в использовании и расширяется круг, выполняемых
задач, как и в гражданском, так и военном секторе. [1, 2, 6-10].
Одной из наиболее значимых задач при создании и конструировании
разных технических систем является обеспечение необходимого качества и
надежности управления объекта при возмущающих воздействиях.
Следовательно, выбор одного из множеств метода управления зависит от вида
задач, возлагаемых на БПЛА [3-5].
Основные преимущества и недостатки основных алгоритмов
управления БПЛА [1, 13-17] представлены в таблице 1.
Таблица 1
Алгоритмы управления
Характеристики
Алгоритмы

Быстродействие
Искусственный
Оптимальность

Управляемость

Шум сигнала
Робастность
управления

Подавление
Адаптация

настройка
интеллект

Простота
Точность
БПЛА

Ручная
помех
1 ПИД-регулятор 1 0 0 0 1 1 2 0 0 2 1
2 ЛК регулятор 0 2 1 0 1 0 1 1 0 1 1
3 Скользящий
регулятор 1 2 1 0 2 2 1 2 1 0 0
4 Бэкстеппинг 0 2 0 0 0 1 0 2 1 0 0
5 Нечетная логика 1 1 1 2 1 1 1 1 0 1 0
6 Нейронные сети 1 2 2 2 1 1 0 1 1 0 0

где, 0 - низкий, 1 - средний, 2 - высокий показатель.

Инерциальный модуль MPU-6050

В данной статье рассмотрено управление электроприводами БПЛА типа


квадрокоптер путем использования инерциального модуля MPU-6050 и ПИД-
регулятора на базе микроконтроллера Arduino Nano.
В состав инерциального модуля MPU6050 входят два датчика: гироскоп
и акселерометр. Совместное использование этих двух датчиков позволяет
определить положение квадрокоптера в пространстве [18, 21].
Гироскоп измеряет угловые скорости по трем осям по следующим
формулам [19]:
𝑡2 𝑡2 𝑡2

𝛗giro = ∫ 𝛗̇ 𝑑𝑡 , 𝛉giro = ∫ 𝛉̇ 𝑑𝑡 , 𝛙giro = ∫ 𝛙̇ 𝑑𝑡


𝑡1 𝑡1 𝑡1

(1)
где, 𝛗, 𝛉, 𝛙 – углы крена, тангажа и рыскания соответственно; 𝛗̇, 𝛉̇, 𝛙̇ –
угловые скорости относительно осей X, Y и Z; 𝑡1 , 𝑡2 – время начала и конца
измерения.

Акселерометр предназначен для компенсации увода гироскопа и


вычисления начальных углов отсчета и определяется следующим формулам
[19]:
𝛗𝑎𝑐𝑐
𝛗𝑎𝑐𝑐 = 𝑡𝑎𝑛−1 ,
√𝛉2𝑎𝑐𝑐 + 𝛙2𝑎𝑐𝑐
𝛉𝑎𝑐𝑐
𝛉𝑎𝑐𝑐 = 𝑡𝑎𝑛−1 ,
√𝛗2𝑎𝑐𝑐 + 𝛙2𝑎𝑐𝑐
𝛙𝑎𝑐𝑐
𝛙𝑎𝑐𝑐 = tan−1
√𝛗2𝑎𝑐𝑐 + 𝛉2𝑎𝑐𝑐
(2)

где, 𝛗𝑎𝑐𝑐 , 𝛉𝑎𝑐𝑐 , 𝛙𝑎𝑐𝑐 – углы, измеряемый акселерометром по осям X, Y, Z.

Обработка полученных значений углов от гироскопа и акселерометра


производиться путем использования цифрового комплементарного фильтра,
чтобы снизить погрешность при расчетах гироскопа и уменьшить
чувствительность акселерометра к внешним воздействиям. Комплементарный
фильтр работает на базе фильтров верхних и нижних частот [18, 20, 21].

Комплементарный фильтр может быть представлен в виде [20]:


𝛼𝑖 = 𝑘1 ∙ (𝛼𝑖−1 + 𝛼𝑔𝑦𝑟𝑜 ∙ ∆𝑡) + 𝑘2 ∙ 𝛼𝑎𝑐𝑐 (3)

где, 𝛂𝐢 – выходной сигнал, 𝛂𝑔𝑦𝑟𝑜 – угол гироскопа, 𝛂𝑎𝑐𝑐 – угол акселерометра,


𝑘1 , 𝑘2 –коэффициенты пропорциональности.

Следовательно, обработанные значения углов крена (φ), тангажа (θ) и


рыскания (ψ) системы примут вид [20, 21]:
𝜑 = 0,98 ∙ (𝜑 + 𝜑𝑔𝑖𝑟𝑜 ∙ ∆𝑡) + 0,02 ∙ 𝜑𝑎𝑐𝑐 , 𝜃 = 0,98 ∙ (𝜃 + 𝜃𝑔𝑖𝑟𝑜 ∙ ∆𝑡) + 0,02 ∙ 𝜃𝑎𝑐𝑐 ,

𝜓 = 0,98 ∙ (𝜓 + 𝜓𝑔𝑖𝑟𝑜 ∙ ∆𝑡) + 0,02 ∙ 𝜓𝑎𝑐𝑐 (4)

где, 𝜑, 𝜃, 𝜓 - отфильтрованные значения углов крена (φ), тангажа (θ) и


рыскания (ψ).

Алгоритм управления

На рис.1 представлена схема подключения электромеханических и


электронных компонентов, входящих в состав конструкции БПЛА типа
квадрокоптер [11, 12, 19].

Рис.1. Функциональная схема БПЛА типа квадрокоптер

На рис.2-4 представлен алгоритм управления квадрокоптером в режиме


стабилизации при постоянной высоте с использованием ПИД-регулятора.
Начало

Инициализация компонентов

Определение углов:
крен (φ= φ desired), тангаж(θ= θ desired)

Считывание данных c MPU6050

Определение ошибки между текущих и


желаемых значений углов φ, θ
roll_error = Total_angle_y - roll_desired_angle;

pitch_error = Total_angle_x - pitch_desired_angle;

Введение ПИД-регулятора для


каждого контура: φ, θ

Определение и формирование PWM


pwm_R_F = 115 + input_THROTTLE - roll_PID
- pitch_PID;

pwm_R_B = 115 + input_THROTTLE - roll_PID


+ pitch_PID;

pwm_L_B = 115 + input_THROTTLE + roll_PID


+ pitch_PID;

pwm_L_F = 115 + input_THROTTLE + roll_PID


- pitch_PID;

да
Новая цель?

нет

Конец

Рис.2. Основной алгоритм управления электроприводами мультироторной


системы при помощи MPU-6050
Считывание данных
c MPU6050

Измерение ошибки гироскопа и акселерометра по осям X, Y:

Gyro_raw_error_x =Gyro_raw_error_x + (Gyr_rawX/32.8);


Gyro_raw_error_y = Gyro_raw_error_y +(Gyr_rawY/32.8);

Acc_angle_error_x = Acc_angle_error_x +
((atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) +
pow((Acc_rawZ),2)))*rad_to_deg));

Acc_angle_error_y = Acc_angle_error_y + ((atan(-


1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) +
pow((Acc_rawZ),2)))*rad_to_deg));

Gyro_raw_error_x = Gyro_raw_error_x/200;
Gyro_raw_error_y = Gyro_raw_error_y/200;

Acc_angle_error_x = Acc_angle_error_x/200;
Acc_angle_error_y = Acc_angle_error_y/200;

Определение текущих значений углов, измеренных


гироскопом и акселерометром по отдельности:
Gyr_rawX = (Gyr_rawX/32.8) - Gyro_raw_error_x;

Gyr_rawY = (Gyr_rawY/32.8) - Gyro_raw_error_y;

Gyro_angle_x = Gyr_rawX*elapsedTime;

Gyro_angle_y = Gyr_rawY*elapsedTime;

Acc_angle_x = (atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) +
pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_x;

Acc_angle_y = (atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) +
pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_y;

Использование цифрового комплементарного фильтра.


Total_angle_x = 0.98 *(Total_angle_x + Gyro_angle_x) +
0.02*Acc_angle_x;

Total_angle_y = 0.98 *(Total_angle_y + Gyro_angle_y) +


0.02*Acc_angle_y;

Конец

Рис.3. Считывание данных c MPU6050


Введение ПИД-регулятора
для каждого контура: φ, θ

Введение коэффициентов kp, ki, kd:


roll_pid_p = roll_kp*roll_error;
pitch_pid_p = pitch_kp*pitch_error;

roll_pid_i = roll_pid_i+(roll_ki*roll_error);
pitch_pid_i = pitch_pid_i+(pitch_ki*pitch_error);

roll_pid_d = roll_kd*((roll_error -
roll_previous_error)/elapsedTime);
pitch_pid_d = pitch_kd*((pitch_error -
pitch_previous_error)/elapsedTime);

Реализация ПИД-регулятора по контурам 𝜑, 𝜃:


roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;

Конец

Рис.4. Введение ПИД-регулятора для каждого контура: φ, θ


Обработка полученных сигналов от инерциального модуля MPU-6050
производится путем использования цифрового комплементарного фильтра.

Рис.5. Обработка сигналов от инерциального модуля MPU-6050:


сигнал от акселерометра, сигнал от гироскопа, обработанный сигнал
Далее в среде Arduino IDE написан код программы для управления
электроприводами БПЛА типа квадрокоптер для достижения заданных
оператором углов крена 𝜑 и тангажа 𝜃 при условии, что угол рыскания 𝜓 =
0𝑜 .
На рис. 5-6 представлены результаты работы алгоритма управления
квадрокоптером в режиме стабилизации при постоянной высоте с
использованием ПИД-регулятора.

Рис.6. Управление БПЛА типа квадрокоптер по углу крена 𝜑:


желаемое значение, реальное значение

Рис.6. Управление БПЛА типа квадрокоптер по углу тангажа 𝜃:


желаемое значение, реальное значение
Заключение
В данной статье проведен анализ существующих методов управления
беспилотными летательными аппаратами, предложен алгоритм управления
электроприводами БПЛА типа квадрокоптер. Введен цифровой
комплементарный фильтр для обработки сигналов, полученных от
инерциального модуля MPU-6050. Использованный ПИД-регулятор
позволяет управлять самолетными углами для обеспечения стабилизации и
устойчивости к внешним воздействиям БПЛА.

Библиографический список

1. Яцун С.Ф., Емельянова О.В., Казарян К.Г. Алгоритм управления


беспилотным летательным аппаратом типа конвертоплан//
Всероссийский научно-практический семинар «Беспилотные
транспортные средства с элементами искусственного интеллекта» (БТС-
ИИ-2016). Иннополис: Перо, 2016. С. 147–157.

2. Павловский В.Е., Яцун С.Ф., Емельянова О.В., Савицкий А.В.


Моделирование и исследование процессов управления квадрокоптером
// Научно техн. журнал «Робототехника и техническая кибернетика».
2014. № 4. C. 49–57.

3. Яцун С.Ф., Емельянова О.В., Савин А.И. Моделирование движения


квадрокоптера при отклонении осей несущих винтов относительно
корпуса //Вибрационные технологии, мехатроника и управляемые
машины. 2014. С. 329-338.

4. Яцун С.Ф., Емельянова О.В., Савин А.И., Стуканева С.П. Моделирование


полета конвертоплана в различных режимах движения. // Известия
ЮЗГУ, 2015. №1. С. 55 – 66.

5. Попов Н.И., Яцун С.Ф., Емельянова О.В., Савин А.И. Исследование


движения квадрокоптера при внешнем периодическом воздействии //
Инженерный журнал с приложением «Справочник.». 2014. №. S4. С. 17-
21.

6. Емельянова О. В., Попов Н. И., Яцун С. Ф. Моделирование движения


квадроротационного летающего робота //Актуальные вопросы науки.
Материалы VIII Международной научно-практической конференции.
М.: Спутник, 2013. С. 6-8.

7. Емельянова О.В., Казарян К.Г., Мартинез Леон А.С., Яцун С. Ф. Синтез


параметров электроприводов БПЛА типа конвертоплан-трикоптер // IV
Международная школа-конференция молодых ученых «Нелинейная
динамика машин» – School-NDM 2017: Сборник трудов. М.: ИМАШ
РАН, 2017. С. 239-249.

8. Павловский В.Е., Яцун С.Ф., Емельянова О.В., Стуканева С.П.


Математическое моделирование робота с переменным вектором тяги//
Всероссийский научно-практический семинар «Беспилотные
транспортные средства с элементами искусственного интеллекта» (БТС-
ИИ-2015). СПб.: Политехника сервис, 2015. С. 99 –106.

9. Патент на полезную модель РФ № 157967. 2015. Движитель воздушный


с изменяемым вектором тяги. С.Ф. Яцун, С.В. Ефимов, В.Я. Мищенко,
А.С. Яцун, О.В. Емельянова, А.С. Мартинез.

10. Патент на полезную модель РФ №166334. 2016. Мультироторный


аппарат с изменяемым вектором тяги / С.Ф. Яцун, В.Я. Мищенко, О.В.
Емельянова, А.И. Савин.

11. Atsushi Oosedo, Satoko Abiko, Shota Narasaki, Atsushi Kuno, Atsushi Konno,
Masaru Uchiyama. Flight control systems of a quad tilt rotor unmanned aerial
vehicle for a large attitude change. // IEEE International Conference on
Robotics and Automation (ICRA). 2015. Pp. 2326-2331.

12. Åström K. J., Hägglund T., Astrom K. J. Advanced PID control. // Research
Triangle Park. NC.: ISA-The Instrumentation, Systems, and Automation
Society. 2006. 461 c.

13. Красильщиков М.Н. Управление и наведение беспилотных маневренных


летательных аппаратов на основе современных информационных
технологий/ под ред. Г.Г. Себряков. М.: Физматлит, 2003. 280 с.

14. Фетисов В.С. Беспилотная авиация: терминология, классификация,


современное состояние: моногр. Уфа: ФОТОН, 2014. 217 с

15. Дорф Р., Бишоп Р. Современные системы управления. / пер. с англ; под
ред. Б. И. Копылова. М.: Лаборатория базовых знаний, 2002. 832 с.
16. Girish C. V. et al. Nonlinear flight control techniques for unmanned aerial
vehicles // Handbook of Unmanned Aerial Vehicles. 2015. С. 577-612.

17. Zulu A., John S. A review of control algorithms for autonomous quadrotors //
Open Journal of Applied Sciences. 2014. № 4. Pp. 547-556

18. Caйт «InvenSense I». [Электронный ресурс] / Режим доступа:


https://store.invensense.com/datasheets/invensense/MPU6050_DataSheet_V
3%204.pdf , свободный

19. Сайт «electronoobs» [Электронный ресурс] / Режим доступа:


http://www.electronoobs.com/eng_robotica_tut9.php , свободный.

20. Euston M. et al. A complementary filter for attitude estimation of a fixed-wing


uav with a low-cost imu // 6th International Conference on Field and Service
Robotics. 2007. С. 1-6.

21. Islam T. et al. Comparison of complementary and kalman filter based data
fusion for attitude heading reference system //AIP Conference Proceedings.
2017. С. 020002.
Сведения об авторах

Фамилия, Имя, Мартинез Леон Андрес Martinez Leon Andres


Отчество (полностью) Сантьяго Santiago
Место работы, Юго-западный Southwest State University
должность государственный университет (SWSU), Ph.D. Student,
(ЮЗГУ), аспирант кафедры Department of Mechanics,
механики, мехатроники и Mechatronics and Robotics
робототехники

Ученая степень Магистр Master of Sciences


Ученое звание - -
Область научных Мобильные роботы, Mobile robots, Unnamed
интересов беспилотные летательные Aerial Vehicles (UAV),
аппараты (БПЛА), Mathematical modelling of
математическое моделирование mechatronical systems,
мехатронными системами, Adaptative and Optimal
адаптивное и оптимальное Control Systems, Artificial
управление, искусственный Intelligence and Neuronal
интеллект и нейроны сети. Networks.
Контактный телефон, +7(915) 079-94-15
факс
Электронный адрес asml1992@yandex.ru
Фотография в
электронном виде
Фамилия, Имя, Ян Найнг Со Yan Naing Soe
Отчество (полностью)
Место работы, Юго-западный Southwest State University
должность государственный университет (SWSU), Ph.D. Student,
(ЮЗГУ), аспирант кафедры Department of Mechanics,
механики, мехатроники и Mechatronics and Robotics
робототехники

Ученая степень Магистр Master of Sciences


Ученое звание - -
Область научных Мобильные роботы, шагающие Mobile robots, legged robots,
интересов роботы, искусственный artificial intelligence and
интеллект и нейроны сети neuronal networks
Контактный телефон, +7(951) 085-12-50
факс
Электронный адрес boyan.243@gmail.com
Фотография в
электронном виде
Фамилия, Имя, Яцун Сергей Федорович Jatsun Sergey Fedorovich
Отчество (полностью)
Место работы, Юго-западный Southwest State University
должность государственный университет (SWSU), Head of the
(ЮЗГУ), заведующий кафедрой Department of Mechanics,
механики, мехатроники и Mechatronics and Robotics
робототехники

Ученая степень Доктор технических наук Doctor of Technical Sciences


Ученое звание Профессор Professor
Область научных Мобильные роботы, механика, Mobile robots, mechanical
интересов теория управления, engineering, control theory,
математическое моделирование mathematical modeling of
мехатронных систем, mechatronical systems,
экзоскелеты. exoskeleton robotics.
Контактный телефон, +7 (4712) 22-2626
факс
Электронный адрес teormeh@inbox.ru
Фотография в
электронном виде