Построение локальной карты проходимости робота

Привет, Хабр!

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

Эта статья рассчитана на тех, кто только входит в мир робототехники или пытается разобраться с построением карты проходимости. Я старалась изложить все максимально простым и понятным языком, понятным для большинства людей.

Что такое локальная карта проходимости

Итак, локальная карта проходимости — это то, что видит робот в данный момент времени.
Это та информация, которая приходит с «глаз» робота и впоследствии обрабатывается и выводится в удобном нам виде.

Если робот стоит на месте, то его локальная карта при неизменных условиях окружающей среды остается постоянной.

Если робот движется, то в каждый момент времени его окружающая обстановка разная, соответственно, локальная карта тоже меняется.

Локальная карта обычно имеет постоянные размеры. Размер высчитывается исходя из максимальной длины лучей, испускаемых дальномером. В моем случае эта длина составляет 6 метров.

Чтобы упростить себе задачу, карту решено было сделать квадратной.Также было решено, что условно дальномер будет находиться ровно по центру карты (это место будет точкой, где x = y = 0). Центр был выбран таким образом, потому что дальномер, который я использую, испускает лучи в плоскости более, чем на 180° (он испускает лучи на 240°, но об этом чуть позже), то есть какие-то лучи непременно уйдут за сканер и при неверном выборе центра их можно потерять. При грамотном выборе центра все лучи будут корректно отображены. Исходя из этого, размер карты я сделала в 2 раза больше, чем максимальная длина испускаемых лучей.
Размер моей карты равен 12 * 12 метров.

image

Датчик, который я использовала

На самом деле для решения подобного рода задачи можно использовать любой дальномер.
Дальномер — это прибор для определения расстояния до чего-либо (в моем случае до потенциального препятствия).

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

Лазерные дальномеры дороже, но точнее, так как их лучи узконаправленны.

Для решения своей задачи я использовала лазерный сканер Hokuyo URG-04LX-UG01. Этот датчик способен испускать лучи на 240° и дает достаточно точную информацию о препятствиях, которые встали на пути лучей. Его максимальная дальность — примено 5 — 6 метров. Стоит отметить, что данный дальномер испускает лучи только в 2D плоскости. Этот факт обязывает ставить датчик на робота в определенное место, обычно спереди снизу робота, для получения более точной картины. Опять же, можно использовать и 3D-сканеры, которые дают гораздо более точную и полную информацию об окружающей среде, но и стоят они гораздо дороже.

Считаю, что именно этот сканер прекрасно подходит для обучения в соотношении цена-качество.

image
Hokuyo URG-04LX-UG01

Коротко о принципе действия лазерного сканера:
Дальномер испускает лучи вдоль плоскости. Луч, который встретил на своем пути препятствие, отражается от него и возвращается обратно. По разности фаз между посылаемым и принимаемым сигналами, можно судить о том, насколько далеко расположено препятствие.

Соответственно, если испускаемый луч не вернулся, то в 5 — 6 метрах вдоль прямой его испускания либо нет никаких препятствий, либо луч не смог корректно отразиться.

image

С лазерного сканера можно получить следующие данные:

Для каждого испущенного луча:

  • Расстояние до препятствия
  • Угол испускания

*Каждый из параметров хранится в отдельном массиве и соответствует данным для одного из лучей.

О построении карты

Для построния карты и ее отрисовки мною были использованы средства ROS (Robot Operating System), а именно: программа Rviz и тип данных nav_msgs::OccupancyGrid. Я создала публикатор (publisher) локальной карты с типом сообщений nav_msgs::OccupancyGrid под соответстующим топиком local_map. В Rviz, подписываясь на данный топик, принимала данные о карте и выводила их в форме типа Map.

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

Что здесь происходит?

Для тех, кто впервые сталкивается с подобным типом данных ROS: это необычно, так как карта привычным образом представяется в виде двумерного массива с определенным числом столбцов и строк.

Так было и у меня. Карту на основе данных со сканера я храню в двумерном массиве, а при формировании сообщения для отправки в Rviz, я преобразую двумерный массив в одомерный, необходимый для OccupancyGrid.

На самом деле карта в OccupancyGrid только хранится и передается в одномерном массиве. При расшифровке ее данных она автоматически превратится в квадратную двумерную карту.
Но чтобы это произошло корректно, необходимо записывать этот одномерный масив определенным образом.

А именно: строчка за строчкой из двумерного хранилища записывать в одну строку.

Вуаля! Это весь секрет.

Обращение к какому-либо элементу такого одномерного массива происходит следующим образом:

$localMap[mapSize * j + i]$

mapSize — размер локальной карты
j — номер столбца
i — номер строки

Ячейки карты (опять же в соответствии с типом данных OccupancyGrid) должны иметь значения от 0 до 100. Чем меньше значение, тем больше вероятность того, что ячейка является проходимой и наоборот.

Для упрощения задачи мною были выбраны 3 основные цвета раскрашивания ячеек.

  • Белый — проходимая зона = 0
  • Черный — непроходимая зона = 100
  • Серый — неизвестная зона = 50

Важный момент!

До прихода данных со сканера карта является полностью неизвестной (значения всех ячеек = 50) и каждый раз после прорисовки вновь обновляется до неизестного состояния. Это делается для того, чтобы карта не наслаивала на себя лишних, предыдущих значений. Ведь локальная карта отражает состояние окружающей обстановки только в данный момент времени.

image
Неизвестная карта

Строятся лучи c помощью преобразования из полярной системы координат (ПСК)
в декартову систему координат (ДСК).

$$display$$\left\{\begin{gather} x = r * cos φ \\ y = r * sin φ \end{gather}\right.$$display$$

x, y — новые координаты в ДСК
r — расстояние до препятствия
φ — угол, на который был отброшен луч
r, φ — старые координаты в ПСК

Алгоритм обработки данных с датчика:

Полностью проходим по массивам расстояний r и углов φ лучей (данные ПСК). Для каждого элемента выполняем следующее:

  1. Преобразуем координаты из ПСК в ДСК для конечных r и φ. Закрашиваем получившуюся ячейку в черный цвет. Это препятствие.
  2. Проходим по прямой от местоположения сканера до ячейки с препятствием с определенным шагом, в простейшем случае равным величине ячейки.
  3. Вновь преобразуем данные из ПСК в ДСК и закрашиваем новую ячейку в белый цвет. Это проходимая зона

image
Простейший пример того, как строится проходимая дорожка до препятствия

А что, если испускаемый луч не вернулся?

Если такое произошло, это может означать следующее:

  • Луч «потерялся», то есть отразился не полностью или отразился в другом направлении
  • На пути луча не возникло никаких препятствий и из-за этого ему просто не от чего было отражаться

*«Потеряться» луч может обычно потому, что отразился не под углом 180° (то есть под любым, который не дойдет обратно), так как препятствие могло стоять неперпендикулярно лучу. А как известно еще из физики: угол падения равен углу отражения.

image

Или потому, что препятствие было слишком черным и забрало в себя большую часть энергии луча и лучу не хватило энергии, чтобы вернуться обратно.

Таким образом, невозможно быть полностью уверенным в том, что произошло, если луч не вернулся.

Что делать в подобных ситуациях?

Делаем следующее:

  1. Считаем расстояние до препятствия такого луча максимально возможным для сканера (в нашем случае это 6 метров)
  2. Считаем все ячейки по прямой до препятствия полупроходимыми и присваиваем им промежуточное число 25. Это проходимые ячейки, но в них мы не до конца уверены.

Мы ничего не теряем, если лучи дейтвительно не встретили препятствий, а если какое-то препятствие все-таки ускользнуло от «глаз» робота, то оно обязательно обнаружится очень быстро.

Разрешение карты

Наконец, последние штрихи!

У каждой карты есть разрешение. Проще говоря, это количество ячеек, которые могут поместиться в 1 клетку.

Например
Если в 1 клетке 1 ячейка (простейший случай), значит разрешение 1.
Если в 1 клетке 5 ячеек, то разрешение 0.2.

Разрешение моей карты — 0.04. То есть в каждой клетке находится 25 ячеек. Таким образом, мой минимальный шаг — это 4 см. А 1 клетка равняется 1м.

image
Разница клетки и ячейки на моей карте

Что же получилось в итоге?

image
Пример построения локальной карты проходимости
*Желтым цветом обозначены цвета ячеек

Я считаю, что в целом проделанная мною работа была успешна, но понимаю, что алгоритм несовершенен и требует уточнений и доработок.

FavoriteLoadingДобавить в избранное
Posted in Без рубрики

Добавить комментарий