Привет!
В этой публикации мне бы хотелось рассказать о том, как я строила локальную карту проходимости для робота. Данная задача была необходима как для повышения навыков в программировании и освоении датчиков, так и для последующего внедрения собственных алгоритмов в работу реальных роботов на таких робототехнических соренованиях, как «Робокросс» и «Робофест».
Эта статья рассчитана на тех, кто только входит в мир робототехники или пытается разобраться с построением карты проходимости. Я старалась изложить все максимально простым и понятным языком, понятным для большинства людей.
Что такое локальная карта проходимости
Итак, локальная карта проходимости — это то, что видит робот в данный момент времени.
Это та информация, которая приходит с «глаз» робота и впоследствии обрабатывается и выводится в удобном нам виде.
Если робот стоит на месте, то его локальная карта при неизменных условиях окружающей среды остается постоянной.
Если робот движется, то в каждый момент времени его окружающая обстановка разная, соответственно, локальная карта тоже меняется.
Локальная карта обычно имеет постоянные размеры. Размер высчитывается исходя из максимальной длины лучей, испускаемых дальномером. В моем случае эта длина составляет 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 только хранится и передается в одномерном массиве. При расшифровке ее данных она автоматически превратится в квадратную двумерную карту.
Но чтобы это произошло корректно, необходимо записывать этот одномерный масив определенным образом.
А именно: строчка за строчкой из двумерного хранилища записывать в одну строку.
Вуаля! Это весь секрет.
Обращение к какому-либо элементу такого одномерного массива происходит следующим образом:
$$display$$localMap[mapSize * j + i]$$display$$
mapSize — размер локальной карты
j — номер столбца
i — номер строки
Ячейки карты (опять же в соответствии с типом данных OccupancyGrid) должны иметь значения от 0 до 100. Чем меньше значение, тем больше вероятность того, что ячейка является проходимой и наоборот.
Для упрощения задачи мною были выбраны 3 основные цвета раскрашивания ячеек.
- Белый — проходимая зона = 0
- Черный — непроходимая зона = 100
- Серый — неизвестная зона = 50
Важный момент!
До прихода данных со сканера карта является полностью неизвестной (значения всех ячеек = 50) и каждый раз после прорисовки вновь обновляется до неизестного состояния. Это делается для того, чтобы карта не наслаивала на себя лишних, предыдущих значений. Ведь локальная карта отражает состояние окружающей обстановки только в данный момент времени.
Неизвестная карта
Строятся лучи c помощью преобразования из полярной системы координат (ПСК)
в декартову систему координат (ДСК).
$$display$$left{begin{gather} x = r * cos φ \ y = r * sin φ end{gather}right.$$display$$
x, y — новые координаты в ДСК
r — расстояние до препятствия
φ — угол, на который был отброшен луч
r, φ — старые координаты в ПСК
Алгоритм обработки данных с датчика:
Полностью проходим по массивам расстояний r и углов φ лучей (данные ПСК). Для каждого элемента выполняем следующее:
- Преобразуем координаты из ПСК в ДСК для конечных r и φ. Закрашиваем получившуюся ячейку в черный цвет. Это препятствие.
- Проходим по прямой от местоположения сканера до ячейки с препятствием с определенным шагом, в простейшем случае равным величине ячейки.
- Вновь преобразуем данные из ПСК в ДСК и закрашиваем новую ячейку в белый цвет. Это проходимая зона
Простейший пример того, как строится проходимая дорожка до препятствия
- Луч «потерялся», то есть отразился не полностью или отразился в другом направлении
- На пути луча не возникло никаких препятствий и из-за этого ему просто не от чего было отражаться
*«Потеряться» луч может обычно потому, что отразился не под углом 180° (то есть под любым, который не дойдет обратно), так как препятствие могло стоять неперпендикулярно лучу. А как известно еще из физики: угол падения равен углу отражения.
Или потому, что препятствие было слишком черным и забрало в себя большую часть энергии луча и лучу не хватило энергии, чтобы вернуться обратно.
Таким образом, невозможно быть полностью уверенным в том, что произошло, если луч не вернулся.
Что делать в подобных ситуациях?
Делаем следующее:
- Считаем расстояние до препятствия такого луча максимально возможным для сканера (в нашем случае это 6 метров)
- Считаем все ячейки по прямой до препятствия полупроходимыми и присваиваем им промежуточное число 25. Это проходимые ячейки, но в них мы не до конца уверены.
Мы ничего не теряем, если лучи дейтвительно не встретили препятствий, а если какое-то препятствие все-таки ускользнуло от «глаз» робота, то оно обязательно обнаружится очень быстро.
Разрешение карты
Наконец, последние штрихи!
У каждой карты есть разрешение. Проще говоря, это количество ячеек, которые могут поместиться в 1 клетку.
Например
Если в 1 клетке 1 ячейка (простейший случай), значит разрешение 1.
Если в 1 клетке 5 ячеек, то разрешение 0.2.
Разрешение моей карты — 0.04. То есть в каждой клетке находится 25 ячеек. Таким образом, мой минимальный шаг — это 4 см. А 1 клетка равняется 1м.
Разница клетки и ячейки на моей карте
Что же получилось в итоге?
Пример построения локальной карты проходимости
*Желтым цветом обозначены цвета ячеек
Я считаю, что в целом проделанная мною работа была успешна, но понимаю, что алгоритм несовершенен и требует уточнений и доработок.
Автор: lizatish
