CC BY
29
Генерация траекторий оптимальных кривых беспилотного летательного аппарата для обхода статического препятствия
Д.Л. Винокурский, К.Ю. Ганьшин, О.С. Мезенцева, Ф.В. Самойлов.
Северо-Кавказский федеральный университет, Ставрополь
Аннотация: В данной работе представлен алгоритм обнаружения и обхода препятствия одиночным беспилотным летательным аппаратом. Предложенный способ представляет собой расширенную версию предыдущей работы с добавлением возможности по предотвращению столкновений на основе построения дополнительной кривой обхода на основе алгоритма де Кастельжо. В этой работе авторы предложили способ генерации траекторий, в котором используются кривые Безье со свойствами годографа Пифагора. Ключевые слова: беспилотный летательный аппарат, полиномы Бернштейна-Безье, годограф Пифагора, генерация траекторий, обход препятствия, программное движение.
Введение
Траекторное управление - сложная задача, которая зачастую требует описания определённого решения при разработке робототехнических систем. В данной задаче наиболее часто выделяются две подзадачи: генерация оптимальных траекторий и определение методов обхода препятствий. На сегодняшний день в различных публикациях описаны методы построения оптимальных траекторий в зависимости от кинематических и динамических ограничений, влияния окружающей среды и выполняемой задачи.
Основными алгоритмами построения траектории беспилотного летательного аппарата (БПЛА) являются: диаграмма Вороного [1-2], алгоритм Дейкстры [3], метод теории графов [4], методы оптимизации (линейное и нелинейное программирование) [56], целочисленное программирование [7-8], методы нечеткой логики и оптимизации траектории генетическими алгоритмами [9-11].
В данной статье авторы используют алгоритм генерации траектории на основе кривых Безье пятого порядка со свойствами годографа Пифагора с разложением по полиномам Бернштейна [12-13]. Применение подобного генератора траекторий обусловлено сниженной вычислительной сложностью задания кривых, легкостью их дифференцирования и возможностью точного определения длины пути. Для успешной работы необходимо иметь возможность перестраивать траектории за как можно меньшее время для избежания столкновений. В данной работе представлен метод определения
оптимальной кривой Безье, основанный на предыдущей работе авторов [14], а также модифицированный метод обхода препятствий на основе алгоритма де Кастельжо, предложенный в работе [15]. Описываемый в настоящей работе способ позволяет избегать неподвижных препятствий.
Описанный метод имеет ограничение в виде отсутствия способности обхода динамических препятствий. Траектория кривой представлена как вектор разделения препятствия и транспортного средства в зависимости от времени. Для предотвращения столкновений строится обходная кривая на основе годографа Пифагора.
Кривые Безье со свойствами годографа Пифагора выбраны в качестве траекторий следования БПЛА. Если принять во внимание тот факт, что данные кривые третьего порядка (кубики) по сути своей являются лишь одной кривой - кривой Чирнхауза, а значит не обладают должной «гибкостью», было решено использовать кривые пятого порядка (квинтики).
Задание кривых с годографом Пифагора осуществляется за счёт указания первых двух и последних двух контрольных точек (р0, р4, р3),
определяющих кривую Безье пятого порядка. Однако, в случае использования БПЛА, возможно представить задание не четырёх точек, а двух точек (начальную и конечную) и скоростей при них (р0, р5, р0, р5); из
последних легко вычислить координаты точек р± и р4. Далее производится
определение оставшихся точек р2 и р3, для чего применятся интерполяция
Эрмита. Учитывая, что годографы заданы, как:
Генерация траектории
*'(*:) = и2 (£)- V2 (г), у'(1) = 2и(£>(£)
(1)
, где и()и можно представить в виде многочленов: и а) = Ц0(1 - Г)2 + и! 2(1 - Х)Х + и/
(2)
КО = 1?о(! - о2 + уг2(1 - £)£ +
2
(3)
и
а контрольные точки р2 и р3 определяются, как:
2 1 Рз = Р2 + (и1 - 2и1у1) + — (и0и2 - У0У2,и0У2 + и2 У о)
(4)
(5)
Интерполяция Эрмита позволяет определить коэффициенты
где
а = 16 ^ и2 ~ у2) + | С"о;"2 - + у ~ Р1х)
9 5 15
(6)
(7)
(8)
51дп(х)
' 1,при х > О ±1 , при Л = 0. 1,при х < О
Отмечается, что решений всего 4, среди которых следует выбрать наиболее оптимальное. В данном случае выбор оптимального решения производится по критерию наименьшей длины получаемой кривой и
и
наименьшей кривизны в конечных точках. Такой подход выбран в связи с тем, что 4 разных решения интерполяции объединяются в две пары с равными длинами кривых, потому определение лишь наименьшего её значения оказывается недостаточным для однозначного выбора «хорошей» кривой. Длина кривой определяется следующим образом:
у™-1 я-
5 = ^^ (9)
п
, здесь п—5, а\ - коэффициенты параметрической скорости <т(£), которые в
случае кривой пятого порядка представлены следующим образом: Со = и1 + У1
°2 = 7 (и? + vi) + hu0u2 + v0v2)
(10)
(Т3 = щи2 + v±v2
<т4 = и\ + v\
Кривизна плоской кривой задаётся параметрически:
Кривизна в конечных точках:
и01'1-и1_1'0
begin
= ¿(0) = 4
к+о
2 ;
kend = Л(1) =4
Ui_V2-U2 V1 (и l+vlf
(11)
(12)
Для поиска оптимальной кривой произведём суммирование кривизны конечных точек:
(13)
Далее, определим критерий оптимальной кривой следующим образом:
М Инженерный вестник Дона, №9 (2021) ivdon.ru/ru/magazine/arcliive/n9y2021/7192
Таким образом, определяется сочетание знаков при вычислении коэффициентов (щ,У0),(и2,У2^ такое, что определяется произведение
для каждого решения интерполяции Эрмита, среди которых
находится лишь наименьшее.
Сами же кривые рассчитываются за счёт представления их в форме многочленов Бернштейна [14]:
Для возможности указания реальных временных границ генерации параметрической кривой, используется минимакс-нормализация:
Ь =
■Г-тт(Г) тах(Г)-тт(Г)
(16)
, где Т- набор дискретных значений времени.
Обход статичного препятствия
Предположим, что препятствие - статичная точка на плоскости
. Определим условие наличия
столкновения следующим образом:
|г(£)"Р0| <й
(17)
где d - скаляр, определяющий радиус безопасности вокруг препятствия.
Зададим новую параметрическую кривую Безье гс(1), по которой
возможно определять наличие пересечения между препятствием и заданной траекторией г(1;):
?1Лг) = г(г) -рй = и=ьркф1-к\ъ-ктк-рй (18)
и
Таким образом, условие наличия столкновения можно переопределить
как:
шп \гс (£) | < (I
(19)
Отсюда, определим минимальную дистанцию между траекторией и препятствием с1т1п, и момент времени данного сближения tm^n следующим
образом:
^Ш1П
ппп I гс (£) 1, 1пйп = аг§тт|гс(£)| се[о,1] Се [од]
(20)
В виду того, что в процессе полёта затруднительно пересчитывать всю кривую траектории, применим алгоритм де Кастельжо для разделения существующей траектории в двух точках, как описано в [15]:
Вырезаемый сегмент кривой определяется на интервале таким образом алгоритм де Кастельжо выполняется на точках £ = и . Данные параметры возможно определить следующим образом:
Здесь tc - момент времени обнаружения препятствия, £сс - момент
времени с предполагаемым столкновением, т - время, отсчитываемое с начала обнаружения препятствия, определяет так же точки разреза основной
М Инженерный вестник Дона, №9 (2021) ivdon.ru/ru/magazine/arcliive/n9y2021/7192
кривой. и - границы окна столкновения. Очевидно, что £ возможно спроецировать в г, причём сделать это можно, применив выражение (16):
Определим траекторию обхода следующим образом:
rdet[тcc'ProJ со) = Кз(т
(24)
, здесь К- масштабный коэффициент, который определяется условием
(26)
Таким образом, итоговая кривая вместе с кривой обхода, при условии, что 1 находится в границах времени разделения изначальной траектории , примет вид:
Аппаратно-программный комплекс
Предлагаемый подход экспериментально апробирован на миниатюрном четырёхроторном БПЛА СгагуШе 2.1. В виду малой массы (34 г) и ограниченной вычислительной мощности данного устройства, все вычисления и контроль производится на станции наземного контроля (СНК), представленной в виде персонального комьютера (ПК) и системы радиосвязи.
пуп ( \г^ссгТ) + г,иЬ(т)\)> с!
Т ГГ)Тг г
Обмен данными между СНК и БПЛА производится посредством системы радиосвязи диапазона ISM 2.4 ГГц CrazyRadio PA с заданной пропускной способностью в 2 Мб/с. БПЛА передаёт на СНК сведения о собственном местоположении в локальной навигационной системе (ЛНС) и о собственных углах поворота относительно горизонтальной плоскости земли. Для снижения ошибки позиционирования в ЛНС на БПЛА установлена дополнительно плата расширения с простой системой визуальной одометрии Flow Deck, определяющей смещение БПЛА относительно горизонтальной плоскости земли. ЛНС представляет из себя набор из 8 широкополосных СВЧ-радиомаяков, чьё местоположение относительно друг друга известно, расположенных в вершинах параллелепипеда, который и определяет допустимую область полёта БПЛА. Квадрокоптер использует оценочную функцию на основе расширенного фильтра Калмана, благодаря которому объединяет сведения от бортового приёмника ЛНС, системы визуальной одометрии и собственной инерциальной системы, что позволяет ему достаточно точно определять собственное местоположение в пространстве.
ПК собран на базе процессора Intel 2 поколения i5-2540m, оборудован 8 ГБ DDR3 RAM. Используется операционная система Ubuntu 20.04 LTS, метаоперационная система ROS Noetic, интерпретатор Python 3.6. Программное обеспечение написано на языке программирования Python 3.
Эксперимент
Для проведения испытаний производится предварительная генерация траектории, состоящей из двух кривых Безье с годографом Пифагора. Забор данных о позиции с БПЛА производился каждые 25 мс с дальнейшей фильтрацией в дискретном ФНЧ для получения наиболее гладкой траектории при построении графиков. Препятствие представляет собой виртуальную точку, располагающуюся по координатам (9.3, 1.9), с безопасным радиусом,
равным 1 метру. Как видно из рис. 1 и 2, первый разрыв траектории произведён в точке (9.59, 3.19), второй разрыв - в точке (9.84, 0.97), между которыми добавлена новая траектория. В виду того, что кривые Безье с годографом Пифагора после интерполяции Эрмита определяются выражениями (11) - (14), а также невозможно построить в интервале разрыва путь обхода такой, чтобы итоговая кривая в местах разрыва оказалась дифференцируемой, произведено дополнительно переопределение основных секций (начальной и конечной) траекторий.
На рисунке 3 представлены графики скоростей: желаемой (сгенерированная кривая, сиреневая линия) и действительной (скорость реального БПЛА, красная линия). На графиках a, Ь рис.3 можно увидеть всплески в сгенерированной траектории в окрестностях точек 1=0.45, 1=0.65, а также на графиках с, ё в окрестностях точки Ь=0.5„ свидетельствующие о наличии "склейки" кривых Безье, однако это не оказывает критического влияния на движение квадрокоптера в виду его инерциальности.
м - о
one trajectory
" —--
XY-plot, with obstacle
Рис. 1 — Начальная траектория, красным обозначена базовая траектория,
чёрным - реальная траектория БПЛА
Рис. 2 — Обход препятствия, красным обозначена базовая траектория, синим - обход, зелёным - границы препятствия, чёрным - реальная траектория
БПЛА
— Тга^есЬэгу — Огопе че1ос11у ■1у Тгв)к[о1у »Ноо^ Огопе уе1ос№у ,/Х
-0.5 Г! //
/ / Х V \\ \)
/г\\ / — (- —у \ 1
\ / //
• —
Тг^ейогу уе1йс№у 1.25 1.00 - Огопе уе1осЛу Г\
0.50 0.25 —1
: /
0.00 41,25 \ //
у V/
Рис. 3 — a, Ь - скорости по осям X и Y с препятствием; с, d - скорости по осям X и Y без обнаружения препятствия соответственно
Выводы
В статье был представлен метод поиска оптимальной кривой Безье с годографом Пифагора, который был применён в также представленном алгоритме обнаружения и обхода препятствий на основе алгоритма де Кастельжо. Как показывают результаты представленного эксперимента,
выбор оптимальной кривой Безье работает удовлетворительно для задач построения траекторий полёта БПЛА: перегибы функций возникают не близ точек начала и конца кривых Безье и обладают малой кривизной, которая не вызывает больших расхождений между желаемыми и действительными координатами БПЛА. Тем не менее, возможно дальнейшее развитие метода оптимизации с возможностью указания требуемой кривизны для соответствия полётным характеристикам модели БПЛА.
Предложенный способ обхода препятствий позволяет изменять уже существующую траекторию для предотвращения столкновений. Как показывают результаты испытаний, в местах разрывов и соединений кривых Безье наличествуют выбросы функций уже при взятии первой производной. Подобное явление говорит о необходимости внедрения методов оптимизации соединения кривых по производным второго порядка, что приведёт к меньшим энергетическим затратам БПЛА. В будущем планируется расширение алгоритма и его улучшение с добавлением возможности обхода динамических препятствий и генерации более плавной траектории.
Исследование выполнено при финансовой поддержке РФФИ в рамках научного проекта № 20-31-90060.
Литература
1. Tong H. et al. Path planning of UAV based on Voronoi diagram and DPSO // Procedia Engineering. - 2012. - Т. 29. - pp. 4198-4203.
2. Chen X., Chen X. The UAV dynamic path planning algorithm research based on Voronoi diagram // The 26th chinese control and decision conference (2014 ccdc). - IEEE, 2014. - pp. 1069-1071.
3. Medeiros F. L. L., Da Silva J. D. S. A Dijkstra algorithm for fixed-wing UAV motion planning based on terrain elevation // Brazilian Symposium on Artificial Intelligence. - Springer, Berlin, Heidelberg, 2010. - pp. 213-222.
4. Toan T. Q., Sorokin A. A., Trang V. T. H. Using modification of visibility-graph in solving the problem of finding shortest path for robot //2017 International Siberian Conference on Control and Communications (SIBCON). - IEEE, 2017. -pp. 1-6.
5. Ragi S., Mittelmann H. D. Mixed-integer nonlinear programming formulation of a UAV path optimization problem //2017 American Control Conference (ACC). - IEEE, 2017. - pp. 406-411.
6. Borrelli F. et al. MILP and NLP techniques for centralized trajectory planning of multiple unmanned air vehicles //2006 American Control Conference. - IEEE, 2006. - p. 6.
7. Glover F. Improved linear integer programming formulations of nonlinear integer problems //Management Science. - 1975. - Т. 22. - №. 4. - pp. 455-460.
8. Ioan D. et al. Mixed-integer programming in motion planning //Annual Reviews in Control. - 2020. pp.85-87.
9. Silva Arantes J. et al. Heuristic and genetic algorithm approaches for UAV path planning under critical situation // International Journal on Artificial Intelligence Tools. - 2017. - Т. 26. -№. 01. - pp. 1760008.
10. Chen G., Cruz J. B. Genetic algorithm for task allocation in UAV cooperative control //AIAA Guidance, Navigation, and Control Conference and Exhibit. - 2003. - p. 5582.
11. Wilburn B. K., Perhinschi M. G., Wilburn J. N. A modified genetic algorithm for UAV trajectory tracking control laws optimization //International Journal of Intelligent Unmanned Systems. - 2014. pp. 58-90.
12. Vanegas G. et al. Smooth 3D path planning for non-holonomic UAVs //2018 7th International Conference on Systems and Control (ICSC). - IEEE, 2018. - pp. 1-6.
13. Bestaoui Y. 3D flyable curves for an autonomous aircraft //AIP Conference Proceedings. - American Institute of Physics, 2012. - Т. 1493. - №. 1. - pp. 132-139.
14. Винокурский Д. Л. и др. Планирование траектории группы беспилотных летательных аппаратов с использованием годографа Пифагора и составных кривых Бернштейна-Безье на плоскости //Вестник КРАУНЦ. Физико-математические науки. -2020. - Т. 31. - №. 2. - С. 70-78.
15. Mehdi S. B. et al. Collision avoidance through path replanning using Bézier curves //AIAA Guidance, navigation, and control conference. - 2015. - p. 0598.
References
1. Tong H. Procedia Engineering. 2012. T. 29. pp. 4198-4203.
2. Chen X., Chen X. The 26th chinese control and decision conference (2014 ccdc). IEEE, 2014. pp. 1069-1071.
3. Medeiros F. L. L., Da Silva J. D. S. Springer, Berlin, Heidelberg, 2010. pp. 213-222.
4. Toan T. Q., Sorokin A. A., Trang V. T. H. 2017 International Siberian Conference on Control and Communications (SIBCON). IEEE, 2017. pp. 1-6.
5. Ragi S., Mittelmann H. D. 2017 American Control Conference (ACC). IEEE, 2017. pp. 406-411.
6. Borrelli F. et al. 2006 American Control Conference. IEEE, 2006. p. 6.
7. Glover F. Management Science. 1975. T. 22. №. 4. pp. 455-460.
8. Ioan D. et al.Annual Reviews in Control. 2020. pp.85-87.
9. Silva Arantes J. International Journal on Artificial Intelligence Tools. 2017. T. 26. №. 01. p. 1760008.
10. Chen G., Cruz J. B. AIAA Guidance, Navigation, and Control Conference and Exhibit. 2003. p. 5582.
11. Vanegas G. et al. Smooth 3D path planning for non-holonomic UAVs 2018 7th International Conference on Systems and Control (ICSC). IEEE, 2018. pp. 1-6.
12. Vanegas G. 2018 7th International Conference on Systems and Control (ICSC). IEEE, 2018. pp. 1-6.
13. Bestaoui Y. AIP Conference Proceedings. - American Institute of Physics, 2012. T. 1493. №. 1. pp. 132-139.
14. Vinokurskij D. L. Vestnik KRAUNC. Fiziko-matematicheskie nauki. 2020. T. 31. №. 2. pp. 70-78.
15. Mehdi S. B. et al. Guidance, navigation, and control conference. 2015. p. 0598.
