CC BY
43УДК
A.V. Erokhin, V.I. Erokhin
METHODS AND MODELS OF MULTI-ROBOT PATH-FINDING WITH SIMULTANEOUS LOCALIZATION AND MAP COMPILATION BASED ON AN ON-BOARD SCANNING DEVICE DATA
The Russian State Scientific Center for Robotics and Technical Cybernetics, Tikhorecky Pr., 21, St Petersburg, 194064, Russia
St. Petersburg State Institute of Technology (Technical University) Moskovsky Pr. 26, St Petersburg, 190013, Russia e-mail: [email protected]
The theoretical and practical aspects of navigation and multirobot path finding problems are considered. The goal of thise paper is to introduce a consistent algorithm for optimal path-finding for a group of robots and simultaneous localization with dynamic map compilation based on on-board scanning device data. Path-finding methods among which both the simplest obstacle avoidance methods and more complex path-planning algorithms originate from the graph theory and theory of artificial intelligence are analyzed. Modern methods of path-planning and navigation control from mobile robotics are analyzed. The problem of simultaneous localization and dynamic map compilation based on scanner laser rangefinder is studied and solved. The problem of multi-robot path-finding when moving toward a single target with an unknown environment with fixed obstacles is studied and solved. The developed algorithm is implemented on a computer model. The results of computational experiments with maps of varying complexity are given.
Keywords: mobile robot, simultaneous localization and mapping based on on-board scanning device data, multirobot path finding, optimal path-finding
19.7
А.В. Ерохин1, В.И. Ерохин2
МЕТОДЫ И МОДЕЛИ ГРУППОВОГО САМОУПРАВЛЕНИЯ ДВИЖЕНИЕМ КОЛЛЕКТИВА РОБОТОВ С ОДНОВРЕМЕННОЙ ЛОКАЛИЗАЦИЕЙ И ПОСТРОЕНИЕМ КАРТЫ ПРЕПЯТСТВИЙ ПО ДАННЫМ СКАНИРУЮЩИХ УСТРОЙСТВ
Центральный научно-исследовательский и опытно-конструкторский институт робототехники и технической кибернетики», Тихорецкий пр., д. 21, Санкт-Петербург, 194064, Россия
Санкт-Петербургский государственный технологический институт (технический университет), Московский пр., д. 26, Санкт-Петербург, 190013, Россия e-mail: [email protected]
Рассматриваются теоретические и практические аспекты проблем навигации и группового управления коллективом автономных мобильных устройств (роботов). Работа посвящена созданию алгоритма построения оптимальных для каждого робота путей до цели с использованием динамически создаваемой при перемещении роботов карты препятствий по данным бортовых сканирующих устройств и одновременной локализации роботов на ней. Проведен обзор методов нахождения пути к цели, включающий как простейшие методы уклонения от столкновений с препятствиями, так и более сложные алгоритмы планирования маршрутов в теории графов и искусственного интеллекта. Выполнен анализ методов управления движением, используемых в современно мобильной робототехнике. Исследованы и решены задачи динамической локализации робота и одновременного построения карты препятствий на основе данных лазерного сканирующего дальномера, а также планирования перемещения группы мобильных роботов при движении к единой цели на неизвестной карте с неподвижными препятствиями. Разработанный алгоритм реализован в рамках компьютерной модели и проверен в вычислительных экспериментах на картах препятствий разных типов сложности.
Ключевые слова: мобильный робот, одновременная локализация и уточнение карты препятствий по данным бортовых сканирующих устройств, групповое управление движением, поиск оптимального пути к цели
1 Ерохин Александр Владимирович, техник лаб. систем автономного и адаптивного управления робототехническими комплексами ЦНИИ РТК, e-mail: [email protected]
Erokhin Aleksandr V. technician of laboratory of systems of off-line and adaptive control by robotic complexes CNII RTK, e-mail: [email protected]
2 Ерохин Владимир Иванович, д-р физ.-мат. наук, зав. каф. инноватики и информационных технологий СПбГТИ(ТУ) e-mail: [email protected] Erokhin Vladimir I., Dr Sci., (Phys - a mat), Professor, Heat of Department of Innovatics and Information Technologies,SPbGTI(TU), e-mail: [email protected]
Дата поступления - 15 сентября 2014 года Received September, 15, 2014
Введение
Навигация и управление автономными мобильными устройствами (роботами) - общепризнанные фундаментальные проблемы робототехники, находящиеся в стадии интенсивных исследований [1, 2], акценты которых в последнее десятилетие смещаются к методам и алгоритмам планирования движения группы мобильных роботов (группового управления). Причина - потенциал группы выше потенциала отдельного мобильного устройства, что особенно важно для задач, с которыми единственное устройство не в состоянии справиться или справляется неэффективно [3, 4].
Задачи навигации и управления (в том числе - группового) автономными мобильными устройствами могут быть сформулированы в большом количестве различных постановок, а также различным образом декомпозированы на вспомогательные подзадачи. В настоящей статье уделено особое внимание одной из них, также является фундаментальной проблемой робототехники [5] - нахождению пути к цели с учетом заранее неизвестных (не обозначенных на карте) препятствий.
В промышленной и военной робототехнике описанная проблема возникает при покрытии площади для уборки территории супермаркетов или вокзалов, доставке грузов внутри складских помещений, планетарных исследованиях и добыче образцов породы, нахождении радиоактивных или химических загрязнений и др. [6]. Кроме того, поиск пути к цели необходим не только в робототехнике, но и, например, при разработке компьютерных игр, исследовании сетей и в сервисах навигации [7].
Дискретизация координат и другие допущения
За исключением сравнительно редко встречающихся особых случаев реальная среда (время и пространственные координаты) для задач навигации и управления мобильными устройствами является непрерывной. В то же время, модельная среда (время и пространственные координаты, использующиеся в математических моделях и алгоритмах задач навигации и управления) может быть как непрерывной так и дискретной. В дальнейшем будет использовано дискретное представление координат. Примем, что снятие (сканирование) координат, а также их обработка, осуществляются в дискретные моменты времени. Таким образом, по формальным признакам все изложенные в работе методы и алгоритмы можно отнести к так называемым методам навигации на сетке [8].
Дискретное (сеточное) представление области пространства допускает важную для последующего изложения интерпретацию в виде неориентированного графа, вершинами (узлами) которого являются ячейки дискретной сетки, а дуги - соединяют соседние ячейки. Если область пространства является плоской (двумерной), взаимное соответствие её сеточного представления и представления в виде неориентированного графа легко проиллюстрировать графически, например, так, как это сделано на приведенном ниже рисунке 1.
Рисунок 1. Взаимное соответствие двух представлений плоскости координат мобильного устройства: (а) - набор ячеек (сетка), (б) - неориентированный граф
Описание всех задач и алгоритмов управления и навигации, представленных в настоящей статье, выполнено для плоского (двумерного) случая. Определенным образом организованную и доступную бортовому вычислительному устройству робота информацию о проходимых и непроходимых участках данной области, указанную с точностью до ячейки сетки (вершины графа), будем называть картой местности, картой препятствий или просто картой. Неизвестной картой будем называть пустую карту (не содержащую информацию об имеющихся в реальности препятствиях). Ячейку карты, принадлежащую препятствию, будем называть непроходимой, в противном случае - проходимой.
В рамках статьи все препятствия предполагаются статическими (неподвижными).
Обзор алгоритмов нахождения пути к цели с учетом заранее неизвестных препятствий
Алгоритмы, не имеющие жесткой привязки к сеточному представлению пространственных координат
Простейшим подходом при организации движения мобильного устройства при наличии препятствий является игнорирование препятствий до контакта с ними и движение в соответствии со стратегией преодоления препятствия - после столкновения. Семейство алгоритмов, реализующих указанных подход, получило название алгоритмов нахождения пути в движении [7].
Семейство алгоритмов BUG. Алгоритмы семейства BUG получили широкое распространение и большое количество модификаций: Algl, Al2, DistBug, Classl, Revl, Rev2, OneBug, LeaveBug и TangentBug и др. [9]. Их популярность обусловлена простотой и дешевизной необходимого оборудования. Так, для работы оригинального алгоритма BUG на роботе должен быть установлен лишь датчик столкновений, а требуемая память бортового вычислителя оставляет несколько машинных слов [10].
Рассмотрим некоторые алгоритмы этого семейства.
Оригинальный алгоритм BUG, а точнее две его модификации Bugl и Bug2 были предложены Владимиром Люмельски и Александром Степановым в 1987 г. [10]. Суть алгоритма Bugl заключается в том, что мобильный робот движется непосредственно по направлению к цели до тех пор, пока не встретит препятствие. Далее роботом исследуются контуры препятствия до тех пор, пока путь движения к цели не будет вновь доступен. Как только датчик столкновения фиксирует наличие препятствия, робот начинает движение вдоль границы препятствия по направлению против часовой стрелки для определения точки выхода. Точкой выхода является ближайшая к цели точка границы препятствия. После достижения точки выхода робот продолжает движение в направлении цели пока не достигнет ее или не будет встречено новое препятствие. Во втором случае повторяется процедура, описанная выше.
Метод имеет низкую эффективность, но гарантирует достижение роботом любой доступной точки назначения [9].
Алгоритм Bug2, в котором мобильный робот движется вдоль прямой, соединяющей стартовую позицию S и точку назначения Т является «жадным» [11]. Движение вдоль указанной прямой продолжается до тех пор, пока не робот достигнет препятствия. После того, как датчиками робота обнаружено препятствие, дальнейшее движение осуществляется вдоль границ этого препятствия, пока не будет достигнута начальная прямая.
Каждый из алгоритмов Bug1 и Bug2 реализует достаточно ясную и простую идею, каждый имеет достоинства и недостатки. Например, алгоритм Bug1 никогда не создает локальных циклов, но для поиска точки выхода всегда будет исследован весь периметр препятствия.
Алгоритм Bug2, в свою очередь, более «интуитивен» и использует более простые решения, однако может оказаться менее эффективным в сложных случаях.
Оригинальные алгоритмы Bug1 и Bug2 имеют ряд существенных ограничений:
- Границы препятствий должны представлять собой замкнутые кривые, такие, что прямая может пересечь каждую такую кривую конечное число раз.
- Препятствия не могу касаться друг друга, т.е. точка, принадлежащая одному препятствию, не может так же принадлежать любому другому препятствию.
- Мобильный робот рассматривается как материальная точка, или, что эквивалентно, проход между препятствиями считается проходимым независимо от ширины.
- Сенсоры робота кроме факта контакта с препятствием так же должны предоставлять информацию о текущих координатах робота.
Большинство последующих модификаций алгоритма Bug направлены на преодоление обозначенных ограничений ценой по возможности минимального усложнения логики алгоритма.
Алгоритм Dist-Bug [12] отличается от классического Bug1 условием определения точки выхода, а именно: во время движения вдоль препятствия одновременно рассчитывается расстояние до цели в каждой точке пройденного пути, а точкой выхода считается точка с минимумом рассчитанного расстояния.
Алгоритм DH-Bug [13] имеет в своей структуре «совещательный» (deliberative) и «адаптивный» (reactive) слои. Совещательный слой несет ответственность за создание предварительной оценки пути, основанной на неполной информации, а адаптивный слой отвечает за реакцию системы на появление неопределенностей. Для сокращения длины пути алгоритмом DH-Bug допускается движение робота в каждом из возможных направлений, отклоняющихся от направления на цель не более чем на 90 Данная тактика откладывает обход препятствия вдоль его границ в классическом алгоритме Bug2, являющийся главной причиной генерации длинных путей. А если движение вдоль границ препятствия уже началось, оно раньше прекратится.
Алгоритм DH-Bug позволяет также решать задачу обхода движущихся препятствий. Однако рассмотрение этой задачи выходит за рамки статьи.
В оригинальном алгоритме Bug предполагается, что датчик (сенсор), способен фиксировать только столкновения с препятствиями. Однако мобильное устройство можно оснастить более информативным сенсором, например, дальномером. Имея эту дополнительную информацию, можно предпринимать попытки создания более эффективных алгоритмов построения пути к цели. Алгоритм TangentBug [14] является результатом одной из первых подобных попыток.
TangentBug использует дополнительную структуру данных - так называемый граф локальных касательных (LTG) к границам препятствий, «видимых» в данный момент времени дальномером. При этом TangentBug, также как и оригинальный Bug, использует два варианта движения мобильного устройства: по направлению к цели и вдоль препятствия. На каждом шаге работы алгоритма (в каждый дискретный момент времени) роботом строится LTG, на основе которого планируется следующий шаг. В процессе движения по направлению к цели робот перемещается в локально оптимальном, согласно LTG, направлении.
Наиболее значимые модификации алгоритма Bug, существенно расширяющие его эффективность, были реализованы в алгоритме CBUG [15]. В логической структуре алгоритма учтен размер (максимальная ширина плоского тела произвольной формы) D > 0 мобильного работа и решена задача оптимизации генерируемого пути.
Сущность алгоритма заключается в следующем. По заданным координатам точек старта S, цели T стоит-ся эллипс с фокусами в точках S, T и площадью A0. Поиск цели осуществляется алгоритмом Bug1 внутри указанного эллипса, причем его границы рассматриваются в качестве виртуального препятствия. Если цель достигнута - работа алгоритма завершается. Иначе поиск повторяется для эллипсов, площади которых составляют 2A для i=1, 2, ... до тех пор, пока цель не будет достигнута или определено, что она недостижима из S. Расширяющиеся эллипсы на определенном этапе обязательно заключат внутри себя путь до цели или препятствие, полностью окружающее цель. Таким образом, в результате работы CBUG всегда будет найден путь до цели, если он существует, или определено, что цель недостижима. Для работы CBUG, как и для работы классического алгоритма BUG, требуется фиксированное количество памяти, вне зависимости от сложности карты. В отличие от оригинального BUG, алгоритму требуется также хранить точку старта S и текущую область поиска.
Еще более интересен алгоритм MRBUG [6], использующий следующие допущения:
1) Каждый робот представляет свободно перемещающееся плоское тело размером D > 0 (например, диск, при этом D может расцениваться как радиус этого диска).
2) Каждый робот оснащен двумя идеальными (безошибочными) датчиками. Первый предназначен для измерения абсолютного положения робота, второй - для обнаружения препятствий, как контактным, так и дальномером ближнего действия, позволяющим определять границы препятствия.
3) Роботы имеют бортовой вычислитель с памятью, достаточной для выполнения операций алгоритма;
4) Роботы имеют возможность передачи информации друг другу или базовой станции.
5) Скорость движения всех роботов одинакова.
В работе алгоритма MRBUG используется вспомогательный алгоритм PBUG1, являющийся модификацией алгоритма Bug1 для пары роботов.
Пара осуществляет совместное прямолинейное движение к цели до тех пор, пока не будет обнаружено препятствие. В каждой точке встречи роботов с препятствием H' роботы разделяются: робот Rl поворачивает налево, а робот Rr - направо, таким образом, пара роботов осуществляет обход препятствия с разных сторон. Во время своего движения каждый робот рассчитывает расстояние до цели и запоминает положение, из которого расстояние минимально. Когда роботы, обогнув препятствие, встретятся, происходит обмен данными и сравнение точек с минимальными расстояниями до цели. Точкой выхода L1 становится та, из которой путь до цели минимален. Пара роботов осуществляет перемещение в заданную точку и процедура повторяется, пока цель не будет достигнута.
Алгоритм MRBUG запускает уже не одну, а n пар роботов из общей точки старта S и привязывает каждую пару j' к собственному эллипсу с фокальными точками в S и T - точке назначения. Первая пара роботов (j' = 1) привязана к эллипсу с площадью A0, а каждая следующая пара роботов выполняет поиск внутри эллипсов, площади которых больше A0 в a > 0 раз, т.е. площади эллипсов составляют A0, A0a, A0a2. Процедура поиска осуществляется согласно алгоритму PBUG1, при этом границы эллипсов являются виртуальными препятствиями. Каждая пара повторяет процедуру PBUG1 на незанятых эллипсах, пока цель не будет достигнута.
В методе искусственного потенциального поля [16] робот рассматривается как материальная точка, движущаяся в потенциальном поле сил двух типов: отталкивания от препятствий и притяжения к цели. Потенциал поля имеет вид Uarl (x) = U (x) +UO(x), где Xd - целевая точка, Uo, Uxd - потенциалы «создаваемые» препятствиями и целью. F = — grad (Uat) - сила, действующая на «ма-
термальную точку» (мобильное устройство). Путь к цели прокладывается по направлению, задаваемым вектором F, т. е. по направлению наискорейшего уменьшения потенциала поля.
Преимущество метода заключается в том, что он строит путь к цели в обход препятствий с минимальными вычислительными затратами. Полученный путь может привести в локальный минимум потенциала поля и задача навигации не будет решена. Для преодоления указанной трудности предлагаются различные решения [17].
Алгоритмы, существенно использующие сеточное представление пространственных координат и формулируемые в терминах теории графов
Алгоритм «поиска в ширину». В настоящее время термин «Поиск в ширину» прочно ассоциируется с классическим алгоритмом обхода вершин графа [11], а сама проблема обхода вершин графа имеет много различных интерпретаций. При этом начинают забываться первоначальные постановки данной проблемы (виде задачи поиска кратчайшего пути в лабиринте [18] и задачи разводки проводников на печатных платах [19]), имеющие наиболее близкое отношение к теме настоящей статьи.
Поиск в ширину заключается в следующем. Для заданного графа, начиная со стартовой вершины, исследуются (проходятся) все соседние узлы (т.е. узлы, соединенные ребрами с исходной вершиной). Затем процедура повторяется для каждой из найденных вершин и так далее, пока цель (целевая вершина) не будет достигнута. Как правило, при реализации алгоритма используется структура под названием список открытых узлов [20]. В него помещаются все не пройденные соседние узлы, которые исследуются, пока не будет найдена целевая вершина или в списке не останется узлов.
Адаптация алгоритма поиска в ширину для решения задачи поиска пути к цели при использовании сеточного представления пространственных координат сводится к сопоставлению вершин неориентированного графа ячейкам области физических координат (см. рис. 1.1). Иллюстрация работы алгоритма поиска в ширину представлена на рисунке 2.
\ \ \ \ \ / / / / / /
\ \ \ \ \ / / / / X
N \ \ \ \ Ч/ X / / X \
\ \ \ \ \ \/ / / А —\ \ \
\ \ \ \ \ \/ / X Ч \ \
N \ \ \ \ \/ \ \ \ \
\ \ \ \ ^ • -х \ ■ ч •
\ \ \ У 7 ¥ \ ■ ■ I1 \
\ \ У / 3 >
\ У / / / / /\ ■
У / / X / / / ¥ г
У / / У / У / 1 Ь
Кроме классического поиска в ширину существует двунаправленный поиск в ширину, в котором две процедуры оригинального алгоритма стартуют с начальной и целевой позиций. Алгоритм завершается, когда одна из процедур поместит в список открытых узлов узлы, участвующие в другой процедуре (рисунок 3).
/ / \ \ \ / / /
\ \ ч / / / \ \ / / А
\ \ А \ \ \ У Л ~Ч \
\ к |\ * -ч \ \
V / \ к ■ 7 \ Ч
Л V У 1 ¡ч / / / \ Л\ \
/ К' / / / ч
Рисунок 3. Иллюстрация работы алгоритма двунаправленного поиска в ширину: • - стартовая ячейка, целевая ячейка, □ - проходимая ячейка, ■ - непроходимая ячейка (препятствие), _ путь от стартовой ячейки к целевой ячейке, построенный алгоритмом, _ «цепочка» ячеек, последовательно проверенная алгоритмом
Описанный подход способен сэкономить до 50 % времени по сравнению с оригинальным алгоритмом [7].
Алгоритм Дейкстры разрабатывался для решения задачи нахождения кратчайших путей из заданной вершины во все остальные на взвешенном ориентированном графе с известными неотрицательными весами ребер V) между вершинами и и V [11]. Кратчайший путь - путь в графе (последовательность соответствующих вершин и дуг), обладающий минимальным весом. Вес пути - сумма весов составляющих его дуг.
Алгоритм Дейкстры в может быть адаптирован для нахождения кратчайшего пути из стартовой ячейки в целевую ячейку на карте с сеточным представлением при соответствующей модификации его логической структуры и (см. рисунок 4) назначении весов ребер графа соответствующим ячейкам сетки, интерпретируя их как цену занятия ячейки.
Рисунок 2. Иллюстрация работы алгоритма поиска в ширину:
• - стартовая ячейка, •- целевая ячейка, □ - проходимая ячейка, ■ - непроходимая ячейка (препятствие), _ путь от стартовой ячейки к целевой ячейке, построенный алгоритмом, _ «цепочка» ячеек, последовательно проверенная алгоритмом
Рисунок 4. Пример взаимного соответствия между взвешенным направленным графом и сеточным представлением области координат мобильного устройства: (а) - взвешенный направленный граф, - веса ребер, (б) - карта с сеточным представлением, - цена перехода из соседней ячейки в ячейку с номером
Пусть S - множество вершин, для которых уже известны кратчайшие пути из стартовой вершины s, V - множество всех вершин графа. В начале работы алгоритма S = 0, VveF|v^s^v.d = х (вес кратчайшего пути от вершины s к любой другой вершине v eV равен бесконечности), Vv e V ^ v.n = NIL (в кратчайшем пути от вершины s к любой другой вершине v eV у вершины v нет предшественника).
Суть алгоритма в исходной постановке (в терминах теории графов) заключается в следующем: поочередно выбирается вершина u eV \ S, которой соответствует минимальная оценка кратчайшего пути. После добавления вершины u во множество S, осуществляется ослабление всех исходящих из нее ребер. При этом оценки кратчайших путей могут быть уменьшены.
Процедура ослабления ребра (u, v) заключается в проверке возможности уменьшить вес кратчайшего пути к вершине v, проведя его через вершину u, и в обновлении атрибутов v, d и v, п :
Если v.d > u.d + w(u, v) то положить v.d = u.d + w(u,v), v.n = u.
Отметим два усовершенствования алгоритма Дейкстры по сравнению с алгоритмом поиска в ширину. Во-первых, в расчет принимается вес для построенного пути, во-вторых, обновляются веса узлов, до которых находится более короткий путь.
При реализации списка открытых узлов в алгоритме Дейкстры используется очередь с приоритетами [21]. Она отличается от классической очереди тем, что ее элементы имеют ассоциированные с ними приоритеты. В такой очереди элемент с большим приоритетом находится выше элемента с меньшим и будет извлечен из нее раньше. В алгоритме Дейкстры в качестве приоритета выступает вес узла: чем они ниже - тем выше приоритет узла в списке открытых узлов.
Алгоритм A* («А со звездой») [7] был предложен П. Хертом, Н. Нильсоном и Б. Рафаэлем в 1968 г. [22] и представлял собой модернизацию алгоритма Дейкстры. Внесенные улучшения оказались удачными, алгоритм A* показал высокую эффективность. К настоящему времени известно большое количество его модификаций для решения широкого круга прикладных задач, таких как планирование маршрутов в сервисах навигации [23], нахождение путей в коммерческих компьютерных играх [24], планирование движения мобильного робота [25], оптимизация прокладки электрических и телекоммуникационных сетей [26], составление расписания занятий [27] и пр.
Оригинальный (классический) алгоритм A* строит кратчайший путь на графе из заданного исходного узла s в заданный конечный t из множества узлов T поиском по первому наилучшему совпадению с использованием эвристической информации, уменьшающей количество исследуемых узлов. Порядок обхода вершин определяется эвристической функцией f (n), определенной для каждого узла n.
Классический алгоритм A* имеет следующий вид:
1) Пометить узел s как открытый (поместить в список открытых узлов) и вычислить f (s) .
2) Выбрать „из списка открытых узлов узел n, значение которого f (n) минимально. Ситуации с совпадением значений f (n) разрешать случайным образом, но всегда в пользу узлов n eT. Если список открытых узлов пуст - выдать сообщение, что путь от узла s к узлам из множества T не существует, работу алгоритма закончить3) Если n eT, пометить узел n как закрытый (удалить из списка открытых узлов). Сохранить путь P, двигаясь назад от целевого узла к его родителю, пока не будет достигнут стартовый узел s. Работу алгоритма закончить.
4) Если n &T, пометить узел n как закрытый (удалить из списка открытых узлов).
5) Для каждого n, соседнего с узлом n рассчитать
f (n).
6) Если узел n,, не является закрытым, пометить его как открытый (поместить в список открытых узлов).
7) Если узел n,, является закрытым, пометить его как открытый (поместить в список открытых узлов), если рассчитанное значение f(ц) стало меньше значения f для этого узла, рассчитанного на шаге 1 или на шаге 5 при предыдущем выполнении шагов 2-7 .
8) Перейти к шагу 2.
Для работы алгоритма необходима некоторая функция f (n), значение которой показывает вес оптимального пути из s в t, проложенного через узел n. Такая функция^не известна заранее, поэтому используется ее оценка f.
Представим функцию f (n) в виде суммы:
f(n) = g (n) + h(n), (1)
где g(n) - вес кратчайшего пути из узла s в узел n; h(n) - вес кратчайшего пути из узла n в узел t.
Пусть g(n) - оценка g(n). Возможный выбор для g(n) - вес пути из s в n с наименьшим (на данный момент) весом, найденный на алгоритмом, что подразумевает выполнение условия
g (n)> g (n). (2)
В качестве оценки для h(n) может быть выбрана некоторая функция h(n) - оценка веса оптимального пути из n в t, формально позволяющая записать f (n ) = g (n) + h (n). Требования к выбору h (n) определяет
Теорема 1 [22]. Если
h (n)< h (n) (3)
для любого узла n, т.е. когда оценка веса кратчайшего пути не превышает действительный вес пути, алгоритм A* гарантированно находит кратчайший путь.
Задача планирования движения группы роботов по неизвестной карте к единой цели
Группу мобильных роботов, оснащенных датчиками сканирования пространства и определения относительного положения членов группы, необходимо переместить в целевую ячейку неизвестной карты. Препятствия считаются неизменными и неподвижными. Взаимодействие роботов с препятствиями не допускается, навигация через движущиеся преграды [28] не рассматривается.
Задача считается решенной, если все члены группы роботов достигают некоторой окрестности целевой ячейки карты. Задача считается неразрешимой, если при построении карты выясняется, что целевая ячейка и ее окрестность не достигаемы, например, находятся внутри области, отделенной со всех сторон препятствиями от стартового положения всех мобильных устройств.
Примем ряд допущений и рассмотрим необходимые вспомогательные задачи.
1) Группой роботов формируется общая карта, доступная для каждого члена группы, которая уточняется по мере передвижения роботов.
2) Координаты цели заданы относительно начального положения роботов и доступны для всех членов группы.
Вспомогательные задачи:
1) Локализация каждого члена группы роботов и построение карты местности по данным бортовых датчиков мобильных роботов.
2) Организации коммуникации и взаимодействия членов группы роботов.
Оговорим, что рассмотрение второй задачи лежит за рамками статьи.
Локализация членов мобильной группы роботов и совместного построения карты по данным бортовых датчиков сканирования пространства.
Управление автономными мобильными устройствами, включая поиск пути к цели при наличии препятствий и пр., невозможны без решения задачи непрерывной во времени пространственной локализации каждого члена мобильной группы, т.е. определения координат в некоторой, заранее заданной системе координат (СК). Для мобильного робота, чаще всего, это статическая СК, связанная с рабочей зоной робота, называемая базовой СК (БСК).
Известны некоторые решения указанной задачи как для отдельных роботов [14, 29, 30], так и для групп [4]. Алгоритмы взаимодействия и коммуникации членов группы являющиеся темами отдельных исследований [31, 32] и не рассматриваются в статье.
Одним из наиболее распространенных датчиков, используемых для локализации, является лазерный сканирующий дальномер (ЛСД) - прибор, измеряющий расстояния до непрозрачных препятствий в некотором заранее известном сегменте с заданной дискретностью. Принцип действия ЛСД основан на фиксации времени возвращения лазерного луча, отраженного от поверхности препятствия.
Будем рассматривать ЛСД, сканирующий в одной плоскости. Основные технические характеристики типичного ЛСД являются следующими: ширина диапазона углов сканирования - 240°, угловое разрешение - 0.36° время получения одного скана - 28 мс [33].
Наиболее интересной как в теоретическом, так и в практическом плане, а также наиболее трудной среди задач локализации является одновременная локализация и построение карты (simultaneous localization and mapping, SLAM) (см., например, [34]). Указанная задача, как правило, решается с использованием дополнительной информации - данных одометрии (см., например, [29]).
Предварительная апробация излагаемых далее подходов, включая имитационное компьютерное моделирование, проведена в работах [35-37].
Разрешающая способность сеточного представления физических координат. Как было отмечено в п.1, карта, с которой будут работать мобильные устройства, может иметь размеры узлов сетки, сопоставимые с разрешающей способностью бортовых сканирующих устройств. Однако при достаточно высоких разрешающих способностях современных измерительных устройств такая карта может содержать чрезмерно большое количество узлов и из-за конечных размеров мобильных роботов группы оказаться существенно избыточной, а необходимость проверки каждого узла карты при построении маршрута может негативно влиять на потребление памяти и быстродействие алгоритма.
Пусть каждый член группы мобильных роботов имеет конечный размер в диаметре не более D. Тогда, разбив исходную карту в разрешении сканирующих устройств на ячейки размером DxD, получим рабочую карту, по которой осуществляется построение пути.
Такой подход с одной стороны позволит уменьшить количество обрабатываемых узлов, с другой стороны может привести к неоднозначностям, в случае, когда препятствие занимает неполную часть ячейки. В нашей модели робот занимает только определенную ячейку карты и не может находиться между ячейками. Поэтому ячейки карты, содержащие части препятствий, будут считаться непроходимыми (рисунок 5).
Рисунок 5. План помещения и упрощенная карта (темно-серым цветом выделены непроходимые Участки)
Сканы и локализация. Сканом будем называть результат однократного считывания ЛСД расстояний до препятствий в пределах рабочего диапазона углов (сканирования). Большинство методов локализации с использованием ЛСД основаны на сопоставлении двух, сделанных один за другим сканов [38]. Идею подобного сопоставления рассмотрим на примере, соответствующем повороту мобильного устройства на некоторый угол а (рисунок 6).
Рисунок 6. Иллюстрация задачи совмещения двух последовательных сканов, соответствующих повороту мобильного устройства на угол а : (а) - скан до поворота, (б) - скан после поворота, (в) - результат совмещения сканов
Известны работы (см., например [38]), в которых отмечается, что для решения задачи локализации достаточно сопоставления не всего скана, а только некоторых выделенных его участков - «особенностей». В этом качестве могут, например, рассматриваться участки резких скачков (бифуркаций) расстояний или некоторые связные множества точек, например, прямые линии.
Отметим ряд существенных недостатков указанного подхода:
- Выделение участков скана является трудоемким как с точки зрения процессорного времени, так и из-за необходимости выделения новой памяти, по сути, для дублирования имеющейся информации.
- Уменьшается количество полезной информации, которую содержит каждый скан. Например, при выделении прямых линий не учитываются препятствия, не имеющие прямых внешних граней, что существенно ограничивает применимость алгоритма, в т.ч. при локализации внутри помещений: колонны, округлые подставки приборов не обеспечивают информацию для локализации.
- Информация о классах присутствующих на ска-не объектов, их количестве и особенностях отделения друг от друга не важна и не используется в дальнейшем, а на ее определение необходимо затратить вычислительные ресурсы (память и время).
- Процедура выделения «особенностей» является дополнительным промежуточным шагом, на котором могут быть внесены дополнительные ошибки.
В настоящей статье с целью создания быстрого и надежного алгоритма локализации по данным ЛСД предлагается работать с «сырыми» данными дальномера. Изложенные ниже методы и алгоритмы были предварительно апробированы в работах [35-37].
Подход, основанный на сопоставлении сканов, предполагает, что за время, прошедшее между снятиями сопоставляемых сканов, робот «далеко не уехал». Формализуем указанное предположение. Будем считать, что за время Д^ разделяющее моменты снятия двух сканов, мобильный робот может переместиться лишь на некоторое расстояние г < К внутри области и и совершить поворот на угол а < А внутри сектора Э (рисунок 7).
карту при помещении оси дальномера в одну из точек области положений до выполнения критерия успешности сопоставления сканов, иллюстрирует рисунок 6. В данном подходе используется вся информация, содержащаяся в скане, каждый очередной скан накладывается не на предыдущий, а на составленную к этому моменту карту.
Контрольной окрестностью 1П размерности п узла на дискретной карте, содержащего препятствие, назовем квадратную область из п х п узлов с заданным узлом в центре (рисунок 8).
Рисунок 8. Некоторый узел карты, содержащий препятствие (отмечен черным кружком) и его контрольная окрестность
Будем утверждать, что в процессе сопоставления сканов узел очередного скана «ложится на карту», если его контрольная окрестность не пуста и содержит хотя бы один узел карты, о котором по результатам предыдущего скана, содержащего препятствие (или принадлежащего некоторому препятствию).
Критерий успешности процедуры наложении сканов должен учитывать количество узлов последующего скана, которые «ложатся на карту».
Рассмотрим объединение контрольных
п
окрестностей узлов очередного скана I = у^. Будем
1=1
называть это объединение контрольной областью скана.
Критерием степени успешности процедуры наложения скана выберем мощность пересечения множества
репятс-
М ячеек карты, помеченных, как содержащих г твие, с контрольной областью скана I: Ф = М ПI
«Наиболее успешное» наложение очередного скана на карту соответствует максимуму критерия Ф.
Рисунок 7. Область положений мобильного робота
Область и и сектор Э ограничены, оценка R и а может быть сделана на основе анализа динамических характеристик мобильного робота. Декартово произведение множеств и и Э будем называть областью положений, а отдельную точку \ с некоторыми координатами (К - положением. '
Лазерный сканирующий дальномер редко является единственным устройством, участвующим в решении задачи локализации. Как правило, при работе с мобильным роботом так же доступны данные одометрии, которые позволяют приблизительно оценить перемещение и сузить области и и Э.
Локализацию сопоставлением «сырых» сканов, с помощью оценки области положения робота по данным одометрии и последовательном наложении сканов на
Рисунок 9. Иллюстрация использования критерия наложения скана: (а) - максимальное наложение, (б) - непустое наложение
Оптимальное значение размерности п контрольной окрестности 1п узла скана зависит от скоростей передвижения мобильного работа и снятия полного скана дальномером, задержек между снятиями сканов, уровня зашумленности данных дальномера и т.д.
С учетом вышеизложенного, алгоритм локализации мобильного устройства по данным сканирующего дальномера, может быть представлен в следующем виде:
1) Принять текущее положение робота за начало системы координат, получить первый скан, наложить данные скана на карту.
2) Робот сохранил положение? Да - перейти к шагу 2, нет - перейти к шагу 3.
3) Определить область положений робота по данным одометрии.
4) Нанести данные скана на карту, считая положением робота одну из точек области положений.
5) Критерий соответствия удовлетворен? Да -принять за новую позицию робота текущую точку из области положений, наложить данные скана на карту, перейти к шагу 2, нет - выбрать новую точку из области положений, перейти к шагу 4.
Алгоритм планирования движения к цели группы мобильных устройств
Планирование группового движения и соответствующие формальные и неформальные модификации алгоритма А*
Ключевым элементом разрабатываемой процедуры планирования движения к цели группы мобильных устройств является алгоритм А*. При этом собственно алгоритм планирования движения к цели группы мобильных устройств окажется своеобразной надстройкой над ним.
Формально, задача, которую необходимо решить при использовании алгоритма А* в исходной редакции (без модификаций), заключается в выборе конкретного вида функций /(п) (g(п), h(п)) и /(п) (И(п), И(п)).
Главная неформальная задача, рассматриваемая в данном параграфе - расширение области применимости алгоритма А* до группы роботов, совершающих движение к единой цели. Формализация ее решения позволит конкретизировать выбор функций /(п) (и(п), И (п)) и / (п) (И (и), И (п)).
Рассмотрим ряд вспомогательных задач.
1. Наличие общей карты подразумевает доступ к ней всех членов группы, что требует обеспечения целостности данных об узлах карты (в нашем случае это веса узлов, их проходимость и пр.).
2. Для исключения одновременного использования одного и того же узла несколькими роботами и предотвращения возможных столкновений необходим контроль пересечения маршрутов.
3. Обеспечение эффективности продвижения к цели группы роботов требует определенного пересмотра и корректировки задачи построения отдельным роботом оптимального пути к цели.
Возможные следующие пути решения указанных
задач:
Целостность данных может быть обеспечена разграничением доступа членов группы к общей карте. При этом порядок доступа может определяться приоритетом члена группы или приоритетом операции над общими ресурсами.
Задачи контроля пересечения маршрутов и обеспечения эффективности путей к цели могут быть решены путем внесением необходимых модификаций алгоритма А*.
Для решения задачи контроля пересечения маршрутов рассмотрим сначала некоторые известные приемы исключения коллизий между перемещающимися объектами, такие как методы блокировки пути, пошаговое вычисление и др. [39].
В методах блокировки пути каждый объект, вычисляющий свой путь, помечает занятые узлы как недоступные для участия в пути других объектов. Далее, по мере передвижения объекта, блокировки с пройденных узлов снимаются. Такой подход имеет как преимущества, так и недостатки. К преимуществам можно отнести высо-
кую скорость работы, простоту реализации и исключение коллизий на стадии вычисления пути. Недостаток заключается в том, что с ростом количества движущихся объектов более сложным становится пространство поиска путей, а сами пути удлиняются.
При пошаговом вычислении путь для каждого объекта рассчитывается без учета остальных членов группы. Когда при движении возникает ситуация, которая может привести к коллизии, существует несколько способов ее разрешения. Например, один из объектов может дождаться освобождения узла другим объектам, или построить траекторию в обход его. При таком подходе недостатком является то, что при наличии выделенного оптимального пути все объекты выберут именно его и будут вынуждены ожидать в очереди.
Заметим, что для программной реализации описанных выше и других возможных приемов исключения коллизий необходимо хранить и обрабатывать некоторую структуру логических значений (флагов). В рассматриваемом далее решении предлагается связывать указанную информацию непосредственно с узлами (ячейками карты).
Итак, пусть имеется некоторая функция ^п), заданная формулой и определяющая фактический вес оптимального пути из узла э в узел и проходящего через узел п. Вид этой функции, а также ее оценки / (п) и ее слагаемых И(п) и И(п) мы ранее не конкретизировали, но теперь это необходимо для построения механизма предотвращения коллизий оценки эффективности движения группы в целом. Рассмотрим расстояние между центрами двух соседних ячеек карты, понимаемое в традиционном геометрическом и физическом смысле (или некоторая величина, пропорциональная расстоянию, которую будем называть ценой перемещения в указанную ячейку). При этом единицы измерения могут указанной величины быть условными, однако важно понимать и учитывать, что в случае, когда все ячейки карты - квадраты с одинаковой длиной стороны, расстояние между двумя соседними ячейками по диагонали в 42 раз больше расстояния между двумя соседними ячейками по вертикали или по горизонтали. Это также справедливо для цены перехода в указанную ячейку.
Примем, что () - длина кратчайшего пути из ячейки карты э в ячейку карты и измеренная как сумма геометрических (физических) расстояний между центрами соседних ячеек, образующих указанный путь. Соответственно, д(п) и h(n) - это измеренные аналогичным образом длины кратчайшего пути из ячейки э в ячейку п и кратчайшего пути из ячейки п в ячейку I
Функцию И(п) можно вычислять как д(п) но мы внесем важное изменение: пусть каждый робот, который включает ячейку п в свой путь, добавляет к значению И (п) некоторую поправку Д. Несложно убедиться, что при этом не нарушается основное требование, накладываемое на функцию И(п) - неравенство . Пусть расчет пути к цели осуществляется для роботов-членов группы последовательно, в порядке очереди, которую можно организовать, присвоив роботам порядковые номера. Тогда для некоторого робота с порядковым номером i, исследующим ячейку карты п может оказаться, что функция д(п) имеет поправку кД, где 0 < к <1 - количество роботов, уже включивших ячейку п в собственный путь. Таким образом, робот с номером \ «понимает», что ячейка п уже входит в путь одного или нескольких членов группы и возможна ситуация, когда указанная ячейка будет занята другим роботом, а ему придется ждать. Другими словами, для каждого последующего робота ячейка п «прибавляет в цене» и путь в обход может оказаться «дешевле». При этом робот имеет способ оптимального разрешения альтернативы: «выбрать путь, который идет в обход занятых узлов» или «стоять в очереди на их доступность».
Рассмотрим пример. Пусть дана карта, представленная на рисунке 10. Положения препятствий указаны
серыми блоками, положения членов группы - красными окружностями с лучом из середины, желтая звезда - цель. Для упрощения рассуждений в этом примере не будем акцентировать внимание на окрестности цели и предположим, что в итоге все роботы должны оказаться строго в целевой ячейке. Пусть робот в рассматриваемой модели может перемещаться на соседние ячейки по вертикали, горизонтали и диагонали. Условимся, что при этом расстояние между соседними ячейками по вертикали и горизонтали составляет 10 единиц, а расстояние между соседними ячейками по диагонали - 14 единиц (к 10-12). Для поправки Д, являющейся инструментом разрешения коллизий, примем значение 10 единиц.
Рисунок 10. Карта местности для модельного эксперимента по разрешению коллизий
Кратчайший путь к цели проходит между препятствиями и, если рассчитывать пути движения отдельно для каждого члена группы, то все получат один и тот же путь и будут вынуждены ждать окончания движения друг друга или строить пути обхода. Рассчитаем пути, воспользовавшись методикой наращивания весов, описанной ранеПредположим для определенности, что горизонтальный строй роботов, являющийся их начальным положением, определяет и очередность, с которой роботы выстраивают свои пути к цели: первым рассчитывает свой путь крайний левый робот, а затем это по одному слева-направо делают остальные роботы. Тогда путь первого робота проходить между препятствиями, как показано на рисунке 11а. При этом длина пути первого робота составит 34 единицы, а веса задействованные в этом пути ячеек, увеличиваются с 0 до 10 единиц.
Рисунок 11. Маршруты роботов
Путь второго робота так же пройдет между препятствиями, как более «выгодный» (рисунок 11б). Его длина с учетом дополнительных весов ячеек составит 60 единиц, а веса задействованных во втором пути ячеек также увеличатся на 10 единиц. Заметим, что второму роботу придется пропустить один такт перемещений, чтобы пропустить вперед первого робота.
Без учета стартовых ячеек, которые не получают весов, пути первого и второго роботов состоят из одних и тех же ячеек, веса указанных ячеек возрастают до 20 единиц после завершения вторым роботом планирования своего пути.
Рассмотрим теперь возможные пути третьего робота. Несложно определить, что если путь третьего робота пройдет по тем же ячейкам, что и у его предшественников, то длина такого пути с учетом весов ячеек составит
94 единицы. При этом третьему роботу придется пропустить два такта перемещений, чтобы пропустить вперед первого и второго робота. В итоге, все роботы группы достигнут цели только спустя 5 тактов работы алгоритма.
В то же время, путь третьего робота в обход правого препятствия (рисунок 11в), хоть и осуществляется диагональными переходами, имеет цену 38 единиц. Таким образом, движение по нему является более выгодным. При этом все члены группы достигнут цели за 4 такта работы алгоритма. Таким образом, в конкретном примере получен выигрыш в 1 такт работы алгоритма.
Рассмотрим теперь еще одну причину возникновения коллизий, которую будем называть тупиковой ситуацией, при которой два или более членов группы осуществляют взаимную блокировку своих путей (рисунок 12). Если не предпринимать специальных мер, то члены группы, попавшие в тупиковую ситуацию, постоянно будут пропускать шаги движения в ожидании освобождения узлов [2].
Ф
б
Рисунок 12. Тупиковая ситуация: (а) - причина возникновения, (б) - взаимная блокировка
На рисунке 12а представлена иллюстрация возможной причины возникновения взаимной блокировки: два робота, отмеченные значками синего и красного цвета, прокладывают встречные пути через узкий проход на карте. Через определенное количество шагов возникает ситуация взаимной блокировки, при которой каждому из роботов для дальнейшего движения необходимо занять узел, заблокированный другим роботом (рисунок 12б).
В большинстве случаев для преодоления взаимных блокировок достаточно осуществить перестройку маршрутов роботов, оказавшихся тупиковой ситуации. Тем не менее, необходим надежный и оперативный механизм программного распознавания и разрешения возникшей блокировки.
Пусть роботу с номером у из группы численностью N, находящемуся в ячейке э, необходимо осуществить перемещение в соседнюю ячейку п.
В качестве метода разрешения тупиковых ситуаций может быть предложен алгоритм разрешения тупиковых ситуаций
(Предполагается, что роботы с номерами i = 1...у уже построили свои пути, причем пути роботов с номерами \ = 1...у - 1 гарантировано не содержат тупиковых ситуаций. При этом роботам известен некоторый алгоритм построения пути, позволяющий обходить препятствия и недопускающий коллизий, отличных от тупиковых ситуаций)
1) Для каждого робота с номером \ = 1...у - 1 проверить, не является ли ячейка п ожидаемой для следующего шага его передвижения. При нахождении такого \ перейти к шагу 2, иначе перейти к шагу 3.
2) Для робота с номером у временно отметить ячейку п как занятую препятствием, найти новый путь с помощью алгоритма построения пути, отметить ячейку п как незанятую препятствием. Перейти к шагу 1, рассматривая вместо ячейки п ячейку п' - первую ячейку нового пути.
3) Оставить ячейку п в пути робота с номером у. (Теперь пути всех роботов с номерами \ = 1...у гарантировано не содержат тупиковых ситуаций)
В приведенном выше примере для наглядности рассматривалась только одна составляющая оценки веса узла §(п), при этом принималось /(п) = §(п), И(п) = 0. При этом отсутствует приоритетность рассмотрения узлов при поиске маршрута (которая специально вводилась в алгоритм А* для сокращения числа исследуемых узлов), и алгоритм А* редуцируется к своему частному случаю -алгоритму Дейкстры [11].
Таким образом, для того, чтобы использовать алгоритм А* в полной мере, необходимо наполнить содержательным смыслом функцию И(п), предоставляющую эвристическую оценку веса, или, что эквивалентно, «длины» кратчайшего пути из текущей ячейки п в целевую ячейку I
Как отмечено ранее, согласно [22, теорема 1], алгоритм А* гарантированно находит кратчайшийпуть, если выполняется условие . Таким образом, пока И (п) не переоценивает фактическую длину пути из текущей ячейки п в целевую ячейку t - найденный путь будет гарантированно оптимальным.
Пусть
И( п ) = (|Дс| + |Ду|) /2, (4)
где Дх и Ду - значения разностей координат центров текущей ячейки п и целевой ячейки I Несложно убедиться, что
И (п )<р( п )< И (п), (5)
где р(п) = ^Дс2 + Ду2 - геометрическое расстояние между центрами ячеек п и I Равенство в левой части двойного неравенства достигается тогда и только тогда, когда Дх = Ду = 0, а равенство в правой части достигается тогда и только тогда, когда путь между ячейками п и tявляется вертикальным, горизонтальным или диагональным и в промежуточных ячейках, составляющих указанный путь, не содержится препятствий.
Таким образом, функция И (п), задаваемая формулой, отвечает достаточным ус- ловиям оптимальности алгоритма А*.
Следующей является задача неполноты данных. В начале группе роботов неизвестна местность, по которой происходит движение. Карта местности компилируется во время движения на основе данных, получаемых с бортовых датчиков мобильных роботов. При этом возможна ситуация, когда пути, рассчитанные на одном из этапов движения, могут оказаться непроходимыми при компиляции карты по новым данным на следующем этапе.
Следовательно, на каждом шаге передвижения необходимо проверять доступность узлов построенных путей, в связи с уточнением карты. При недоступности очередного узла необходимо повторить процедуру поиска пути для конкретного члена группы, учитывая новые данные.
Слишком частые перерасчеты маршрутов следо-
вания могут негативно сказаться на быстродействии алгоритма и загрузке расчетных модулей роботов. С другой стороны, недостаточно частые корректировки могут привести к неадекватному обстановке исполнению команд роботами.
Компромиссом между быстродействием и точностью может служить, например, следующее решение: осуществлять пересчеты маршрутов после внесения новой информации на карту одним из членов группы при условии, что хотя бы один из рассчитанных путей пересекает новый непроходимый участок.
На этом необходимые предварительные выкладки закончены, и можно привести описания соответствующих алгоритмов - модифицированного алгоритма А* и алгоритма планирования движения к цели группы мобильных устройств.
Алгоритм планирования движения группы роботов по неизвестной карте к единой цели. Определим алгоритм планирования движения группы роботов по неизвестной карте к единой цели в виде следующей совокупности шагов, одна часть которых выполняется параллельно (одновременно), а другая - последовательно:
0) Задать численность группы роботов, начальные координаты, координаты точки назначения и другие необходимые данные.
1) Выполнить сканирование (параллельный шаг - выполняется всеми роботами одновременно).
2) Осуществить предварительную индивидуальную локализацию и индивидуальное уточнение карты (параллельный шаг - выполняется всеми роботами одновременно по методике, описанной в параграфе 3.1.2).
2) Коллективно уточнить индивидуальные локализации, создать уточненную общую карту (коллективный шаг - к настоящему моменту не разработан; в приведенных ниже модельных вычислительных экспериментах роботы позиционируются точно и считается, что получаемые ими сканы формируют безошибочную информацию о новых препятствиях, которая без проверок добавляется в коллективную карту).
3) Для каждого робота группы:
- Если цель достигнута - последующие шаги не выполнять.
- Если цель достигнута всеми роботами - работу алгоритма завершить.
- Модифицированным алгоритмом А* с использованием алгоритма разрешения тупиковых ситуаций построить проект пути к цели. В качестве стартовой ячейки э учитывать текущую ячейку, занятую роботом (модифицированный алгоритм А* учитывает как известные на данный момент препятствия, так и занятие (и освобождение) ячеек роботами, проекты путей которых уже рассчитаны).
- Если построение проекта пути к цели окончилось неудачей - выдать сообщение о недоступности цели и работу алгоритма закончить.
- В соответствии с построенным проектом пути переместиться в очередную ячейку или остаться на месте, пропуская других роботов.
4) Перейти к шагу 1.
Модифицированный алгоритм А*. Для того, чтобы описание приведенного выше алгоритма планирования движения группы роботов по неизвестной карте к единой цели приобрело законченный вид, необходимо описать используемый в нем модифицированный алгоритм А*. Отметим, что в отличие от описания оригинального алгоритма, ориентированного на работу с узлами графа, представленное ниже описание ориентировано на сеточное представление карты, а функции ^п), д(п), ||(п), /(п), й (п), к(п) конкретизированы в соответствии с описаниями, представленными выше.
Модифицированный алгоритм А*.
1) Пометить узел э как открытый (поместить в список открытых узлов) и вычислить / (5) .
„2) Выбрать из списка открытых узлов узел п, значение / (п) которого минимально. Ситуации с совпадением значений /(п) разрешать случайным образом, но в пользу узлов п еТ. Если список открытых узлов пуст, то вернуть в главный алгоритм признак безуспешности построения пути к цели, работу алгоритма для заданного агента закончить.
3) Если пеТ, пометить узел п как закрытый (удалить из списка открытых узлов). Положить /(п) = /(п) + А . Сохранить путь, двигаясь назад от целевого узла к его родителю, пока не будет достигнут стартовый узел э. Вернуть в главный алгоритм признак успешности построения пути к цели. Работу алгоритма для заданного агента закончить.
4) Если п£Т, пометить узел п как закрытый (удалить из списка открытых узлов).
5) Для каждого из 8 узлов п1, соседних с п, кроме помеченных как непроходимые, рассчитать /(ц) с учетом поправки NД, где N количество агентов, использующих данный узел.
6) Если узел ni не является закрытым, то пометить его как открытый (поместить в список открытых узлов).
7) Если узел п, является закрытым, пометить его как открытый (поместить в список открытых узлов), если рассчитанное значение /(п) меньше значения у для этого узла, рассчитанного на шагах 1 или 5 при предыдущем выполнении шагов 2-7.
8) Перейти к шагу 2.
Краткое описание вычислительных экспериментов
К сожалению, ограниченный объем статьи не позволяет привести детальное описание вычислительных экспериментов, поэтому ниже будут рассмотрены только основные особенности данного этапа исследования предлагаемого алгоритма.
1. Тестирование проводилось на картах с двумя типами топологии: открытого типа и лабиринтах. К открытому типу были отнесены карты, свободные участки которых по размерам превосходят или соизмеримы с размерами самих препятствий и существенно больше размеров роботов. К лабиринтам были отнесены карты с препятствиями протяженной формы, соединенными между собой, и свободными областями (проходами), ширина которых была сопоставима с размерами робота.
2. В вычислительных экспериментах для каждой модельной карты варьировалась численность группы роботов.
3. Карты местности представляли собой монохромные изображения, хранимые в виде файлов формата Ьтр, на которых белые области обозначали свободное пространство, а черные - препятствия. Если препятствие имело замкнутый контур, белая область внутри этого контура свободной не считалась.
4. В качестве критерия эффективности исследуемого алгоритма была выбрана длина (измеренная числом ячеек карты) наиболее длинного пути, пройденного роботом-членом группы до достижения заданной окрестности цели. Такой показатель относительно легко поддается подсчету, при постоянном времени шага движения предоставляет оценку затрачиваемого времени и для группы роботов может быть интерпретирован как количество шагов до достижения цели.
Эксперименты выявили ряд интересных закономерностей. Так, в результате моделирования «прохождения» группой роботов карт открытого типа обнаружилось, что количество шагов до достижения цели растет с увеличением численности группы (см. таблицу 1).
Таблица 1. Результаты работы алгоритма на картах открытого типа
Численность Количество шагов до достижения цели
группы Карта «склад» Карта «офис»
1 62 66
5 81 71
10 85 74
15 98 85
Вычислительные эксперименты с картами типа «лабиринт» дали качественно другие результаты: обнаружилось, что количество шагов до достижения цели с увеличением численности группы проходит через минимум (см. таблицу 2).
Таблица 2. Результаты работы алгоритма на картах типа лабиринт
Численность Количество шагов до достижения цели
группы Карта «коридор» Карта «лабирин»
1 157 603
5 121 591
10 130 529
15 144 546
В целом, для работы алгоритма по картам открытого типа характерно наличие хороших результатов при прохождении единичного робота; стабильное прохождение групп роботов средней численности и резкое увеличение количества необходимых шагов до достижения цели при перенасыщении карты роботами.
Для карт типа «лабиринт» можно говорить о некоторой оптимальной численности группы для конкретной карты, при которой на прохождение всей группы будет затрачено минимально возможное количество шагов.
Заключение
В работе были рассмотрены постановки задач локализации и планирования движения группы роботов. Аналитический обзор отечественных и зарубежных литературных источников показал отсутствие универсальных методов решения указанных задач, что послужило стимулом для разработки оригинальных методов локализации и планирования движения группы роботов на основе информации с бортовых датчиков сканирования пространства.
Задачу построения алгоритма планирования движения группы роботов (фактически - алгоритма группового самоуправления) удалось решить, с использованием одного из наиболее эффективный алгоритмов поиска кратчайшего пути на графе - алгоритма A*. Внесенные в него модификации позволили распространить решение поиска пути на группу роботов, а также учесть и исключить возможности коллизий членов группы и возникновение тупиковых ситуаций.
Тестирование алгоритма на компьютерной модели показало как высокую эффективность предложенного решения задачи планирования движения в общем случае, так и выигрыш в производительности, достигаемый группой перед отдельным роботом при выполнении задания на картах определенных типов.
Литература
1. Юревич Е.И. Основы робототехники. СПб.: БХВ-Петербург, 2005. 416 с.
2. Hoy M.C. Methods for collision-free navigation of multiple mobile robots in unknown cluttered environments // Cornell University Library. URL: http://arxiv-web.arxiv.org/ pdf/1401.6775 (дата обращения: 29.08.2014).
3. Fujimori A., Kubota H., Shibata N., Tezuka Y. Leader-follower formation control with obstacle avoidance using sonar-equipped mobile robots // J. of Systems and Control Engineering. 2014. V. 228. № 5. P. 1-13.
4. Fox D., Burgard W., Kruppa H., Thrun S. Callobo-rative multi-robot localization // Lecture notes in computer science. 1999. V. 1701. P. 255-266.
5. Antich J., Ortiz A., M nguez J. A bug-inspired algorithm for efficient anytime path planning // IEEE/RSJ International Conference on Intelligent Robots and Systems. 1015 Oct. 2009. IROS. 2009. P. 5407-5413.
6. Sarid S., Shapiro A., Gabriely Y. MRBUG: A competitive multi-robot path finding algorithm // 2007 IEEE International Conference on Robotics and Automation. 10-14 Apr. Roma. IEEE 2007. P. 877- 882.
7. Stout B. Smart moves: intelligent pathfinding // Game Developer. 1996. V. 10. P. 28-35.
8. Garrido S., Moreno L., Blanco D., Jurewicz P. Path planning for mobile robot navigation using Voronoi diagram and fast marching // International Journal of Robotics and Automation. V. 2. Issue 1. 2011. P. 42- 64.
9. Ng J., Braunl Th. Performance comparison of bug navigation algorithms // Journal of Intelligent and Robotic Systems. 2007. Vol. 50. Issue 1. P. 73-84.
10. Lumelsky V.L., Stepanov A.A. Path-planning strategies for a point mobile automaton moving amidst unknown obstacles of arbitrary shape // Algoritmica. 1987. V. 2. № 1-4. P. 403-430.
11. Кормен Т., Лейзерсон Ч., Ривест Р., Штайн К. Алгоритмы: Построение и анализ. Изд. 3-е. М.: ИД «Вильяме», 2013. 1323 с.
12. Zohaib M., Pasha M., Riaz R.A., Javaid N., Ilahi M., Khan R.D.. Control strategies for mobile robot with obstacle avoidance URL: http://arxiv.org/pdf/1306.1144v1 (дата обращения: 29.08.2014).
13. Zhu Y., Zhang T., Song J., Li X. A new hybrid navigation algorithm for mobile robots in environments with incomplete knowledge // Knowledge-Based Systems. 2012. V. 27. P. 302-313.
14. Kamon I., Rivlin E., Rimon E. A new rangesensor based globally convergent navigation algorithm for mobile robots // Proc. 1996 IEEE International Conference on Robotics and Automation. 22-28 Apr.1996.Minneapolis, Minnesota. 1996. Vol. 4. P. 429-435.
15. Gabriely Y., Rimon E. CBUG: A Quadratically Competitive Mobile Robot Navigation Algorithm //IEEE Transactions on Robotics. 2008. V. 24. №. 6. P. 1451-1457.
16. Hamani M., Hassam A. Mobile robot navigation in unknown environment using improved APF method // Proc. 13th International Arab Conference on Information Technology. 10-13 Dec. 2013. Zarqa University, Jordan: P. 453-458.
17. Wang C. Collision free autonomous navigation and formation building for non-holonomic ground robots // Cornell University Library. URL: http://arxiv.org/pdf/1402.3188 (дата обращения: 29.08.2014).
18. Moore E.F. The shortest path through a maze // Proc. Intern. Symp. Theory Switching. 1957. Part II. (H. Aiken, ed.), Harvard University Press, Cambridge, Massachusetts. 1959. P. 285-292.
19. Lee C.Y. An algorithm for path connection and its applications // IRE Transactions on Electronic Computers. 1961. V. EC-10. Issue 3. P. 346-365.
20. Кнут Д.Э. Искусство программирования, Т. 1. Основные алгоритмы. М.: И.Д. «Вильямс», 2010. 720 с.
21. Джейсуол Н. Очереди с приоритетами. М.: Мир, 1973. 279 с.
22. Hart P. E., Nilsson N. J., Raphael B. A formal basis for the heuristic determination of minimum cost paths // IEEE Transactions on Systems Science and Cybernetics. 1968. V. 4. № 2. P. 100-107.
23. Zhao L., Wang W. A new path search algorithm for providing paths among multiple origins and one single
destination // International J. of Computer Science and Application. 2014. V. 3. Issue 1. P. 29-33.
24. Botea A., Müller M., Schaeffer J. Near optimal hierarchical path-finding // J. of Game Development. 2004. V. 1. №. 1. P. 7-28.
25. Das P.K., Mandhata S.C. Improved real time A* algorithm for path planning of mobile robot in quadrant based environment // International J. on Advanced Computer Theory and Engineering. 2012. V.1. Issue 1. P. 7-126. Gholami De-hbalaei M.R., Delshad E. Presenting a model for affordable choice of wiring route in the electrical and telecommunications networks in the residential areas based on the artificial intelligence A-STAR algorithm // Life Science J. 2013. V.10. № 1. P. 1068-1070.
27. Pokorny K.L., Vincent R.E. Multiple constraint satisfaction problems using the A-STAR (A*) search algorithm: classroom scheduling with preferences // J. of Computing Sciences in Colleges. 2013. V. 28. Issue 5. P. 152-159.
28. Wu H., Levihn M., Stilman M. Navigation among movable obstacles in unknown enviroments // IEEE/RSJ International Conference on Intelligent Robots and Systems. 18-22 Oct. IEEE. 2010. P. 1433-1438.
29. Thrun S., Montemerlo M., Koller D., Wegbreit B., Nieto J., Nebot E. Fastslam: An efficient solution to the simultaneous localization and mapping problem with unknown data association // J. of Machine Learning Research. 2004. V. 4. №. 3. P. 380-407.
30. Eliazar A., Parr R. DP-SLAM: Fast, robust simultaneous localization and mapping without predetermined landmarks // IJCAI. 9-15 Aug. 2003.Acopulco. Mexico. T. 3. C. 1135-1142.
31. Jing J.H. Multi-robot path finding with wireless multihop communications // IEEE Commun. Magaz. 2010. V. 48. Issue 7. P. 126-132.
32. Kim S.-L., Burgard W., Kim D. Wireless communications in networked robotics // IEEE Wireless Commun. 2009. V.16. № 1. P. 4-5.
33. Hokuyo UBG-04LX-F01 (Rapid URG) Scanning Laser Rangefinder - RobotShop // URL:http://www.ro-botshop.com/en/hokuyo-ubg-04lx-f01-laser-rangefinder.html. (Дата обращения: 29.08.2014)
34. Aulinas J., Petillot Y.R., Lladô X.. The SLAM problem: a survey // Frontiers in Artificial Intelligence and Applications. 2008. V. 184. P. 363-371.
35. Ерохин А.В., Волков В.В. Алгоритм локализации движущегося робота по данным сканирующего дальномера: свидетельство о регистрации электронного ресурса № 19557; опубл. 21.10.2013. Хроники объединного фонда электронных ресурсов «Наука и образование» 2013. № 10(53).
36. Ерохин А.В., Ерохин В.И. Алгоритм группового самоуправления коллектива роботов при построении пути к цели с учетом препятствий по неизвестной карте на основании данных сканирования пространства и определения относительного положения членов группы // Матер. научн. конф., посвященной 185-й годовщине образования СПбГТИ(ТУ). СПб: СПбГТИ(ТУ), 2013. С. 259.
37. Ерохин А.В., Ерохин В.И. О некоторых подходах к оптимизации алгоритма группового самоуправления коллектива роботов при построении пути к цели по неизвестной карте // Материалы научно-технич. конф. молодых ученых «Неделя науки-2014».СПб: СПбГТИ(ТУ), 2014. С. 164.
38. Васильев И.А. Локализация мобильного робота. // Вестник компьютерных и информационных технологий. 2012. № 4. С. 24-27.
39. Vincke S. Real-time pathfinding for multiple objects // Game Developer Magazine. 1997. June Issue. P. 36-44.
