Теперь мы рассмотрим задачу локализации робота в пространстве. Мы будем использовать лазерный дальномер RPLidar и алгоритм Hector SLAM на Raspberry Pi 3 с использованием ROS Kinetic. Давайте начнем!
Что такое SLAM в двух словах
Задача локализации робота заключается в определении точной позиции робота в некоторой среде, например, в офисных помещениях. Для решения задачи локализации роботу необходимо иметь информацию об этой среде. Для этого роботу может быть достаточно построить карту помещений — визуальное представление среды, в которой он действует. Карта содержит информацию о расположении стен и других препятствий. Это позволяет роботу перемещаться в пространстве и планировать путь к цели таким образом, чтобы обходить препятствия в виде стен и объектов. Одним из методов построения карт и локализации является SLAM.
SLAM является аббривиатурой для Simultaneous Localization and Mapping. Суть SLAM заключается в том, что робот параллельно строит карту незнакомой местности на основе данных сенсоров (лидар, камера глубины или обычная RGB камера в зависимости от алгоритма) и одновременно определяет свое местоположение по отношению к построенной карте. Это может использоваться в дальнейшем для перемещения робота из точки A в точку B в автономном режиме. Для использования Hector SLAM нам нужно будет установить и настроить Ubuntu 16.04 для корректной работы ROS Kinetic, а также драйвера для лидара RPLidar.
Аппаратная основа
Как я уже сказал, нам понадобится лазерный дальномер RPLidar и микрокомпьютер Raspberry Pi последней третьей версии.
Почему именно RPLidar? Для построения карты нам нужен либо лазерный дальномер (лидар) либо камера (обычная RGB или камера глубины). Мы будем использовать простой алгоритм Hector SLAM, который использует данные 2D лидара. RPLidar является недорогим и довольно производительным лидаром, разработанным в компании RoboPeak. Лидар способен сканировать окружающее пространство с частотой 5.5 Гц в угловом охвате 360 градусов на расстоянии до 6 метров. Он создает 2D облако точек, так называемый плоский срез. Каждая точка облака имеет точные координаты относительно системы координат лазера.
Лидар состоит из двух частей: фиксированной основы и вращающегося сканера. Благодаря системе мотора круглый сканер вращается в направлении часовой стрелки и получает 360 точек за один полный оборот.
Подключается RPLidar к Raspberry Pi через USB вход. Стоит отметить, что мы будем использовать RPLidar A1. Более детально можно прочитать о лидаре в официальной спецификации. Он поставляется в коробке вместе с USB-microUSB кабелем, UART-USB кабелем и маленьким манулом.
Почему именно Raspberry Pi 3? Это самая последняя версия микрокомпьютера всего за 36 долларов. Raspberry Pi 3 более производительный, чем его предшественники. Он имеет процессор 1.2 Гц с четырьмя ядрами, 1 Гб RAM. Внешне Raspberry Pi 3 полностью повторяет вторую версию: те же 4 порта USB, Ethernet порт, 40 входов GPIO, HDMI вход и microUSB для питания, тот же слот для microSD карт. Из новинок третья версия может похвастаться встроенным модулем беспроводной сети 802.11n Wireless LAN. Так получилось, что я недавно приобрел один такой экземпляр и решил попробовать его для своих экспериментов с использованием Hector SLAM и RPLidar. Что из этого получилось, мы увидим дальше в статье.
Установка Ubuntu 16.04 на Raspberry Pi 3
Итак, у нас на руках последняя версия Raspberry Pi. Имеет смысл установить на него Ubuntu последней версии 16.04. Это позволит нам попробовать последнюю выпущенную версию ROS Kinetic. Скачать образ Ubuntu 16.04 можно отсюда.
Пожалуй начнем. Скачаем образ, разархивируем его и запишем на карту:
unxz ubuntu-standard-16.04-server-armhf-raspberry-pi.img.xz sudo ddrescue -D --force ubuntu-standard-16.04-server-armhf-raspberry-pi.img /dev/mmcblk0 Здесь /dev/mmcblk0 это точка монтирования microSD карты. Это можно проверить с помощью команды lsblk.
Теперь загрузим Raspberry Pi. Логин и пароль администратора будут одинаковые — ubuntu. Сначала зададим имя хоста в файлах /etc/hostname и /etc/hosts:
nano /etc/hostname nano /etc/hosts Конкретно, в файле /etc/hosts добавим строку:
127.0.1.1 ubuntu-pi Здесь ubuntu-pi — имя вашего хоста.
Создадим нового пользователя:
sudo adduser username Дадим права администратора новому пользователю используя команду visudo:
newuser ALL=(ALL:ALL) ALL Актуализуем систему:
sudo apt-get update sudo apt-get upgrade Последнее, что нужно сделать, это ресайз файловой системы, т. е. выделить системе все доступное пространство на карте. По умолчанию при записи образа только часть доступного пространства на карте выделяется под систему. Мы выполним команду:
sudo fdisk /dev/mmcblk0 Удалим второй раздел введя с клавиатуры: d, 2. Затем заново создадим раздел используя установки по умолчанию: n, p, 2, enter, enter. Наконец, запишем изменения и выйдем (w). Сделаем ребут и выполним команду:
sudo resize2fs /dev/mmcblk0p2 После перезапуска может возникнуть одна проблема: может стать недоступным сетевой интерфейс eth0. При выполнении команды ifconfig, мы не увидим информации о eth0. Укажем IP адрес Raspberry Pi, добавив строку типа ‘ip = 192.168.0.4’ в конец файла cmdline.txt:
sudo nano /boot/cmdline.txt Проверьте, что вы не ввели лишних пробелов. Теперь просто сделайте ребут и проверьте, что IP адрес был установлен командой: hostname -l.
Установка ROS Kinetic
Установка ROS Kinectic почти в точности повторяет установку ROS Indigo на Raspbian.
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 0xB01FA116 sudo apt-get update sudo apt-get install python-rosdep python-rosinstall-generator python-wstool python-rosinstall build-essential sudo rosdep init rosdep update Установка пакетов ROS Kinetic предельно проста — используем apt-get:
sudo apt-get install ros-kinetic-desktop Установим rviz — графическую утилиту для визуализации данных, получаемых с сенсоров, в ROS:
sudo apt-get install ros-kinetic-rviz Добавим путь к пакетам ROS в системный путь с помощью команды source:
source /opt/ros/kinetic/setup.bash Добавим команду в файл ~/.bashrc, чтобы не приходилось выполнять эту команду при открытии нового окна терминала:
echo ‘source /opt/ros/kinetic/setup.bash’ >> ~/.bashrc Проверим, что ROS и rviz установлены:
roscore rosrun rviz rviz Должно открыться окно rviz подобное этому
Про rviz можно подробно прочитать здесь. Также нам нужно создать рабочий каталог (catkin workspace) для наших пакетов:
mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src catkin_init_workspace cd ~/catkin_ws catkin_make source devel/setup.bash
Установка драйверов и настройка окружения для RPLidar и hector_slam
Теперь нам осталось только установить необходимые пакеты для Hector SLAM и драйвер для нашего RPLidar. Для работы с Hector SLAM установим специальный ROS пакет hector_slam через apt-get:
sudo apt-get install ros-kinetic-hector-slam Нам будет также необходим пакет rplidar_ros. Этот пакет отвечает за обработку данных, получаемых с лазера RPLidar, и публикацию этих данных на специальный топик. Установим rplidar_ros драйвер для RPLidar:
cd ~/catkin_ws/src git clone https://github.com/robopeak/rplidar_ros.git cd ~/catkin_ws catkin_make source devel/setup.bash
Как работает Hector SLAM
Для работы Hector SLAM требует только данные 2D лидара в формате sensor_msgs/LaserScan (публикуются в топик /scan) и способен выполнять построение карты и локализацию на той же частоте, на которой лидар выполняет сканирование. При этом Hector SLAM работает без данных одометрии. Больше информации об алгоритме можно найти в презентации.
Сам лидар дает множество точек в пространстве, для каждой точки он вычисляет угол этой точки относительно начала отсчета. hector_slam для работы кроме наличия источника данных sensor_msgs/LaserScan (лидара) требует выполнения преобразования из системы координат лидара в систему координат плоскости движения робота. Об этом будет сказано дальше.
Использование hector_slam с RPLidar
Мы наконец-то может попробовать все на практике. Для начала, запустим наш драйвер для RPLidar:
roscore roslaunch rplidar_ros rplidar.launch Мы можем в первый раз получить ошибку доступа к порту /dev/ttyUSB0:
Error, cannot bind to the specified serial port /dev/ttyUSB0. Нам нужно дать права на чтение / запись порта для всех пользователей:
sudo chmod a+rw /dev/ttyUSB0 Теперь все пойдет без проблем:
roslaunch rplidar_ros rplidar.launch rosrun rviz rviz Подобно тому, как мы делали в статье, найдем в окне rviz в левой панели Displays секцию Global options, развернем ее и установим значение /laser для поля Fixed frame.
В той же левой панели внизу нажмите кнопку Add и выберите в списке в открывшемся окне LaserScan и нажмите OK. В левой панели Displays появится новый пункт LaserScan. Разверните его и введите значение топика /scan.
Создадим новый пакет my_hector_mapping:
cd ~/catkin_ws catkin_create_pkg my_hector_mapping cd src/my_hector_mapping Внутри пакета создадим лаунч файл my_launch.launch для создания карты:
//середина карты
Здесь мы запускаем узлы tf и hector_mapping. hector_mapping требует точной настройки некоторых параметров и имен систем координат (reference frame). Лидар может быть установлен с некоторым смещением и под некоторым углом к поверхности, по которой движется робот. Для корректного построения карты нам необходимо выполнить преобразование из системы координат лидара к системе координат поверхности движения. Соотношение всех основных систем координат, участвующих в движении робота, можно посмотреть здесь.
Преобразование между системами координат лидара /laser и плоскости движения /base_link реализуется с помощью узла static_transform_publisher из пакета tf:
Параметры в значении args соответствуют формату “x y z yaw pitch roll”, т. е. сдвиг по x, y, z и поворот вокруг осей z, y и x соответственно.
Запускаем узел hector_mapping:
Лидар у нас зафискирован в плоскости движения робота и одометрия отсутствует, поэтому мы будем использовать только некоторые из систем координат:
В конечном итоге мы имеет преобразование /laser → /base_link → /base_footprint → /map_frame. Компилируем и запускаем:
cd ~/catkin_ws catkin_make source devel/setup.bash roslaunch my_hector_mapping my_launch.launch Покажем список топиков:
rostopic list Среди вывода мы заметим новые узлы, созданные hector_slam:
/initialpose /map /map_metadata /move_base_simple/goal /poseupdate /slam_out_pose Самые важные топики /map и /slam_out_pose. Топик /map представляет карту, которая строится в процессе SLAM. Сообщения, публикуемые в топик map, имеют тип nav_msgs/OccupancyGrid и представляют собой 2D сеточную карту, каждая ячейка которой определяет вероятность занятости определенной дискретной позиции в пространстве (это может быть часть стены или объект). Ячейка имеет определенный цвет в зависимости от степени занятости: черный цвет, если занята (вероятная максимальна), светло-серый, если свободна. Покажем карту в rviz.
Добавим новый дисплей Map с топиком map.
Я также добавил дисплей Axes чтобы показать позицию робота относительно карты.
Здесь мы видим, что данные лидара (разноцветные точки на сетке) совпадают с некоторыми черными границами карты, которые представляют стены.
Для построения полной карты нужно объехать помещение управляя роботом с клавиатуры. hector_slam работает довольно быстро благодаря высокой частоте сканирования лидара, поэтому много времени процесс не займет.
У меня в момент проведения экспериментов не было в наличии робота, поэтому я просто перемещал лидар из одной точки пространства в другую вручную. У меня часто возникало несоответствие при построении карты (например, лидар был повернут на некоторый угол относительно предыдущего положения) подобно как на картинке:
В таком случае можно очистить карту, опубликовав сообщение «reset» в служебный топик syscommand (позволяет контролировать процесс SLAM с помощью команд):
rostopic pub syscommand std_msgs/String "reset" В своих экспериментах я организовал маленькое ограниченное со всех сторон пространство из подручных материалов площадью около 2 кв. метров. В конечном итоге у меня была получена такая карта:
Второй важный топик slam_out_pose с типом сообщений geometry_msgs/PoseStamped. Он указывает положение робота в пространстве. Добавим дисплей Pose с топиком slam_out_pose в rviz. В rviz появится красная стрелка как на последней картинке.
Красная стрелка указывает в направлении оси X лидара. Система координат лидара RPLidar выглядит как на картинке:
В ROS есть замечательный пакет по управлению картами map_server. С его помощью например можно сохранить сгенерированную карту в файл. Установим пакет:
sudo apt-get install ros-kinetic-map-server Запустим узел map_saver из пакета:
rosrun map_server map_saver После выполнения будет создано два файла: map.pgm и map.yaml в текущей директории. В файле map.pgm будет сохранена карта. В файле map.yaml хранятся метаданные о карте: разрешение карты (количество метров на пиксель), путь до файла pgm и другие. Подробно можно прочитать о формате yaml здесь. Карты в формате yaml используются для локализации, например в алгоритме amcl.
Карта в pgm формате выглядит подобным образом:
Здесь каждый пиксель имеет один из трех цветов: белый — пространство свободно, черный — пространство занято препятствием или серый — пока не исследовано роботом.
Интересно отметить, что даже на Raspberry Pi 3 все работает быстро, никаких задержек с получением данных лидара или выполнением Hector SLAM не возникает, все работает real-time.
На этом пока все. Мы установили ROS Kinetic на Raspberry Pi 3, познакомились с алгоритмом Hector SLAM и построили карту помещения. Желаю всем успеха и до новых встреч!
Источник
