Хабрахабр

Робот-тележка на ROS. Часть 7. Локализация робота: gmapping, AMCL, реперные точки на карте помещения

Продолжение цикла статей.

Предыдущие посты серии:

Часть 6 Часть 5 Часть 4 Часть 3 Часть 2 Часть 1

Однако робот, несмотря на наличие карты и настройку одометрии с оптических датчиков все еще чувствует себя неуверенно в окружающей обстановке.
Вернее, он ее вообще не видит. В прошлый раз, после установки бюджетного лидара RPlidar-A1 удалось построить карту помещения, поработать с одометрией. Это и радует, и огорчает одновременно. И ездит по готовой карте вдоль и поперек, препятствия не для него. Поэтому поговорим о локализации робота в пространстве, используя алгоритмы, которые предоставляет ROS, а также наш джентельменский набор из лидара и энкодеров. С одной стороны не стоит беспокоится о препятствиях и ездить, куда душа пожелает, с другой стороны — поехать в другую комнату или на кухню, вряд ли получится. Познакомимся с gmapping.
Но перед тем как перейдем непосредственно к локализации, поговорим еще об одном пакете ROS, который также позволяет строить 2D карты помещения и порой это у него получается получше, чем у ROS пакета из предыдущего поста.

Gmapping из клана ROS.

Не будем оригиналами и воспользуемся наработками из уже существующей статьи Хабра по теме, но расширим, обновим и углубим, содержащиеся в ней сведения. Статья называется Построение карты и локализация мобильного робота в ROS без одометрии с использованием laser_scan_matcher

Часть манипуляций (высоконагруженные приложения — rviz) будут производиться на Компьютере (вне робота), остальное (драйвер движения, нода запуска лидара) — на Роботе.

Установим по сценарию вышеуказанной статьи сначала laser_scan_matcher для ROS-kinetic на Компьютере (в статье было indigo):
sudo apt-get install ros-kinetic-laser-scan-matcher

Теперь запустим.
Ноду с лидаром на Роботе:
roslaunch rplidar_ros rplidar.launch
На Компьютере:
roslaunch laser_scan_matcher demo.launch
*roscore запускать нет необходимости, так как master-нода стартует каждый раз при загрузке Робота.
В запустившемся rviz будут видны контуры комнаты и мусора в ней:
image

Сам gmapping устанавливать нет необходимости, он уже есть на виртуальной машине в составе ROS Kinetic. laser_scan_matcher понадобится нам для работы с gmapping пакетом ROS. Проверим наличие пакета в системе:

Теперь создадим на Компьютере (не на Роботе), как и в вышеупомянутой статье, launch файл, использующий gmapping:
roscd roscd rosbots_description/launch
nano my_gmapping_launch.launch
Поместим туда тот же

код

<?xml version="1.0"?>
<launch> <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" /> <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen"> <param name="fixed_frame" value = "odom"/> <param name="use_odom" value="true"/> <param name="publish_odom" value = "true"/> <param name="use_alpha_beta" value="true"/> <param name="max_iterations" value="10"/> </node> <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"> <param name="map_udpate_interval" value="1.0"/> <param name="delta" value="0.02"/> </node>
</launch>

Как видно из кода, автор запускает 3-и ноды: tf, laser_scan_matcher и gmapping.

Cнова запустим лидар на Роботе:
roslaunch rplidar_ros rplidar.launch
На Компьютере вновь испеченный launch файл и редактор rviz:
roslaunch rosbots_description my_gmapping_launch.launch
rosrun rviz rviz

Только в этот раз работает пакет gmapping.
И надо признать, работает он не дурно. В rviz получим картинку, схожую с той, что была получена при построении карты в нашем предыдущем посте о Роботе-тележке. Если hector_slam оставлял многочисленные артефакты на карте при повороте лидара вокруг своей оси, то в этот раз артефактов почти нет:

После поездок по помещению, построенная карта сохраняется также:
rosrun map_server map_saver -f map-1, где map-1 имя карты для сохранения.

Локализация с amcl

AMCL использует многочастичный фильтр для отслеживания положения робота на карте. Алгоритм, который применяется для определения местоположения робота на карте, называется AMCL. В нашем роботе для реализации AMCL мы используем пакет ROS (http://wiki.ros.org/amcl).

Запустим AMCL для нашего робота.
Для этого на Компьютере в папке проекта создадим еще один launch файл.
Назовем его

amcl-2.launch

<launch> <param name="/use_sim_time" value="false"/> <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" /> <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen"> <param name="fixed_frame" value = "odom"/> <param name="use_alpha_beta" value="true"/> <param name="max_iterations" value="10"/> </node> <node name="map_server" pkg="map_server" type="map_server" args="/home/pi/catkin_ws/src/rosbots_description/maps/map-3.yaml"/> <node pkg="amcl" type="amcl" name="amcl" output="screen" > <!-- Publish scans from best pose at a max of 10 Hz --> <param name="odom_model_type" value="diff"/> <param name="odom_alpha5" value="0.1"/> <param name="transform_tolerance" value="0.2" /> <param name="gui_publish_rate" value="10.0"/> <param name="laser_max_beams" value="30"/> <param name="min_particles" value="500"/> <param name="max_particles" value="5000"/> <param name="kld_err" value="0.05"/> <param name="kld_z" value="0.99"/> <param name="odom_alpha1" value="0.2"/> <param name="odom_alpha2" value="0.2"/> <!-- laser, translation std dev, m --> <param name="laser_min_range" value="-1"/> <param name="laser_max_range" value="-1"/> <param name="odom_alpha3" value="0.8"/> <param name="odom_alpha4" value="0.2"/> <param name="laser_z_hit" value="0.5"/> <param name="laser_z_short" value="0.05"/> <param name="laser_z_max" value="0.05"/> <param name="laser_z_rand" value="0.5"/> <param name="laser_sigma_hit" value="0.2"/> <param name="laser_lambda_short" value="0.1"/> <param name="laser_lambda_short" value="0.1"/> <param name="laser_model_type" value="likelihood_field"/> <!-- <param name="laser_model_type" value="beam"/> --> <param name="laser_likelihood_max_dist" value="2.0"/> <param name="update_min_d" value="0.2"/> <param name="update_min_a" value="0.5"/> <param name="odom_frame_id" value="odom"/> <param name="base_frame_id" type="str" value="base_link" /> <param name="global_frame_id" type="str" value="map" /> <param name="resample_interval" value="1"/> <param name="transform_tolerance" value="0.1"/> <param name="recovery_alpha_slow" value="0.0"/> <param name="recovery_alpha_fast" value="0.0"/> <param name="use_map_topic" value="true" /> <param name="first_map_only" value="true" />
</node> </launch>

Код полностью идентичен уже упомянутой статье, за исключением:
— исключена нода, запускающая лидар hokuyo (он запускается на Роботе)
— путь и название карты помещения иное (map-3.yaml)
В коде запускаются 4-е ноды:
— tf
— map_server
— laser_scan_matcher
— amcl
Нода amcl использует карту, которую публикует map_server для последующей локализации робота.

Запустим launch файлы и посмотрим на результат.
Но Роботе:
roslaunch rplidar_ros rplidar.launch
На Компьютере:
1-й терминал: roslaunch rosbots_description amcl-2.launch
2-й терминал: roslaunch rosbots_description rviz.launch

После того как запустится rviz, следующие шаги в этом редакторе будут следующие:
— добавить displays в rviz:
• LaserScan
• Map
• PoseArray

Нужна «первоначальная инициализация».
Для это в rviz надо выбрать «2D Pose Estimate» и зеленой стрелкой прямо в окне, где изображен робот обозначить его позицию:

Данную операцию необходимо проделать, выбрав Frame «map» в rviz:

В терминале получим координаты (pose) робота:
[ INFO] [1572374324. — локализовать робота на начальной стадии, так как при запуске amcl не знает где робот стартовал и где он находится. 454806): -0. 454855505]: Setting pose (1572374324. 012 0. 014 -0. 655
Выставлять позиция робота на карте с помощью зеленой стрелки на карте можно многократно.
При этом желательно, чтобы данные с лидара (красная окантовка) на карте сопадала или была близка к реальному расположению стен помещения:

В окне визуализации rviz получим характерные стрелки красного цвета вокруг робота*:

*в качестве робота у нас оси для наглядности (вся расписная rviz модель пока скрыта).
**Если стрелки не появились, то можно попробовать снять и вновь поставить галочку в добавленном в rviz дисплее PoseArray.

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

У нас из этого набора: лидар и энкодеры.
Но мы поездим по карте только с запущенным лидаром, заодно выяснив можно ли достоверно локализовать робота только с его (лидара) помощью.
— Чтобы система более точно поняла, где именно робот, надо поездить по карте с запущенными нодами, которые определяют расположение робота.

поездить на роботе по карте

На роботе:
rosrun rosbots_driver part2_cmr.py

На Компьютере:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel

.

Во время поездки amcl начнет читать топики /scan, /map,/tf и публиковать расположение робота в топики /amcl_pose и /particlecloud.
По мере поездки можно наблюдать, что количество стрелок уменьшается и они все больше сгущаются в точке с реальным расположением робота:

И также видно, что лидар не достаточно точно в одиночку справляется с локализацией при неоднозначных границах помещения. На картинке видно как путешествует модель (в виде дерева связей).

Что означают другие параметры в коде ноды amcl?

Условно они делятся на основные (General), параметры фильтров (Filter), Параметры лазера (лидара) (Laser Parameters).

Основные параметры:

У нас diff, что означает дифференциальная. • odom_model_type (default: «diff»): определяет какую модель одометрии использовать. Она обычно публикуется в топик odom.
• base_frame_id (default: «base_link»): frame для базы робота.
• global_frame_id (default: «map»): frame карты, как правило map-server ее публикует в топик map
• use_map_topic (default: false): определяет будет ли загружена карта через топик или путем вызова сервиса (мы помним, что кроме топиков в ROS есть еще сервисы(services) и действия (actions).
Может быть изменен на «omni,» «diff-corrected,» или «omni-corrected.»
• odom_frame_id (default: «odom»): определяет frame (читай топик), с которым будет аасоциироваться одометрия.

Параметры фильтров (Filter Parameters).

Эти параметры позволят вам настроить способ работы фильтра частиц.

У нас 500.
• max_particles (default: 5000): Устанавливает максимально допустимое количество частиц для фильтра.
• kld_err (default: 0. • min_particles (default: 100): Устанавливает минимально допустимое количество частиц для фильтра. У нас 0. 01): Устанавливает максимально допустимую ошибку между истинным распределением и расчетным распределением. 2): Устанавливает линейное расстояние (в метрах), которое должен пройти робот, чтобы выполнить обновление фильтра.
• update_min_a (default: пи/6. 05
• update_min_d (default: 0. У нас 0. 0): Устанавливает угловое расстояние (в радианах), которое робот должен переместить, чтобы выполнить обновление фильтра. У нас 1.
• transform_tolerance (default: 0. 5
• resample_interval (default: 2): Устанавливает количество обновлений фильтра, необходимых перед повторной выборкой. 0): Максимальная скорость (в Гц), с которой сканы и пути публикуются для визуализации. 1): Время (в секундах), в течение которого следует датировать опубликованное преобразование, чтобы указать, что это преобразование действительно в будущем.
• gui_publish_rate (default: -1. 0, эта функция отключена. Если это значение -1. У нас 10.

Параметры лазера (лидара) (Laser Parameters)

0): Минимальный диапазон сканирования, который следует учитывать; -1. Эти параметры позволяют настроить способ взаимодействия amcl с лазером лидара.
• laser_min_range (default: -1. 0): Максимальный диапазон сканирования, который следует учитывать; -1. 0 приведет к использованию минимального диапазона, указанного в отчете лазера.
• laser_max_range (default: -1. 95): Веса z_hit составляющей модели робота.
• laser_z_short (default: 0. 0 приведет к использованию максимальной дальности лазера.
• laser_max_beams (default: 30): Сколько равномерно распределенных лучей в каждом сканировании будет использоваться при обновлении фильтра.
• laser_z_hit (default: 0. 05): Веса z_max составляющей модели робота.
• laser_z_rand (default: 0. 1): Веса z_short составляющей модели робота.
• laser_z_max (default: 0. 05): Веса z_rand составляющей модели робота.

Если уменьшить их значения, то при запуске launch файла количество частиц в визуальном редакторе будет наглядно меньше. Посмотрим на что влияют параметры min_particles и max_particles.

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

Реперные точки на карте помещения.

Название уловное и подразумевает под собой позицию робота на карте в данный конкретный момент.
Для чего они нужны? Для того, чтобы понять, что робот приехал из точки А в комнате в точку В на кухне.
Данные о позиции (pose) робота можно получить при работающей ноде amcl (то, что запускалось в статье выше).
И посмотреть в топике /amcl_pose:
rostopic echo -n1 /amcl_pose
*ключ n1 — для «фиксации» потока сообщений в топике.

В папке создадим файл: Создадим сервис, который при вызове будет отдавать позицию (координаты) робота, чтобы каждый раз не заглядывать в топик.
1.Создадим новый ros пакет.
cd catkin_ws/src
catkin_create_pkg get_pose rospy
cd get_pose/src
2.

get_pose_service.py

#! /usr/bin/env python import rospy
from std_srvs.srv import Empty, EmptyResponse # Import the service message python classes generated from Empty.srv.
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose robot_pose = Pose() def service_callback(request): print "Robot Pose:" print robot_pose return EmptyResponse() # the service Response class, in this case EmptyResponse def sub_callback(msg): global robot_pose robot_pose = msg.pose.pose rospy.init_node('service_server') my_service = rospy.Service('/get_pose_service', Empty , service_callback) # create the Service called get_pose_service with the defined callback
sub_pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, sub_callback)
rospy.spin() # mantain the service open.

*Не забудем сделать его исполнимым chmod +x get_pose_service.py
3.Создадим launch для файла с кодом ноды:
cd ..
mkdir launch && cd launch

nano

get_pose_service.launch


<launch> <node pkg="get_pose" type="get_pose_service.py" name="service_server" output="screen"> </node>
</launch>

4.Не забудем пересобрать catkin:
cd catkin_ws
catkin_make

Теперь все заново запустим, включая новый launch файл.
Но Роботе:
roslaunch rplidar_ros rplidar.launch
На Компьютере:
1-й терминал: roslaunch rosbots_description amcl-2.launch
2-й терминал: roslaunch rosbots_description rviz.launch
3-й терминал: roslaunch get_pose get_pose_service.launch

Обратимся к новому сервису, который должен отдавать нам текущую позицию робота на карте (вызовем сервис ROS):
rosservice call /get_pose_service

В терминале с запущенным launch get_pose, получим координаты робота на карте:

Возможные ошибки.
[rviz.launch] is neither a launch file in package [rosbots_description] nor is [rosbots_description] a launch file name
решение:
cd catkin_ws
source devel/setup.bash

Показать больше

Похожие публикации

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

Ваш адрес email не будет опубликован. Обязательные поля помечены *

Кнопка «Наверх»