CC BY
56
УНИВЕРСИТЕТ итмо
НАУЧНО-ТЕХНИЧЕСКИИ ВЕСТНИК ИНФОРМАЦИОННЫХ ТЕХНОЛОГИИ, МЕХАНИКИ И ОПТИКИ сентябрь-октябрь 2021 Том 21 № 5 http://ntv.ifmo.ru/
SCIENTIFIC AND TECHNICAL JOURNAL OF INFORMATION TECHNOLOGIES, MECHANICS AND OPTICS September-October 2021 Vol. 21 No 5 http://ntv.ifmo.ru/en/
ISSN 2226-1494 (print) ISSN 2500-0373 (online)
ИНФОРМАЦИОННЫХ ТЕХНОЛОГИЙ. МЕХАНИКИ И йПТИКИ
doi: 10.17586/2226-1494-2021-21-5-653-663 УДК 351.814.334.3
Геометрический подход к решению задачи для машин Дубинса при формировании программных траекторий движения
Сергей Петрович Хабаров1, Мария Львовна Шилкина2Н
Санкт-Петербургский государственный лесотехнический университет им. С.М. Кирова, Санкт-Петербург, 194021, Российская Федерация
1 Serg.Habarov@mail.ru, https://orcid.org/0000-0003-1337-0150
2 тЛетоЬау@тЬох.ти, https://orcid.org/0000-0001-6501-5789
Аннотация
Предмет исследования. Рассмотрен подход к формированию управляющих программных траекторий движения подвижных объектов (самолетов, судов) как решение оптимальной по быстродействию задачи для машин Дубинса. Метод. Предложено вместо прямого решения принципа максимума Понтрягина воспользоваться простым перебором возможных стратегий управления с целью определения среди них оптимальной по быстродействию. Основные результаты. Решена задача поиска кратчайшей траектории движения объекта из одной точки в другую, причем для обеих точек заданы их координаты и курсовые углы. Заданы три абсолютных значения радиусов циркуляции, соответствующие сигналам управления на каждом из трех участков траектории. Задача поиска кривых Дубинса сводится к поиску параметров двух промежуточных точек, в которых происходит смена управления. Рассмотрены возможные направления вариантов смены управлений с учетом имеющихся ограничений. Вычислены длины траекторий движения и выбрана оптимальная. Решена задача построения траектории, которая обеспечит гладкое сопряжение двух прямолинейных фрагментов траекторий и проходит через точку их пересечения. Решение задачи поиска оптимальной траектории движения с использованием машины Дубинса дает единственную траекторию. Предлагаемый метод рассматривает нескольких допустимых по ограничениям траекторий, из которых перебором выбирается оптимальная. Наличие нескольких допустимых стратегий дает преимущества при выборе траектории в зависимости от окружающей обстановки. Практическая значимость. Вместо прямого решения принципа максимума Понтрягина используется простой перебор возможных стратегий управления с целью определения среди них оптимальной по быстродействию, что обусловлено ограниченным для машин Дубинса количеством возможных стратегий управления. Физически ограничения на управление (радиус поворота) связаны с ограниченностью угла поворота руля. Простота аналитических расчетов для каждой стратегии позволяет выполнять эти расчеты в реальном времени. Быстрота расчетов для задачи определения оптимальной траектории связана с тем, что в предложенном методе не требуется выполнение сложных расчетов для решения задачи нелинейной оптимизации, следующей из принципа Понтрягина. Ключевые слова
машина Дубинса, траектория движения, алгоритмы, оптимальное управление, кривые сопряжения Ссылка для цитирования: Хабаров С.П., Шилкина М.Л. Геометрический подход к решению задачи для машин Дубинса при формировании программных траекторий движения // Научно-технический вестник информационных технологий, механики и оптики. 2021. Т. 21, № 5. С. 653-663. doi: 10.17586/2226-1494-2021-21-5-653-663
© Хабаров С.П., Шилкина М.Л., 2021
Geometric approach to the solution of the Dubins car problem in the formation
of program trajectories
Sergey P. Khabarov1, Maria L. Shilkina2®
St. Petersburg State Forest Technical University, Saint Petersburg, 194021, Russian Federation
1 Serg.Habarov@mail.ru, https://orcid.org/0000-0003-1337-0150
2 mchernobay@inbox.ru®, https://orcid.org/0000-0001-6501-5789
Abstract
The paper considers an approach to the formation of control program trajectories of moving objects (UAVs, ships) as a solution to the optimal problem in terms of Dubins path search. Instead of directly solving the Pontryagin's maximum principle, it is proposed to use a simple analysis of possible control strategies in order to determine among them the optimal one in terms of time spent on a trajectory. The problem of finding the shortest trajectory of movement of an object from one point to another is solved, and for both points their coordinates and heading angles at these points are given, as well as three absolute values of the circulation radii corresponding to the given control signals on each of the three sections of the trajectory. The problem of finding the Dubins curves is reduced to determining the parameters of two intermediate points at which the control changes. All possible directions of control change options are considered, taking into account the existing constraints, also the lengths of the corresponding motion trajectories are calculated, and the optimal one is selected. The problem of constructing a trajectory is solved as well, which ensures a smooth conjugation of two linear fragments of trajectories and passes through the point of their intersection. The solution of the optimal trajectory problem using the Dubins car gives a single trajectory. In contrast to this, the proposed method considers several trajectories admissible by the constraints, from which the optimal one is selected by exhaustive search. The presence of several feasible strategies gives advantages for each specific situation of choosing a trajectory depending on the environment. Instead of directly solving the Pontryagin's maximum principle and constructing a three-dimensional optimal trajectory, the authors used a simple analysis of possible control strategies in order to determine among them the optimal one in terms of elapsed time. The approach was motivated by the limited number of possible control strategies for Dubins paths, as well as the simplicity of analytical calculations for each of them, which allows performing these calculations in real time. The high speed of calculations for the problem of determining the optimal trajectory is due to the fact that the proposed method does not require complex calculations to solve the problem of nonlinear optimization, which follows from the Pontryagin's principle. Keywords
Dubins car, trajectory, algorithms, optimal control, conjugation curves
For citation: Khabarov S.P., Shilkina M.L. Geometric approach to the solution of the Dubins car problem in the formation of program trajectories. Scientific and Technical Journal of Information Technologies, Mechanics and Optics, 2021, vol. 21, no. 5, pp. 653-663 (in Russian). doi: 10.17586/2226-1494-2021-21-5-653-663
Введение
В настоящее время для мониторинга линейных объектов, таких как линии электропередач и ветки газопроводов, используют беспилотные летательные аппараты (БПЛА) самолетного типа, а для сканирования прибрежных морских акваторий — исследовательские суда и подводные роботы. Объединяет перечисленные объекты то, что все они должны осуществлять свое движение по строго определенным программным траекториям и обладать ограниченной управляемостью по угловым скоростям.
Программные траектории определяют генеральное направление движения подвижных объектов, в окрестности которых должны стабилизироваться все кинематические параметры их движения. При этом программные траектории, как правило, задаются в виде упорядоченного набора путевых точек {Pг■, г = 1, ..., п}, который интерпретируется как совокупность прямолинейных участков траектории, соединяющих между собой путевые точки Pi и Pг■+l. В этих условиях практически важной является задача гладкого сопряжения всех участков опорной траектории между собой [1-3]. При решении задачи на сфере в [4, 5] было предложено использовать гладкую кривую сопряжения в виде заданного радиуса поворота.
Однако этот подход ограничен в своем применении, особенно для сложных программных траекторий и специфических требований к прохождению каждого из ее участков. Более полезным в ряде случаев оказывается подход к формированию кривых сопряжения как кратчайших траекторий движения объекта из некоторой исходной точки в заданную конечную точку, причем эти точки лежат на сопрягаемых участках программной траектории.
Результаты, полученные Л. Дубинсом [6], оказались очень полезными для исследования движения объектов с ограничением радиуса поворота и с постоянной по величине линейной скоростью. Такие объекты стали называть «машина Дубинса», хотя подобными задачами еще в 1889 г. занимался А.А. Марков [7]. Они используют простейшую модель движения объекта в горизонтальной плоскости, которая описывается нелинейной системой дифференциальных уравнений третьего порядка, где две фазовые переменные характеризуют геометрическое положение объекта на плоскости, а третья — угол направления вектора скорости.
В рамках решения этой задачи с использованием принципа максимума Л.С. Понтрягина установлено, что самый быстрый переход из точки в точку с заданными начальным и конечным направлениями линейной скорости осуществляется при помощи кусочно-постоянно-
го управления не более чем с двумя переключениями. Модель Дубинса нашла применение для решения широкого класса задач, например, определение множества достижимости при ограничениях на управление [8, 9], поиске оптимального управления подводным глайдером [10] и применительно к управлению БПЛА [11, 12].
Результаты, полученные Л. Дубинсом [7], полезны при исследовании объектов с ограниченным радиусом разворота и постоянной скоростью движения. Такие модели применяются при построении траекторий движения в горизонтальной плоскости [13, 14]. Для машины Дубинса известен синтез оптимального управления в задаче быстродействия в симметричном [15] и несимметричном [16] случаях ограничений на управление.
В настоящей работе предложено использовать аналогичный подход как для формирования оптимальных по быстродействию режимов маневрирования подвижных объектов, так и для поиска гладких кривых сопряжения участков программной траектории. Машина Дубинса предполагает решение плоской задачи. При поиске оптимальной траектории движения судна в пределах нескольких десятков километров можно пренебречь кривизной земной поверхности и считать, что речь идет о решении плоской задачи. Аналогично и БПЛА, которые осуществляют мониторинг больших территорий и их фото- и видеосъемку, должны быть самолетного типа и иметь стабилизацию по высоте для корректного ведения съемки поверхности. Таким образом, задача поиска оптимальной траектории, проходящей через заданные точки, тоже может считаться плоской задачей для поиска траектории между двумя соседними точками, и для ее решения может использоваться машина Дубинса. Помимо этого, искомая траектория должна иметь ограничения на углы поворота. Физически ограничения на управление (радиус поворота) связаны с ограниченностью угла поворота руля у судна и БПЛА самолетного типа.
В основе данной работы лежит парадигма решения задачи машины Дубинса и использование простейших геометрических расчетов для решения задачи поиска оптимальной по быстродействию траектории. Это позволит упростить аналитическое исследование и решение прямой и сопряженной систем, вытекающих из принципа максимума Понтрягина. Решение задачи поиска оптимальной траектории выполняется перебором возможных стратегий управления с целью определения среди них оптимальной по быстродействию. В пользу этого говорит ограниченное для машин Дубинса количество возможных стратегий управления, а также простота аналитических расчетов для каждой из них, что позволяет выполнять эти расчеты в реальном времени. Быстрота расчетов для задачи определения оптимальной траектории связана с тем, что в предложенном методе не требуется выполнение сложных расчетов для решения задачи нелинейной оптимизации, следующей из принципа Понтрягина. Исследование выигрыша в быстроте расчетов по сравнению с решением задачи на основе принципа максимума Понтрягина выходит за рамки данного исследования, но очевидно, он более чем существенен. Необходимо отметить, что при проведении вычислений в реальном времени важно
обеспечить надежность системы и своевременность результатов [17, 18].
Отметим, что цель работы — построение не оптимального управления, а именно оптимальной траектории, так как для построения оптимального управления необходимо учитывать инерционность рулевого управления. В связи с этим судну или БПЛА самолетного типа лишь задается некоторая траектория, а чтобы следовать ей, придется несколько раз переключать управление, перекладывая руль слева направо и справа налево.
Постановка задачи
Требуется найти кратчайшую траекторию движения объекта из некоторой точки A(x0, y0, ф0) в заданную точку B(xk, yk, ф^), где x(t), y(t) — координаты объекта в неподвижной системе координат, а ф(0 — угол курса (угол между направлением вектора скорости и направлением на Север). Показано [3, 19], что такие траектории на плоскости XOY состоят из конечного числа прямолинейных участков траекторий (S) и дуг окружностей, соответствующих повороту направо (R) и налево (L). При этом Дубинс показал, что оптимальными являются только следующие шесть стратегий управления:
{RLR, LRL, LSL, LSR, RSL, RSR}. (1)
Кратчайший путь между любыми двумя конфигурациями всегда можно охарактеризовать одной из этих стратегий. При этом исходная задача может быть сведена к задаче поиска параметров двух промежуточных точек P и Q (рис. 1), соответствующих моментам переключения управления. На рис. 1 обозначены: N — направление на Север; V — вектор скорости; A и B — соответственно начальная и конечная точки траектории; L — отрезок прямолинейного участка траектории. Это позволит определить не только вид отдельных фрагментов траектории движения, но и длину всей траектории движения.
Стратегии управления RLR и LRL
Общим для стратегий RLR и LRL является то, что на первом и третьем интервалах сигнал управления имеет один и тот же знак, а на втором — другой знак. Считая положительным сигнал управления, вызывающий поворот объекта в сторону увеличения курсового угла, примем Ra = + ra, Rb = + rb, Rc = - rc (для стратегии RLR) и Ra = - ra, Rb = - rb, Rc = + rc (для стратегии LRL). Здесь ra, rb, rc — абсолютные значения радиусов циркуляции, соответствующие заданным сигналам управления на каждом из трех участков.
В качестве примера рассмотрим задачу поиска кривой Дубинса для случая RLR, которая сводится к поиску параметров двух промежуточных точек P и Q (рис. 2).
Для решения задачи необходимо найти координаты центров двух окружностей, одна из которых является исходящей из точки А (обозначим ее центр P1), а вторая — входящей в точку В (с центром в точке P2):
Рис. 1. Пример реализации стратегий управления RLR (а) и RSL (b) Fig. 1. An example of the implementation of RLR (а) and RSL (b) control strategies
Pi = A + [^фа; -si^JRa; P2 = B + [cosфè; -s^b]Rb.
(2) (3)
данных. Для проверки его существования необходимо определить расстояние между точками Р1 и Р2
Центр окружности радиуса Rc которая является касательной к окружностям Ra и Яь, должен находиться в точках P3 или P4. Их особенность состоит в том, что они должны располагаться на расстоянии Rj от точки Pi и на расстоянии R2 от точки P2, где
Ri = Ral + Rcl; R2 = |Rbl + Rcl.
Другими словами, для вычисления координат точки P3 необходимо решить задачу определения точек пересечения двух окружностей, радиусов Rj и R2, исходящих из точек Pj и P2 соответственно. Однако это решение существует не на всех наборах исходных
Рис. 2. Варианты реализации стратегии управления RLR, когда окружность радиуса rc находится слева или справа от
траектории движения Fig. 2. Cases for implementing the RLR control strategy when a circle of radius rc is to the left or right of the trajectory
d = IP - Pill
(4)
Далее выполним проверку двух условий. Во-первых, если + ^2 < й, то окружности и ^2 не пересекаются, так как располагаются отдельно друг от друга. Во-вторых, если Я1 - > й, то окружности также не пересекаются, так как одна окружность находится внутри другой. Если выполняется хотя бы одно из этих условий, то рассматриваемая стратегия управления (RLR или LRL) не может быть реализована, и следует перейти к рассмотрению иной стратегия управления.
При возможности реализации рассматриваемой стратегии управления для поиска координат центра окружности Яс, касательной к окружностям Яа и Яь, требуется найти величины отрезков а и h — расстояния между точками Р1 и Р0 и точками Р3 и Р0 соответственно, где Р1Р0 — проекция Р1Р3 на Р1Р2:
а = (Я? - Я\ + йР)/2й; h = Уя? - а2.
Координаты точки Ро могут быть определены с помощью векторного параметрического уравнения прямой:
Р0 = Р1 + (Р2 - Р1\а/й.
Введем в рассмотрение матрицу
W = [0 -Ый; Ый 0].
Тогда координаты точки пересечения окружностей радиусов Я1 и Я2 для случая, когда окружность Яс находится слева от отрезка АВ (если смотреть в направлении от точки А к точке В — рис. 2), могут быть определены из выражения
Рз = Р0 + W(P2 - Р1).
А координаты второй точки пересечения окружностей Я1 и Я2 для случая, когда окружность Яс находится справа от отрезка АВ (если смотреть в направлении от А к В) могут быть определены из выражения
Р4 = Р0 + WT(P2 - Р1) .
Таким образом, знание координат точки Р0 позволило найти координаты точек Р3 и Р4. Для этого направляющая от Р1 к Р2 была повернута на угол +90° при определении координат точки Р3 (на угол -90° при определении координат точки Р4) и умножена на h.
Проведение этих простейших предварительных расчетов позволяет перейти к решению основной задачи, а именно, поиску параметров двух промежуточных точек рассчитываемой траектории (Р и 0, которые соответствуют моментам переключения управляющего сигнала.
В частности, принимая Р3 за центр окружности Яс, точка сопряжения двух окружностей Яа и Яс (т. е. точка Р), может быть найдена путем деления отрезка, соединяющего точки и Р3, в заданном отношении:
т = №1;
Р = (Р1 + тР3)/(1 + т).
Аналогично могут быть найдены и координаты точки Q, которая является точкой сопряжения окружностей
Яъ и Я-.
п = Яъ/Яс1;
Q = (Р2 + пР3)/(1 + п).
Данные расчеты выполнены в предположении, что любая точка описывается вектором-столбцом, первый элемент которого соответствует абсциссе точки (х), а второй — ординате точки (у). С учетом данных замечаний, а также зная координаты (хР1, уР1), (хР2, уР2) и (хр3, Ур3) точек Р1, Р2 и Р3, значения курсовых углов в точках переключения Р и Q могут быть определены на основе следующих соотношений:
Фр = аг^((хР3 - хр1)/(ур3 - УР1)) + sign(Яa)п/2;
Фч = arctg((xрз - хр2)/(ур3 -УР2)) + sign(Яъ)п/2.
Знание исходных значений фа, фъ и определение фр, фд позволяет на основе разностей курсовых углов определить длину дуги на каждом участке траектории. С учетом радиуса поворота на каждом из участков может быть определена общая длина траектории для каждой из возможных стратегий управления. Что, в свою очередь, позволяет путем простого сравнения определить среди них и оптимальную для конкретных исходных данных.
На рис. 3 представлены все траектории типа RLR и LRL для перехода подвижного объекта из точки А = [0 м, 0 м, 30°] в точку В = [1000 м, 1000 м, 95°] с заданными радиусами поворота га = 400 м, гъ = 600 м и гс = 500 м.
Figure 1: Движение судна из точки А с курса Ка = 30е в точку В на курс Kb = 95е (КЬ-Ка = 65)
RLR-1 - Кр=212,7 Kq=261,9 (Sap=1275,7 Spq=2712,88 Зф=2022Л8) - S = 6011 RLR-2- Kp=57,2 Kq=8 (Sap=189,72 Spq=428,71 Sqb=910,55) - S= 1529
1000
500
-500
-1000
t »
f !
I «1 Д&/1 93,1 ° ;
A
■¿310,9' J
■ o er*
1500
1000
500
-500
О
1 1
t 49 .1? f 7 » 1
I \J\ r - л - 17° > 1 1
n / \ "s V "'<27 К » 1
1000
2000
-500
500 1000 1500 2000
LRL-1 - Kp=324,6 Kq=127,2 (Sap=456,82 Spq=1419,4S Sqb=337,43) - S = 2213,7 LRL-2 - Kp=305,5 Kq=142£ (Sap=590,1 Spq=1722,14 Sqb=S00,75) - S = 2813
2000
1500
1000
500
-500
1 1 1 г «2,2° 1
\ ''' 1
/ Ô /<■ m *
2000
1500
1000
500
-500
1 1 ■ % #7,8° !
eeeeJ^,,«' : : /
#9-7,3»^ / : * /
f ^4,5 r— о
"-—y '"o
-1000
1000
2000
-1000
1000
2000
Рис. 3. Пример траекторий для стратегий управления RLR и LRL Fig. 3. An example of trajectories for the RLR and LRL control strategies
Отметим, что расчеты для случая LRL выполняются также по описанной выше схеме. Только начальные расчеты по формулам (2) и (3) определяют точки P1 и P2 (рис. 2), симметричные точкам Pi и Pj относительно вектора скорости в точках A и B соответственно, и все геометрические построения выполняются относительно этих точек.
Стратегии LSL, LSR, RSL, RSR
Отличие стратегий LSL, LSR, RSL, RSR от рассмотренных ранее в том, что на втором участке выполняется прямолинейное движение, при котором объект, двигаясь из P{xp, yp, фр}, должен попасть в точку Q{xq, Уд, фд} с тем же курсовым углом, с которым он начал свое движение на этом участке. Таким образом, при реализации стратегий LSL, LSR, RSL, RSR курсовой угол в точке Q должен быть всегда равен курсовому углу в точке P, т. е. фд = фр. Геометрически этот угол соответствует углу наклона касательной сразу к двум окружностям радиусов Ra и Rb.
Дуги окружностей определяют движение объекта на начальном и конечном фрагментах искомой траектории. При этом для конкретной стратегии управления центры окружностей Pi и Pj, как и ранее, определяются выражениями (2) и (3), а расстояние d между точками по формуле (4).
Для дальнейших расчетов важным является угол ф0, который соответствует углу между P1P2 и направлением на Север. Получив координаты (xP1, yP1) и (xp2, yP2) точек Pi и P2 этот угол можно найти из выражения вида:
фо = arctg((xP2 - XPi)/(yP2 - yPi)).
При заданных исходных и полученных начальных данных промежуточной задачей для поиска оптимальной траектории движения объекта является задача по-
иска всех необходимых параметров для двух промежуточных точек P и Q, которые определяют моменты переключения сигналов управления.
Отметим, что решение поставленной задачи зависит от исследуемой стратегии управления. Так в случае, если Ra и Rb имеют одинаковые знаки, то задача сводится к построению внешней касательной к двум дугам окружностей (рис. 4, а). В случае, когда Ra и Rb имеют разные знаки, то задача нахождения точек P и Q сводится к построению внутренней касательной к двум дугам окружностей (рис. 4, b).
Для построения касательной к двум дугам окружностей Ra и Rb следует из центра окружности, которая имеет больший радиус, дополнительно построить еще одну окружность радиусом
Ra1 - Rl если sign(Ra) = sign(Rb)
Rai + Rb|, если sign(Ra) Ф sign(Rb).
Первый вариант будет соответствовать стратегиям RSR и LSL, а второй — RSL и LSR. При этом следует особо отметить, что любая из стратегий будет реализуема только в случае, если |d| > |г|.
Если стратегия реализуема, то можно определить длину прямолинейного участка траектории и угол поворота этого участка траектории относительно направления P1P2
L = id2 - r2; ф1 = arctg(|r|/L).
На вычисление курсового угла прямолинейного участка траектории существенное влияние оказывает то, какой из радиусов циркуляции Ra или Rb имеет большее по абсолютной величине значение. Так при
Rai > Rbl
U = Sign(Ra);
Рис. 4. Геометрическая интерпретация реализации стратегий RSR (а) и RSL (b) Fig. 4. Geometric interpretation of the implementation of the RSR (а) and RSL (b) strategies
Фр = Фо + иФъ Ро = Р1.
В противном случае справедливы следующие соотношения:
и = sign(Яb);
Фр = Фо - иФ1; Ро = Р2.
После определения курсового угла в точке Р(Фр), который для траекторий с прямолинейным участком всегда равен курсовому углу в точке 0>(фр = Фд), появляется возможность определить координаты точек:
W =
'-cos^p .sin^)
P = Pi + W^; Q = P2 + WPb; P3 = Po + W|r|u.
На основе исходных данных (фа, фД и полученных результатов расчета (фр, Ь, фр) найдем для каждой из стратегий управления общую длину траектории, которая включает в себя: дугу окружности радиуса Яа от
курсового угла фа до курсового угла фр в направлении sign(Яa); прямолинейный участок траектории длиной Ь; дугу окружности радиуса Яь от курсового угла ф? = фр до курсового угла ф6 в направлении sign(Яb).
Определим путем простого сравнения среди всех возможных стратегий управления ту из них, которая обеспечивает оптимальную траекторию для конкретных исходных данных. В частности, на рис. 5 представлены возможные траектории перехода объекта из точки А в точку В с параметрами, аналогичными рис. 3, т. е. для перехода подвижного объекта из точки А = [0 м, 0 м, 30°] в точку В = [1000 м, 1000 м, 95°] с заданными радиусами поворота га = 400 м, гь = 600 м.
Особенностью программной реализации предлагаемого алгоритма является то, что он позволяет не только получить оптимальную траекторию перехода из точки в точку, но и при желании просмотреть все возможные варианты, что может быть очень полезно с практической точки зрения. Особенно в случае наличия внешних ограничений в заданной акватории маневрирования.
В таблице приведены длины кривых Дубинса (1), представленных на рис. 3 и 5, для всех возможных стратегий управления. Очевидно, что две из них — RSR и RLR-2 — дают близкие результаты, но более короткой и проще реализуемой является стратегия RSR.
Figure 2: Движение судна из точки А с курса Ка = 30е в точку В на курс Kb = 95е (Kb - Ка = 65)
RSR- Кр=31,4 Kq=31,4 (Sap=9,5 Spq=827,23 Sqb=666,42) - S = 1503,2 RSL- Kp=52,6 Kq=52,6 (Sa p=157,92 Spq=1652,29 Sqb=3326,12) - S = 5136,3
1000
500
-500
о
1
1 *
! ' /А <63,6° 1
/ \ О ---- у
2000 1500 1000 500 0
-500
<817,6 4
1 Jr jâj
* ✓ • ✓ ; /
7 «
/1 '-22,6'
-----
-500 0 500 1000 1500 2000
1000
2000
LSL- Kp=50,8 Kq=50,8 (Sap=2367,89 Spq=1967,23 Sqb=3307,31) - S = 7642,4 LSR - Kp=31,3 Kq=31,3 (Sap=2503,88 Spq=845,96 Sqb=666,58) - S = 4016,4
1500
2000 1500 1000 500 0
-500
«815,8°î
яй Як
✓ • ---- г
I <839, i '' 0 ¡io X F '
"в
1000
500
-500
I о
I ■ \ \
<858,7 / • Ï / <36 3,7°
"в
-1000
1000
2000
-500
500 1000 1500 2000
Рис. 5. Пример траекторий для стратегий с прямолинейным участком: RSR (a), RSL (b), LSL (c) и LSR (d) Fig. 5. An example of trajectories for strategies with a straight-line segment RSR (a), RSL (b), LSL (c) and LSR (d)
Таблица. Длины кривых Дубинса перехода из точки A = [0 м, 0 м, 30°] в B = [1000 м, 1000 м, 95°] с радиусами поворота
ra = 400 м, rb = 600 м
Table. Lengths of Dubins curves of transition from point A= [0 m, 0 m, 30°] to B = [1000 m, 1000 m, 95°] with turning radii
ra = 400 m, rb = 600 m
Стратегия управления Длина траектории, м Стратегия управления Длина траектории, м
RLR-1 6011 RSR 1503,2
RLR-2 1529 RSL 5136,3
LRL-1 2213,7 LSL 7642,4
LRL-2 2813 LSR 4016,4
Второй аспект возможности использования данного алгоритма, на котором следует остановиться, состоит в том, что полетное задание для БПЛА или движения исследовательского судна задается совокупностью путевых точек, определяемых их географическими координатами, а гладкое сопряжение двух прямолинейных траекторий в окрестности путевых точек выполняется дугами с заданным радиусом поворота.
В частности, на рис. 6 приведен фрагмент траектории с заданными семью путевыми точками {Рг,1 = 1, ..., 7} и стандартная сглаженная опорная траектории движения объекта.
Предположим, что движение от точки Pз к точке P4 осуществляется вдоль линии газопровода или линии электропередач, а точка P4 — координата газокомпрессорной станции или силовой электрической подстанции, то в этом случае требуются иные подходы к формированию сглаживающих алгоритмов.
Для решения данной задачи применим предложенный алгоритм. Так, если требуется обязательное и точное прохождение всего участка траектории от точки Pз до точки P4, то исходная задача сводится к возврату в исходную точку P4, но с новым значением курса.
В тех случаях, когда га = тъ = гс и допустимо некоторое отклонение от заданной траектории на конечной части участка Pз-P4, но при этом обязательным является попадание в точку P4, то становится необходимым решение обратной задачи, а именно, задачи поиска значений Х0, >>0 и хк, ук равноотстоящих от точки P4 вдоль фрагментов траектории Р3-Р4 и Р4-Р5.
На рис. 7 представлены примеры реализации двух описанных выше задач при изменении в точке Р4 курса сопрягаемых прямолинейных фрагментов траектории с
Ф0 = 255° на ф^ = 103° и допустимых радиусах циркуляции га = тъ = гс = 700 м.
Определение точек сопряжения двух путевых траекторий
При решении задачи сопряжения двух путевых траекторий требуется построить траекторию, которая обеспечит гладкое сопряжение двух прямолинейных фрагментов траекторий и проходит через точку их пересечения. Задача имеет два решения. Путем использования стратегии RLR — в случае выполнения перехода на траекторию, находящуюся слева от исходной, а также стратегии LRL — при переходе на траекторию, которая располагается по правому борту относительно исходной траектории.
Требуется определить координаты точек А(ха, уа) и В(хь, уь), находящихся на одинаковом расстоянии от точки С(хс, ус) = Р4 (рис. 7, Ь), т. е. точки пересечения траекторий Р3Р4 и Р4Р5, заданных своими курсовыми узлами фа и фь. Так как точки А и В должны принадлежать соответствующим траекториям, то для них известны и курсовые углы. Что касается курсового угла фс в точке С, то надо принять во внимание, что сопрягающая траектория симметрична относительно биссектрисы угла Р3Р4 Р5, образованного фрагментами траекторий Р3Р4 и Р4Р5.
Угол фс можно определить, как нормаль к биссектрисе угла Р3Р4 Р5 с учетом направления вращения Яс. Кроме того, свойство симметричности дает возможность при поиске величины равноотстояния (5) вместо всей траектории рассматривать только ее половину. Например, только ее начальную часть: от А(ха, уа, фа)
Рис. 6. Пример задания траектории движения семью путевыми точками (а) и сглаженная опорная траектория движения
объекта(b)
Fig. 6. An example of setting the trajectory of movement with seven waypoints (a) and a smoothed reference trajectory of the
object's movement (b)
Рис. 7. Примеры траекторий: точка Р4 — начало и конец траектории по стратегии LRL (а); RLR с отклонением от заданной
траектории на конечной части участка Р3Р4 и с заходом в Р4 (b) Fig. 7. An example of trajectories (a): point Р4 is the beginning and the end of the trajectory according to the LRL strategy; RLR with a deviation from the given trajectory at the end of the section Р3Р4 and passing the point Р4 (b)
к промежуточной точке Р(хь У1, Ф1) и далее к точке С (Хс, Ус, Фс).
Так как исходными данными являются только координаты точки С и два курсовых угла сопрягаемых траекторий фа и Фъ, то сначала необходимо определить, какая из стратегий может реализовать поставленную задачу.
(RLR (Яа = Яъ = г, Яс = -г), если sin(фъ - Фа) < 0 1LRL (Яа = Яъ = -г, Яс = г), если sin(фъ - Фа) > 0.
*с3 = *с + ^сСОБфсУсЗ = Ус - ^csin9c;
(8)
state = i
Допущение, что точки А и В находятся на одинаковом расстоянии от точки С, позволяет записать выражения для определения координат точек:
Ха = Хс - &тфдаУа = Ус - (5)
Хъ = Хс + Ssmфъ, Уъ = Ус + ^СОБфъ. (6)
Вычислим расстояние между точками с использованием (5) и (6):
ь = у1(Ха - хъ)2 + (Уа - Уъ)2 = 5 ,
что позволит воспользоваться теоремой косинусов для определения угла между траекториями Р3Р4 и Р4Р5, который обозначим как в:
соф = 5 + & - Ь2)/(2Я2) = - co¡(фa - фъ).
Получив значение угла в, появляется возможность вычисления курсового угла в точке С, который может быть определен из следующего выражения:
Фс = Фа - ¡^п(Яа)(П - в)/2.
Фрагмент сопрягающей траектории от точки А(ха, Уа, фа) к промежуточной точке Р(х1, У1, ф1), и далее к точке С(хс, Ус, фс) будет состоять из дуг двух окружностей радиусов Яа и Яс. Обозначим центры этих двух окружностей, как точки с1 и с3, координаты которых будут определяться следующим образом:
xc1 = xa + RcC^a, Ус1 = Уа - Rasi^
(7)
Отметим, что расстояние между этими точками должно быть равно двум радиусам, т. е. выполняется условие:
>/(Хс1 - Хс3)2 + (Ус1 - Ус3)2 = 2Г. (9)
Подставляя выражения (7), с учетом (5) и (8), в выражение (9), получим квадратное уравнение относительно 5 вида:
(т - 5япф0)2 + (п + Sco¡ф0)2 = 4г2, (10)
где т = Яaco¡фa - Яcco¡фc, п = Яаяпфа + Яc¡inфc.
При решении уравнения (10) для исходных данных, приведенных на рис. 7, можно видеть результаты расчета сглаживающей траектории, проходящей через точку Р4. Эта траектория проходит через точки начала и окончания данного режима, равноотстоящие от точки Р4 на величину 5 ~ 2,538га.
Заключение
В основе предложенного алгоритма используется машина Дубинса, т. е. задача определения линии кратчайшей длины с ограниченным радиусом кривизны, соединяющей на плоскости две точки - входа и выхода траектории - с заданными координатами и курсовыми направлениями на входе и выходе. Особенность предложенного алгоритма состоит в том, что вместо прямого решения принципа максимума Понтрягина предложено воспользоваться простым перебором возможных стратегий управления с целью определения среди них оптимальной по быстродействию. В пользу использования данного подхода говорит то, что для машин Дубинса существует ограниченное количество возможных стратегий управления, а также простота аналитических расчетов для каждой из них, что позволяет выполнять эти расчеты в реальном времени. Быстрота расчетов для задачи определения оптималь-
ной траектории связана с тем, что в предложенном методе не требуется выполнение сложных расчетов для решения задачи оптимизации на основании принципа Понтрягина. Предлагаемый метод решает задачу геометрически с последующим перебором полученных решений с учетом ограничений. Данные условия связаны не только с ограничениями на радиус поворота, но и с географическими особенностями конкретной местности: для беспилотных летательных аппаратов — здания и рельеф, а для судов — береговая линия, мели, острова и т. д. В связи с этим может оказаться, что найденное оптимальное по быстродействию решение не может быть реализуемым на практике. Тогда предложенный в работе метод имеет возможность выбора среди менее оптимальных по быстродействию траекторий.
Траектории, для которых выполняются ограничения на радиус поворота, и которые проходят территориально в обход естественных географических ограничений, могут оказаться реализуемы с практической точки зрения. Помимо вычислительной сложности, в методе Дубинса в результате его использования получается единственная оптимальная траектория, и при невозможности ее практической реализации нет вариантов для выбора из других траекторий, допустимых по ограничениям на радиус поворота.
В дальнейшем исследование и разработку рассмотренного алгоритма предполагается вести в направлении его модификации с учетом влияния условий окружающей среды — водных течений и ветра.
Литература
1. Parlangeli G., Indiveri G. Dubins inspired 2D smooth paths with bounded curvature and curvature derivative // IFAC Proceedings Volumes. 2010. V. 43. N 16. P. 252-257. https://doi. org/10.3182/20100906-3-IT-2019.00045
2. Jha B., Chen Z., Shima T. Shortest bounded-curvature paths via circumferential envelope of a circle // IFAC-PapersOnLine. 2020. V. 53. N 2. P. 15674-15679. https://doi.org/10.1016/j. ifacol.2020.12.2554
3. Kumar M., Keil E., Rao A.V. Chance-constrained path planning in narrow spaces for a Dubins vehicle // International Robotics & Automation Journal. 2021. V. 7. N 2. P. 46-61. https://doi. org/10.15406/iratj.2021.07.00277
4. Вагизов М.Р., Хабаров С.П. Алгоритм формирования гладких программных траекторий движения БПЛА // Информация и космос. 2021. № 2. С. 122-130.
5. Хабаров С.П., Шилкина М.Л. Формирование программных траекторий движения БПЛА с учетом ограничений на их управляемость // Цифровые технологии в лесном секторе: материалы II Всероссийской научно-технической конференции-вебинара. СПб.: Санкт-Петербургский государственный лесотехнический университет имени С.М. Кирова, 2021. С. 141-143.
6. Dubins L.E. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents // American Journal of Mathematics. 1957. V. 79. N 3. P. 497-516. https://doi.org/10.2307/2372560
7. Марков A.A. Несколько примеров решения особого рода задач о наибольших и наименьших величинах // Сообщения Харьковского математического общества. Вторая серия. 1889. Т. 1. № 2. С. 250-276.
8. Пацко В.С., Федотов А.А. Аналитическое описание множества достижимости для машины Дубинса // Труды института математики и механики УрО РАН. 2020. Т. 26. № 1. С. 182-197. https:// doi.org/10.21538/0134-4889-2020-26-1-182-197
9. Patsko V. S., Fedotov A.A. Reachable set for Dubins car and its application to observation problem with incomplete information // Proc. 27th Mediterranean Conference on Control and Automation (MED). 2019. P. 489-494. https://doi.org/10.1109/MED.2019.8798511
10. Liu Y., Ma J., Ma N., Zhang G. Path planning for underwater glider under control constraint // Advances in Mechanical Engineering. 2017. V. 9. N 8. P. 1-9. https://doi.org/10.1177/1687814017717187
11. Бердышев Ю.И. Об оптимальном по быстродействию управлении обобщенной машиной Дубинса // Труды института математики и механики УрО РАН. 2016. Т. 22. № 1. С. 26-35.
12. Patsko V.S., Fedotov A.A. Three-dimensional reachable set at instant for the Dubins car: Properties of extremal motions // Proc. 60th Israel Annual Conference on Aerospace Sciences (IACAS). 2020. P. 10331049.
13. Silverberg L., Xu D. Dubins waypoint navigation of small-class unmanned aerial vehicles // Open Journal of Optimization. 2019. V. 8. N 2. P. 59-72. https://doi.org/10.4236/ojop.2019.82006
14. Meyer Y., Isaiah P., Shima T. On Dubins paths to intercept a moving target // Automatica. 2015. V. 53. P. 256-263. https://doi. org/10.1016/j.automatica.2014.12.039
References
1. Parlangeli G., Indiveri G. Dubins inspired 2D smooth paths with bounded curvature and curvature derivative. IFAC Proceedings Volumes, 2010, vol. 43, no. 16, pp. 252-257. https://doi. org/10.3182/20100906-3-IT-2019.00045
2. Jha B., Chen Z., Shima T. Shortest bounded-curvature paths via circumferential envelope of a circle. IFAC-PapersOnLine, 2020, vol. 53, no. 2, pp. 15674-15679. https://doi.org/10.1016/j. ifacol.2020.12.2554
3. Kumar M., Keil E., Rao A.V. Chance-constrained path planning in narrow spaces for a Dubins vehicle. International Robotics & Automation Journal, 2021, vol. 7, no. 2, pp. 46-61. https://doi. org/10.15406/iratj.2021.07.00277
4. Vagizov M.R., Khabarov S.P. Algorithm for the formation of smooth programmed trajectories of uav motion. Information and Space, 2021, no. 2, pp. 122-130. (in Russian)
5. Khabarov S.P., Shilkina M.L. Formation of programmed trajectories for UAV movement, taking into account their controllability. Digital technologies in forestry. Proceedings of the 2nd All-Russian scientific and technical webinar conference. St. Petersburg, Saint Petersburg State Forest Technical University, 2021, pp. 141-143. (in Russian)
6. Dubins L.E. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, 1957, vol. 79, no. 3, pp. 497-516. https://doi.org/10.2307/2372560
7. Markov A.A. Some examples of the solution of a special kind of problem on greatest and least quantities. Soobshhenija Har 'kovskogo matematicheskogo obshhestva. Vtoraja serija, 1889, vol. 1, no. 2, pp. 250-276. (in Russian)
8. Patsko V.S., Fedotov A.A. Analytic description of a reachable set for the Dubins car. Trudy instituta matematiki i mekhaniki UrO RAN, 2020, vol. 26, no. 1, pp. 182-197 (in Russian). https://doi. org/10.21538/0134-4889-2020-26-1-182-197
9. Patsko V.S., Fedotov A.A. Reachable set for Dubins car and its application to observation problem with incomplete information. Proc. 27th Mediterranean Conference on Control and Automation (MED), 2019, pp. 489-494. https://doi.org/10.1109/ MED.2019.8798511
10. Liu Y., Ma J., Ma N., Zhang G. Path planning for underwater glider under control constraint. Advances in Mechanical Engineering, 2017, vol. 9, no. 8, pp. 1-9. https://doi.org/10.1177/1687814017717187
11. Berdyshev Yu.I. On a time-optimal control for the generalized Dubins car. Trudy instituta matematiki i mekhaniki UrO RAN, 2016, vol. 22, no. 1, pp. 26-35. (in Russian)
12. Patsko V.S., Fedotov A.A. Three-dimensional reachable set at instant for the Dubins car: Properties of extremal motions. Proc. 60th Israel Annual Conference on Aerospace Sciences (IACAS), 2020, pp. 10331049.
13. Silverberg L., Xu D. Dubins waypoint navigation of small-class unmanned aerial vehicles. Open Journal of Optimization, 2019, vol. 8, no. 2, pp. 59-72. https://doi.org/10.4236/ojop.2019.82006
14. Meyer Y., Isaiah P., Shima T. On Dubins paths to intercept a moving target. Automatica, 2015, vol. 53, pp. 256-263. https://doi. org/10.1016/j.automatica.2014.12.039
15. Pecsvaradi T. Optimal horizontal guidance law for aircraft in the terminal area // IEEE Transactions on Automatic Control. 1972. V. 17. N 6. P. 763-772. https://doi.org/10.1109/TAC.1972.1100160
16. Bakolas E., Tsiotras P. Optimal synthesis of the asymmetric sinistral/ dextral Markov-Dubins problem // Journal of Optimization Theory and Applications. 2011. V. 150. N 2. P. 233-250. https://doi. org/10.1007/s10957-011-9841-3
17. Bogatyrev V.A., Bogatyrev A.V., Bogatyrev S.V. Redundant servicing of a flow of heterogeneous requests critical to the total waiting time during the multi-path passage of a sequence of info-communication nodes // Lecture Notes in Computer Science (including subseries Lecture Notes in Artificial Intelligence and Lecture Notes in Bioinformatics). 2020. V. 12563. P. 100-112. https://doi. org/10.1007/978-3-030-66471-8_9
18. Bogatyrev V.A., Bogatyrev S.V., Derkach A.N. Timeliness of the reserved maintenance by duplicated computers of heterogeneous delay-critical stream // CEUR Workshop Proceedings. 2019. V. 2522. P. 26-36.
19. LaValle S.M. Planning Algorithms. Cambridge University Press, 2006. 1023 p. https://doi.org/10.1017/CB09780511546877
15. Pecsvaradi T. Optimal horizontal guidance law for aircraft in the terminal area. IEEE Transactions on Automatic Control, 1972, vol. 17, no. 6, pp. 763-772. https://doi.org/10.1109/ TAC.1972.1100160
16. Bakolas E., Tsiotras P. Optimal synthesis of the asymmetric sinistral/ dextral Markov-Dubins problem. Journal of Optimization Theory and Applications, 2011, vol. 150, no. 2, pp. 233-250. https://doi. org/10.1007/s10957-011-9841-3
17. Bogatyrev V.A., Bogatyrev A.V., Bogatyrev S. Redundant servicing of a flow of heterogeneous requests critical to the total waiting time during the multi-path passage of a sequence of info-communication nodes. Lecture Notes in Computer Science (including subseries Lecture Notes in Artificial Intelligence and Lecture Notes in Bioinformatics), 2020, vol. 12563, pp. 100-112. https://doi. org/10.1007/978-3-030-66471-8_9
18. Bogatyrev V.A., Bogatyrev S.V., Derkach A.N. Timeliness of the reserved maintenance by duplicated computers of heterogeneous delay-critical stream. CEUR Workshop Proceedings, 2019, vol. 2522, pp. 26-36.
19. LaValle S.M. Planning Algorithms. Cambridge University Press, 2006, 1023 p. https://doi.org/10.1017/CB09780511546877
Авторы
Хабаров Сергей Петрович — кандидат технических наук, доцент, доцент, Санкт-Петербургский государственный лесотехнический университет им. С.М. Кирова, Санкт-Петербург, 194021, Российская Федерация, 57220744173, https://orcid.org/0000-0003-1337-0150, Serg.Habarov@mail.ru
Шилкина Мария Львовна — кандидат технических наук, доцент, доцент, Санкт-Петербургский государственный лесотехнический университет им. С.М. Кирова, Санкт-Петербург, 194021, Российская Федерация, 57246047100, https://orcid.org/0000-0001-6501-5789, mchernobay@inbox.ru
Authors
Sergey P. Khabarov — PhD, Associate Professor, Associate Professor, St. Petersburg State Forest Technical University, Saint Petersburg, 194021, Russian Federation, S3 57220744173, https://orcid.org/0000-0003-1337-0150, Serg.Habarov@mail.ru
Maria L. Shilkina — PhD, Associate Professor, Associate Professor, St. Petersburg State Forest Technical University, Saint Petersburg, 194021, Russian Federation, gg 57246047100, https://orcid.org/0000-0001-6501-5789, mchernobay@inbox.ru
Статья поступила в редакцию 19.08.2021 Одобрена после рецензирования 06.09.2021 Принята к печати 27.09.2021
Received 19.08.2021
Approved after reviewing 06.09.2021
Accepted 27.09.2021
©
Работа доступна по лицензии Creative Commons «Attribution-NonCommercial»
