CC BY
500
Моделирование движения квадрокоптера по траектории с минимальной ошибкой отклонения
Кочкаров А.А., НТЦ-3 ОАО «РТИ», Финансовый университет при Правительстве РФ
[email protected] Агишев Р.Т., МФТИ (ГУ) [email protected]
траектории [Meilinger D., Michael N., and
Аннотация
Данная статья описывает компьютерную модель квадрокоптера. Основной задачей спроектированной модели является моделирование движения БПЛА вдоль различных траекторий с минимальной ошибкой отклонения. Представлены результаты облета вдоль прямой линии и спирали, а также вдоль гладкой кривой, задаваемой контрольными точками. Для каждого варианта облета траектории предоставлены графики зависимости координат и скоростей БПЛА от времени, демонстрирующие малую величину ошибки отклонения от желаемой траектории.
1 Введение
В последние годы все большее применение в различных областях находят беспилотные летательные аппараты [Кочкаров А.А., 2015; Малинецкий Г.Г., Кочкаров А.А., 2014; Кочкаров А.А., Яцкин Д.В., Рахманов О.А, 2016; Кочкаров А.А.,Калинов И.А, 2016; Кочкаров А.А., Яцкин Д.В., Калинов И.А., 2016]. В гражданской сфере чаще всего используются квадрокоптеры, т.е. дроны с четырьмя винтами. Легкость, малый размер, маневренность, простота управления -основные достоинства квадрокоптеров, которые позволяют использовать их во многих отраслях. Установка на квадрокоптеры дополнительного
оборудования (например, камер) позволяет справляться с проблемами преследование нарушителей, обзора местности в тяжелых условиях (наводнение). В настоящее время квадрокоптеры широко используются в городских условиях с плотной застройкой. Поэтому движение по траектории является важным вопросом. Существует множество алгоритмов реализации траекторной задачи. Результатом данной работы является реализация алгоритма с минимальной ошибкой следования БПЛА желаемой
Kumar V, 2010].
2 Математическая модель движения БПЛА
Движением квадрокоптера можно управлять, изменяя скорости вращения винтов. Аппарат движется относительно неподвижной инерциальной системы отсчета, связанной с Землей. Угловое положение аппарата задаем тремя углами Крылова: углами крена ф, тангажа 0 и рыскания у, определяющими вращение вокруг осей соответственно.
Рис. 1 Конфигурация "+" квадрокоптера
Второй закон Ньютона для квадрокоптера будет иметь вид:
dv .. т— = т, сИ
/ — вектор суммарной силы, приложенной к БПЛА
При переходе системы из неподвижной системы координат в подвижную уравнение движение принимает вид:
т — = т + м х V) = Т ,
Связь между линейными скоростями и инерциальной и подвижной системой отчета задается соотношением:
У 2
=
R
матрица поворота:
R =
СщСе СщВеВр ВщСр ЯщСе + СщСр
ЯрСе
СщЯеСр + ВщВеСр - СщВр
С С
ре
где = sin(x),
Сх = cos( х). На квадрокоптер действует сила тяжести. В подвижной системе координат сила тяжести принимает вид:
mg Бт(е) -mg sin(р)cos(е) -mg cos(р)cos(е)
^ =
действует
Также на квадрокоптер подъемная сила:
F = k (с^ + с2 + с32 + с4),
где к - экспериментально определяемая постоянная, а - скорость вращения 1-ого винта.
Описание движения квадрокоптера задает
следующая совокупность уравнений:
—
т
Эту систему необходимо дополнить силой сопротивления набегающего потока воздуха:
F = с В*- В
сопр d ^
Допустим далее, что тело квадрокоптера является симметричным, представляет собой
Л и массой Ы.
х "0" СщЯеСр +
у = - g 0 + ВщВеСр - СщВр
2 1 С С
на
шар радиусом „ ---------
расстоянии от которого находятся моторы, представленные в виде материальных точек массой Ыт на расстоянии длины лучей I от центра, что изображено на рис. 2:
Рис. 2 Схематическое изображение квадрокоптера Тензор инерции такого тела имеет вид:
3 =
Л о о 0 3У о 0 0 3,
где компоненты тензора следующим образом:
3^=3=ЫЛ1+2,2 Ы.,
х у
выглядят
3 = 2ЫЛ2
5
5
+ 412 ТЫ.
Угловые ускорения уравнениями Эйлера:
определяются
т2 - F4)
Ц Fз - F1)
Ы1 - Ы2 + Ы3 - Ы4
Р Р
- Я X I Я
г г
Линеаризованная с учетом малости углов тангажа и крена математическая модель квадрокоптера в подвижной системе отсчета имеет вид:
X = -gе - sign(X)с
рх
Я.
РУ
у = gр- sign(уК "уЯу
2 = g - — - ^П(2)cd Я т 2
р = -21кс0 (с4 - с2)
= — 1кс0 (с3 - с1)
щ = — Ьс0 (-с1 + с2 - с3 + с4)
2
3 Управление движением БПЛА
В задаче следования БПЛА заданной траектории важно быстрое стремление к нулю ошибки отклонения по координате. Для этой цели необходимо потребовать, чтобы координаты БПЛА удовлетворяли уравнениям:
(Г,Т - Г i,des ) + kd,i (- Г, ) + kp . (riJ - r ) = 0
i = 13
Последние равенства описывают работу ПД-регулятора [Вадутов О.С., 2014]. Решения каждого из трех уравнений экспоненциально стремятся к желаемым значениям ri,des.
Управляющие движением квадрокоптера величины (силы тяги четырех моторов и создаваемые ими моменты) далее выражаются через переменные,
определяющие положение (координаты x,y,z) и ориентацию (углы крена, тангажа и рысканья) БПЛА в пространстве. Получаем полный набор соотношений для задачи следования траектории [N. Michael, D. Mellinger, Q. Lindsey, and V. Kumar, 2010]:
F = m • (g - KdzZ - Kp z (z - zdes)),
XT = X des + Kp,x (xdes - x) + Kd,x (X des - Xl
yT = У des + Kp,y ( У des - У) + Kd,y ( У des - У),
^ds = -(XT • Sin( Wdes ) - yT ■ C0S(Wdes )), g
edes = -( XT • C0S(W des ) + yT ' Sm(W des )), g
P des 0, q des 0, rdes ^ des ,
M =
KpA$des -фф + Kd,ф(Pdes - P)
KP,e(edes -e) + Kd- q) Kp,W (¥des -w) + Kd,v (rdes - r)
С помощью данной системы уравнений реализована модель движение квадрокоптера по двум заданным траекториям: вдоль прямой линии и спирали, см. рис. 3, 4.
Рис. 3. Движение квадрокоптера вдоль прямой
линии.
Рис. 4. Движение квадрокоптера вдоль спирали.
На графиках красными точками отмечена траектория БПЛА, синими - желаемая траектория. Ошибки траекторий
отслеживались с помощью графиков зависимости координат и скоростей от времени, рис. 5-8. С их помощью производилась настройка ПД-регуляторов.
time [s]
time [s]
Рис. 5. Графики г(£) при движении вдоль прямой линии.
0.5 О
Рис. 6. Графики х>(£) при движении вдоль прямой линии.
Рис. 8. Графики х>(£) при движении вдоль спирали.
Рис. 7. Графики г(£) при движении вдоль спирали.
4 Траектория, задаваемая контрольными точками
Более интересной и полезной с практической точки зрения выглядит задача проектирования движения БПЛА по траектории, задаваемой лишь контрольными точками. В реальности зачастую приходится иметь дело с препятствиями. Благодаря своей маневренности и способности зависать в воздухе, квадрокоптеры используются в городской плотно застроенной среде. Важным ограничением также является малое отклонение БПЛА от намеченной траектории. Из уравнений
движения квадрокоптера можно получить, что управляющие сигналы (силы тяги и создаваемые
f (4)
моменты) зависят от tttt . Поэтому для построения гладких кривых, проходящих через заданный набор точек, используется вариационный метод минимизации четвертой производной по времени от координаты [Daniel Mellinger and Vijay, 2011]. Т.е. для нахождения кривой, соединяющей две соседние контрольные точки траектории, решается математическая задача:
p*(t) = argmin J (p(4))2 dt p(t) 0 .
Здесь p*(t) - искомая траектория, T - время, затрачиваемое на ее прохождение до следующей контрольной точки. Решение подобной вариационной задачи эквивалентно решению дифференциального уравнения 8-ого порядка Эйлера-Лагранжа. Поэтому искомую траекторию нужно задать как полином 7-ой степени времени, например:
f t - V
7
t - Si
pi(t ) = a 0 + an T +... + a
i i
T
S o = 0, S, = £ Tk
Здесь введены обозначения: k=1 - время достижения i-ой контрольной точки при
движении из начальной. Все описанные таким образом полиномы pi, i=l..n, должны удовлетворять 8n условиям для нахождения всех констант aij, i=1..n, j=0..7, а именно:
Рг (S,-i) = wi-1, Р, ) = W, i = 1, n
plk \S o) = p(nk \Sn) = 0, k = 13
p(k )(Si) = p^KS,), k = 16
Такая система уравнений относительно неизвестных aij далее записывается в матричном виде: Ла = b.
Здесь A - матрица, размером 8n*8n, a - столбец искомых коэффициентов, b - матрица 8n*3. Решая систему уравнений отдельно для каждого из столбцов матрицы b, получаем коэффициенты, задающие траектории x(t), y(t), z(t) соответственно. Результаты моделирования движения через 6 контрольных точек приведены на рис. 9.
О S 10 15 20
time [s]
iteration: 468, time: 23.40
Рис. 9. Движение вдоль гладкой кривой, заданной контрольными точками: (0 0 0)^(1 1 1)^(2 0 2)^ (3 -1 1)^(4 - 2 2)^ (5 -1 3)^(6 0 0)
На рис. 10 далее приведены графики полиномов, полученных в результате решения
вариационной задачи.
Благодарности
Работа выполнена при поддержке РФФИ (грант № 16-01-00342, грант № 16-29-04268) и гранта
Президента РФ (НШ-6831.2016.8).
Рис. 10. Зависимость координат от времени при движении через 6 контрольных точек.
Список литературы
Кочкаров А.А. Некоторые особенности применения малых и сверхмалых беспилотных летательных аппаратов // Труды Второй Всероссийской научно-технической конференции молодых конструкторов и инженеров «Минцевские чтения», посвященной 120-летию со дня рождения академика А.Л. Минца и 60-летию аспирантуры Радиотехнического института. - М.: Издательство МГТУ им. Н.Э. Баумана, 2015. -С. 301-304.
Малинецкий Г.Г., Кочкаров А.А. Будущее российского оружия и междисциплинарные подходы // Интеллект и технологии. - 2014. - № 1(7). - С. 48-51.
Кочкаров А.А., Яцкин Д.В., Рахманов О.А. Особенности решения задачи геометрического мониторинга // Известия ЮФУ. Технические науки. - 2016. - № 2(175). - С. 158-168.
Кочкаров А.А., Калинов И.А. Создание программно-аппаратного комплекса пространственной навигации и мониторинга мультироторного БПЛА на основе модифицированного алгоритма визуальной одометрии // Наука и Образование. МГТУ им. Н.Э. Баумана. Электрон.журн. - 2016. - № 09. - С. 74-91.
Кочкаров А.А., Яцкин Д.В., Калинов И.А. Новый подход в применении малых БПЛА для мониторинга сложных пространств // Интеллект и технологии. - 2016. - № 2(14). - С. 68-71.
Mellinger D., Michael N., and Kumar V. Trajectory Generation and Control for Precise Aggressive Maneuvers with Quadrotors // Int. Symposium on Experimental Robotics, December 2010.
Вадутов О.С. Настройка типовых регуляторов по методу Циглера-Никольса // Издательство Томского политехнического университета, 2014.
Michael N., Mellinger D., Lindsey Q., and Kumar V. The GRASP Multiple Micro-UAV Testbed, IEEE Robotics and Automation Magazine, 2010.
Mellinger D., Kumar V. Minimum Snap Trajectory Generation and Control for Quadrotors // 2011 IEEE International Conference on Robotics and Automation Shanghai International Conference Center May 9-13, 2011, Shanghai, China.
