Построение карты и локализация мобильного робота в ROS без одометрии с использованием laser_scan_matcher

    Добрый день читатели! Мы уже однажды коснулись темы локализации и SLAM в статье о Hector SLAM. Продолжим знакомство с алгоритмами построения карт местности и локализации в ROS. Сегодня мы попробуем построить карту местности без источника одометрии, используя только лидар Hokuyo URG-04LX-UG01 и алгоритм gmapping и локализовать робота на построенной карте с помощью алгоритма amcl. В этом нам поможет laser_scan_matcher. Кому интересно, прошу под кат.

    Установка пакета laser_scan_matcher


    Итак, начнем! Для экспериментов будем использовать ROS Indigo, но вы можете использовать другую версию ROS (Jade, Kinetic, Hydro). Установка пакетов должна выполняться аналогично (возможно только в ROS Kinetic не будут доступны некоторые пакеты через apt-get).

    Пакет laser_scan_matcher это инструмент для инкрементальной регистрации данных лазера, который реализован на основе метода Canonical Scan Matcher, о котором можно почитать здесь. О пакете можно прочитать здесь. Пакет может быть использован без данных одометрии, он сам выполняет оценку одометрии.

    Установим пакет:

    sudo apt-get install ros-indigo-laser-scan-matcher
    

    Попробуем демо:

    roscore
    roslaunch laser_scan_matcher demo.launch
    

    Мы увидим нечто подобное в rviz:



    Здесь показан laser_scan_matcher в действии на данных лидара, записанных в bag файл. Теперь попробуем пакет вживую. Чтобы не терять время, сразу попробуем его с алгоритмом gmapping. Создадим пакет с файлом my_gmapping_launch.launch для запуска gmapping:

    cd ~/catkin_ws/src
    catkin_create_pkg my_laser_matcher
    cd src/my_laser_matcher
    mkdir launch
    vim launch/my_gmapping_launch.launch
    

    Скопируйте следующий код в файл my_gmapping_launch.launch:

    Код 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>
    


    Здесь мы запускаем static_transform_publisher из пакета tf для публикации трансформации между системами координат base_link → laser, узлы laser_scan_matcher и slam_gmapping. Исходный код файла можно скачать отсюда. Для использования лидара Hokuyo нам нужно установить пакет ROShokuyo_node:

    sudo apt-get install ros-indigo-hokuyo-node
    

    Запустим узел getID из пакета hokuyo_node чтобы получить информацию о лидаре:

    rosrun hokuyo_node getID /dev/ttyACM0
    

    Может возникнуть ошибка:

    Error: Failed to open port. Permission denied.
    [ERROR] 1263843357.793873000: Exception thrown while opening Hokuyo.
    Failed to open port: /dev/ttyACM0. Permission denied (errno = 13). You probably don't have premission to open the port for reading and writing. (in hokuyo::laser::open) You may find further details at http://www.ros.org/wiki/hokuyo_node/Troubleshooting
    

    В этом случае нам нужно добавить права доступа для порта /dev/ttyACM0:

    sudo chmod a+rw /dev/ttyACM0
    

    Запустим getID из пакета hokuyo_node еще раз и получим подобный вывод:

    Device at /dev/ttyACM0 has ID H0906078
    

    Теперь запустим узел hokuyo_node:

    rosrun hokuyo_node hokuyo_node
    

    И наконец запустим наш лаунч файл my_gmapping_launch.launch:

    roslaunch my_laser_matcher my_gmapping_launch.launch
    rosrun rviz rviz
    

    Выведем список топиков:

    rostopic list
    

    Среди топиков мы увидим следующие:

    /initialpose
    /move_base_simple/goal
    /odom
    /pose2D
    ...
    /imu/data
    

    Таким образом мы получаем одометрию и позицию робота благодаря laser_scan_matcher.

    Добавим в rviz дисплей типа LaserScan с топиком /scan так, как это описано в статье. Также добавим дисплей для карты Map и для трансформации TF. Развернем секцию TF и внутри нее Frames, затем отметим пункты: odom, map, base_link. Напомню, что это системы координат одометрии, карты и робота соответственно. Не забудьте установить значение /map для поля Fixed frame в левой панели Displays в секции Global options.

    В rviz мы увидим подобную картинку:

    image

    Дальше просто перемещаем робота в пространстве для построения полной карты местности. Используем утилиту map_saver из пакета map_server для сохранения карты:

    rosrun map_server map_saver
    

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


    Сейчас попробуем локализовать робота с помощью алгоритма amcl. Создадим файл my_localize.launch внутри нашего пакета со следующим содержимым:

    Код my_localize.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 name="hokuyo" pkg="hokuyo_node" type="hokuyo_node" respawn="false" output="screen">
        <param name="calibrate_time" type="bool" value="false"/> 
        <param name="port" type="string" value="/dev/ttyACM0"/> 
        <param name="intensity" type="bool" value="false"/>
      </node>
    
      <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/vladimir/catkin_ws/map.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"/>
      <!-- translation std dev, m -->
      <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>
    


    Здесь мы аналогично лаунчеру для gmapping публикуем трансформацию /laser → /base_link с помощью узла static_transform_publisher, узлы hokuyo_node и laser_scan_matcher. Затем мы запускаем map_server для публикации нашей построенной карты, где в args передаем путь до карты в файле yaml. Наконец запускаем узел amcl с параметрами. О параметрах amcl можно прочитать на официальной странице алгоритма.

    Код лаунч файла также можно скачать из github репозитория. Запустим наш лаунч файл:

    roslaunch my_laser_matcher my_localize.launch
    

    Теперь перейдем в rviz. Установим значение map для Fixed frame в секции Global options. Выведем список топиков:

    rostopic list
    

    В списке появятся новые топики:

    ...
    /amcl/parameter_descriptions
    /amcl/parameter_updates
    /amcl_pose
    ...
    /map
    /map_metadata
    /map_updates
    ...
    /particlecloud
    

    Топик amcl_pose соответствует позиции робота, публикуемой amcl.
    Посмотрим сообщения в топике:

    rostopic echo /amcl_pose
    

    Получим данные о позиции робота:

    header: 
      seq: 15
      stamp: 
        secs: 1482430591
        nsecs:  39625000
      frame_id: map
    pose: 
      pose: 
        position: 
          x: 0.781399671581
          y: 0.273353260585
          z: 0.0
        orientation: 
          x: 0.0
          y: 0.0
          z: -0.636073020536
          w: 0.771628869694
      covariance: [0.2187289446708912, -0.010178711317316846, 0.0, 0.0, 0.0, 0.0, -0.010178711317316819, 0.23720047371620548, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.07106236846890918]
    ---
    

    В rviz мы получим такую картинку:

    image

    Как видим точки скана с лидара частично совпадают со стенами на карте. Вот как выглядела моя установка в действительности:

    image

    Попробуем переместить робота. Позиция робота и точка обзора карты должны одновременно измениться в окне rviz. После перемещения робота позиция робота может быть определена алгоритмом amcl не точно. Нам нужно выполнить корректировку положения робота с помощью инструмента 2D Pose Estimate. Для этого нажмем в верхней панели инструментов в rviz на кнопку 2D Pose Estimate, кликнем на приблизительную точку центра системы координат робота на карте в rviz (система координат base_link) и будем удерживать зажатой кнопку мышки. Появится зеленая стрелка, исходящая из центра робота:

    image

    Потянем стрелку изменяя ее направление и пытаясь совместить точки скана с лидара с черными краями (стены) на карте. Получив наилучшее совмещение отпустим кнопку мышки.

    image

    Мы получим такие сообщения в терминале, где запущен my_localize.launch:

    [ INFO] [1482431993.717411186]: Setting pose (1482431993.717383): -0.413 -0.071 0.057
    

    На коротком видео можно посмотреть все в действии:



    Топик /particlecloud представляет данные о неопределенности положения робота в виде ориентированных позиций (Pose) или так называемое облако частиц. Тип сообщений — geometry_msgs/PoseArray.

    Добавим дисплей по названию топика /particlecloud:

    image

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

    image

    Чем гуще скопление частиц, тем выше вероятность положения робота в этой позиции. Об инструменте 2D Pose estimate, облаке частиц и других понятиях в amcl можно прочитать в туториале на портале ros.org.

    На этом все! Все рассмотренные алгоритмы (gmapping и amcl) являются частью большого стека Navigation stack в ROS, о нем можно найти много информации в Интернете. Сегодня мы попробовали в действии инструмент laser_scan_matcher, алгоритмы построения карт gmapping и локализации amcl. Теперь можно легко начать работать над локализацией и навигацией мобильного робота и создать полностью автономного робота, способного ориентироваться в пространстве без необходимости в ручном управлении.
    Подписывайтесь на нашу группу о ROS Вконтакте и будьте в курсе о появлении новых статей и новостей о платформе ROS. Желаю всем удачи в экспериментах и до новых встреч!

    PS: Всех с наступающим 2017 годом!
    • +22
    • 11,2k
    • 6
    Поддержать автора
    Поделиться публикацией
    AdBlock похитил этот баннер, но баннеры не зубы — отрастут

    Подробнее
    Реклама

    Комментарии 6

      +1
      Очень полезная статья и очень нужная. Для Ros таких не хватает. Спасибо!
        0
        спасибо за хорошие слова!
        0
        Какова частота публикаций данных лидара в топик?
          0
          Хороший вопрос. К сожалению не проверял частоту. Могу проверить когда будет время.
            0
            Просто подобные узлы локализации очень требовательны к частоте обновления данных. Например на каком-нибудь лидаре неато получить разумную одометрию практически не возможно :( либо надо Очень медленно перемещаться в пространстве)
              0
              проверил. Частота публикаций данных лидара: 10.046Hz.

        Только полноправные пользователи могут оставлять комментарии. Войдите, пожалуйста.

        Самое читаемое