В этой публикации мне бы хотелось рассказать о том, как я строила локальную карту проходимости для робота. Данная задача была необходима как для повышения навыков в программировании и освоении датчиков, так и для последующего внедрения собственных алгоритмов в работу реальных роботов на таких робототехнических соренованиях, как «Робокросс» и «Робофест».
Эта статья рассчитана на тех, кто только входит в мир робототехники или пытается разобраться с построением карты проходимости. Я старалась изложить все максимально простым и понятным языком, понятным для большинства людей.
Что такое локальная карта проходимости
Итак, локальная карта проходимости — это то, что видит робот в данный момент времени.
Это та информация, которая приходит с «глаз» робота и впоследствии обрабатывается и выводится в удобном нам виде.
Если робот стоит на месте, то его локальная карта при неизменных условиях окружающей среды остается постоянной.
Если робот движется, то в каждый момент времени его окружающая обстановка разная, соответственно, локальная карта тоже меняется.
Локальная карта обычно имеет постоянные размеры. Размер высчитывается исходя из максимальной длины лучей, испускаемых дальномером. В моем случае эта длина составляет 6 метров.
Чтобы упростить себе задачу, карту решено было сделать квадратной.Также было решено, что условно дальномер будет находиться ровно по центру карты (это место будет точкой, где x = y = 0). Центр был выбран таким образом, потому что дальномер, который я использую, испускает лучи в плоскости более, чем на 180° (он испускает лучи на 240°, но об этом чуть позже), то есть какие-то лучи непременно уйдут за сканер и при неверном выборе центра их можно потерять. При грамотном выборе центра все лучи будут корректно отображены. Исходя из этого, размер карты я сделала в 2 раза больше, чем максимальная длина испускаемых лучей.
Размер моей карты равен 12 * 12 метров.
Датчик, который я использовала
На самом деле для решения подобного рода задачи можно использовать любой дальномер.
Дальномер — это прибор для определения расстояния до чего-либо (в моем случае до потенциального препятствия).
В робототехнике применяются в основном 2 типа дальномеров: ультразвуковые и лазерные.
Ультразвуковые дальномеры значительно дешевле, но ультразвуковые лучи достаточно широки и для точных измерений не годятся.
Лазерные дальномеры дороже, но точнее, так как их лучи узконаправленны.
Для решения своей задачи я использовала лазерный сканер Hokuyo URG-04LX-UG01. Этот датчик способен испускать лучи на 240° и дает достаточно точную информацию о препятствиях, которые встали на пути лучей. Его максимальная дальность — примено 5 — 6 метров. Стоит отметить, что данный дальномер испускает лучи только в 2D плоскости. Этот факт обязывает ставить датчик на робота в определенное место, обычно спереди снизу робота, для получения более точной картины. Опять же, можно использовать и 3D-сканеры, которые дают гораздо более точную и полную информацию об окружающей среде, но и стоят они гораздо дороже.
Считаю, что именно этот сканер прекрасно подходит для обучения в соотношении цена-качество.
Hokuyo URG-04LX-UG01
Коротко о принципе действия лазерного сканера:
Дальномер испускает лучи вдоль плоскости. Луч, который встретил на своем пути препятствие, отражается от него и возвращается обратно. По разности фаз между посылаемым и принимаемым сигналами, можно судить о том, насколько далеко расположено препятствие.
Соответственно, если испускаемый луч не вернулся, то в 5 — 6 метрах вдоль прямой его испускания либо нет никаких препятствий, либо луч не смог корректно отразиться.
С лазерного сканера можно получить следующие данные:
Для каждого испущенного луча:
- Расстояние до препятствия
- Угол испускания
*Каждый из параметров хранится в отдельном массиве и соответствует данным для одного из лучей.
О построении карты
Для построния карты и ее отрисовки мною были использованы средства ROS (Robot Operating System), а именно: программа Rviz и тип данных nav_msgs::OccupancyGrid. Я создала публикатор (publisher) локальной карты с типом сообщений nav_msgs::OccupancyGrid под соответстующим топиком local_map. В Rviz, подписываясь на данный топик, принимала данные о карте и выводила их в форме типа Map.
В соответствии с таким алгоритмом необходимо было программно настроить обработку данных с лазерного сканера и запись их в необходимый формат для передачи. В OccupancyGrid карта хранится и передается в одномерном массиве.
Так было и у меня. Карту на основе данных со сканера я храню в двумерном массиве, а при формировании сообщения для отправки в Rviz, я преобразую двумерный массив в одомерный, необходимый для OccupancyGrid.
На самом деле карта в OccupancyGrid только хранится и передается в одномерном массиве. При расшифровке ее данных она автоматически превратится в квадратную двумерную карту.
Но чтобы это произошло корректно, необходимо записывать этот одномерный масив определенным образом.
А именно: строчка за строчкой из двумерного хранилища записывать в одну строку.
Вуаля! Это весь секрет.
Обращение к какому-либо элементу такого одномерного массива происходит следующим образом:
mapSize — размер локальной карты
j — номер столбца
i — номер строки
Ячейки карты (опять же в соответствии с типом данных OccupancyGrid) должны иметь значения от 0 до 100. Чем меньше значение, тем больше вероятность того, что ячейка является проходимой и наоборот.
Для упрощения задачи мною были выбраны 3 основные цвета раскрашивания ячеек.
- Белый — проходимая зона = 0
- Черный — непроходимая зона = 100
- Серый — неизвестная зона = 50
Важный момент!
До прихода данных со сканера карта является полностью неизвестной (значения всех ячеек = 50) и каждый раз после прорисовки вновь обновляется до неизестного состояния. Это делается для того, чтобы карта не наслаивала на себя лишних, предыдущих значений. Ведь локальная карта отражает состояние окружающей обстановки только в данный момент времени.
Неизвестная карта
Строятся лучи c помощью преобразования из полярной системы координат (ПСК)
в декартову систему координат (ДСК).
x, y — новые координаты в ДСК
r — расстояние до препятствия
? — угол, на который был отброшен луч
r, ? — старые координаты в ПСК
Алгоритм обработки данных с датчика:
Полностью проходим по массивам расстояний r и углов ? лучей (данные ПСК). Для каждого элемента выполняем следующее:
- Преобразуем координаты из ПСК в ДСК для конечных r и ?. Закрашиваем получившуюся ячейку в черный цвет. Это препятствие.
- Проходим по прямой от местоположения сканера до ячейки с препятствием с определенным шагом, в простейшем случае равным величине ячейки.
- Вновь преобразуем данные из ПСК в ДСК и закрашиваем новую ячейку в белый цвет. Это проходимая зона
Простейший пример того, как строится проходимая дорожка до препятствия
- Луч «потерялся», то есть отразился не полностью или отразился в другом направлении
- На пути луча не возникло никаких препятствий и из-за этого ему просто не от чего было отражаться
*«Потеряться» луч может обычно потому, что отразился не под углом 180° (то есть под любым, который не дойдет обратно), так как препятствие могло стоять неперпендикулярно лучу. А как известно еще из физики: угол падения равен углу отражения.
Или потому, что препятствие было слишком черным и забрало в себя большую часть энергии луча и лучу не хватило энергии, чтобы вернуться обратно.
Таким образом, невозможно быть полностью уверенным в том, что произошло, если луч не вернулся.
Что делать в подобных ситуациях?
Делаем следующее:
- Считаем расстояние до препятствия такого луча максимально возможным для сканера (в нашем случае это 6 метров)
- Считаем все ячейки по прямой до препятствия полупроходимыми и присваиваем им промежуточное число 25. Это проходимые ячейки, но в них мы не до конца уверены.
Мы ничего не теряем, если лучи дейтвительно не встретили препятствий, а если какое-то препятствие все-таки ускользнуло от «глаз» робота, то оно обязательно обнаружится очень быстро.
Разрешение карты
Наконец, последние штрихи!
У каждой карты есть разрешение. Проще говоря, это количество ячеек, которые могут поместиться в 1 клетку.
Например
Если в 1 клетке 1 ячейка (простейший случай), значит разрешение 1.
Если в 1 клетке 5 ячеек, то разрешение 0.2.
Разрешение моей карты — 0.04. То есть в каждой клетке находится 25 ячеек. Таким образом, мой минимальный шаг — это 4 см. А 1 клетка равняется 1м.
Разница клетки и ячейки на моей карте
Что же получилось в итоге?
Пример построения локальной карты проходимости
*Желтым цветом обозначены цвета ячеек
Я считаю, что в целом проделанная мною работа была успешна, но понимаю, что алгоритм несовершенен и требует уточнений и доработок.
Комментарии (19)
mazkorulez
30.07.2018 08:31Должен сказать, что получилась очень интересная работа. Для себя я получил минимальное представление о том, как строить локальную карту у робота, какие при этом возникают проблемы и… как работать с локатором.При этом большим плюсом является рассмотрение в рамках реального оборудования и программного обеспечения, на рабочем проекте.
Также очень порадовал оригинальный способ кодирования ячеек.
По вопросу дальнейших исследований могу лишь посоветовать рассмотреть алгоритмы интерполяции и экстраполяции (можно даже эвристические, например, заполнять 4-см ячейки значением 25, если вокруг её окружают 25 и т.д.).
Работа очень хорошая, в ней освещены основные вопросы построения локальной карты, возможные «подводные камни» и «пути их обхода».
iliasam
30.07.2018 08:44+1Поправлю. Лазерный сканер «URG-04LX-UG01» использует фазовый, а не времяпролетный (TOF) принцип измерения расстояния:
Principle of distance measurement is based on calculation of the phase difference, due to which it is possible to obtain stable measurement with minimum influence from object’s color and reflectance.
За счет этого достигается разрешение в 1 мм.
Этот сканер достаточно дорогой (1000$), в любительских конструкциях можно использовать лазерные сканеры от пылесосов Neato — они продаются как запчасти по 100$.vektory79
30.07.2018 09:10А еще для совсем простых проектов недавно появился совсем дешевый датчик GY-530 VL53L0X (на али 200-300 руб всего!). Сам правда пока его не пробовал. Но было бы неплохо попробовать.
iliasam
30.07.2018 09:30Вот именно, что для самых простых. У этого датчика достаточно низкая скорость измерений и довольно широкий угол излучения, так что полноценный сканер на нем сделать нельзя.
degressor
30.07.2018 10:20Когдя я баловался с ардуинками, видел похожую идею, только на сонаре. Стоит копейки и для побаловаться снонсый результат.
trapwalker
30.07.2018 11:58+1Жаль, что карта каждый раз чистится и строится заново.
Можно попробовать учитывать данные прошлых сканирований, а за одно еще и улучшить точность навигации.
Положим робот у нас автономный и делает выводы об изменении своего положения по показаниям энкодеров на колёсах. При этом возможны проскальзывания в ту или иную сторону, что по мере движения ухудшает интегральную точность.
После некоторого перемещения можно вычислить теоретическое новое положение робота относительно абсолютной карты. Проведя повторное сканирование, а также сдвинув и повернув предыдущую карту на нужный угол (в соответствии с вычисленным по энкодерам перемещением) можно получить ожидаемую и фактическую карту. Они очевидно будут отличаться.
Итак, у нас есть две «абсолютные» карты: до (назовём её M') и после перемещения (M).
Мы знаем из каких сегментов состояло наше перемещение, например, это был поворот на (примерно) 30 градусов и движение на (примерно) 1 метр. Каждое из этих действий вносит ошибку (вычисляемую эмпиоически). Например, заставляем робота поверунться много раз на 1000 градусов, и каждый раз замеряем реальный угол поворота. Среднеквадратичное отклонение от 1000 градусов будет мерой ошибки. Её можно пронормировать и вычислять примерно какой ошибки следует ожидать в каких смещениях и каких поворотах. На самом деле ошибка состоит из двух частей, одна зависит от величины перемещения, а другая нет, и каждую надо учитывать, но это уже детали.
В конце концов зная величины возможных ошибок после перемещения у нас есть, грубо говоря, диапазоны в пределах которых нужно подёргать карту M' сравнивая её с актуальной. Сравнивать можно введя понятие нормы резонанса. Например, перемножаем карты попиксельно, а затем суммируем элементы. Наша задача найти такие перемещения, при котороых преобразованная карта входит в резонанс с актуальной. Это будут уточненные данные о нашем фактическом перемещении.
Новую карту мы можем закрасить поверх полупрозрачным серым, а сверху нанести на неё актуальную карту.
Так наша карта станет абсолютной, наш робот не будет мгновенно забывать что было «за углом», навигация станет более точной, поскольку будет опираться на характерные элементы местности, наезд на край ковра или шнур на полу не собьют фатально навигацию.
Вместо заполнения всей старой карты серым перед смешиванием с новой можно, кстати, делать размытие Motion Blur с характеристиками, соответствующими потенциальным ошибкам перемещения. Так сдвиг будет размывать карту вдоль оси перемещения пропорционально расстоянию. Поворот будет делать радиальное размытие пропорционально углу.lizatish Автор
30.07.2018 12:04Если правильно вас поняла, речь здесь уже скорее не о локальной карте (которая актуальна только в данный момент времени), а о так называемой глобаной (запоминание и наложение локальных карт с учетом перемещения). Эта задача была реализована мной, подумываю написать один из постов об этом.
trapwalker
30.07.2018 13:32Здорово! Ждём. Всегда интересны альтернативные подходы и нюансы в интересных задачах.
Я считал, что локальной картой стоит считать карту, имеющую максимальную актуальность в окрестности бота. Координаты здесь абсолютные, но локальные, заданные от произвольной точки начала ориентирования.
Глобальная карта — это карта, полученная извне с привязкой к абсолютным координатам. В этом случае мы можем её уточнять и ориентироваться по ней.
В общем для изолированного бота глобальной карты не будет. Откуда он возьмёт глобальную привязку?romalik
30.07.2018 13:54Глобальная карта получается из локальной в момент замыкания кольца и/или других оптимизаций графа. Она, конечно, не будет привязана к каким-то глобальным координатам (типа GPS), если нет внешних данных, и вообще говоря может быть искривленной, но это не будет мешать локализации и планированию траекторий, если искривления в окрестности робота незначительны
trapwalker
31.07.2018 12:38Я постарался пояснить свою классификацию глобальной/локальной карты. Для меня карта глобальная, если у неё есть абсолютная привязка, которая что-то значит для нескольких ориентирующихся сущностей. Локальная карта — это карта, которую бот строит и использует только сам.
Если бот заколеьцевал и тем самым уточнил локальную карту, то от этого она и останется локальной. Если боты скинули свои карты в облако и на их основе была синтезирована единая карта в единой системе координат — это глобальная карта. Не вижу причин не привязывать локальную координатную сетку к глобальной при первом удобном случае.
romalik
30.07.2018 12:05+1Трассирование лучей по матрице занятости для маркировки свободных ячеек — не самый удачный способ определить свободное пространство, с одной стороны — из-за возможных потерь лучей, с другой — из-за недостаточного углового разрешения (на большом удалении получится ежик из лучей, а не непрерывная поверхность). Еще и неэффективно по вычислительным затратам. Рекомендую отрисовать все полученные точки занятыми, а затем сгенерировать полигон из всех точек скана, а также положения лидара, путем сортировки списка этих точек по углу в полярных координатах и соединения их последовательно. Получится многоугольник, который можно разом отрендерить в карту цветом свободных ячеек (ну или замешать с каким-то коэффициентом).
Следующая прикольная задача — собирать эту карту в движении.lizatish Автор
30.07.2018 12:07Спасибо за совет! Карта собирается в движении, планирую написать об этом пост.
OriSvet
В Mindrover не играли случаем?
lizatish Автор
Нет)