CC BY
51
Компьютерные инструменты в образовании, 2014 № 5: 25-41 УДК: 519.682.2 http://ipo.spb.ru/journal
Дао Дуй Нам, Фирсов Михаил Александрович
информационные
системы
Аннотация
Рассматриваются два приближённых алгоритмы локализации мобильного робота, снабженного картой в виде простого многоугольника. Гипотезам локализации соответствуют экземпляры карты с отметкой предполагаемого положения робота. Первый алгоритм основан на использовании триангуляции простого многоугольника в качестве предобработки для реализации основных операций, таких как пересечение многоугольников, вычисление многоугольников видимости, нахождение кратчайших путей. Во втором алгоритме в дополнение к триангуляции используется понятие окна в пересечении экземпляров карты. «Заглядывая» в окно, робот отсекает ложные гипотезы. Проведены экспериментальные исследования этих алгоритмов и сравнение с другими алгоритмами.
Ключевые слова: вычислительная геометрия, робототехника, локализация робота, пересечение многоугольников, триангуляция многоугольника, многоугольник видимости, скелет многоугольника видимости, приближённые алгоритмы.
1. ВВЕДЕНИЕ
Прикладная задача локализации мобильного робота (ЗЛМР) относится к области робототехники, а для её формализации и получения алгоритма решения используются методы вычислительной геометрии. Содержательно ЗЛМР для случая плоскости формулируется следующим образом. Мобильный робот может перемещаться во внешней среде, которую можно представить как свободное пространство на плоскости, ограниченное стеной (преградой). Будем считать, что эту внешнюю среду можно описать простым плоским многоугольником, внутренность которого соответствует свободному пространству, а граница многоугольника (без самопересечений) соответствует преграде. Пусть робот снабжен картой внешней среды в виде плоского простого многоугольника P с п вершинами без отверстий. В исходном состоянии мобильный робот помещен в заранее неизвестное место среды, то есть в некоторую точку p в пределах многоугольника P (см. рис. 1). Робот снабжен компасом и сенсорным устройством, с помощью которого он осуществляет круговой обзор и определяет расстояние до преграды. Перед роботом стоит задача определить свое истинное местоположение во внешней среде, то есть локализовать себя на карте. Для этого робот, во-пер-
© Дао Дуй Нам, Фирсов М.А., 2014
Рис. 1. Многоугольник карты Р и начальное положение р (слева),
I í Т а оаТ ëйí её аеае! Т пое V (в центре) и два возможных начальных местоположений р1 и р2, соответствующих многоугольнику видимости (справа)
вых, может осмотреть свою окрестность и соотнести видимую область (так называемый многоугольник видимости) V = V(p) с картой. Если на карте имеется лишь один фрагмент, совпадающий с многоугольником видимости, то задача решена. Если таких фрагментов несколько, то необходимо определить, какой из них соответствует начальному местоположению робота. Для этого на основании анализа многоугольников Р и V робот должен сгенерировать множество Н всех гипотез о своем местоположении р{ е Р таким образом, что область видимости в точке р{ конгруэнтна V. Далее робот, перемещаясь и обозревая окрестность, может устранить все неправильные гипотезы о своем местоположения и таким образом определить свое истинное начальное местоположение. При этом требуется, чтобы суммарная длина проделанных перемещений робота была минимальной.
Отметим, что в случае отсутствия компаса робот не сможет правильно сориентировать карту (то есть «привязать» её по направлению), и задача локализации в этом случае, вообще говоря, не будет иметь единственного решения (например, в случае квадрата однозначная локализация возможна только при положении робота в центре квадрата, во всех остальных точках имеются 4 решения, отличающиеся поворотами на угол л/2, л и 3л/2).
Оказывается, оптимизационная задача локализации мобильного робота является NP-трудной задачей [1]. Поэтому в основном рассматриваются приближённые (полиномиальные по вычислительной сложности) алгоритмы локализации мобильного робота [2, 3, 4]. При этом, как правило, внимание акцентируется на характеристиках, описывающих отклонения от стоимости оптимального решения (суммарной длины перемещений робота). Характеристики вычислительной сложности таких алгоритмов оцениваются асимптотически, а данные о реальном времени работы алгоритмов, как правило, не приводятся, в том числе, видимо, из-за высокой вычислительной сложности алгоритмов, например O(n5log п) [2] или 0(п12) [4].
Нашей целью далее будет разработка таких модификаций ранее известных алгоритмов или новых алгоритмов локализации робота, которые имеют меньшую вычислительную сложность или при асимптотически такой же сложности оказываются на практике более эффективными, то есть выполняются быстрее. Анализ используемых алгоритмов локализации показывает, что есть несколько общих понятий и связанных с ними алгоритмических действий, которые используются во всех или во многих таких алгоритмах. Это, например, построение многоугольников видимости, вычисление кратчайшего пути для перемещения робота из одной точки внутри многоугольника в другую. Основная идея предлагаемых далее алгоритмов заключается в систематическом использовании триангуляции простого многоугольника в качестве предобработки на различных этапах алгоритма локализации.
2. ЗАДАЧА ЛОКАЛИЗАЦИИ МОБИЛЬНОГО РОБОТА
Известные алгоритмы локализации мобильного робота [2, 3, 4] включают две фазы: генерацию гипотез [5] и проверку гипотез. В фазе генерации гипотез вычисляется множество гипотетических местоположений робота pj,P2,. .,Pk е P, которые соответствуют наблюдениям роботом окружающей среды в его начальном положении. В фазе проверки гипотез исключаются неправильные гипотезы.
В фазе генерации гипотез строится множество гипотез H = {hj, h2,...., hk}, (Vi е 1..k | hj: p = pt), определяемых гипотетическими местоположениями в P, в которых робот мог бы быть расположен первоначально. Далее выбирается произвольное гипотетическое pt местоположение из H, чтобы служить точкой привязки. Затем для каждого гипотетического местоположения p., 1 < j < k, j Ф i определяется вектор переноса tj = p/ — Pj, который соответствует переводу местоположения p. в pt. (p/ = pj + tj ). Вычисляются копии Pj, P2, ..., Pk многоугольника карты P, соответствующие множеству гипотез H таким образом, что P. преобразуется в P = Pt смещением на t. (можно считать, что при этом экземпляр Pt преобразуется в себя смещением на ti = 0).
Пусть 6P обозначает границу простого многоугольника P. Граница многоугольника 6P разбивает плоскость на внутреннюю Int P и внешнюю Out P грани, а стороны многоугольников рассматриваются как ребра такого подразбиения плоскости.
Для двух простых многоугольников P и Q рассмотрим их пересечение Intersection (P, Q), которое является, вообще говоря, многосвязной областью, а её составными частями являются простые многоугольники, которые можно рассматривать как грани подразбиения плоскости, порождаемого наложением многоугольников P и Q [6]. Простой многоугольник F есть грань в пересечении P и Q, если и только если его внутренность Int F является максимальным связным подмножеством в Int P n Int Q. На рис. 2 приведен пример пересечения Intersection (P1, P2) многоугольников P1 и P2, полученных в соответствии с гипотезами и смещением из многоугольника P, приведенного на рис. 1.
В рассмотренных далее алгоритмах будет необходимо вычислять не всё пересечение многоугольников Intersection (P1, P2), а лишь одну его связную компоненту, а именно ту, в которой находится точка гипотетического местоположения робота p. Далее для краткости такую связную компоненту будем называть фрагментом пересечения и обозначать как InterS (P1, P2). Точка местоположения робота p в этом обозначении не указана и будет ясна из контекста.
Фрагмент пересечения многоугольников InterS (P1, P2) относительно выделенного экземпляра многоугольника карты P = Pt определим как фрагмент пересечения смещенных многоугольников P, 1 < j < к. Индуктивно можно определить InterS (P1, P2, ..., Pк) = InterS (InterS(P1,..., Pk_ 1), Pk), к > 3.
Вычисление пересечения многоугольников требуется в алгоритмах [3] и [4], а также в двух рассматриваемых далее алгоритмах [7] и [8]. Алгоритм построения пересечения на основе предварительной триангуляции простых многоугольников описан далее в разделе 5. Отметим, что
1) при вычислении кратчайшего пути внутри простого многоугольника из одной точки в другую алгоритм [9], использующий предварительную триангуляцию многоугольника, более эффективен, чем другие подходы, например, на основе построения графа видимости [10];
2) на основе использования триангуляции многоугольника можно эффективно вычислять многоугольники видимости и их скелеты (см. раздел 4).
Для формального определения многоугольника видимости и его скелета рассмотрим следующие определения, для краткости опуская некоторые детали.
/ РиРг
Рис. 2. Пересечение многоугольников Pl и P2, полученных из многоугольника P (см. рис. 1) смещением в соответствии с гипотезами \ : p = p1 и h2 : p = p2. Пересечение состоит из трёх граней (закрашено)
Рис. 3. Разбиение многоугольника P на ячейки видимости
Рис. 4. Скелет V*(p) многоугольника видимости
Две точки границы простого многоугольника 8P видимы друг из друга, если отрезок, соединяющий их, лежит в P и пересекает 8P только в этих конечных точках. Две точки a и b многоугольника P видимы друг из друга, если отрезок ab, соединяющий их, полностью лежит в P (ab с P) и если пересекает границу 8P, то только в этих конечных точках (ab n 8P с {a,b}). Многоугольник видимости V(p), для любой точкиp из P, состоит из всех точек P, которые являются видимыми из p. Например, на рис. 1 (в центральной части) показан многоугольник видимости V(p) с вершинами a, b, c, d, e, из которых a, b, d, e являются вершинами исходного многоугольника P. Ребра (a, b), (d, e) и (e, a) есть ребра исходного многоугольника P, а ребро (b, c) есть часть ребра многоугольника.
Определение. В заданном простом многоугольнике P ячейкой видимости C е P называется максимальное (по включению) связное подмножество P со свойством, что любые две точки в C видят одно и то же подмножество вершин P.
Один из первых алгоритмов [2], решающих ЗЛМР, основан на специальной предобработке многоугольника карты - разбиении на ячейки видимости (см. рис. 3). Сложность этого алгоритма O(n5 log n) [2].
Определение. Скелет V*(p) многоугольника видимости V(p) - это многоугольник, образованный такими вершинами многоугольника видимости V(p), которые одновременно являются вершинами исходного многоугольника-карты P. Скелет многоугольника видимости также можно рассматривать как многоугольник, образованный теми ребрами из V(p), которые являются ребрами исходного многоугольника P и которые, в свою очередь, связаны в последовательность ребер границы многоугольника V*(p) дополняющими ребрами, то есть такими сторонами многоугольника V(p), которые не являются сторонами исходного многоугольника P или их частями (см. рис. 4). Важно, что при этом ребра скелета промар-
кированы (как исходные или дополняющие). Далее в разделе 4 описан алгоритм вычисления скелета многоугольника видимости за время О(п), основанный на предварительной триангуляции простого многоугольника.
3. РЕШЕНИЕ ЗАДАЧИ ЛОКАЛИЗАЦИИ С ИСПОЛЬЗОВАНИЕМ ТРИАНГУЛЯЦИИ МНОГОУГОЛЬНИКА
Дадим описание двух предлагаемых алгоритмов локализации робота, представленных ранее в [7] и [8]. Наше описание отличается уточнением и более подробным представлением некоторых шагов этих алгоритмов (а также оценок их сложности и в дальнейшем их более эффективной программной реализацией), в том числе за счет более систематического использования триангуляции карты.
Начнем с алгоритма локализации мобильного робота с использованием триангуляции карты. Как уже отмечалось, триангуляцию многоугольника карты целесообразно использовать в алгоритмах локализации как вспомогательное действие при вычислении кратчайшего пути из одной точки внутри простого многоугольника в другую. Однако, используя триангуляцию исходного многоугольника как предобработку, во-первых, можно эффективно реализовать и некоторые другие действия, например, построение многоугольника видимости, а во-вторых, можно использовать разбиение карты на множество треугольников для выбора перемещений робота на этапе отсечения гипотез. Например, выбор точек, устраняющих гипотезы, можно осуществлять обследованием центров треугольников и средних точек ребер триангуляции. Можно рассматривать триангуляцию как альтернативу разбиению многоугольника на ячейки видимости, использованному в [2, 5]. Ячейки видимости обладают очень полезным при локализации свойством: перемещение робота из одной ячейки видимости в соседнюю (смежную через ребро) открывает обзор, по крайней мере, одной новой (ранее невидимой) вершины и (или) исключает из поля видимости, по крайней мере, одну из вершин. Однако число ячеек видимости многоугольника есть 0(п3). Ячейки триангуляции (то есть треугольники) таким полезным свойством не обладают, однако их количество есть О(п), что позволяет надеяться на ускорение работы алгоритма локализации при их использовании.
Алгоритм локализации робота с использованием триангуляции карты [8] можно описать с учетом данных ранее определений. Пусть на входе алгоритма заданы многоугольник карты Р и робот, помещенный в неизвестное начальное местоположение в Р. Алгоритм состоит из следующих действий:
1. Вычислить в относительных координатах многоугольник видимости V(p) и его скелет V*(p) по данным сенсоров робота в текущем начальном местоположении робота р, соответствие которому на карте неизвестно. Пусть т - число вершин многоугольника V*(p).
2. Выполнить триангуляцию многоугольника карты.
3. Сгенерировать множество Низ k гипотез на карте Р, которые соответствуют полученному многоугольнику видимости V. Для этого скелет многоугольника видимости V*, заданный в относительных координатах, сопоставляется с последовательностью вершин многоугольника с учетом маркировки ребер скелета. В местах совпадений на основе использования триангуляции карты вычисляется скелет многоугольника видимости относительно предполагаемого положения робота, и эти два скелета сравниваются. В случае совпадения не только скелетов, но и многоугольников видимости фиксируется новая гипотеза.
4. (Операции в пп.4-9 осуществляются далее для всех активных (не отклоненных пока) гипотез hj (/ = 1, 2,..., начиная с к? = k.). Выбрать произвольную гипотезу hj из множества активных гипотез Н и соответствующую точку гипотетического местоположения как исходную для построения пересечений. Переносами вершин получить триангулированные «смещенные» многоугольники, соответствующие прочим гипотезам (фактически триангу-
ляция, заданная номерами вершин, не изменяется, а «смещаются» лишь координаты вершин).
5. Для активных гипотез вычислить связный компонент 1Мв^ (Р1, Р2, ..., Р^), содержащий стартовую точку. Вычислить триангуляцию F.
6. Найти точку г в множестве точек на серединах ребер триангуляции и центров треугольников в пределах многоугольника F как такую точку, которая является ближайшей к текущему положению робота среди точек, возможно, устраняющих лишние гипотезы. Для этого запускается поиск в ширину на графе триангуляции. В каждой обследуемой вершине определяется скелет многоугольника видимости V*(q) из соответствующей ей точки q. Этот скелет сопоставляется со скелетами видимости соответствующих точек q,|. в экземплярах карты активных гипотез, и определяется возможность отклонения гипотез при планируемом перемещении робота в эту точку. Для всех таких точек вычисляется кратчайший путь от текущего положения робота и выбирается точка г с минимальной длиной пути. При этом дерево поиска в ширину обходится не полностью, так как обрываются поддеревья, корнями которых являются найденные точки возможного устранения гипотез. Это ограничение поиска в ширину, возможно, приведет к удлинению суммарного перемещения робота, но сокращает время работы алгоритма.
7. Осуществить перемещение робота в точку г.
8. Устранить гипотезы, сравнивая данные о текущем многоугольнике видимости, выдаваемые роботом в точке г, с данными о видимости, вычисленными во всех эквивалентных точках, соответствующих активным гипотезам. При этом, если подтверждается одна гипотеза, то устраняется одна или более других гипотез, то есть в итоге устраняется хотя бы одна из гипотез.
9. Пусть Е - множество гипотез, устраненных на предыдущем шаге. Заменить к на к - |Е|. Повторять шаги 4-9, пока в множестве активных гипотез Н не останется только одна гипотеза (к = 1), которая и будет соответствовать истинному начальному местоположению робота.
Вычислительная сложность этого алгоритма по этапам и суммарно приведена в табл. 1. Отметим, что при выполненной предварительной триангуляции вычисление кратчайшего пути от текущего положения робота до анализируемой точки выполняется в худшем случае за время О(п) [9], однако эту операцию требуется применять многократно на шаге 6 каждой итерации алгоритма, что в значительной степени определяет суммарную сложность алгоритма локализации.
Триангуляция простого многоугольника теоретически имеет сложность О(п) [11] и мо-
Табл. 1
Шаг Действия Сложность укрупненных действий
1-2 Триангуляция многоугольника карты * O(nlog n)
3,4 Генерация гипотез O(mn) + O(kn)
5 Построение пересечений относительно выбранной гипотезы и триангуляция F (к' - число активных гипотез) * (k — 1)O(n) + O(nlog n)
6,7 Обследование 4 к'(п - 2) точек на ребрах и центрах треугольников для устранения гипотез. Вычисление кратчайших путей до точек, устраняющих гипотезы, для определения ближайшей из них 4k' (n — 2)O(n) = k'O(n2)
8 Сравнение данных о многоугольниках видимости для активных гипотез и текущего положения робота k'O(n)
* v-^k—1 2 4 Полная сложность: O(mn) + O(kn) + O(nlog n) + ^^ 1 k'O(n ) + k'O(n) = O(n )
жет быть реализована, например, известным эффективным и практичным алгоритмом [12] за время O(n log*n) (здесь log*n — итерированный логарифм), что на практике в ЗЛМР дает фактически O(n). Выходом этого алгоритма является множество треугольников, заданных номерами своих вершин. Для дальнейшего использования в алгоритме локализации множество треугольников преобразуется за время O(n) в специальную структуру данных [6]. Это представление, по сути, является одним из адаптированных к триангуляции вариантов реберного списка, используемого для представления планарного подразбиения плоскости [10]. Каждый треугольник в этой структуре представлен своими тремя вершинами и тремя указателями на соседние треугольники, смежные с ним через его стороны.
Структура триангуляции получается в два этапа. Сначала по заданному множеству треугольников, образующему триангуляцию, для каждой вершины триангуляции формируется список треугольников, в которые входит эта вершина. Затем формируется собственно структура триангуляции, при этом информация о прилегающих треугольниках получается для каждой пары вершин данного треугольника путем анализа списков, полученных на первом этапе. Структура триангуляции дополняется массивами входов для вершин и ребер триангуляции.
Использование структуры триангуляции позволяет эффективно реализовать некоторые базовые операции алгоритма локализации. Например, локализация точки в триангуляции, если необходимо проводить эту операцию многократно, осуществляется с использованием приема кэширования треугольников [6], а систематически используемая операция построения многоугольника видимости и его скелета реализована на основе поиска в ширину на графе, двойственном триангуляции. При этом удается, как правило, избежать просмотра всех вершин многоугольника карты, анализируя треугольники, смежные с текущим.
Второй из рассматриваемых алгоритмов локализации мобильного робота — это приближенный алгоритм с использованием окон в многоугольнике карты. В работе [4] было введено понятие окна и на его основе предложен приближенный алгоритм локализации, который опирается на решение обобщенной (групповой) задачи Штейнера специального
вида ( — -Group Steiner problem). Оценка сложности этого алгоритма Q(n12) приведена в
другой работе [13] одного из авторов работы [4].
Далее рассматривается более простой, чем [4], вариант приближенного алгоритма, не использующий решение задачи Штейнера. Первоначальный вариант этого алгоритма был рассмотрен в [7]. В этом алгоритме наряду с вычислением окон карты также систематически используется триангуляция многоугольника карты для реализации большинства вспомогательных операций.
Рассмотрим гипотезу h (h Ф h) и обозначим через Fj такую грань в Intersection (P,, P), которая содержит начальную позицию у0 (см. например F12 на рис. 5), то есть в обозначениях, введенных в разделе 2, Fj = InterS(P,, P). Грань Fj имеет не более 2n ребер [4].
Г
г
Го
Рис. 5. Слева многоугольник Р с двумя гипотезами hl и h2 и справа наложение Р1 и Р2 с выделенной гранью F12, содержащей у0
Каждое из О(п) ребер е е Fjj может быть ребром одного из трех типов: (1) е лежит на границе Рно не Р.; (ii) е лежит на границе Р., но не Р.; (iii) е лежит на границе и Ри Р.. Робот может различить hi и h., если и только если он видит ребро е типа (1) или (и) (см. рис. 6).
Если из точки у0 видно какое-либо ребро типа (1) или типа (и), то робот может различить h i и hj, не перемещаясь из у0. Предположим, что все ребра F ., которые видимы из у0, имеют тип (ш). Пусть ребро е грани F . имеет тип (i) или тип (и). Множество УР(е) точек F ., которые видимы из некоторой точки ребра е, является простым многоугольником (многоугольник видимости е) в пределах F ., который по нашему предположению не включает точку у0. В грани F . существует хорда ^(е), которая лежит на границе УР(е) и отделяет е от у0. Отрезок ^(е) можно назвать окном [4] (см. рис. 6). Содержательно ясно, что, переместившись и «выглянув» в окно, робот мог бы увидеть ребро е и различить гипотезы h{ и
С учетом данных определений можно описать предлагаемый алгоритм локализации робота с использованием окон карты. Пусть на входе алгоритма заданы многоугольник карты Р и робот, помещенный в неизвестное начальное местоположение в Р. Алгоритм состоит из следующих действий (те действия, которые основаны на триангуляции и аналогичны действиям первого алгоритма, здесь указываются более кратко):
1. Вычислить в относительных координатах многоугольник видимости V(p) и его скелет V*(p) по данным сенсоров робота в текущем начальном местоположении робота р, соответствие которому на карте неизвестно. Пусть т — число вершин многоугольника V*(p).
2. Выполнить триангуляцию многоугольника карты.
3. Сгенерировать множество Низ kгипотез на карте Р, которые соответствуют полученному многоугольнику видимости V.
4. (Операции в пп. 4-11 осуществляются для всех активных (не отклоненных пока) гипотез , начиная с к = к) Выбрать произвольную гипотезу h{ из Н и соответствующую точку гипотетического местоположения как исходную для построения пересечений.
5. Построить для экземпляров карты Р 1 и Р. связный компонент, содержащий стартовую точку.
6. Построить все окна компонента пересечения F ...
7. Вычислить средние точки всех окон в F . и найти среди них такую точку г., которая является ближайшей к текущему положению робота, то есть найти «ближайшее» окно в Fjj.
8. Вычислить для активных гипотез связный компонент пересечения, содержащий стартовую точку. Среди тех точек г., которые попадают в F, найти ближайшую к текущему положению робота точку г.
9. Переместить робота в точку г.
10. Устранить гипотезы, сравнивая данные о текущем многоугольнике видимости, выдаваемые роботом в точке г, с данными о видимости, вычисленными во всех эквивалентных точках, соответствующих активным гипотезам.
Рис. 6. Слева изображен многоугольник видимости VP(e) и показаны ребро е типа (1) и соответствующая хорда (окно) ^(е). Справа показаны все окна ^(е) для ребер типа (1) или (И) грани F.
У
11. Пусть E — это множество гипотез, устраненных на предыдущем шаге. Заменить k на k - |E|. Повторять шаги 4-11, пока в множестве активных гипотез H не останется только одна гипотеза (k = 1), которая и будет соответствовать истинному начальному местоположению робота.
Подсчет вычислительной сложности этого алгоритма производится аналогично тому, как это было показано в табл. 1, и учитывает реализацию основных операций (нахождение пересечений, скелетов многоугольников видимости, кратчайших путей) на базе триангуляции многоугольника карты. Отличаясь в деталях, этот подсчет приводит к асимптотической оценке O(n4), такой же как и для алгоритма на основе триангуляции, описанного выше.
4. ВЫЧИСЛЕНИЕ СКЕЛЕТА МНОГОУГОЛЬНИКА ВИДИМОСТИ
Алгоритмы вычисления многоугольника видимости из заданной точки в простом многоугольнике давно и активно использовались. Ретроспектива этого вопроса изложена в [14]. До начала 1980-х годов были известны алгоритмы с вычислительной сложностью O(n2). В 1981 году H. ElGindy and D. Avis, а в 1983 году D.T. Lee получили алгоритмы сложности O(n) (см. [14]). Учитывая особенности задачи локализации робота и основываясь на подходе с использованием триангуляции, в [8] предложено вычислять многоугольник видимости и его скелет на основе триангуляции простого многоугольника. Независимо в [15] был предложен аналогичный подход и описан алгоритм вычисления многоугольника видимости на основе триангуляции для более общего случая многоугольника с «дырами». Детали реализации этих алгоритмов, конечно, отличаются. Например, в [8] использован поиск в ширину в графе, двойственном триангуляции, в то время как в [15] используется поиск в глубину.
Общая схема алгоритма построения скелета многоугольника видимости на основе триангуляции [8] такова:
1. {Локализация начальной точки в триангуляции} Найти треугольник, который содержит заданную точку p, используя кэширование триангуляции.
2. С помощью поиска в ширину совершить обход структуры триангуляции (фактически обход графа, двойственного триангуляции), основываясь на отношении смежности треугольников. В результате в массив Vзаписать вершины видимых из точкиp треугольников.
3. Сортировать массив вершин V по возрастанию их номеров в исходном многоугольнике. Массив V будет содержать скелет многоугольника видимости. Наличие соседних номеров у соседних вершин скелета означает, что они связаны ребром исходного многоугольника.
Псевдокод построения скелета многоугольника видимости на основе триангуляции (см. листинг 1).
На рис. 7 показан процесс построения скелета многоугольника видимости данным алгоритмом для трех вариантов расположения точки p: в точках pp p2 и p3. Стрелками показаны переходы из треугольника в треугольник при обследовании (обходе) триангуляции в ширину. Основной операцией здесь является определение видимости некоторой точки треугольника из заданной точки p. Для этого при обходе треугольников
Рис. 7. Примеры построения скелетов многоугольников видимости в точках рр р2 и р3 на основе триангуляции. Стрелками показаны переходы из треугольника в треугольник при обследовании (обходе) триангуляции в ширину
Листинг 1
CTriangle Triangles[n];// структура триангуляции (массив структур) // Вход: структура триангуляции Triangles[n], точка p. // Выход: массив V вершин скелета многоугольника видимости sl1=1; // sl1 - число обследуемых треугольников в очереди d1[1] = номер треугольника, содержащего заданную точку p; Записать вершины треугольника d1[1] в массив V;
Пометить все треугольники, кроме первого, как новые (не посещенные); do {
sl2=0; // sl2 - размер новой порции смежных треугольников for (int i=1;i<=sl1;i++) { for (int j=0;j<3;j++)
if (не помечен треугольник j, смежный с треугольником d1[i]) { Пометить треугольник j;
if (третья вершина треугольника j видима от точки p) { Записать вершину в массив V; sl2++;
d2[sl2]=Triangles[d1[i]].get_Triangles(j);
}
else if (p видит какую-либо точку в треугольнике) { sl2++;
d2[sl2]=Triangles[d1[i]].get_Triangles(j);
}
}
}
sl1=sl2;
for (i=1;i<=sl2;i++) d1[i]=d2[i];// новая порция записана в очередь } while (sl1!=0);
Отсортировать массив V по возрастанию номеров вершин; //Массив V содержит скелет многоугольника видимости
Рис. 8. Пример генерации карты. Здесь размер карты п = 746, а число гипотез k = 7 для любого из обозначенных положений робота
формируется сектор видимости из точки p. Этот сектор ограничен углами, под которыми видны вершины триангуляции, такие, что ребра (стороны многоугольника), концами которых они являются, заслоняют точки, расположенные за ними относительно точки p.
Для анализа эффективности предложенного алгоритма проводилось экспериментальное исследование, в котором генерировались карты (простые многоугольники) различных типов, для каждой карты выбирались точки, относительно которых вычислялись скелеты многоугольников видимости. Один из вариантов карты (при n = 746) приведен на рис. 8.
При усреднении по выборке, например, из 11 случайных точек на этой карте были получены следующие соотношения времен работы алгоритмов. При этом наряду с предлагаемым алгоритмом для сравнения рассматривались и другие алгоритмы. Так, например, промежуточный алгоритм (с упрощенным вычислением скелета), который также использовал обход триангуляции, но при анализе видимости вершин треугольников перебирал, вообще говоря, все ребра многоугольника, требовал времени больше в 26 раз, а «наивный» алгоритм, имеющий сложность O(n2), работал примерно в 81 раз дольше. Интересно отметить, что попытка ускорить работу «наивного» алгоритма за счет параллельной реализации на графическом процессоре в технологии CUDA [16] приводила к ускорению работы в 4 раза, но такая реализация была, в свою очередь, в 20 раз медленнее, чем предлагаемый алгоритм на основе триангуляции. Величины аналогичного порядка были получены в результате экспериментов на других конфигурациях карты.
5. ПЕРЕСЕЧЕНИЕ МНОГОУГОЛЬНИКОВ
В [6, 17] описан алгоритм триангуляции пересечения простых многоугольников, действующий по следующей схеме (на примере двух многоугольников): сначала объединяются множества рёбер многоугольников и строится триангуляция Делоне на объединённом множестве вершин с ограничениями (в виде объединённого множества рёбер), затем происходит разметка треугольников полученной триангуляции по принадлежности к исходным многоугольникам, после чего строится пересечение и его триангуляция. Основной вклад в сложность этого алгоритма даёт процедура триангуляции Делоне с ограничениями — O(n log n).
Рассмотрим другую схему. Пусть сначала строятся триангуляции двух исходных простых многоугольников. Затем путём обхода и сравнения этих триангуляций строится пересечение многоугольников и вычисляется триангуляция этого пересечения. Существенно, что в ЗЛМР нет необходимости строить пересечение карт полностью, требуется лишь та его связная компонента, в которой находится начальное местоположение робота (как и в разделе 2, эту компоненту назовём фрагментом пересечения). Вклад процедуры триангуляции простого многоугольника в сложность этого алгоритма есть O(n), и эта схема может быть более эффективной в ЗЛМР.
Итак, предлагается алгоритм построения фрагмента пересечения двух многоугольников на основе их триангуляций, учитывающий особенности пересечения многоугольников в задаче локализации робота. Первое действие - триангулирование простых многоугольников, пересечение которых требуется найти.
Помимо результатов триангуляции, алгоритму в качестве входных данных требуются две стартовые вершины — первого и второго многоугольника. Эти вершины должны совпадать и входить в состав полигонального фрагмента пересечения, который будет построен (такими вершинами будут соответствующие вершины двух гипотез — карты и сдвинутой карты).
Триангуляции, используемые алгоритмом, представлены структурой триангуляции в формате «Узлы и треугольники», кратко описанной в разделе 3. Вершины треугольника перечисляются против часовой стрелки. Массив входов по рёбрам содержит для каждого ребра указатель на треугольник, содержащий это ребро (на самом деле в алгоритме построения
пересечения такие указатели нужны только для сторон многоугольника). Вершины исходных многоугольников перечисляются против часовой стрелки.
Далее для краткости те рёбра триангуляции, которые являются сторонами исходного многоугольника, будем называть структурными рёбрами. Структурные рёбра являются направленными, то есть ориентированными в направлении обхода границы многоугольника против часовой стрелки. Прочие рёбра триангуляции назовём, соответственно, неструктурными. Алгоритм построения пересечения триангулированных многоугольников состоит в движении по структурным рёбрам «верхней» триангуляции над треугольниками «нижней» триангуляции, при этом триангуляции меняются ролями в определённых точках. По мере движения вычисляются вершины искомого фрагмента-пересечения. Алгоритм завершится, когда в качестве очередной вершины будет получена стартовая точка.
Переход от одной точки к другой называется шагом. Не всякая новая точка является вершиной фрагмента-пересечения: она может лежать и на его ребре. Шаг содержит два действия:
1. Подготовка к переходу - определение верхней и нижней триангуляции (выполняется обмен ролями, если требуется); определение луча (проходящего по ребру верхней триангуляции, которое назовём переходным), направленного на новую точку из текущей точки; определение ребра или треугольника нижней триангуляции, в котором следует искать новую точку.
2. Переход — вычисление новой точки, а также определение треугольника, в котором оказалась новая точка, и, возможно, того ребра нижней триангуляции, на которое она попала. Эти треугольник и ребро назовём треугольником и ребром попадания.
Каждая точка, полученная в результате перехода, соответствует одному из следующих случаев:
1. Вершина верхней триангуляции, лежащая внутри треугольника нижней триангуляции.
2. Точка пересечения с ребром нижней триангуляции, не совпадающая с вершиной нижней триангуляции.
2.1. Вершина верхней триангуляции, лежащая на ребре нижней триангуляции (и не совпадающая с его концами):
Рис. 9. Построение пересечения VGHWIJQREN многоугольников ABCDEF (1) и VGHWIJKLMN (2) по их триангуляциям
2.1.1. На неструктурном ребре.
2.1.2. На структурном ребре.
2.2. Точка пересечения переходного ребра с ребром нижней триангуляции (не совпадающая с концами пересекаемых рёбер):
2.2.1. С неструктурным ребром.
2.2.2. Со структурным ребром.
3. Точка пересечения совпадает с вершиной нижней триангуляции.
3.1. Точка пересечения совпадает также с вершиной верхней триангуляции (концом переходного ребра).
3.2. Точка пересечения лежит на переходном ребре, не совпадая с его концами.
Примеры всех случаев есть на рисунке 2, на котором изображено построение пересечения многоугольников ABCDEF (1) и VGHWIJKLMN (2) по их триангуляциям.
Пусть стартовая пара вершин — А = V (другим вариантом могла бы быть пара С = W). Верхней триангуляцией сначала станет триангуляция 2 (то есть многоугольника 2). Построение выполняется за 14 шагов: (случай 1), 2^Н (2.1.1), 3-HW (3.1), 4^1 (1), 5-1Р (2.2.1), 6^ (2.1.2), 74К (2.1.2), 8-^ (2.2.2, верхней стала триангуляция 1), 9^ (2.2.2, верхней стала триангуляция 2), 10-Н5 (2.2.1, верхней стала триангуляция 1), 11^Е (1), 12-ЕТ (2.2.1), 13-ТК (3.2), 14-№У (3.1, верхней стала триангуляция 2).
Действия, выполняемые при подготовке к переходу, зависят от случая текущей точки. Используются треугольник и ребро попадания, определённые при предыдущем переходе.
Алгоритм начинается со стартовой точки, и считается, что она была получена переходом типа 3.1. Обмен ролей триангуляций может происходить, только если полученная на предыдущем переходе точка относится к случаю 2.2.2, 2.1.2, 3.1 или 3.2, причём в случае 2.2.2 обмен происходит всегда. В случаях 2.1.2, 3.1 и 3.2 верхней становится та триангуляция, чьё структурное ребро выбирается в качестве очередного переходного ребра. При этом из двух возможных рёбер переходным ребром следует выбрать ребро с максимальным углом поворота налево относительно направления обхода. Если оба ребра реализуют поворот направо, то выбрать ребро с минимальным углом правого поворота. Если направления рёбер совпадают, то следует выбрать ребро верхней триангуляции.
Сложность алгоритма в худшем случае есть 0(п2). Для пояснения рассмотрим пример многоугольника, изображённого на рис. 10. При вычислении фрагмента пересечения с учё-
Рис. 10. Пример карты: вверху — схема карты из повторяющихся фрагментов (включая фрагменты «гармошка» и «вилка»), внизу — детализация части схемы. Показаны также начальное положение робота р и положения, соответствующие гипотезам локализации
том гипотетического положения робота p при работе алгоритма будет происходить следующее. Поскольку фрагменты «гармошка» и «вилка» будут накладываться друг на друга (см. рис. 11), то в процессе прохода по сторонам (структурным рёбрам) фрагмента «вилка» будут многократно пересекаться рёбра триангуляции фрагмента «гармошка». Если число и тех и других рёбер будет O(n), то число таких пересечений и соответствующих действий будет O(n2).
Отметим, что ситуации, подобные приведённому примеру, в большинстве конфигураций карт, характерных для ЗЛМР, либо не возникают, либо соответствующие гипотезы быстро отсекаются. Поэтому алгоритм построения пересечения на основе триангуляции должен быть эффективен: в построении будут использоваться только те треугольники триангуляций, которые содержат точки фрагмента пересечения, и в среднем это даст сложность О(п).
6. ЭКСПЕРИМЕНТАЛЬНОЕ СРАВНЕНИЕ АЛГОРИТМОВ ЛОКАЛИЗАЦИИ
Для анализа эффективности предложенных алгоритмов проводилось экспериментальное исследование, основанное на их программной реализации (Visual C++ 2010), а также реализациях алгоритмов [2, 3, 4] и сравнении таких их характеристик, как величина суммарного пути перемещения робота и время работы алгоритма локализации. Ранее было установлено [18], что алгоритм [3], использующий рандомизацию при проверке гипотез, более эффективен, чем алгоритм [2], использующий декомпозицию карты на ячейки видимости и имеющий асимптотическую сложность O(n5log п), а также более эффективен, чем алгоритм [4], основанный на решении полугрупповой задачи Штейнера и имеющий вычислительную сложность Q(n12). Поэтому далее приводится сравнение именно с алгоритмом [3]. На алгоритмы, участвовавшие в эксперименте, будем ссылаться, обозначив их для краткости римскими цифрами:
I — алгоритм локализации мобильного робота (АЛМР) с использованием триангуляции карты;
II — АЛМР с использованием рандомизации при проверке гипотез [3] (число точек, случайным образом размещаемых в исследуемой области X = 100);
III — тот же АЛМР [3], но число точек, случайным образом размещаемых в исследуемой области X = 500;
IV — АЛМР с использованием окон в многоугольнике карты.
При проведении эксперимента использовалась генерация карт различных модельных типов. Генерация осуществлялась по нескольким задаваемым шаблонам: первый из них задает общую укрупненную схему карты в виде регулярного дерева, вершины которого соответствуют фрагментам карты «комната» или «коридор», для которых, в свою очередь, задаются свои шаблоны. Сочетание параметров, задающих размеры шаблонов, определяет общий размер карты п, и при такой генерации его числовое значение можно регулировать, как правило, лишь приближенно. На рис. 8 приведен пример генерации карты размера п = 746 и с числом гипотез k = 7 при любом из обозначенных на рисунке начальных положений pt (i = 1..7).
Предварительный анализ показал, что полученные характеристики алгоритмов (длина пути d, пройденного роботом до окончания локализации, и время t работы алгоритма ло-
Рис. 11. Триангулированные фрагменты карты «гармошка» и «вилка» и их наложение
я У
'В
СЯ
е Й
и" <о и К
Я
и
1,6 1,5 1,4 1,3 1,2 1,1 1
0,9 0,8
IV Ч Л
I
- » \Х'\\
V \ •
л. \ .....
1 . / \ ^
.............у \ ч №
*. N
1 1
й
©
в?
е.
ч
и £ ГС а
О
160 140 120 100 80 60 40 20 О
170
370
570
770
170
370
570
770
Рис. 12. Зависимость средних значений для отношений длин путей
кализации) существенно зависят от начального расположения робота (от номера гипотезы). По этой причине целесообразно усреднять по гипотезам не сами характеристики, полученные для разных алгоритмов, а их отношения, которые и будут характеризовать сравнительную эффективность алгоритмов. Выберем характеристики алгоритма с триангуляцией (номер алгоритма I) в качестве «эталона» для сравнения. То есть для алгоритмов (с номером а) будут вычислены (здесь 1 - номер гипотезы)
у(а) =-
х(1)
, где х = d или х = t.
у(а) = I у* у(а)
, /—II =1 1
Рис. 13. Зависимость средних значений для отношений времени работы алгоритмов
Усреднением по к гипотезам получим
При этом 1) =1 и У(1) = 1.
Рис. 14. Зависимость средних значений для отношений времени работы алгоритмов (укрупненный вид)
к'
В табл. 3 приведены значения отношений у (а) для конфигурации, изображенной на рис. 8.
Алгоритм I на основе триангуляции показывает здесь значительно лучшее время, но при этом дает несколько большую длину пути, чем алгоритм с рандомизацией в варианте III, и чем лучший по этому показателю алгоритм IV. Аналогичные соотношения получены и на других конфигурациях карты.
На рис. 12 и 13 приведены графики зависимостей средних значений указанных отношений от размера многоугольника карты п. Различные типы линий графиков на рис. 12-13 относятся к алгоритмам с соответствующими номерами.
На рис. 14 показана увеличенная часть рис. 13 (данные алгоритма III находятся выше на графике и не попадают в область рис. 14).
Табл. 3
Номер алгоритма
Характеристика (х) I II III IV
Длина пути 1 1,16 0,98 0,85
Время локализации 1 41,9 117,1 17,5
Данные на этих рисунках показывают (более наглядно на рис. 14), что в приведенном диапазоне значений размера карты п алгоритм I показывает значительно меньшее время работы, а вторым по времени работы оказывается алгоритм IV.
ЗАКЛЮЧЕНИЕ
Проведенные эксперименты с другими модельными конфигурациями карты показали аналогичные результаты и позволяют сделать вывод, что рассмотренные приближенные алгоритмы ЛМР обеспечивают сравнимую точность, но время работы двух алгоритмов ЛМР с использованием триангуляции карты и с использованием окон в многоугольнике карты меньше, чем время работы других. При этом лучшие по времени результаты показывает алгоритм на основе триангуляции карты.
Однако и время работы, и асимптотическая сложность O(n4) предложенных алгоритмов для многих практических задач все-таки велики. Их уменьшения можно ожидать за счет оптимизации наиболее трудоемких шагов алгоритма (например вычисления кратчайшего пути для перемещения робота из текущего положения в новое), а также за счет их реализации на параллельных структурах, например, с использованием графических ускорителей и гетерогенных вычислительных структур [16], [19], [20]. Такая перспектива позволяет рассматривать оба предложенных алгоритма, и это соображение основано на наличии эффективного параллельного алгоритма триангуляции [21].
Литература
1. Dudek G., Jenkin M. Computational Principles of Mobile Robotics, Cambrigde University Press, 2rd rev. ed, 2010.
2. Dudek G., RomanikK., Whitesides S. Localizing a robot with minimum travel// SIAM J. Comput, 1998. Vol. 27. P. 583-604.
3. RaoM., DudekG., WhitesidesS. Randomized algorithms for minimum distance localization// Internat. J. Robotics Research. 2007. Vol. 26. P. 917-934.
4. Koenig S., Mitchell J.S.B., Mudgal A., Tovey C. A near-tight approximation algorithm for the robot localization problem// SIAM J. Comput. 2009. Vol. 39. P. 461-490.
5. GuibasL.J., MotwaniR., Raghavan P. The robot localization problem// SIAM J. Comput, 1997. Vol. 26. P. 1120-1138.
6. СкворцовА.В. Триангуляция Делоне и её применение. Томск: Изд-во Том. ун-та, 2002.
7. Дао Зуй Нам, Ивановский С.А. Приближенный алгоритм локализации мобильного робота с использованием окон в многоугольнике карты // Известия СПбГЭТУ «ЛЭТИ», 2014. № 3. С. 38-43.
8. Дао Зуй Нам, Ивановский С.А. Приближённые алгоритмы локализации мобильного робота// Научный вестик НГТУ, 2014. № 2. С. 109-121.
9. Hershberger J., Snoeyink J. Computing minimum length paths of a given homotopy class // Computational Geometry. Theory and Applications, 1994. № 4. P. 63-98.
10. De Berg M., Cheong O., Van Kreveld M., Overmars M. Computational Geometry: Algorithms and Applications, Berlin, Heidelberg: Springer-Verlag, 3rd rev. ed., 2008.
11. Chazelle B. Triangulating a simple polygon in linear time // Discrete & Computational Geometry, SpringerVerlag, 1991. № 6(1). P. 485-524,
12. SeidelR. A simple and fast incremental randomized algorithm for computing trapezoidal decompositions and for triangulating polygons // Computational Geometry. Theory and Applications, 1991. № 1(1). P. 51-54.
13. Koenig S., Tovey C. Localization: Approximation and Performance Bounds to Minimize Travel Distance // IEEE Transactions On Robotics, 2009. Vol. 26. P. 320-330.
14. Ghosh S.K. Visibility algorithms in the plane. Cambridge University Press, 2007.
15. Bungiu F., Hemmer M., Hershberger J., Huang K., Kroller A. Efficient computation of visibility polygons // EuroCG, 2014.
16. Сандерс Дж., Кэндрот Э. Технология CUDA в примерах: введение в программирование графических процессоров. М.: ДМК Пресс, 2011.
17. Скворцов А.В. Построение объединения, пересечения и разности произвольных многоугольников в среднем за линейное время с помощью триангуляции // Вычислительные методы и программирование, 2002. Т. 3. С. 116-123.
18. Дао Зуй Нам, Ивановский С.А. Экспериментальный анализ алгоритмов локализации мобильного робота // Известия СПбГЭТУ «ЛЭТИ», 2014. № 1. С. 19-24.
19. ФирсовМ.А., Ивановский С.А. Параллельная реализация алгоритма построения пересечения простых полигонов с использованием технологии CUDA // Известия СПбГЭТУ «ЛЭТИ», 2013. № 9. С. 29-34.
20. Фирсов М.А. Сравнение параллельных реализаций алгоритма пересечения полигонов на многоядерном и на графическом процессорах // 67-я науч.-техн. конф. профессорско-преподавательского состава СПбГЭТУ «ЛЭТИ» 2014: тез. докл. СПб., 27 янв.-3 февр. 2014. СПб.: Изд-во СПбГЭТУ «ЛЭТИ», 2014. С. 103-107.
21. QiM., Cao T.-T., Tan T.-S. Computing 2D Constrained Delaunay Triangulation Using Graphics Hardware / School of Computing; National University of Singapore // Technical Report # TRB3/11, March 2011.
USING TRIANGULATION OF POLYGONS IN THE PROBLEM OF LOCALIZATION OF A MOBILE ROBOT
Dao Duy Nam, Firsov M. A.
Abstract
Two approximate algorithms of the robot localization problem with the map in the form of a simple polygon is considered. Hypotheses localization correspond to copies maps from assumed mark of robot's position. The first algorithm is based on a triangulation of a simple polygon as a preprocessing for the implementation of the basic operations, such as the intersection of polygons, computing visibility polygons, finding shortest paths. The second algorithm is used other than the triangulation additional concept of the window at the intersection of copies of the map. Looking» in the window, the robot cuts false hypotheses. Experimental studies of these algorithms and compared with other algorithms.
Keywords: Computational geometry, robotics, robot localization, intersection polygons, polygon triangulation, visibility polygon, skeleton of a visibility polygon, approximate algorithms.
Дао Дуй Нам,
аспирант СПбГЭТУ «ЛЭТИ»,
магистр по направлению Информатика и
вычислительная техника,
duynam81@mail.ru
Фирсов Михаил Александрович, аспирант СПбГЭТУ «ЛЭТИ», магистр по направлению Программная инженерия,
pvptm@mail.ru
(С) Наши авторы, 2014. Our authors, 2014.
