Visibility graph и motion planning

Материал из Викиконспекты
Перейти к: навигация, поиск

Нахождение любого пути между точками с препятствиями

Путь с препятствиями через трапецоидную карту
Такой путь не самый короткий

Для начала рассмотрим движение материальной точки. Случай, когда размером и формой движимого объекта пренебречь нельзя, будет рассмотрен позднее.

Эту задачу можно решить с помощью трапецоидной карты. По ней строится граф, ребра которого соединяют центры трапедоидов, а также начальную и конечную вершины с серединами вертикальных сторон трапецоидов. В таком графе ищется путь между начальной и конечной вершинами.

Если точки лежат внутри одного трапецоида — ответ найден. Иначе идём из стартовой точки в центр её трапецоида, далее по построенным рёбрам ищем трапецоид содержащий финальную точку. Для этого можно использовать поиск в ширину или другой алгоритм нахождения кратчайшего пути в графе. В конечном итоге, соединяем середину последнего трапецоида с конечной вершиной.

Данный алгоритм работает за [math] O(n \log n) [/math] и за линейное количество памяти и хорошо подходит для нахождения какого-нибудь пути между парой данных вершин. Но если нужно найти кратчайший путь, этот алгоритм не подходит, хоть и работает быстро. Однако, решения нахождения кратчайшего пути в лучшем случае работают за [math] O(n^2) [/math] времени и памяти (здесь и далее [math] n [/math] — количество всех вершин).

Нахождение кратчайшего пути между точками с препятствиями

Visibility graph

Рассмотрим точное решение нахождения кратчайшего пути на плоскости между двумя точками с полигональными препятствиями с помощью построения графа видимости. После его построения, как и в случае с трапецоидной картой, кратчайший путь ищется любым стандартным алгоритмом поиска (например, алгоритмом Дейкстры или A*).

Для простоты рассуждений начальную и конечную вершины будем считать вершинами полигонов.

Лемма (О кратчайшем пути):
Любой кратчайший путь между двумя вершинами с полигональными препятствиями представляет собой ломаную, вершины которой — вершины полигонов.
Доказательство:
[math]\triangleright[/math]
Short cut
Пусть кратчайший путь — не ломаная. В таком случае, на пути существует такая точка [math] p [/math], которая не принадлежит ни одному прямому отрезку. Это означает, что существует [math]\epsilon[/math]-окрестность точки [math] p [/math], в которую не попадает ни одно препятствие (случай, когда точка попала на ребро рассматривается аналогично). В таком случае, подпуть, который находится внутри [math]\epsilon[/math]-окрестности, по неравенству треугольника может быть сокращён по хорде, соединяющий точки пересечения границы [math]\epsilon[/math]-окрестности с путем. Раз часть пути может быть уменьшена, значит и весь путь может быть уменьшен, а значит исходное предположение некорректно.
[math]\triangleleft[/math]
Определение:
Говорят, что вершина [math] u [/math] видна (англ. mutually visible) из [math] v [/math], если отрезок [math] uv [/math] не пересекает ни одного препятствия.


Определение:
Граф видимости (англ. visibility graph) — граф, вершины которого — вершины полигонов. Между вершинами [math] u [/math] и [math] v [/math] существует ребро, если из [math] u [/math] видна [math] v [/math].


В худшем случае в таком графе может быть [math] O(n^2) [/math] ребер. Однако по некоторым ребрам кратчайший путь точно не пройдет, и такие ребра из графа можно удалить.

Лемма (О неиспользуемых вершинах):
Удаляем [math] BD [/math]
  1. Если существуют вершины [math] A, B, C [/math] одного препятствия и вершина [math] D [/math] такая, что поворот [math] DBA [/math] не совпадает с поворотом [math] DBC [/math], то ребро [math] DB [/math] не принадлежит кратчайшему пути и его можно удалить из графа. (См. поясняющую картинку справа)
  2. Все внутренние вершины, кроме вырожденного случая, (начальная/конечная точка лежит внутри выпуклой оболочки фигуры) можно игнорировать.
Доказательство:
[math]\triangleright[/math]
Не удаляем [math] BS [/math]
  1. Путь проходящий через ребро [math] BD [/math] будет длиннее, чем через соседей точки [math] B [/math], так как по неравенству треугольника [math] AB + BD \gt AD [/math]
  2. Если случай не вырожденный, значит заход внутрь фигуры только увеличит суммарный путь, так как по неравенству треугольника расстояние между соседними выпуклыми вершинами всегда меньше суммы расстояний с учётом внутренней.
[math]\triangleleft[/math]

По доказанным леммам любое ребро кратчайшего пути содержится в графе. Таким образом, для нахождения кратчайшего пути осталось найти кратчайший путь в этом графе от начальной до конечной вершины.

Построение visibility графа

Наивный алгоритм. [math] O(n ^ 3) [/math]

Для каждой пары вершин проверяем, можно ли добавить ребро между ними, то есть нет ли пересечений с полигонами. [math] O(n^2) [/math] пар вершин и [math] O(n) [/math] ребер, то есть [math] O(n^3) [/math].

Lee’s Algorithm. [math] O(n ^ 2 \log n) [/math]

Заметание плоскости вращающимся лучом

Однако можно это сделать за [math] O(n ^ 2 \log n) [/math]. Идея алгоритма проста: для каждой вершины найдем видимые из нее вершины. Если научиться делать это за [math] O(n \log n) [/math], задача решена, так как всего точек [math] n [/math].

Для каждой вершины будем рассматривать только правую половину плоскости, так как ребра, которые должны идти в левую половину, будут исходить из вершин, для которых текущая вершина будет справа.

Переформулируем задачу. Дано: точка [math] v [/math] и множество отрезков — ребер препятствий. Найти: множество концов отрезков, видимых из [math] v [/math].

Для решения этой задачи будем использовать заметающий луч с началом в точке [math] v [/math]. Его статусом будут отрезки, которые его пересекают, упорядоченные по возрастанию расстояния от точки [math] v [/math] до точки пересечения. Точками событий будут концы отрезков.

Пустим луч из рассматриваемой вершины [math] v [/math] вертикально вверх и добавим в статус все отрезки, которые он пересекает, по увеличению расстояния до них. Теперь будем рассматривать точки [math] w \in V [/math] в порядке сортировки по углу между [math] v [/math] и вертикальной полуосью [math] l [/math]. При таком обходе для проверки видимости вершины достаточно проверить пересечение с ближайшим к [math] v [/math] отрезком, то есть первым в статусе(так как отрезки отсортированы по расстоянию до них). Действительно, если вершина [math] w [/math] не видна, то отрезок [math] vw [/math] пересекает несколько отрезков, лежащих перед [math] w [/math], а значит и ближайший. В противном случае все пересекаемые лучом отрезки лежат за вершиной [math] w [/math] и пересечения отрезка [math] vw [/math] с ближайшим отрезком не будет. Вне зависимости от видимости вершины, необходимо изменить статус заметающего луча. Для этого необходимо удалить из статуса все отрезки, которые заканчиваются вершине [math] w [/math] (лежат слева от прямой [math] vw [/math]) и добавить все отрезки, которые в ней начинаются (лежат справа от прямой [math] vw [/math]).

Псевдокод
graph buildVisibilityGraph(Set<Segment> segments)
   vertices = getVertices(segments) [math] \cup\ \{s,\ t\} [/math]
   graph = visibilityGraph(vertices)                  
   for Vertex [math]v[/math] in vertices
      for Vertex [math]w[/math] in getVisibleVertices([math]v[/math], segments)
         visibilityGraph.addEdge([math]v[/math], [math]w[/math])
   return visibilityGraph

Здесь функция getVisibleVertices([math] v [/math]) возвращает все видимые из [math] v [/math] вершины и выглядит так: 

Set<Vertex> getVisibleVertices(Vertex [math]v[/math], Set<Segment> segments)
   Set<Vertex> answer
   for Segment [math]s[/math] in segments
      if intersect([math] s [/math], [math] l [/math])
         status.add([math]s[/math])
   for Point [math]w[/math] in segments
      if [math]v.x \leqslant w.x[/math]
         currentVertices.add([math]w[/math])
   sort(currentVertices) by angle
   for Point [math]w[/math] in currentVertices
      if not intersect([math]vw[/math], status.closest)
         answer.add([math]w[/math])
      for Segment [math]s[/math] ending in [math]w[/math]
         status.delete([math]s[/math])
      for Segment [math]s[/math] beginning in [math]w[/math]
         status.add([math]s[/math])
   return answer

В качестве статуса нужно использовать структуру данных, позволяющую добавлять и удалять из нее отрезки за [math] O(\log n) [/math] и извлекать минимум за [math] O(1) [/math] или [math] O(\log n) [/math]. В этом случае достигается асимптотика [math] O(n^2 \log n) [/math], так как для каждой из [math] n [/math] точек выполняется сортировка за [math] O(n \log n) [/math], обновление статуса (суммарно [math] O(n \log n) [/math], так как каждый отрезок добавляется и удаляется из статуса не более одного раза) и запросы ближайшего отрезка ([math] O(\log n) [/math] или [math] O(1) [/math] на точку, то есть [math] O(n \log n) [/math] или [math] O(n) [/math]).

Обновление статуса заметающего луча: добавляем ребра [math] w_1 w_5 [/math] и [math] w_1 w_2 [/math] в статус
Добавляем ребра [math] w_3 w_2 [/math] и [math] w_3 w_4 [/math] в статус
Удаляем ребра [math] w_3 w_2 [/math] и [math] w_1 w_2 [/math] из статуса

Motion planning

Изменяем препятствия
Ищем путь для точки

Рассмотрим задачу нахождения кратчайшего пути, когда движимый объект — это выпуклый полигон. Например, робот, которого надо доставить из начальной в конечную точку.

Если полигон вращать нельзя, задачу сводится к движению точки так: выбирается точка на полигоне, которая принимается за начало координат. В такой системе координат для каждого препятствия считается сумма Минковского с полигоном. Получаются бОльшие препятствия, но теперь достаточно двигать выбранную точку, что было описано выше.

Если полигон можно вращать, задача нахождения кратчайшего пути становится достаточно ресурсоёмка, поэтому обычно рассматривают задачу нахождения какого-нибудь пути между конечными точками.

Первый шаг решения этой задачи совпадает с предыдущим случаем: выберем точку и построим сумму Минковского препятствий с полигоном. Рассмотрим малый угол [math] \epsilon [/math]. Представим, что поворот полигона на этот угол — это движение вверх-вниз между слоями, на каждом из которых посчитана сумма Минковского с полигоном, повернутым на этот угол.

На каждом слое построим трапецоидную карту и граф, как описано в начале. Если пересечь соседние слои и добавить между их графами ребра, получится один большой граф, в котором ищется кратчайший путь.

При таком подходе может возникнуть ошибка при пересечении слоев: на каждом слое состояния будут допустимые, а осуществить поворот физически будет невозможно. Обычно, эту проблему решают двумя способами: измельчением угла поворота и изначальным сглаживанием углов полигона. Первый способ повышает не только точность решения, но и вычислительную сложность задачи. Второй подход практически исключает возможность нахождения пути, когда его нет, но повышает вероятность "ненахождения" пути, когда он есть.

Источники информации

  • Mark de Berg, Otfried Cheong, Marc van Kreveld, Mark Overmars, Computational Geometry: Algorithms and Applications — Third edition — Springer, 2008. — Chapter 15. — ISBN 978-3-540-77973-5
  • Academia.edu — 3D Visibility Graph
  • Хабрахабр — Motion planning: граф видимости, дорожные карты
  • igitur-archive.library.uu.nl — Visibility graph при помощи rotation tree за [math]O(n^2)[/math].

Ссылки

  • Github — Реализация алгоритма за [math] O(n^2 \log n) [/math]
Источник — «http://neerc.ifmo.ru/wiki/index.php?title=Visibility_graph_и_motion_planning&oldid=85760»