Алгоритмы планирования перемещения на плоскости Яковлев К.С.

Терминология (русскоязычная) Планирование траектории Планирование перемещений Поиск пути Поиск траектории Навигация

Терминология (англоязычная) Path Planning Path Finding Navigation

Примеры задач Проехать на автомобиле из города А в город Б (по дорогам общего пользования)

Примеры задач Пройти из точки А в точку Б

Примеры задач Пройти из комнаты 1 в комнату 5

История. Начало Университет Стэнфорда. США. Проект Shakey

История. 21 век. Беспилотные транспортные средства Проекты Публикации Программы Стандарты «Соревнования»

Планирование траектории в разрезе управления БТС Планирование Управление вертолетом Управление в нештатных ситуациях Оценка обстановки Взаимодействие Управление коммуникациями Управление ресурсами Функциональные модули Интеллектуальные агенты Базы данных и знаний Системы БТСДатчики/ОружиеКоммуникации

Планирование траектории в разрезе управления БТС Низкая нагрузка на процессор и оперативную память контроль исполнения плана в условиях динамической окружающей среды; Эффективная перепланировка маршрута в случае непредвиденной смены условий окружающей среды и/или целей; построение «субоптимального» плана за короткий промежуток времени и дальнейшее улучшение данного плана исходя из наличия временных и вычислительных ресурсов, отведенных на планирование.

Что общего у задач планирования траектории разных типов?

Планирование как поиск пути на графе G= - неориентированный взвешенный граф, где: S={s} – множество вершин графа соответствующих состояниям рассматриваемой предметной области; E S×S – множество ребер графа, соответствующих переходам из одного состояния в другое; c: E(0;+) – функция, определяющая веса ребер графа, соответствующих трудозатратам, связанных с соответствующими переходами. (s i, s j ) - ребро графа c(si,sj) - вес данного ребра Вершины s, s S - смежные, если существует ребро (s, s) E Adj(s) - множество смежных вершин для вершины s Путь из s в s - последовательность вершин π={s 0, s 1, …, s k }, такая что s 0 =s, s k =s,i:1ik,(s i,s i-1 ) E. Вес пути π: Путь π* - кратчайший, если не существует пути π такого, что: c(π )c(π *). c*(s,s) - вес кратчайшего пути из s в s

Задача планирования Задача планирования: π(s start, s goal ) : c(π)=c*(s start, s goal ) – оптимальное решение

Методы решения «Неинформируемый» поиск BFS DFS Dijkstra Эвристический поиск A A* WA* IDA* Anytime A* И др

Вариации алгоритма A* А А* WА*WА* h(s) >>> h(s): h(s) допустима h(s) >>> w*h(s) Кратчайший путь Путь не более чем w раз «хуже», чем кратчайший

Вариации алгоритма A* Anytime А* ARA* А* LPА Алгоритмы планирования с отсечением по времени Построение плана за малый промежуток времени; «улучшение» плана D Алгоритмы планирования в динамических графах Адаптационное перепланирование в динамической среде Anytime D

Эвристический поиск. Результаты Изучена взаимосвязь между используемыми эвристиками и весом найденного пути Установлены критерии нахождения кратчайшего пути Разработаны модификации алгоритма A*, предназначенные для решения частных задач

Планирование траектории как поиск пути на графе. ПРОБЛЕМЫ. Временная и емкостная сложность эвристического поиска: O ( b d ) b – коэффициент ветвления d – глубина решения Подбор эвристики Построение самого графа

«Открытые» задачи планирования траектории НОВЫЕ Модели Методы

Метрический топологический граф MT-GR= A – множество клеток, представляющее собой матрицу A m×n ={a ij }: a ij =0 1, i, j: 0i

МТ-графы Пусть a ij, a lk A m×n : a ij a lk, a ij 0, a lk 0. Путем из a ij в a lk будем называть последовательность смежных проходимых клеток МТ-графа π= { a i0 j0, a i1 j1,a i2 j2, …, a is js }, a i0 j 0 = a ij, a is j s =a lk. Будем обозначать путь как π ( a ij, a lk ) или просто π. Клетку a ij пути π будем называть начальной, a lk – целевой. Вес пути π определяется как сумма весов переходов по всем смежным клеткам, входящим в данный путь: Кратчайшим путем из a ij в a lk будем называть такой путь π* ( a ij, a lk ), что π π* c(π)c(π*). c(π)=

МТ-графы Две различные клетки МТ-графа a i1j1, a i2j2 A m×n будем называть смежными, если: i 1,j 1,i 2,j 2 : 0 i 1,i 2

МТ-графы и взвешенные графы Любой МТ-граф может имплицировать взвешенный граф Все алгоритмы эвристического поиска, применимые на графах, являются применимыми и для МТ-Графов

Метрики на МТ-графах 1.d(a ij, a lk )=c(π* (a ij, a lk )). 2.d(a ij, a lk )= H(a ij, a lk )= Δ i =Δ i (a ij, a lk )=|i-l| Δ j =Δ j (a ij, a lk )=|j-k| Является а) допустимой б) монотонной эвристикой для алгоритмов семейства A*

Алгоритмы семейства A* при поиске пути на МТ-графе Алгоритмическая сложность (как временная, так и емкостная) O ( d 2 ) Проблема «локального минимума»

Решение Принципиальной иной подход к планированию основанный на иерархическом подходе и знаниях о предметной области – алгоритм HGA*.

Иерархический подход Разбить исходную задача на ряд «элементарных» подзадач

Нуль-траектория Нуль-траекторией между двумя различными клетками a ij и a lk будем называть последовательность смежных клеток МТ-графа tr(a ij, a lk )={a i0j0, a i1j1, a i2j2, …, a isjs }, такую что: 1.a i0j0 =a ij, a lk =a isjs. 2.Если a ij расположена не правее a lk, то a iv jw расположена не правее a iv+1 jw+1 для 0v

Секция Секция - упорядоченная пара клеток МТ-графа Секция проходима ТТТК нуль- траектория tr(a ij, a kl )проходима Вес (длина) секции определяется следующим образом: с(a ij, a kl ) = с(tr(a ij, a kl )), если проходима с(a ij, a kl ) = d(a ij, a kl ), если если непроходима

Задача планирования Пусть на заданном МТ-графе MT-Gr зафиксированы начальная a startI startJ и целевая a goalI goalJ клетки. Задача планирования состоит в отыскании такой последовательности клеток PP={a i0 j0, a i1 j1,a i2 j2, …, a is js }, что a i0 j0 = a startI startJ a is js = a goalI goalJ секции,, … - проходимы PP – частичный план Клетки a ij PP – опорные клетки

МТ-Графы. Взаимное расположение объектов. Левее, Правее, Выше, Ниже Клетка a ij расположена левее (правее) клетки a lk, если j k). Клетка a ij расположена ниже (выше) клетки a lk, если i l). Препятствия Obs={a i0j0, a i1j1, a i2j2, …, a isjs |а ikjk =1, а ikjk adj(а ik-1jk-1 ) k=0,1,2, …, s, s N}. Препятствие Obs лежит между клетками a ij и a lk, если tr(a ij, a lk ) Obs

МТ-Графы. Операция поворота. ROT(A m×n )=RA nxm, где RA nxm ={ra ij }, ra ij =a m-j-1 i. Графически МТ-граф RA представляет собой перевернутый на 90 градусов по часовой стрелке МТ-граф A m×n.

Компоненты планирования Выделение опорных клеток Упорядочивание опорных клеток Выбор опорных клеток для формирования итогового решения

Абстрактный недетерминированный иерархический алгоритм планирования траектории Вход: PP={a startI startJ, a goalI goalJ } Шаг 1. Если PP удовлетворяет Критерию Останова, то вернуть PP Шаг 2. Недетерминировано выбрать пару опорных клеток из PP - a ij, a lk Шаг 3. Построить нуль-траекторию tr(a ij, a lk ) Шаг 4. Если нуль-траектория tr(a ij, a lk ) проходима, то перейти к шагу 1 Шаг 5. Недетерминировано выбрать клетку C A + в качестве опорной Шаг 6. Добавить C в PP (а именно заменить последовательность a ij, a lk в PP на a ij, С, a lk ) Шаг 7. Перейти к шагу 1

Вероятностный иерархический алгоритм планирования траектории Вход: PPС={PP={a startI startJ, a goalI goalJ }} – множество планов- кандидатов Шаг 1. Выбрать лучший план-кандидат PP из PPC согласно Критерию Выбора Частичного Плана Шаг 2. Если PP удовлетворяет Критерию Останова, то вернуть PP Шаг 3. В соответствии с Критерием Выбора Опорных Клеток выбрать пару опорных клеток из PP - a ij, a lk Шаг 4. Построить нуль-траекторию tr(a ij, a lk ) Шаг 5. Если нуль-траектория tr(a ij, a lk ) проходима, то перейти к шагу 1 Шаг 6. Случайным образом выбрать N опорных клеток C 1, C 2, …, C n A + Шаг 7. Разбить секцию на N вариантов, а именно: Для каждого PP PPC, включающего a ij, a lk Шаг 7.1. Разбить PP на N дубликатов Шаг 7.2. Заменить последовательность a ij, a lk на a ij, C 1, a lk, a ij, C 2, a lk,…. a ij, C n, a lk в каждом дубликате соответственно Шаг 8. Перейти к шагу 1

Детерминированный выбор опорных клеток Утверждение Если препятствие Obs лежит между клетками a ij, a lk, то частичный план PP(a ij, a lk ) необходимо содержит клетки, расположенные выше (либо ниже) препятствия Obs.

Детерминированный выбор опорных клеток GetBaseCellsForExtension(cell s, cell g, cell X) int i_up, i_down, j_right, j_left=X.j-1; cell tmp=X; while (tmp==1) tmp.i--; i_up=tmp.i; tmp.i++; while(tmp==1) tmp.j++; j.right=tmp.j;tmp=X; while (tmp==1) tmp.i++; i_down=tmp.i; if (i_up>=0){ A.i=B.i=i_up; A.j=j_left; B.j=j_right } else A=B=null; if (i_down

HGA* Вход: PPС={PP={a startI startJ, a goalI goalJ }} – множество планов- кандидатов Шаг 1. Выбрать лучший план-кандидат PP из PPC согласно Критерию Выбора Частичного Плана Шаг 2. Если PP удовлетворяет Критерию Останова, то вернуть PP Шаг 3. В соответствии с Критерием Выбора Опорных Клеток выбрать пару опорных клеток из PP - a ij, a lk Шаг 4. Построить нуль-траекторию tr(a ij, a lk ) Шаг 5. Если нуль-траектория tr(a ij, a lk ) проходима, то перейти к шагу 1 Шаг 6. Выполнить процедуру GetBaseCellsForExtension для получения опорных клеток A, B, C, D. Шаг 7. Если A=B=C=D=null вернуть failure Шаг 8. Разбить секцию на 2 вариантa: a ij, A, B, a lk и a ij,D, С, a lk Шаг 9. Перейти к шагу 1

Емкостная сложность HGA* «Хранить» клетку – O(1) A* – O ( d 2 ) Число препятствий Число хранимых клеток …… Obs: 2+4*Obs Obs d/2 d = max{|goalI - startI|, |goalJ - startJ|} HGA* – O ( d )

Препятствия нетривиальной формы Обход контура препятствия по (против) часовой стрелке от клетки X до «первого шага в горизонтальном направлении»

Препятствия нетривиальной формы

Свойства алгоритма HGA* «Открытые» задачи планирования Реализация различной логики планирования в рамках одного алгоритма Учет особенностей объекта управления (ограничения на вид траектории на плоскости) Планирование с отсечением по времени Интероперабельность с СУ БТС (состояние планировщика всегда «прозрачно» для СУ) Естественный учет частичной наблюдаемости среды Возможность создания модификаций для учета динамики среды

Экспериментальные результаты. 3 серии экспериментов МТ-графы различных размеров с различной степенью заполнения препятствиями МТ-графы – цифровые карты Москвы в разрешении необходимом для обеспечения маловысотного полета беспилотного вертолета

Экспериментальные результаты. Алгоритмы HGA*, A*, WA*-3, WA*-5 Отслеживаемые индикаторы Q – число сохраненных клеток W – длина траектории E= (Q/W)/(Q*/W*) – удельная эффективность алгоритма

1 серия экспериментов. 150 МТ-графов различного размера и различной степени заполнения препятствиями (СЗП) Препятствия – прямоугольники, шириной в 1 клетку и длиной в l клеток l – случайная величина с МО 10 СЗП = ( l *2+4)*N, где N – число препятсвий = 0.3, 0.5, 0.8

1 серия экспериментов. РазмерКоличество МТ-графов =0,3 51 x 5110 =0,3 101 x =0,3 251 x =0,3 501 x =0, x =0,5 51 x 5110 =0,5 101 x =0,5 251 x =0,5 501 x =0, x =0,8 51 x 5110 =0,8 101 x =0,8 251 x =0,8 501 x =0, x ИТОГО150

1 серия экспериментов.

АлгоритмВыходные параметрыРазмер МТ-графа 50x50100x100250x250500x x1000 Asearch 1вес пути (W1)629, обработано вершин (Q1) (Q1/W1)/(Q1/W1)100% Asearch 3вес пути (W3)703, обработано вершин (Q3)211, (Q3/W3)/(Q1/W1)36%24%10%4%3% Asearch 5вес пути (W5)709, обработано вершин (Q5)208, (Q5/W5)/(Q1/W1)35%23%10%4%3% HGAвес пути (WH) обработано секций (QH) (QH/WH)/(Q1/W1)13%12%6%2% = 0.5

1 серия экспериментов. HGA лучше «лучшей» версии A* в 1,5 – 4 раза в зависимости от плотности препятствий

2 серия экспериментов Размер МТ-графа фиксирован 101х101 СЗП фиксирована = 0.5 Длины препятствий варьируются l =2, 5, 10, 15, 25

2 серия экспериментов

3 серия экспериментов. Маловысотный полет вертолета. 2 МТ-графа (цифровые карты местности Москвы, 2х2 км) Глубина решение d=50 Случайный выбор начальной и целевых клеток (10 повторений на каждый МТ-граф) A*, WA*-5, HGA*

3 серия экспериментов.

Выводы по результатам экспериментов HGA* использует вычислительные ресурсы гораздо эффективней аналогов HGA* лучше масштабируется HGA* эффективно отрабатывает на МТ- графах с любой степенью заполнения любыми типами «элементарных» препятствий HGA* может использоваться в задачах планирования траектории характерных для «городского» ландшафта

Резюме HGA* - новый, гибкий алгоритм (технология) планирования траектории, основанный на иерархическом подходе, и предназначенный для решения задач планирования на «открытой» местности с учетом особенностей управления беспилотным транспортным средством

Спасибо за внимание