a ij

Алгоритмы планирования
перемещения на плоскости
Яковлев К.С.
Терминология (русскоязычная)
□ Планирование траектории
□ Планирование перемещений
□ Поиск пути
□ Поиск траектории
□ Навигация
Терминология (англоязычная)
□ Path Planning
□ Path Finding
□ Navigation
Примеры задач
Проехать на
автомобиле
из города А
в город Б (по
дорогам
общего
пользования)
Примеры задач
Пройти из
точки А в точку
Б
Примеры задач
Пройти из
комнаты 1 в
комнату 5
История. Начало.
□ 1971. Университет
Стэнфорда. США.
Проект Shakey
История. 21 век.
Беспилотные транспортные средства
□ Проекты
□ Публикации
□ Программы
□ Стандарты
□ «Соревнования»
Планирование траектории в
разрезе управления БТС
Управление
ресурсами
Планирование
Управление в
нештатных
ситуациях
Оценка обстановки
Взаимодействие
Функциональные
модули
Интеллектуальные
агенты
Базы данных и
знаний
Управление
вертолетом
Системы БТС
Управление
коммуникациями
Датчики/Оружие
Коммуникации
Планирование траектории в
разрезе управления БТС
□ Низкая нагрузка на процессор и
оперативную память
□ контроль исполнения плана в условиях
динамической окружающей среды;
□ Эффективная перепланировка маршрута в
случае непредвиденной смены условий
окружающей среды и/или целей;
□ построение «субоптимального» плана за
короткий промежуток времени и дальнейшее
улучшение данного плана исходя из наличия
временных и вычислительных ресурсов,
отведенных на планирование.
34
24
14
38
20
10
0
14
24
34
42
Что общего у задач планирования
траектории разных типов?
Планирование как поиск пути
на графе
G=<S,E,c>- неориентированный взвешенный граф, где:
S={s} – множество вершин графа соответствующих состояниям рассматриваемой
предметной области;
E S×S – множество ребер графа, соответствующих переходам из одного состояния
в другое;
c: E→(0;+∞) – функция, определяющая веса ребер графа, соответствующих
трудозатратам, связанных с соответствующими переходами.
(si, sj) - ребро графа
c(si,sj) - вес данного ребра
Вершины s, s’ S - смежные, если существует ребро (s, s’) E
Adj(s) - множество смежных вершин для вершины s
Путь из s в s’ - последовательность вершин π={s0, s1, …, sk}, такая что s0=s,
sk=s’,i:1≤i≤k,(si,si-1 ) E.
Вес пути π: c ( ) 
k
 c( s
i 1
i 1
, si )
Путь π* - кратчайший, если не существует пути π такого, что: c(π )≤c(π *).
c*(s,s’) - вес кратчайшего пути из s в s’
Задача планирования
Задача планирования:
<G, sstart, sgoal >
π(sstart, sgoal) :
c(π)=c*(sstart, sgoal ) – оптимальное решение
Методы решения
□ «Неинформируемый»
поиск
□ BFS
□ DFS
□ Dijkstra
14
0
10
14
24
20
24
34
38
34
□ Эвристический поиск
□A
□ A*
□ WA*
□ IDA*
□ Anytime A*
□ И др.
14
24
42
0
38
14
24
42
Вариации алгоритма A*
WА*
А*
А
h(s) >>> w*h(s)
Кратчайший путь
h(s) >>> h(s): h(s) допустима
Путь не более
чем w раз
«хуже», чем
кратчайший
Вариации алгоритма A*
Anytime А*
ARA*
Построение плана за малый
промежуток времени; «улучшение»
плана
Алгоритмы планирования с отсечением
по времени
А*
Алгоритмы планирования
в динамических графах
Anytime D
Адаптационное
перепланирование в
динамической среде
LPА
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|aijA,
aij=0}
а)
б)
в)
МТ-графы
Пусть aij, alk Am×n: aijalk , aij0, alk0. Путем из aij в alk будем называть
последовательность смежных проходимых клеток МТ-графа
π={ai0 j0, ai1 j1,ai2 j2, …, ais js}, ai0 j0=aij, ais js=alk.
Будем обозначать путь как π(aij, alk) или просто π.
Клетку aij пути π будем называть начальной, alk – целевой.
Вес пути π определяется как сумма весов переходов по всем
смежным клеткам, входящим в данный путь:
s
c(π)=
 c(ai
v 1
v 1 j v 1
, aiv jv )
Кратчайшим путем из aij в alk будем называть
такой путь π*(aij, alk), что  ππ* c(π)≤c(π*).
МТ-графы
Две различные клетки МТ-графа 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

+∞
chv
chv
cd
ADJ A×A – множество всех пар смежных клеток
cd  2  chv
chv,cdR+
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.
МТ-графы и взвешенные
графы
Любой МТ-граф может имплицировать взвешенный граф
Все алгоритмы эвристического поиска, применимые на
графах, являются применимыми и для МТ-Графов
Метрики на МТ-графах
1. d(aij, alk)=c(π* (aij, alk)).
2. d(aij, alk)=H(aij, alk)=
chv  j  cd ( i   j ), если  i (aij , alk )   j (aij , alk )

chv  i  chv ( j   i ), если  i (aij , alk )   j (aij , alk )
Δi=Δi(aij, alk)=|i-l|
Δj=Δj(aij, alk)=|j-k|
Является а) допустимой б)
монотонной эвристикой для
алгоритмов семейства 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. Если kj, то  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
пути:
c(tr(aij, alk))=c(tr)=  c(aiv1 jv1 , aiv jv )
v 1
Секция
□ Секция <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, аikjkadj(аik-1jk-1) k=0,1,2, …, s,
sN}.
Препятствие 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)
d = max{|goalI - startI|, |goalJ - startJ|}
Число
препятствий
Число
хранимых
клеток
0
2
1
6
3
10
…
…
Obs: 2+4*Obs
Obs  d/2
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 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 дистанция
251
63001
Asearch 1 вес пути
обработано вершин
Asearch 3 вес пути
обработано вершин
Asearch 5 вес пути
обработано вершин
HGA
вес пути
обработано секций
250 препятсвий
ср.длин.преп.
абс. площадь перекрытия
% перекрытия
карта 1
карта 2
карта 3
3368
2774
15536
5147
3506
3240
962
971
3512
3302
961
945
3696
2798
676
259
3404
13151
3520
966
3520
966
3736
688
1200
10
12000 отн. S перекрытия
19%
карта 4
карта 5
2826
7005
3160
921
3190
913
3180
547
2820
1108
3410
1040
3446
1014
2972
469
31200
50%
карта 6
карта 7
карта 8
карта 9
карта 10 AVG
2984
2924
2858
2790
3292
2788
8332
4568
11079
5575
9679
10468
3339
3348
3196
3110
3610
3290
977
955
1007
874
986
1084
3389
3476
3288
3174
3616
3364
961
951
955
872
983
1048
3209
3092
3024
2970
3446
3174
545
685
421
541
721
445
Среднее число созданных узлов/секций
10500
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%
9500
8500
7500
6500
Asearch 1
5500
Asearch 3
4500
Asearch 5
3500
HGA
2500
1500
500
-500
AVG 30%
AVG 50%
AVG 80%
Asearch 1
7150
8332
10281
Asearch 3
947
977
951
Asearch 5
941
961
933
HGA
252
545
608
1 серия экспериментов.
Алгоритм
Asearch 1
Asearch 3
Asearch 5
HGA
Выходные параметры
вес пути (W1)
обработано вершин (Q1)
(Q1/W1)/(Q1/W1)
вес пути (W3)
обработано вершин (Q3)
(Q3/W3)/(Q1/W1)
вес пути (W5)
обработано вершин (Q5)
(Q5/W5)/(Q1/W1)
вес пути (WH)
обработано секций (QH)
(QH/WH)/(Q1/W1)
50x50
629,4
525
100%
703,2
211,8
36%
709,4
208,3
35%
651
70
13%
 = 0.5
Размер МТ-графа
100x100 250x250 500x500
1158
2984
6085
1478
8332
39590
100%
100%
100%
1261
3339
6803
383
977
1936
24%
10%
4%
1290
3389
6894
380
961
1926
23%
10%
4%
1235
3209
6577
187
545
930
12%
6%
2%
1000x1000
11813
117558
100%
13592
3854
3%
13789
3807
3%
12827
2176
2%
1 серия экспериментов.
HGA лучше «лучшей» версии A* в 1,5 –
4 раза в зависимости от плотности
препятствий
Во сколько раз HGA "лучше" A Search 5
4,00
3,50
3,00
2,50
ресурсы
2,00
ресурсы с
учетом веса
пути
1,50
1,00
0,50
0,00
30%
50%
% перекрытия карты
80%
2 серия экспериментов
□ Размер МТ-графа фиксирован
101х101
□ СЗП фиксирована  = 0.5
□ Длины препятствий варьируются
□ l=2, 5, 10, 15, 25
2 серия экспериментов
L=2
Asearch 1 вес пути (W1)
обработано клеток (Q1)
(Q1/W1)/(Q1/W1)
Asearch 3 вес пути (W3)
обработано клеток (Q3)
(Q3/W3)/(Q1/W1)
Asearch 5 вес пути (W5)
обработано клеток (Q5)
(Q5/W5)/(Q1/W1)
HGA
вес пути (WH)
обработано клеток (QH)
(QH/WH)/(Q1/W1)
L=5
1143
499
100%
1181
350
68%
1181
350
68%
1146
95
19%
L=10
1166
901
100%
1240
357
37%
1245
357
37%
1189
120
13%
1158
1478
100%
1261
383
24%
1290
380
23%
1235
187
12%
L=15
1225
1991
100%
1348
434
20%
1413
423
18%
1285
154
7%
L=25
1335
2662
100%
1470
568
19%
1500
524
18%
1516
114
4%
2 серия экспериментов
Среднее число сохраненных клеток МТ-графа
2750
2500
2250
2000
1750
Asearch 1
1500
Asearch 3
1250
Asearch 5
1000
HGA
750
500
250
0
L=2
L=5
L=10
L=15
L=25
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
3 серия экспериментов.
Маловысотный полет вертолета.
□ 2 МТ-графа (цифровые карты
местности Москвы, 2х2 км)
□ Глубина решение d=50
□ Случайный выбор начальной и
целевых клеток (10 повторений на
каждый МТ-граф)
□ A*, WA*-5, HGA*
3 серия экспериментов.
3 серия экспериментов.
3 серия экспериментов.
МТ-граф 1 МТ-граф 2
Asearch 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* - новый, гибкий алгоритм
(технология) планирования
траектории, основанный на
иерархическом подходе, и
предназначенный для решения
задач планирования на «открытой»
местности с учетом особенностей
управления беспилотным
транспортным средством
Спасибо за внимание