Работа с SLAM в ROS на Raspberry Pi 3 на примере hector_slam

Добрый день, уважаемые хабрачитатели! В предыдущих статьях мы познакомились с возможностями робототехнической платформы ROS для решения различных задач в робототехнике, главным образом на микрокомпьютере Raspberry Pi. Я напомню, что мы уже научились получать видеопоток с камеры и решать задачи компьютерного зрения с OpenCV, взаимодействовать с микроконтроллером Arduino, контролировать робота с клавиатуры и с помощью игрового контроллера PS3 Dualshock, и наконец, использовать данные сенсора глубины Kinect для задач компьютерного зрения в 3D.

Теперь мы рассмотрим задачу локализации робота в пространстве. Мы будем использовать лазерный дальномер 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 для создания карты:

<?xml version="1.0"?>
<launch>  

  <node pkg="tf" type="static_transform_publisher" name="laser_link" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 50" />

  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
    
    <!-- Frame names -->
    <param name="map_frame" value="map" />
    <param name="odom_frame" value="base_link" />
        
    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="1024"/>
    <param name="map_start_x" value="0.5"/>     //середина карты
    <param name="map_start_y" value="0.5" />
    <param name="map_multi_res_levels" value="2" />
    
    <!-- Map update parameters -->
    <param name="update_factor_free" value="0.4"/>
    <param name="update_factor_occupied" value="0.9" />    
    <param name="map_update_distance_thresh" value="0.4"/>
    <param name="map_update_angle_thresh" value="0.06" />
    <param name="laser_z_min_value" value="-1.0" />
    <param name="laser_z_max_value" value="1.0" />
    
    <!-- Advertising config -->
    <param name="advertise_map_service" value="true"/>
    <param name="scan_subscriber_queue_size" value="5"/>
    <param name="scan_topic" value="scan"/>
    
   </node>
</launch>

Здесь мы запускаем узлы tf и hector_mapping. hector_mapping требует точной настройки некоторых параметров и имен систем координат (reference frame). Лидар может быть установлен с некоторым смещением и под некоторым углом к поверхности, по которой движется робот. Для корректного построения карты нам необходимо выполнить преобразование из системы координат лидара к системе координат поверхности движения. Соотношение всех основных систем координат, участвующих в движении робота, можно посмотреть здесь.
Преобразование между системами координат лидара /laser и плоскости движения /base_link реализуется с помощью узла static_transform_publisher из пакета tf:

<node pkg="tf" type="static_transform_publisher" name="laser_link" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 50" />

Параметры в значении args соответствуют формату “x y z yaw pitch roll”, т. е. сдвиг по x, y, z и поворот вокруг осей z, y и x соответственно.

Запускаем узел hector_mapping:

<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

Лидар у нас зафискирован в плоскости движения робота и одометрия отсутствует, поэтому мы будем использовать только некоторые из систем координат:

<param name="map_frame" value="map" />
<param name="odom_frame" value="base_link" />

В конечном итоге мы имеет преобразование /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 и построили карту помещения. Желаю всем успеха и до новых встреч!

Автор: vovaekb90

Источник