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

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

Яковлев К.С.

Терминология (русскоязычная)

□Планирование траектории□Планирование перемещений□Поиск пути□Поиск траектории

□Навигация

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

□Path Planning□Path Finding

□Navigation

Примеры задач

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


Пройти из точки А в точку Б


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

История. Начало.

□1971. Университет Стэнфорда. США.Проект Shakey

История. 21 век.

Беспилотные транспортные средства

□Проекты□Публикации□Программы□Стандарты□«Соревнования»

Планирование траектории в разрезе управления БТС

Планирование

Управление вертолетом

Управление в нештатных ситуациях

Оценка обстановки

Взаимодействие

Управление коммуникациями

Управление ресурсами

Функциональные модули

Интеллектуальные агенты

Базы данных и знаний

Системы БТС Датчики/Оружие Коммуникации

Планирование траектории в разрезе управления БТС

□Низкая нагрузка на процессор и оперативную память□контроль исполнения плана в условиях

динамической окружающей среды;□Эффективная перепланировка маршрута в

случае непредвиденной смены условий окружающей среды и/или целей;

□построение «субоптимального» плана за короткий промежуток времени и дальнейшее улучшение данного плана исходя из наличия временных и вычислительных ресурсов, отведенных на планирование.

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

разных типов?

0

14

24

34

42

38

10

20

14

24

34

Планирование как поиск пути на графе

G=<S,E,c>- неориентированный взвешенный граф, где:S={s} – множество вершин графа соответствующих состояниям рассматриваемой предметной области;E S×S – множество ребер графа, соответствующих переходам из одного состояния в другое; c: E→(0;+∞) – функция, определяющая веса ребер графа, соответствующих трудозатратам, связанных с соответствующими переходами.

(si, sj) - ребро графа

c(si,sj) - вес данного ребра Вершины s, s’ S - смежные, если существует ребро (s, s’) EAdj(s) - множество смежных вершин для вершины s

Путь из s в s’ - последовательность вершин π={s0, s1, …, sk}, такая что s0=s,

sk=s’,i:1≤i≤k,(si,si-1 ) E.

Вес пути π:

Путь π* - кратчайший, если не существует пути π такого, что: c(π )≤c(π *).

c*(s,s’) - вес кратчайшего пути из s в s’

11

( ) ( , )k

i ii

c c s s

Задача планирования

Задача планирования:<G, sstart, sgoal >

π(sstart, sgoal) :

c(π)=c*(sstart, sgoal ) – оптимальное решение

Методы решения

□ «Неинформируемый» поиск□ BFS□ DFS□ Dijkstra

□ Эвристический поиск□A□A*□WA*□IDA*□Anytime A*□И др.

0

14 24 34

423810 20

14 24 34

0

14 24

4238

14 24

Вариации алгоритма A*

А

А*

WА*

h(s) >>> h(s): h(s) допустима

h(s) >>> w*h(s)

Кратчайший путь

Путь не более чем w раз «хуже», чем кратчайший

Вариации алгоритма A*

Anytime А*

ARA*

А*LPА

Алгоритмы планирования с отсечением по времени

Построение плана за малый промежуток времени; «улучшение» плана

D

Алгоритмы планирования в динамических графах

Адаптационное перепланирование в динамической среде

Anytime D

Эвристический поиск. Результаты

□Изучена взаимосвязь между используемыми эвристиками и весом найденного пути

□Установлены критерии нахождения кратчайшего пути

□Разработаны модификации алгоритма A*, предназначенные для решения частных задач

Планирование траектории как поиск пути на графе. ПРОБЛЕМЫ.

□Временная и емкостная сложность эвристического поиска: O(bd)b – коэффициент ветвленияd – глубина решения

□Подбор эвристики

□Построение самого графа

«Открытые» задачи планирования траектории

НОВЫЕ

□Модели

□Методы

Метрический топологический граф

MT-GR=<A, d>□A – множество клеток, представляющее

собой матрицу Am×n={aij}: aij=0 1, i, j: 0≤i<m, 0≤j<n, m, n N\{0}.

□d – метрика на множестве A+={aij|aijA, aij=0}

а) б) в)

МТ-графы

Пусть aij, alk Am×n: aijalk , aij0, alk0. Путем из aij в alk будем называть последовательность смежных проходимых клеток МТ-графа

π={ai0 j0, ai1 j1,ai2 j2, …, ais js}, ai0 j0=aij, ais js=alk. Будем обозначать путь как π(aij, alk) или просто π.

Клетку aij пути π будем называть начальной, alk – целевой.

Вес пути π определяется как сумма весов переходов по всем смежным клеткам, входящим в данный путь:

Кратчайшим путем из aij в alk будем называть такой путь π*(aij, alk), что ππ* c(π)≤c(π*).

c(π)=

s

vjiji vvvv

aac1

),(11

МТ-графыДве различные клетки МТ-графа ai1j1, ai2j2 Am×n будем называть смежными, если: i1,j1,i2,j2: 0≤ i1,i2<m, 0≤j1,j2<n → |i1-i2|≤1|j1-j2|≤1.

Две различные клетки ai1j1, ai2j2 Am×n будем называть:горизонтально смежными, если:i1,j1,i2,j2: 0≤ i1,i2<m, 0≤j1,j2<n → |i1-i2|=0 |j1-j2|=1вертикально смежными, если:i1,j1,i2,j2: 0≤ i1,i2<m, 0≤j1,j2<n → |i1-i2|=1 |j1-j2|=0диагонально смежными, если:i1,j1,i2,j2: 0≤ i1,i2<m, 0≤j1,j2<n → |i1-i2|=1 |j1-j2|=1

ADJA×A – множество всех пар смежных клетокchv,cdR+

c:ADJ → {chv, cd, +∞}:c(aij, alk)=chv, если aij=0 alk=0 и клетки aij, alk являются горизонтально или вертикально

смежными;c(aij, alk)=cd, если aij=0 alk =0 и клетки aij, alk являются диагонально смежными;c(aij, alk)=+∞, если aij=1 alk=1.

cd

chv

chv

+∞

hvd cc 2

МТ-графы и взвешенные графы

Любой МТ-граф может имплицировать взвешенный граф

Все алгоритмы эвристического поиска, применимые на графах, являются применимыми и для МТ-Графов

Метрики на МТ-графах

1. d(aij, alk)=c(π* (aij, alk)).

2. d(aij, alk)=H(aij, alk)=

Δi=Δi(aij, alk)=|i-l|

Δj=Δj(aij, alk)=|j-k|

),(),(),(

),(),(),(

lkijjlkijiijhvihv

lkijjlkijijidjhv

aaaaеслиcc

aaaaеслиcc

Является а) допустимой б) монотонной эвристикой для алгоритмов семейства A*

Алгоритмы семейства A* при поиске пути на МТ-графе

□Алгоритмическая сложность (как временная, так и емкостная) O(d2)

□Проблема «локального минимума»

Решение

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

Иерархический подход

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

Нуль-траекторияНуль-траекторией между двумя различными клетками aij и alk будем называть

последовательность смежных клеток МТ-графа tr(aij, alk)={ai0j0, ai1j1, ai2j2, …, aisjs}, такую что:

1. ai0j0=aij, alk=aisjs.2. Если aij расположена не правее alk, то aiv jw расположена не правее aiv+1 jw+1 для

0≤v<r, 0≤w<s.3. Если aij расположена не левее alk, то aiv jw расположена не левее aiv+1 jw+1 для

0≤v<r, 0≤w<s.4. Если kj, то aiv jw tr(aij, alk) удовлетворяет соотношению iv=K∙jw+B, 0≤v≤r,

0≤w≤s, где , B=l-k∙K, а x - целая часть числа x.5. Если k=j, то aiv jw tr(aij, alk) удовлетворяет соотношению jw=j=k, iv=iv-1+δ для

0<v≤r, 0≤w≤s, где δ=-1 если aij расположена выше alk, δ=1 если aij расположена ниже alk.

Нуль траектория – отрезок дискретной прямойНуль-траектория tr(aij, alk) проходима ТТКГ aisjs =0 aisjs

tr(aij, alk)

s

vjiji vvvv

aac1

),(11

c(tr(aij, alk))=c(tr)=

Вес нуль-траектории определяется аналогично весу пути:

Секция

□Секция <aij, akl> - упорядоченная пара клеток МТ-графа

□Секция <aij, akl> проходима ТТТК нуль-траектория tr(aij, akl)проходима

□Вес (длина) секции определяется следующим образом:□с(aij, akl) = с(tr(aij, akl)), если <aij, akl>

проходима

□с(aij, akl) = d(aij, akl), если если <aij, akl> непроходима

Задача планирования

Пусть на заданном МТ-графе MT-Gr зафиксированы начальная astartI startJ и целевая agoalI goalJ клетки.

Задача планирования состоит в отыскании такой последовательности клеток PP={ai0 j0, ai1 j1,ai2 j2, …, ais js}, что

ai0 j0= astartI startJ

ais js= agoalI goalJ секции <ai0 j0, ai1 j1>, <ai1 j1,ai2 j2 >, … <ais-1 js-1 ais js> - проходимы

PP – частичный планКлетки aij PP – опорные клетки

МТ-Графы. Взаимное расположение объектов.

□Левее, Правее, Выше, Ниже□ Клетка aij расположена левее (правее) клетки alk, если j<k (j>k).□ Клетка aij расположена ниже (выше) клетки alk, если i<l (i>l).

□ПрепятствияObs={ai0j0, ai1j1, ai2j2, …, aisjs|аikjk=1, аikjkadj(аik-1jk-1) k=0,1,2, …, s, sN}.

Препятствие Obs лежит между клетками aij и alk, если

tr(aij, alk) Obs

МТ-Графы. Операция поворота.

ROT(Am×n)=RAnxm, где RAnxm={raij}, raij=am-j-1 i. Графически МТ-граф RA представляет собой перевернутый на 90 градусов по часовой стрелке МТ-граф Am×n.

Компоненты планирования

□Выделение опорных клеток□Упорядочивание опорных клеток□Выбор опорных клеток для

формирования итогового решения

Абстрактный недетерминированный иерархический алгоритм

планирования траекторииВход: PP={astartI startJ , agoalI goalJ }

Шаг 1. Если PP удовлетворяет Критерию Останова, то вернуть PPШаг 2. Недетерминировано выбрать пару опорных клеток из PP - aij, alk

Шаг 3. Построить нуль-траекторию tr(aij, alk)Шаг 4. Если нуль-траектория tr(aij, alk) проходима, то перейти к шагу 1Шаг 5. Недетерминировано выбрать клетку C A+ в качестве опорнойШаг 6. Добавить C в PP (а именно заменить последовательность aij, alk в PP на aij, С, alk)Шаг 7. Перейти к шагу 1

Вероятностный иерархический алгоритм планирования траектории

Вход: PPС={PP={astartI startJ , agoalI goalJ }} – множество планов-кандидатов

Шаг 1. Выбрать лучший план-кандидат PP из PPC согласно Критерию Выбора Частичного ПланаШаг 2. Если PP удовлетворяет Критерию Останова, то вернуть PPШаг 3. В соответствии с Критерием Выбора Опорных Клеток выбрать пару опорных клеток из PP - aij, alk

Шаг 4. Построить нуль-траекторию tr(aij, alk)Шаг 5. Если нуль-траектория tr(aij, alk) проходима, то перейти к шагу 1Шаг 6. Случайным образом выбрать N опорных клеток C1, C2, …, Cn A+

Шаг 7. Разбить секцию <aij, alk> на N вариантов, а именно:Для каждого PP PPC, включающего aij, alk Шаг 7.1. Разбить PP на N дубликатовШаг 7.2. Заменить последовательность aij, alk на aij, C1, alk, aij, C2, alk,…. aij, Cn, alk в каждом дубликате соответственно

Шаг 8. Перейти к шагу 1

Детерминированный выбор опорных клеток

□УтверждениеЕсли препятствие Obs лежит между клетками aij, alk, то частичный план PP(aij, alk) необходимо содержит клетки, расположенные выше (либо ниже) препятствия 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<m){ C.i=D.i=i_down; C.j=j_left; D.j=j_right}else C=D=null;return {A, B, C, D}

HGA*

Вход: PPС={PP={astartI startJ , agoalI goalJ }} – множество планов-кандидатов

Шаг 1. Выбрать лучший план-кандидат PP из PPC согласно Критерию Выбора Частичного ПланаШаг 2. Если PP удовлетворяет Критерию Останова, то вернуть PPШаг 3. В соответствии с Критерием Выбора Опорных Клеток выбрать пару опорных клеток из PP - aij, alk

Шаг 4. Построить нуль-траекторию tr(aij, alk)Шаг 5. Если нуль-траектория tr(aij, alk) проходима, то перейти к шагу 1Шаг 6. Выполнить процедуру GetBaseCellsForExtension для получения опорных клеток A, B, C, D.Шаг 7. Если A=B=C=D=null вернуть failure

Шаг 8. Разбить секцию <aij, alk> на 2 вариантa: aij, A, B, alk и aij,D, С, alkШаг 9. Перейти к шагу 1

Емкостная сложность HGA*

«Хранить» клетку – O(1)

A* – O(d2)

Число препятствий

Число хранимых клеток

0 2

1 6

3 10

… …

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


Размер КоличествоМТ-графов

=0,3 51 x 51 10

=0,3 101 x 101 10

=0,3 251 x 251 10

=0,3 501 x 501 10

=0,3 1001 x 1001 10

=0,5 51 x 51 10

=0,5 101 x 101 10

=0,5 251 x 251 10

=0,5 501 x 501 10

=0,5 1001 x 1001 10

=0,8 51 x 51 10

=0,8 101 x 101 10

=0,8 251 x 251 10

=0,8 501 x 501 10

=0,8 1001 x 1001 10

ИТОГО 150

1 серия экспериментов.Высота 251 дистанция 250 препятсвий 1200Ширина 251 ср.длин.преп. 10Площадь 63001 абс. площадь перекрытия 12000 отн. S перекрытия 31200

% перекрытия 19% 50%

карта 1 карта 2 карта 3 карта 4 карта 5 карта 6 карта 7 карта 8 карта 9 карта 10 AVGAsearch 1 вес пути 3368 2774 3404 2826 2820 2924 2858 2790 3292 2788 2984

обработано вершин 15536 5147 13151 7005 1108 4568 11079 5575 9679 10468 8332

Asearch 3 вес пути 3506 3240 3520 3160 3410 3348 3196 3110 3610 3290 3339


Asearch 5 вес пути 3512 3302 3520 3190 3446 3476 3288 3174 3616 3364 3389


HGA вес пути 3696 2798 3736 3180 2972 3092 3024 2970 3446 3174 3209

обработано секций 676 259 688 547 469 685 421 541 721 445 545

AVG 30% AVG 50% AVG 80%Asearch 1 вес пути (W1) 2827 2984 3024

обработано вершин (Q1) 7150 8332 10281(Q1/W1)/(Q1/W1) 100% 100% 100%

Asearch 3 вес пути (W3) 3109 3339 3552обработано вершин (Q3) 947 977 951(Q3/W3)/(Q1/W1) 12% 10% 8%

Asearch 5 вес пути (W5) 2950 3389 3662обработано вершин (Q5) 941 961 933(Q5/W5)/(Q1/W1) 13% 10% 7%

HGA вес пути (WH) 2908 3209 3381обработано секций (QH) 252 545 608(QH/WH)/(Q1/W1) 3% 6% 5%

Среднее число созданных узлов/секций

-500

500

1500

2500

3500

4500

5500

6500

7500

8500

9500

10500

Asearch 1

Asearch 3

Asearch 5

HGA

Asearch 1 7150 8332 10281

Asearch 3 947 977 951

Asearch 5 941 961 933

HGA 252 545 608

AVG 30% AVG 50% AVG 80%


Алгоритм Выходные параметры Размер МТ-графа

50x50 100x100 250x250 500x500 1000x1000Asearch 1 вес пути (W1) 629,4 1158 2984 6085 11813

обработано вершин (Q1) 525 1478 8332 39590 117558

(Q1/W1)/(Q1/W1) 100% 100% 100% 100% 100%Asearch 3 вес пути (W3) 703,2 1261 3339 6803 13592

обработано вершин (Q3) 211,8 383 977 1936 3854

(Q3/W3)/(Q1/W1) 36% 24% 10% 4% 3%Asearch 5 вес пути (W5) 709,4 1290 3389 6894 13789

обработано вершин (Q5) 208,3 380 961 1926 3807

(Q5/W5)/(Q1/W1) 35% 23% 10% 4% 3%HGA вес пути (WH) 651 1235 3209 6577 12827

обработано секций (QH) 70 187 545 930 2176

(QH/WH)/(Q1/W1) 13% 12% 6% 2% 2% = 0.5


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

препятствийВо сколько раз HGA "лучше" A Search 5

0,00

0,50

1,00

1,50

2,00

2,50

3,00

3,50

4,00

30% 50% 80%

% перекрытия карты

ресурсы

ресурсы сучетом весапути

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

□Размер МТ-графа фиксирован 101х101

□СЗП фиксирована = 0.5□Длины препятствий варьируются

□l=2, 5, 10, 15, 25


L=2 L=5 L=10 L=15 L=25Asearch 1 вес пути (W1) 1143 1166 1158 1225 1335

обработано клеток (Q1) 499 901 1478 1991 2662(Q1/W1)/(Q1/W1) 100% 100% 100% 100% 100%

Asearch 3 вес пути (W3) 1181 1240 1261 1348 1470обработано клеток (Q3) 350 357 383 434 568(Q3/W3)/(Q1/W1) 68% 37% 24% 20% 19%

Asearch 5 вес пути (W5) 1181 1245 1290 1413 1500обработано клеток (Q5) 350 357 380 423 524(Q5/W5)/(Q1/W1) 68% 37% 23% 18% 18%

HGA вес пути (WH) 1146 1189 1235 1285 1516обработано клеток (QH) 95 120 187 154 114(QH/WH)/(Q1/W1) 19% 13% 12% 7% 4%


Среднее число сохраненных клеток МТ-графа

0

250

500

750

1000

1250

1500

1750

2000

2250

2500

2750

Asearch 1

Asearch 3

Asearch 5

HGA

Asearch 1 499 901 1478 1991 2662

Asearch 3 350 357 383 434 568

Asearch 5 350 357 380 423 524

HGA 95 120 187 154 114

L=2 L=5 L=10 L=15 L=25

3 серия экспериментов. Маловысотный полет

вертолета.□2 МТ-графа (цифровые карты

местности Москвы, 2х2 км)□Глубина решение d=50□Случайный выбор начальной и

целевых клеток (10 повторений на каждый МТ-граф)

□A*, WA*-5, HGA*




МТ-граф 1 МТ-граф 2Asearch 1 вес пути (W1) 570 590

обработано клеток (Q1) 183 303(Q1/W1)/(Q1/W1) 100% 100%

Asearch 5 вес пути (W3) 594 584обработано клеток (Q3) 120 161(Q3/W3)/(Q1/W1) 63% 54%

HGA вес пути (WH) 582 590обработано клеток (QH) 57 19(QH/WH)/(Q1/W1) 31% 6%

Выводы по результатам экспериментов

□HGA* использует вычислительные ресурсы гораздо эффективней аналогов

□HGA* лучше масштабируется□HGA* эффективно отрабатывает на МТ-

графах с любой степенью заполнения любыми типами «элементарных» препятствий

□HGA* может использоваться в задачах планирования траектории характерных для «городского» ландшафта

Резюме

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

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

Documents

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