Motion planning: граф видимости, дорожные карты

Постановка задачи

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

Решение

Оптимальный путь

Можно доказать, что оптимальный путь — это кусочно-линейная ломаная, вершины которой совпадают с вершинами препятствий.

Доказательство

Часть 1. Оптимальный путь — кусочно-линейная ломаная

Предположим, что кратчайший путь T — не является кусочно-линейной ломаной. В таком случае, на пути T существует такая точка p, которая не принадлежит ни одному прямому отрезку из T (рисунок 1). Это означает что существует ε-окрестность точки p (рисунок 2) в которую не попадает ни одно препятствие. В таком случае, подпуть T_ε, который находится внутри ε-окрестности, может быть сокращён по хорде, соединяющий точки пересечения границы ε-окрестности с путем T. Раз часть пути может быть уменьшена, значит и весь путь может быть уменьшен, а значит исходное предположение некорректно. Путь T — кусочно-линейная ломаная.

Рисунок 1. Точка p на пути T	Рисунок 2. ε-окрестность точки p	Рисунок 3. Хорда, являющаяся кратчайшим путём

Часть 2. Вершины пути совпадают с вершинами препятствий
Предположим теперь что вершины пути T не совпадают с вершинами многоугольников-препятствий. Рассмотрим вершину v пути T. Вершина не может лежать внутри свободного пространства, поскольку в противном случае, применив аналогичный подход, мы нашли бы в ε-окрестности вершины v более короткий путь по хорде (рисунок 4). Значит, вершина v обязана лежать на границах препятствий (в их вершинах или на рёбрах). Но и на рёбрах вершина лежать не может, поскольку (опять рассматриваем ε-окрестность), мы сможем срезать по хорде (рисунок 5).

Рисунок 4. ε-окрестность вершины пути, лежащей в свободном пространстве	Рисунок 5. ε-окрестность вершины пути, лежащей на ребре

Вышесказанное означает, что вершины кусочно-линейного пути T обязаны находиться в вершинах препятствий-многоугольников.

Граф видимости

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

Рисунок 6. Пример графа видимости.

Предположим, что мы построили граф видимости VG. Добавим в граф VG вершины s и f, а также рёбра, соединяющие их со всеми видимыми из них вершинами. Получим расширенный граф VG⁺. Нам останется лишь найти кратчайший путь из вершины s в вершину f в графе VG⁺. Это можно сделать, применив алгоритм Дейкстры. Я не буду подробно рассказывать о нём, поскольку в интернете существует огромное множество статей, в том числе и на хабре. Остаётся лишь решить задачу построения графа видимости.

Построение графа видимости: наивный алгоритм

Будем перебирать все пары вершин в графе VG⁺ и для каждой из них проверять, не пересекает ли отрезок, соединяющий вершины рёбра препятствий. Нетрудно видеть, что сложность такого алгоритма O(n³), поскольку у нас есть n² пар вершин и O(n) рёбер.

Построение графа видимости за O(n²log(n))

Попробуем построить граф видимости немного быстрее. Для каждой вершины найдём все видимые из неё вершины при помощи метода плоского заметания. Нам нужно решить следующую задачу: на плоскости дано множество отрезков (рёбер препятствий) и точка p. Найти все концы отрезков, видимые из точки p. Будем заметать плоскость вращающимся лучом с началом в точке p (рисунок 7). Статусом заметающей прямой будет отрезки, которые её пересекают, упорядоченные по возрастанию расстояния от точки p до точки пересечения. Точками событий будут концы отрезков.

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

Упорядоченные по углу относительно оси, проходящей через точку p вершины будут храниться в очереди событий. Инициализация статуса займёт время O(nlog(n)), инициализация очереди событий также займёт время O(nlog(n)). Общее время работы алгоритма заметания есть O(nlog(n)), таким образом, применив алгоритм для каждой вершины графа, мы получим граф видимости за время порядка O(n²log(n)).
Более формально. Пусть количество рёбер множества непересекающихся многоугольников-препятствие будет n. В спойлерах я буду приводить код на псевдоязыке, максимально похожим на Java.

public final G buildVisibilityGraph(final Set<Segment> segments) {   /* Инициализация графа. Добавление всех концов отрезков в качестве вершин графа. Множество рёбер - пустое.  */   final G visibilityGraph = new G(getVertices(segments));      /* Для каждой вершины графа */   for (final Vertex v : visibilityGraph.getVertices()) {     /* Получение всех видимых из неё вершин */     final Set<Vertex> visibleVertices = getVisibleVertices(v, segments);      /* Создание рёбер для всех видимых вершин */     for (final Vertex w : visibleVertices)       visibilityGraph.addEdge(v, w);   }    return visibilityGraph; } 

Функция buildVisibilityGraph принимает на вход множество отрезков — рёбер многоугольников-препятствий и возвращает построенный граф видимости. Пояснения здесь, как мне кажется, не требуются. Рассмотрим псевдокод функции getVisibleVertices.

public final Set<Vertex> getVisibleVertices(final Vertex v, final Set<Segment> segments) {   /* Сортировка всех вершин (концов отрезков) по углу, образуемому отрезком pw (где w - конец отрезка) с вертикальной полуосью, проходящей через вершину v */   final SortedSet<Vertex> sortedVertices = sortSegmentsVertices(v, segments);    /* Пусть I - вертикальная полуось, проходящая через v. Найдём все отрезки, пересекающие эту ось и занесем их в в сбалансированное дерево поиска в порядке удаленности точки пересечения с полуосью */   final BST<Segment> intersectedSegments = new BST<Segment>(v, segments);    /* Изначально нет видимых вершин */   final Set<Vertex> visibleVertices = new HashSet<Vertex>();    /* Заметание плоскости по углу */   for (final Vertex w : sortedVertices) {     if (isVertexVisible(v, w, intersectedSegments))       visibleVertices.add(w);      /* Добавление в список пересечений всех рёбер, инцидентных вершине w и лежащих справа от заметающей прямой (луча vw), иными словами, начинающихся в вершине w */     intersectedSegments.add(getIncidentSegments(v, w, Position.RightOf));      /* Удаление из списка пересечений всех рёбер, инцидентных вершине w и лежащих слева от заметающей прямой (луча vw), иными словами, заканчивающихся в вершине w */     intersecedSegments.remove(getIncidentSegments(v, w, Position.LeftOf));   }    return visibleVertices; } 

Итак, первым делом вершины w ∈ W сортируются по углу между лучом vw и вертикальной полуосью, проходящей через v. Такая сортировка выполняется за O(nlog(n)), например, сортировкой слияниями. Затем происходит инициализация множества видимых вершин (по умолчанию, такое множество пустое). Далее начинается заметание плоскости. В порядке сортировки вершин для каждой из них выполняется проверка: видна ли вершина w из вершины v. Поскольку такая проверка означает наличие пересечений, которые хранятся в сбалансированном дереве, она может быть выполнена за O(log(n)). Если вершина видима, необходимо добавить её в список видимых вершин. И, наконец, вне зависимости от видимости вершины, необходимо изменить статус заметающей прямой. Для этого, для текущей вершины w необходимо удалить из списка текущих пересечений все рёбра (отрезки), которые заканчиваются в этой вершине (лежат слева от прямой vw) и добавить все рёбра (отрезки), которые в ней начинаются (лежат справа от прямой vw).
Рассмотрим рисунки 8-10:

Рисунки 8-10

Рисунок 8. Шаг 1. Обновление статуса заметающей прямой.

Рисунок 9. Шаг 2. Обновление статуса заметающей прямой.

Рисунок 10. Шаг 3. Обновление статуса заметающей прямой.

Итак, на первом шаге (луч vw₁) в дерево добавляются два отрезка w₁w₅ и w₁w₂, поскольку они лежат справа от луча vw₁. Статус заметающей прямой помечен оранжевым цветом.
На втором шаге (вторая в списке вершина — это вершина w₃) добавляются ещё два ребра в статус: w₃w₂ и w₃w₄. Теперь в статусе лежат 4 ребра. Наконец, на третьем шаге (вершина w₂) из статуса убираются 2 ребра: w₃w₂ и w₁w₂.

В конечном итоге, следует посмотреть на функцию проверки видимости вершины.

public final boolean isVisible(final Vertex v, final Vertex w, final BST<Vertex> intersections) {   final Segment currentSegment = new Segment(v, w);    /* Отрезок vw всегда пересекается с отрезками, инцидентными вершине w, поэтому исключаем их */   if (intersects(currentSegment, w.getSegments())     return false;     /* Предыдущая вершина (по углу) лежит на отрезка vw. */   if (currentSegment.contains(w.getPreviousVertex())) {     /* Предыдущая вершина невидима, отрезок продолжается, никаких изменений */     if (!v.getPreviousVertex().isVisible())       return false;      return !intersections.hasIntersectionsWith(new Segment(w.getPrevious(), w));      }    /* Проверка на пересечение луча с самым левого листом (самое близкое пересечение луча vw с отрезком из статуса) */   return intersections.intersectWithLeftMost(currentSegment); } 

Эта функция всего лишь должна вернуть true, если вершина w видима из вершины v и false в противном случае. Для этого достаточно всего лишь сравнить длину отрезка vw и расстояние от точки v до пересечения с ближайшим отрезком, хранящемся в дереве (самый левый лист). Но здесь существует несколько граничных случаев:

• Луч vw пересекает рёбра, инцидентные вершинам v или w
• Луч совпадает с ребром, на котором находятся другие вершины

Таким образом, функция проверки видимости работает в худшем случае за O(log(n)), поскольку использует поиск в сбалансированном дереве. Остальные операции выполняются за константное время.

Дорожные карты

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

Построим прямоугольник, содержащий внутри себя все многоугольники-препятствия (рисунок 11). Рассмотрим множество отрезков — рёбер препятствий и построим для него трапецоидальную карту (рисунок 12), а затем удалим из полученной карты все трапецоиды, лежащие внутри препятствий (рисунок 13). На построение карты требуется время O(nlog(n)), где n — общее число вершин препятствий. Для того чтобы решить, надо ли удалить трапецоид T, достаточно взять ребро top(T) и проверить, сверху или снизу от него лежит препятствие, граница которого содержит это ребро. Таким образом, на удаление всех лишних трапецоидов потребуется время O(n).
Если стартовая и конечная точка находятся внутри одного трапецоида, то можно пройти из первой точки во вторую по прямой. Если же эти точки лежат в разных трапецоидах, то путь пересечёт несколько трапецоидов, и, вероятно, в некоторых из них мы должны будем делать повороты.
Построим карту дорог — граф G. Вершинами G будут середины отрезков вертикальных линий, проведённых через вершины препятствий, и середины трапецоидов. Две вершины графа G соединены ребром в том и только в том случае, если одна из них является серединой некоторого трапецоида, а другая лежит на вертикальном ребре этого трапецоида. Граф G может быть построен из трапецоидальной карты за время O(n). Заметим, что граф G — плоский граф.
Для того чтобы найти путь из точки s в точку f, мы вначале определим, в каких трапецоидах содержатся s и f. Если они попали внутрь одного трапецоида, тогда кратчайший путь — прямая. Иначе мы перемещаемся по прямой из точки s в середину содержащего её трапецоида, затем перемещаемся вдоль рёбер графа в середину трапецоида, содержащего точку f (можно использовать поиск в ширину), и наконец, перемещаемся в точку f по прямой из середины содержащего её трапецоида (рисунок 14). Так как трапецоидальная карта содержит линейное по n число трапецоидов, и число отрезков вертикальных линий, проведённых через вершины препятствий, также линейно по n, то граф G имеет O(n) вершин, а так как он планарен, то число рёбер есть также O(n). Следовательно, поиск в ширину на G можно осуществить за время O(n). Так как для локализации точки в трапецоидальной карте требуется время O(log(n)), то для построения пути из точки s в точку f требуется время O(n). Но пути, построенные с помощью дорожных карт, не будут оптимальными.

Рисунок 11. Построение включающего прямоугольника	Рисунок 12. Трапецоидальная карта	Рисунок 13. Редуцированная трапецоидальная карта

Рисунок 14. Построенная карта дорог

Подзадачи

Построение выпуклой оболочки

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

• Наивный алгоритм. Работает за O(n⁴). Для каждой «не крайней» (то есть такой точки множества, через которую можно провести прямую, относительно которой всё множество будет лежать с одной стороны) выполяется следующий тест. Если она принадлежит хотя бы одному треугольнику, полученному по точкам множества, тогда она является точкой выпуклой оболочки. В множестве из N точек N³ треугольников. Следовательно для каждой из N точек нужно выполнить N³ проверок.
• Чуть менее наивный алгоритм. O(n³). Строится полный граф с множеством узлов — точками исходного множества. Для каждого ребра (которых N²) проверяеются остальные N-2 точки. Лежат ли они все по одно сторону от данного ребра. Если да, то ребро принадлежит выпуклой оболочке.
• Алгоритм Джарвиса. Работает за O(n²). Представьте себе множество точек на плоскости. Выберите среди них самую левую (такое можно сделать всегда (если их несколько, то любую среди левых), она заведомо является точкой выпуклой оболочки), а затем «заворачивайте» множество как подарок. Среди всех остальных точек выбираем такую, которая имеет наименьший полярный угол (по аналогии с построением графа видимости, заметаем лучом)
• Алгоритм Грэхэма. Работает за O(nlog(n)). Описание предоставлять не буду (как Вы понимаете, чем быстрее алгоритм, тем больше писанины) — гуглите

Пересечение отрезков

Наконец, последней задачей, которую необходимо решить в рамках построение графа видимости — это проверка двух отрезков на наличие пересечений. Это очень хорошая тема для отдельной статьи, поскольку писать по этому вопросу тоже достаточно много. Я лишь приведу один из понравившихся мне методов решения задачи: вот он.

Заключение

Надеюсь, кому-нибудь статья показалась интересной, а может быть даже полезной. В ближайшее время планирую опубликовать ещё одну статью, а дальше будет видно — устроим опрос, надо ли продолжать.

Вместо тысячи слов

Ссылки

• Курсы лекций по линейной алгебре, высшей математике, дискретной математике, теории алгоритмов и вычислительной геометрии на кафедре Прикладная математика СПбГПУ
• M. de Berg, O. Cheong Computation «Computational Geometry», 2008
• J. E. Goodman, J. O’Rourke «Discrete and computational geometry», 2004
• Venta library реализация на Java