Биард Р.У., МакЛэйн Т.У. Малые БЛА - теория и практика (2015) (1245764), страница 38
Текст из файла (страница 38)
11.10, где J — угол, образованный линией между crs и cle, l = | |cle crs| | иJ 2 = J р/2 + sin1 (2R/l).Рис. 11.10. Траектория Дубинса, вариант IIИспользуя уравнение (11.7), расстояние, проходимое вдоль crs, дается выражениемp.R 2p + J 2 - cs 2Аналогичным образом, используя (11.8), расстояние, проходимое вдоль cle,можно записать какp.R 2p + J 2 + p - ce +2Общая длина для II-го варианта маршрута дается выражениемL2 = l 2 - 4 R 2 + R 2 p + J 2 - c s -p2+ R 2p + J 2 + p - ce +p.2(11.10)208Глава 11. Система управления маршрутомIII-й вариант: L-S-RГеометрия для III-го случая показана на рис.
11.11, где J — угол, образованный линией между crs и cle, l = | |cre cls| | иJ 2 = cos1 (2R/l).Рис. 11.11. Траектория Дубинса, III-й вариантИспользуя уравнение (11.8), расстояние, проходимое вдоль cls, дается выражениемpR 2p + cs +- J +J2 .2Аналогичным образом, используя (11.7), расстояние, проходимое вдоль cre,можно записать какpR 2p + ce - J +J2 -p .2Общая длина для III-го варианта маршрута дается выражениемL3 = l 2 - 4 R 2 + R 2 p + c s +p- J +J22+(11.11)+ R 2p + ce -p- J +J2 -p .211.2. Траектории Дубинса209IV-й вариант: L-S-LГеометрия для IV-го варианта показана на рис.
11.12, где J — угол, образованный линией между cls и cle. Используя уравнение (11.8), расстояние, проходимое вдоль cls, можно записать какR 2p + cs +pp.- J+22Рис. 11.12. Траектория Дубинса, IV-й вариантАналогичным образом, используя (11.8), расстояние, проходимое вдоль cle,можно записать какpp.R 2p + J +- ce +22Общая длина для IV-го варианта маршрута поэтому дается выражениемL4 = c ls - c le + R 2 p + c s +pp- J+22+ R 2p + J +pp. (11.12)- ce +2211.2.3. Àëãîðèòìû äëÿ îòñëåæèâàíèÿ òðàåêòîðèé ÄóáèíñàАлгоритм для отслеживания траектории Дубинса III-го варианта представленграфически на рис.
11.13. Алгоритм инициализируется для орбиты с движением против часовой стрелки вокруг cls и выдерживает движение по этой орбите,пока МБЛА не войдет в полуплоскость, обозначенную как H1. После входа вH1 начинает работать стратегия ведения МБЛА по прямолинейной траектории, пока МБЛА не войдет в полуплоскость H2. Затем система управления ведет МБЛА по часовой стрелке на орбите вокруг cre, пока МБЛА не войдет вполуплоскость H3, которая определяет завершение траектории Дубинса.210Глава 11. Система управления маршрутомРис. 11.13. Определение переключения полуплоскостей траектории Дубинса. Алгоритм начинается по круговой орбите и переключается на прямолинейное отслеживаниепри входе в H1. Отслеживание по орбите снова инициализируется при входе H2.Полуплоскость H3 определяет завершение траектории ДубинсаОтсюда следует, что траекторию Дубинса можно параметризовать начальной окружностью cs, направлением движения по начальной окружности лs, конечной окружностью ce, направлением движения по конечной окружности лe,параметрами полуплоскости H1 z1 и q1, параметрами полуплоскости H2 z2 иq2 = q1 и параметрами полуплоскости H3 z3 и q3.
Параметры траектории Дубинса, связанные с начальной конфигурацией (ps, чs), конечной конфигурацией(pe, чe) и радиусом R, рассчитываются с помощью алгоритма 7. Так же рассчитывается длина траектории Дубинса L. Rz(J) обозначает матрицу вращенияrдля поворота по часовой стрелке на угол J вокруг оси z и e 1 = (1, 0, 0)Т. Еслизадать последовательность конфигурацийP = {(w1, ч1), (w2, ч2), · · · , (wN, чN)},(11.13)и можно использовать алгоритм управления движением, ведущий по траектории Дубинса между конфигурациями, который представлен ниже алгоритмом 8.
В строке 4 задаются параметры Дубинса для текущего сегмента траектории по путевым точкам, используя алгоритм 7. Поскольку начальнаяконфигурация может быть далеко от окружности, которая уже находится в полуплоскости H1, то начальная окружность будет пребывать в state = 1 (состояние 1), пока не пересечет часть окружности, находящейся в противоположнойстороне от H1 (см.
операторы в строках 7—9). Начальная окружность затем перейдет в state = 2, пока МБЛА не пересечет полуплоскость H1 (см. операторы встроках 10—13). После пересечения H1 будет выполняться следование по прямолинейной траектории в state = 3 (см. операторы в строках 14—18). В строке16 проверяется, пересек ли МБЛА полуплоскость H2. Если пересек, выполняется следование по конечной окружности в state = 4 и state = 5. Снова нужны11.2. Траектории Дубинса211Алгоритм 7. Нахождение параметров Дубинса: (L, cs, лs, ce, лe, z1, q1, z2, z3, q3) == findDubinsParameters (ps, чs, pe, чe, R) (найти параметры Дубинса)Входные данные: Начальная конфигурация (ps, чs), конечная конфигурация(pe, чe), радиус R.Требуется, чтобы: | |ps pe| | ³ 3RТребуется, чтобы: R был больше минимального радиуса поворота МБЛА1: crs ¬ ps + RRz(р/2) (cos чs, sin чs, 0)T2: cls ¬ ps + RRz(-р/2) (cos чs, sin чs, 0)Т3: cre ¬ pe RRz(р/2) (cos чe, sin чe, 0)Т4: cle ¬ pe + RRz(-р/2) (cos чe, sin чe, 0)Т5: Рассчитать с помощью (11.9)—(11.12) L1, L2, L3 и L4.6: L ¬ min{L1, L2, L3, L4}7: if arg min{L1, L2, L3, L4} = 1, then8: cs ¬ crs, лs ¬ +1, ce ¬ cre, лe ¬ +19: q1 ¬ (ce cs)/| |ce cs| |10: z1 ¬ cs + RRz(-р/2) q111: z2 ¬ ce + RRz(-р/2) q112: else if arg min{L1, L2, L3, L4} = 2, тогда13: cs ¬ crs, лs ¬ +1, ce ¬ cle, лe ¬ 114: l ¬ | |ce cs| |15: J ¬ угол(ce cs)16: J 2 ¬ J р/2 + sin1(2R/l)17: q1 ¬ Rz (J 2 + р/2)e118: z1 ¬ cs + R Rz (J 2) e119: z2 ¬ ce + R Rz (J 2 + р) e120: else if arg min{L1, L2, L3, L4} = 3, then21: cs ¬ cls, лs ¬ 1, ce ¬ cre, лe ¬ +122: l ¬ | |ce cs| |,23: J ¬ угол (ce cs),24: J 2 ¬ cos1 (2R/l)25: q1 ¬ Rz (J + J 2 р/2) e1,26: z1 ¬ cs + R Rz (J + J 2) e1,27: z2 ¬ ce + R Rz (J + J 2 р) e128: else if arg min{L1, L2, L3, L4} = 4, then29: cs ¬ cls, лs ¬ 1, ce ¬ cle, лe ¬ 130: q1 ¬ (ce cs)/| |ce cs| |,31: z1 ¬ cs + R Rz(р/2) q1,32: z2 ¬ ce + R Rz(р/2) q233: end if34: z3 ¬ pe35: q3 ¬ Rz (чe)e1212Глава 11.
Система управления маршрутомдва состояния, поскольку МБЛА может уже находиться в H3 в том случае, когда он входит в H2. Если это так, то он следует по конечной окружности, покане войдет в H3 (см. строки 19—23). После того как МБЛА вошел в H3, как показано в строке 25, путевые точки циклически повторяются и находятся новыепараметры Дубинса (см. строки 27—28). На рис. 11.14 приведен пример траектории, созданной с помощью алгоритма 8.Северная координатаСистема управления маршрутом (траектории Дубинса)Восточная координатаРис.
11.14. Пример типов траекторий полета, создаваемых алгоритмом 811.3. Êðàòêîå ñîäåðæàíèå ãëàâûВ этой главе рассматривается несколько схем для переходов между конфигурациями путевых точек с использованием алгоритмов следования по прямолинейной и круговой траекториям, описанным в гл. 10. В разделе 11.1 описывается переход между сегментами маршрута с путевыми точками, использующийполуплоскость и вставление скругления между сегментами.
В разделе 11.2 вводятся в рассмотрение траектории Дубинса и показано, как построить траекторию Дубинса между конфигурациями путевых точек. В следующей главе будетописано несколько алгоритмов планирования маршрута, которые находят участки траекторий с путевыми точками и конфигурации путевых точек для маневрирования через поля препятствий.Замечания и ссылкиРаздел 11.1 главным образом основывается на [62]. Траектории Дубинсабыли введены в [61].
В определенных случаях траектория Дубинса может несодержать одного из трех элементов. Например, если начальная и конечная11.3. Краткое содержание главы213Алгоритм 8. Следование по путевым точкам с параметрами Дубинса: (flag, r, q,c, с, л) = followWppDubins(P, p, R) (следование траекторией Дубинса)Входные данные: Конфигурация траектории P = {(w1, ч1), . . . , (wN, чN)},state МБЛА p = (pn, pe, pd)Т, радиус скругления R.Требование: N ³ 31: if Получена новая конфигурация траектории P, тогда2: Инициализировать указатель путевой точки: i ¬ 2 и конечный автомат:state ¬ 1.3: end if4: (L, cs, лs, ce, лe, z1, q1, z2, z3, q3) < findDubinsParameters(wi1, чi1, wi, чi, R)(найти параметры Дубинса)5: if state = 1, then6: flag ¬ 2, c ¬ cs, с ¬ R, л ¬ лs7: if p Î H(z1, q1), then8: state ¬ 29: end if10: else if state = 2, then11: if p Î H(z1, q1), then12: state ¬ 313: end if14: else if state = 3, then15: flag ¬ 1, r ¬ z1, q ¬ q116: if p Î H(z2, q1), then17: state ¬ 418: end if19: else if state= 4, then20: flag ¬ 2, c ¬ ce, с ¬ R, л ¬ лe21: if p Î H(z3, q3), then22: state ¬ 523: end if24: else if state = 5, then25: if p Î H(z3, q3), then26: state ¬ 127: i ¬ (i + 1) до тех пор, пока не станет i = N.28: (L, cs, лs, ce, лe, z1, q1, z2, z3, q3) < findDubinsParameters(wi1, чi1, wi, чi, R)(найти параметры Дубинса)29: end if30: end if31: return flag, r, q, c, с, л.214Глава 11.
Система управления маршрутомконфигурации находятся на прямой линии, тогда не нужны начальная и конечная дуги. Или, если начальная и конечная конфигурации лежат на окружности радиуса R, тогда не нужны прямая линия и конечная дуга окружности.В этой главе было пропущено описание этих вырожденных случаев.В работе [63] идеи Дубинса использовались для создания практически реализуемых траекторий БЛА при наличии кинематических и маршрутных ограничений при алгоритмическом поиске оптимальных положений окружностей ипрямых отрезков траекторий. В [64] окружности Дубинса накладывались в качестве скруглений на соединения прямолинейных участков траекторий, получаемых из диаграмм Вороного.
В некоторых задачах, таких как при совместнойсинхронизации, описанной в [60], может потребоваться совершать переходымежду путевыми точками так, чтобы при этом сохранялась длина траектории.Система управления маршрутом для этого сценария описана в [62].11.4. Îïûòíî-êîíñòðóêòîðñêàÿ ðàçðàáîòêàЦель этого задания — реализовать алгоритмы 5 и 6 для следования набору путевых точек, обозначаемых как W, и алгоритма 8 для следования набору конфигураций, обозначаемых как P. Входными данными для системы управлениямаршрутом являются W или P, а выходными данными — задание маршрута.y Управл.æ flag öçV d ÷ç g ÷ç rr ÷=ç q ÷ç c ÷ç r ÷ç l ÷øèАлгоритм программы для этой главы находится на веб-странице этогоучебника.11.1. Измените path_manager_line.m, чтобы использовать алгоритм 5 дляследования по траектории с путевыми точками, которая задана в path_planner_chap11.m. Проверьте и отладьте алгоритм в модели наведения, которая определяется уравнением (9.19).
Когда алгоритм станет хорошо работать вмодели наведения, убедитесь, что он надлежащим образом работает в полноймодели с шестью степенями свободы.11.2. Измените path_manager_fillet.m и используйте алгоритм 6 для следования маршрутом с путевыми точками, который был задан в path_planner_chap11.m. Проверьте и отладьте алгоритм в модели наведения, которая11.4. Опытно-конструкторская разработка215определяется уравнением (9.19).
Когда алгоритм станет хорошо работать вмодели наведения, убедитесь, что он надлежащим образом работает в полноймодели с шестью степенями свободы.11.3. Измените path_manager_dubins.m и используйте алгоритм 8 для следования маршрутом с путевыми точками, который был задан в path_planner_chap11.m.
Проверьте и отладьте алгоритм в модели наведения, котораяопределяется уравнением (9.19). Когда алгоритм станет хорошо работать в модели управления, убедитесь, что он надлежащим образом работает в полноймодели с шестью степенями свободы.ÃËÀÂÀ 12ÏËÀÍÈÐÎÂÀÍÈÅÒÐÀÅÊÒÎÐÈÈВ литературе по робототехнике используются два существенно отличающихсядруг от друга подхода планирования траекторий движения: совещательное планирование перемещений, в котором маршруты и траектории рассчитываются вявном виде на основе глобальных мировых сведений [65, 66, 67], и реактивноепланирование движений, которое использует поведенческие методы для формирования отклика на локальную информацию, получаемую от датчика [68,69]. В общем случае совещательное планирование перемещений полезно, когда окружающая среда известна априорно, но в случае среды с высокой степенью динамичности может потребоваться большой объем вычислений.
![](https://s.studizba.com/z.php?f=/templates/si/i/noavatar.png&w=100&h=100&t=1"){kind=avatar}
