Биард Р.У., МакЛэйн Т.У. Малые БЛА - теория и практика (2015) (1245764), страница 37
Текст из файла (страница 37)
10, необходимоследовать по прямолинейному отрезку w i -1 w i до входа в полуплоскость H1, показанную на рис. 11.5. Затем следовать по круговой орбите радиуса R по часовой стрелке до входа в полуплоскость H2, показанную на рис. 11.5, в точке которой начнется следование попрямолинейному отрезку w i w i +1 .Как показано на рис. 11.5, центр закругляющей вставки дается соотношениемæ R ö q i -1 - q i÷÷.c = w i - ççè sinñ / 2 ø q i -1 - q i11.1.
Переходы между путевыми точками201Рис. 11.5. Задание полуплоскостей, связанных со следованием по закругляющей траектории, вставленной между сегментамиАналогичным образом, положение полуплоскости H1 задается с помощьюæ R ö÷÷ q i -1r1 = w i - ççè sinñ / 2 øи вектором нормали qi 1. Положение полуплоскости H2 задается с помощьюæ R ö÷÷ q ir2 = w i + ççè sinñ / 2 øи вектором нормали qi.Алгоритм для маневрирования вдоль маршрута с путевыми точками W, использующий закругляющие вставки для сглаживания перехода между прямолинейными сегментами, представлен в приведенном ниже алгоритме 6.
Оператор if в 1-й строке проверяет, была ли достигнута новая путевая точка,включая моменты, когда алгоритм инстанциируется. Если системой управления маршрутом была достигнута новая путевая точка, тогда во 2-й строке инициализируются указатель путевых точек и конечный автомат. В строках 4—6рассчитываются единичные вектора qi 1 и qi и угол ñ.Когда конечный автомат находится в state = 1, МБЛА получает командуследовать прямолинейным маршрутом вдоль w i -1 w i , который параметризуетсяс помощью r = wi 1 и q = qi 1, и задается в строках 8—10. В строках 11—14выполняется проверка, перешел ли МБЛА в полуплоскость, обозначенную H1на рис. 11.5.
Если МБЛА перешел в H2, тогда конечный автомат обновляет состояние на state = 2.Когда конечный автомат находится в состоянии state = 2, МБЛА получаеткоманду следовать орбитальным маршрутом, который задается закругляющей202Глава 11. Система управления маршрутомАлгоритм 6. Следовать путевыми точками с закруглениями: (flag, r, q, c, с, л) == followWppFillet(W, p, R) (следовать АО-скруглению)Входные данные: Траектория с путевыми точками W = {w1, .
. . , wN}, положение МБЛА p = (pn, pe, pd)Т, радиус закругления R.Требуется: N ³ 31: if Достигнута новая путевая точка маршрута W, тогда2: Инициализировать индекс путевой точки: i ¬ 2, и конечный автомат:state ¬ 1.3: end if4: qi1 ¬ (wi wi1)/| | w i - w i-1 | |5: qi ¬ (wi+1 wi)/| | w i +1 - w i | |6: ñ ¬< cos1(qТi1qi)7: if state = 1, then8: flag ¬ 19: r ¬ wi110: q ¬ qi111: z ¬ wi (R/tan(ñ/2))qi112: if p Î H(z, qi1), then13: state ¬ 214: end if15: в противном случае, если state = 2, then16: flag ¬ 217: c ¬ wi (R /sin(ñ/2))(qi1 qi)/| | q i +1 - q i | |18: с ¬ R19: л ¬ sign(qi1,nqi,e qi1,eqi,n)20: z ¬ wi + (R/tan(r/2)) qi21: if p Î H(z, qi ) then22: i ¬ (i + 1) до тех пор, пока не станет i = N 1.23: state ¬ 124: end if25: end if26: return flag, r, q, c, с, л.вставкой.
Центр, радиус и направление орбиты задаются в 17—19 строках.В строке 19 qi1, n и qi1,e обозначают северную и восточную компоненты qi 1.В строках 21—24 выполняется проверка, перешел ли МБЛА в полуплоскость,обозначенную на рис. 11.5, через H2. Если МБЛА перешел в H2, тогда указатель11.1. Переходы между путевыми точками203Северная координатаСистема управления маршрутом (закругления)Восточная координатаРис.
11.6. Пример типов маршрутов полета, создаваемых алгоритмом 6путевых точек увеличивается и конечный автомат переключается обратно в состояние state = 1, чтобы следовать по сегменту w i w i +1 . Алгоритм 6 создает маршруты, которые похожи на приведенные на рис. 11.6.Одним из недостатков метода закругления маршрута, согласно алгоритму6, является то, что при вставке закруглений меняется длина маршрута. Дляопределенных задач, таких как коллективная синхронизация, описанная в[60], важно иметь высококачественные оценки длины маршрута или времени,требуемого для пересечения определенной путевой точки маршрута.
Этот раздел главы будет завершен выводом выражения для длины маршрута W послетого, как были установлены закругляющие маршрут участки.Соблюдая точность, пустьN|W | @ åi =2w i - w i -1будет длиной маршрута W. Определим |W|F как длину маршрута с поправкой научасток скругления, которая будет получена с помощью алгоритма 6. Из рис. 1.4видно, что длина участка скругления, пересекаемого скорректированной траекторией, равна R(ñi). Кроме того, ясно, что длина прямолинейного сегмента, удаленного из |W| за счет прокладки скругления, равна 2R tan(ñi/2). ПоэтомуNæ2R ö÷,| W | F = |W | + å çç R(ñ i ) tanñ i / 2 ÷øi =2 èгде ñi дается уравнением (11.3).(11.4)204Глава 11. Система управления маршрутом11.2.
Òðàåêòîðèè Äóáèíñà11.2.1. Îïðåäåëåíèå òðàåêòîðèé ÄóáèíñàВ этом разделе рассматриваются траектории Дубинса, в которых, в отличие отследования траекториями с путевыми точками, целью является переход из одной конфигурации (положения и курса) в другую. В [61] было показано, чтодля транспортного средства с кинематикой, определяемой соотношениямиp& n = V cos q,p& e = V sin q,J& = u,где V — постоянная, а u Î [u , u ], оптимальный по времени следования маршрут между двумя различными конфигурациями состоит из дуги окружности, закоторой следует отрезок прямой, и завершается другой дугой окружности кконечной конфигурации, где радиус дуги окружности равен V/u . Эти траектории «поворот — прямая — поворот» составляют несколько вариантов траекторий Дубинса, определяемых для оптимальных переходов между конфигурациями. В контексте неуправляемых летательных аппаратов внимание будетограничено сценариями полетов при постоянной высоте и постоянной скорости относительно земли.Радиус дуги окружности, которая задает траекторию Дубинса, будем обозначать R, при этом предположим, что R по меньшей мере больше минимального радиуса поворота МБЛА.
На протяжении этого раздела конфигурацияМБЛА определяется (p, ч), где p является положением в инерциальной системе отсчета, а ч — курсовым углом.Обозначив начальную конфигурацию как (ps, чs), конечную — как (pe, чe),построим траекторию Дубинса, которая состоит из дуги радиуса R, начинающейся в начальной конфигурации с последующим прямолинейным участком, изавершается другой дугой радиуса R, заканчивающейся в конечной конфигурации. Как показано на рис.
11.7, для любых заданных начальной и конечнойконфигураций имеются четыре возможные траектории, состоящие из дуги соследующей за ней прямой линией и новой дуги окружности. I-й вариант(R-S-R) относится к дуге с движением по часовой стрелке, за которой следуетотрезок прямой и другая дуга окружности с движением по часовой стрелке. II-йвариант (R-S-L) относится к дуге с движением по часовой стрелке с последующим отрезком прямой и дугой с движением против часовой стрелки. Случай III(L-S-R) относится к дуге с движением против часовой стрелки, последующейпрямой линией и дугой с движением по часовой стрелке.
IV-й вариант (L-S-L)11.2. Траектории Дубинса205pначалоВариант I: R-S-RpначалоВариант II: R-S-LpначалоВариант III: L-S-RВариант IV: L-S-LРис. 11.7. При заданной начальной конфигурации (ps, чs) и конечной конфигурации (pe,чe), а также заданном радиусе R имеются четыре возможных траектории, состоящие из дуги, прямой линии и другой дуги. Траектория Дубинса задается как вариант, который приводит к наикратчайшему пути, которым для данного сценария является случай Iотносится к дуге с движением против часовой стрелки, прямой линии и другойдуге с движением против часовой стрелки. Траектория Дубинса определяетсякак вариант с кратчайшей траекторией движения.11.2.2. Ðàñ÷åò äëèíû ìàðøðóòàДля задания маршрута Дубинса необходимо рассчитать длину траектории длячетырех вариантов, представленных на рис. 11.7.
В этом разделе будут получены явные формулы для длины траектории каждого из вариантов. Если положение обозначить через p, курс — через ч, а радиус — через R, то центрыокружностей при движении по часовой и против часовой стрелки можноопределить с помощью соотношенийcr = p + R (cos(ч + р/2 ), sin(ч + р/2 ), 0)Т,(11.5)cl = p + R (cos(ч р/2 ), sin(ч р/2 ), 0)Т.(11.6)Для вычисления длин пути различных траекторий необходимо получитьобщее уравнение для угловых расстояний на окружности. На рис.
11.8 показана геометрия для окружностей с движением как по часовой стрелке (CW), таки против часовой стрелки (CCW).206Глава 11. Система управления маршрутомРис. 11.8. Угловое расстояние между J1 и J2 для движения по окружности по часовой (CW)и против часовой (CCW) стрелкиПредположим, что J 1 и J 2 лежат в интервале 0—2р. Для окружностей с движением по часовой стрелке угловое расстояние между J 1 и J 2 дается выражением|J 2 J 1|CW @ 2 р + J 2 J 1 ,(11.7)гдеj @ j mod 2р.Аналогично для окружностей с движением против часовой стрелки получим|J 2 J 1|CCW @ 2 р J 2 + J 1 .(11.8)I-й вариант: R-S-RГеометрия для этого варианта показана на рис.
11.9, где J — угол, образованный линией между crs и cre. Используя уравнение (11.7), расстояние, проходимое вдоль crs, можно записать какR 2p + J -pp.- cs 22Рис. 11.9. Траектория Дубинса, I-й вариант11.2. Траектории Дубинса207Аналогичным образом, используя (11.7), расстояние, проходимое вдоль cre,можно записать какpp.R 2p + ce - J22Общая длина для I-го варианта маршрута дается выражениемL1 = c rs - c re + R 2 p + J -pp- cs 22+ R 2p + ce -pp.- J22(11.9)II-ой вариант: R-S-LГеометрия для этого варианта показана на рис.
![](https://s.studizba.com/z.php?f=/templates/si/i/noavatar.png&w=100&h=100&t=1"){kind=avatar}
