Картографическая навигация Limo Pro

введение

Краткое описание перспектив: Выше мы представили проект использования кобота LIMO для реализации сценария применения составной роботизированной системы, способной выполнять сложные задачи, со следующих трех аспектов: концептуальный дизайн, архитектура системы и ключевые компоненты.

Эта статья в основном посвящена основной части ядра проекта, которая также в основном разделена на три части: визуальный захват роботизированной руки, функция LIMO Pro в ROS, картографирование, навигация и объезд препятствий и т. д., как а также интеграция двух систем.

Подготовка оборудования

myCobot 280 M5stack

myCobot Adaptive Gripper

myCobot Camera Flange 2.0

LIMO PRO

Визуальный захват роботизированной руки

Вот как выглядит роботизированная рука после установки Adaptive Gripper и фланца камеры 2.0.

Используемый нами фланец камеры 2.0 представляет собой 2D-камеру. Он не может полагаться на саму камеру для получения трехмерных данных (длины, ширины и высоты) объекта, но мы можем использовать маркеры для получения длины и ширины цели. объект высокий. Общие из них включают ArUco, STag, AR code и AprilTags. Сегодня мы используем алгоритм STag для визуального распознавания.

Система маркировки STag

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

Ниже представлено видео, демонстрирующее эффект распознавания кода тега STag, ARToolKit+, ArUco и кода RUNE-Tag в одной и той же среде.

https://www.youtube.com/watch?v=vnHI3GzLVrY

Из видео видно, что высокая адаптируемость STag к изменениям окружающей среды и высокая надежность в сложных сценах делают его лучшим выбором в приложениях, требующих высокоточного отслеживания и позиционирования. Существует также документ, посвященный объяснению стабильной системы реперной маркировки STag. Если вам интересно, вы можете нажать на ссылку, чтобы узнать об этом самостоятельно.

https://arxiv.org/abs/1707.06292

Система STag может быть адаптирована к ROS. Она имеет пакет программного обеспечения ROS, написанный на C++, а также может поддерживать использование Python.

C++/ROS:GitHub - bbenligiray/stag: STag: A Stable Fiducial Marker System

Python：GitHub - ManfredStoiber/stag-python: Python Package for STag - A Stable, Occlusion-Resistant Fiducial Marker System

Напишите простой пример на Python

import cv2
import stag
import numpy as np
 
# Загрузить параметры камеры
camera_params = np.load("camera_params.npz")
mtx, dist = camera_params["mtx"], camera_params["dist"]
 
# Инициализация детектора STag
stag_detector = stag.detectMarkers(mtx, dist)
 
# Инициализировать захват видео
cap = cv2.VideoCapture(0)
while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break
    
    # Применить коррекцию камеры (необязательно)
    frame_undistorted = cv2.undistort(frame, mtx, dist, None, mtx)
    # Обнаружение тегов STag
    (corners, ids, rejected_corners) = stag.detectMarkers(frame_undistorted, 21)
    # Постройте обнаруженные маркеры и их идентификаторы
    stag.drawDetectedMarkers(frame_undistorted, corners, ids)
    # Нарисуйте отклоненные регионы-кандидаты и установите красный цвет.
    stag.drawDetectedMarkers(frame_undistorted, rejected_corners, border_color=(255, 0, 0))
 
    # Показать результаты
    cv2.imshow("STag Detection", frame_undistorted)
 
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
 
cap.release()
cv2.destroyAllWindows()

После маркировки кода STag можно получить параметры четырех углов маркирующего кода.

(array([[[257., 368.],
        [185., 226.],
        [345., 162.],
        [420., 302.]]], dtype=float32),)

После получения координат вы можете использовать функцию opencv «cv2.solvePnP» для расчета поворота и смещения маркера относительно камеры. Для этой функции требуются трехмерные координаты маркера (положение в физическом мире) и соответствующие координаты двумерного изображения (т. е. обнаруженная угловая точка), а также внутренние параметры камеры и коэффициенты искажения. solvePnP возвращает вектор вращения (rvec) и вектор перемещения (tvec), которые описывают преобразование из системы координат маркера в систему координат камеры. Таким образом, на основе этих параметров можно рассчитать положение и ориентацию маркера.

Ниже приведен псевдокод для простоты понимания.

def estimate_pose(corners):
    #Do some calculations with PnP, camera rotation and offset
    return rvec,tvec
    
def convert_pose_to_arm_coordinates(rvec, tvec):
    # Преобразуйте вектор вращения и вектор перемещения в x в системе координат робота, y, z, rx, ry, rz
    return x, y, z, rx, ry, rz
    
def convert_grab(object_coord_list):
    #Do some calculations to convert the coordinates into grasping coordinates for the robotic arm
    return grab_position
    
    
cap = cv2.VideoCapture(0)
while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break
        
    
    maker = (corners, ids, rejected_corners) = stag.detectMarkers(image, 21)
    
    rvec, tvec = stag.estimate_pose(marker)
    
    object_coord_list = convert_pose_to_arm_coordinates(rvec, tvec)
    grab_position = convert_grab(object_coord_list)
    mycobot.move_to_position(grab_position)

Управление роботизированной рукой

Приведенный выше код представляет собой грубый процесс сканирования. Более сложные части уже решены. Далее мы занимаемся управлением движением роботизированной руки с помощью библиотеки pymycobot.

from pymycobot import MyCobot
 
#Create an instance and link the robotic arm
mc = MyCobot('com3',115200)
 
#Control the robotic arm with angle
mc.send_angles(angles_list,speed)
 
#Control the robotic arm using coordinates
mc.send_coords(coords_list,speed,mode)
 
#Control gripper,value-0~100
mc.set_gripper_value(value,speed)

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

Картографическая навигация Limo Pro

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

Сначала нам нужно увидеть карту. После того, как у нас есть карта, мы можем выполнять навигацию по ней, круиз по фиксированной точке и другие функции. мы выбираем относительно статично. Для достижения этого используйте алгоритм gmapping.

Картирование

Gmapping — это широко используемый метод Открытый, основанный на системе фильтрации SLAM. исходный Алгоритм кодSLAM. Gmapping эффективно использует информацию одометра колес.,Требования к частоте для лидара не высоки,При построении небольшой карты сцены,Требуемые расчеты невелики, а точность высокая. Здесь Картирование лимузина реализовано с использованием пакета функций GMapping, инкапсулированного ROS.

Примечание. Все следующие функции упакованы и могут использоваться напрямую.

Для начала вам необходимо запустить радар, открыть новый терминал и ввести в терминале команду:

roslaunch limo_bringup limo_start.launch pub_odom_tf:=false

Затем запустите алгоритм gmapping Картирование,Откройте еще один новый терминал,Введите команду в терминале:

roslaunch limo_bringup limo_gmapping.launch

После успешного запуска откроется инструмент визуализации rviz. Интерфейс, который вы видите в данный момент, показан на рисунке.

В это время вы можете перевести ручку в режим дистанционного управления и управлять лимузином Картирование.

После построения карты вам необходимо запустить следующую команду, чтобы сохранить карту в указанном каталоге:

1. Перейдите в каталог, в котором необходимо сохранить карту. Здесь сохраните карту в ~/agilex_ws/src/limo_ros/limo_bringup/maps/ и введите в терминале команду:

cd ~/agilex_ws/src/limo_ros/limo_bringup/maps/

2. После перехода в /agilex_ws/limo_bringup/maps продолжайте вводить команду в терминале:

rosrun map_server map_saver -f map1

map1 — имя сохраненной карты. При сохранении карты следует избегать дублирования названий карт.

навигация

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

(1) move_base: реализовать оптимальное планирование пути при роботонавигации.

(2) amcl: реализует позиционирование робота на двумерных картах.

На основе двух вышеуказанных функциональных пакетов,ROS предоставляет полный набор средств навигации.,

Роботу нужно только опубликовать необходимую информацию от датчиков и целевую позицию навигации.,ROS может выполнить функцию навигации. в этих рамках,Пакет функций move_base обеспечивает основной рабочий и интерактивный интерфейсы навигации. Чтобы обеспечить точность пути навигации,роботу также необходимо точно определить ваше местоположение,Эта часть функции реализуется пакетом функций amcl.

В процессе навигации,Используются два алгоритма: алгоритмы DWA и TEB.,Эти два алгоритма обрабатывают планирование глобального и локального пути соответственно.,Чтобы гарантировать безопасное движение автомобиля к месту назначения.,Избегайте столкновений с препятствиями.

(1) Сначала запустите радар и введите команду в терминал:

roslaunch limo_bringup limo_start.launch pub_odom_tf:=false

(2) Запустите функцию навигации.,Введите команду в терминале:

roslaunch limo_bringup limo_navigation_diff.launch

После успешного запуска откроется интерфейс rviz, как показано на рисунке.

Нам нужно импортировать только что созданную карту. Откройте файл limo_navigation_diff.launch, чтобы изменить параметры. Каталог, в котором находится файл: ~/agilex_ws/src/limo_ros/limo_bringup/launch. Измените map02 на имя карты, которую необходимо заменить.

После включения навигации,Вы обнаружите, что форма, сканированная лазером, не совпадает с картой.,Нам нужно вручную исправить,Исправьте фактическое положение шасси в сцене на карте, отображаемой в rviz.,С помощью инструментов в rviz,Напишите примерное местоположение,Укажите приблизительное местоположение лимузина,Затем используйте ручку для дистанционного управления вращением лимузина.,Пусть само исправится,Когда форма лазера перекрывает форму сцены на карте,Калибровка завершена. Этапы работы показаны на рисунке:

После завершения калибровки

через 2D Nav Goal Set навигация по целевой точке

дорожный патруль

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

(1) Сначала запустите радар, откройте новый терминал и введите в терминал команду:

roslaunch limo_bringup limo_start.launch pub_odom_tf:=false

(2) Запустите функцию навигации.,Открыть новый терминал,Введите команду в терминале:

roslaunch limo_bringup limo_navigation_diff.launch

Примечание. Если это режим движения Аккермана, запустите

roslaunch limo_bringup limo_navigation_ackerman.launch

(3) Запустите функцию записи пути, откройте новый терминал и введите команду в терминале:

roslaunch agilex_pure_pursuit record_path.launch

После завершения записи пути завершите программу записи пути и введите в терминале команду: Ctrl+c

(4) Запустите функцию проверки дорожного патруля.,Открыть новый терминал,Введите команду в терминале:

Примечание. Установите дескриптор в командный режим.

roslaunch agilex_pure_pursuit pure_pursuit.launch

Интеграция двух систем

Приведенный выше дистрибутив завершает визуальный захват роботизированной руки myCobot.,Картографическая навигация LIMO,дорожный функция патрульпроверки,Теперь нам нужно интегрировать их в систему ROS. Наш сценарий по умолчанию,LIMO проводит инспекции в фиксированных точках,Остановить движение при встрече с маркером,Ожидание, пока рука робота схватит объект,После завершения ЛИМО переходит к следующей точке.

Распределение функциональных узлов

Процесс реализации функции в ROS (операционная система робота) включает в себя несколько шагов и компонентов, включая узлы, темы, службы, серверы параметров и действия. Согласно функциональной узловой архитектуре ROS мы определяем распределение узлов и способ их взаимодействия:

1. Узел распознавания изображений

Обязанности: Постоянно получать потоки изображений с камер и использовать алгоритмы распознавания изображений (такие как OpenCV или модели глубокого обучения) для обнаружения конкретных маркеров.

Вход: поток изображения с камеры.

Вывод: при обнаружении маркера опубликовать сообщение в определенной теме (например, /marker_detected).

2. Узел управления

Обязанности: Управлять движением робота, включая запуск, остановку и продолжение осмотра.

Ввод: подпишитесь на тему /marker_detected, чтобы прослушивать выходные данные узла распознавания изображений. Также можно подписаться на тему, специально предназначенную для получения команд ручного управления (например, /control_commands).

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

3. Узел выполнения задач

Обязанности: Выполнение конкретных задач после обнаружения маркеров. Этими задачами могут быть сбор данных, отчеты о состоянии и т. д.

Ввод: прослушивает инструкции от узла управления, которые указывают, когда начинать выполнение задачи.

Выходные данные: обратная связь о состоянии завершения задачи, которая может быть отправлена ​​на управляющий узел или в специальную тему состояния (например, /task_status).

4. узел навигации и планирования пути (Navigation and Path Planning Node）

Обязанности: Управление логикой планирования и навигации пути робота, чтобы гарантировать, что робот может безопасно перемещаться в окружающей среде.

Ввод: Получите инструкции от узла управления о запуске, остановке или корректировке пути навигации.

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

Подвести итог

Эта сцена изначально завершена. Фактически, можно добавить множество деталей, например добавить некоторые движущиеся препятствия во время движения или настроить такой объект, как светофор, чтобы он был ближе к реальной сцене. Если вы считаете, что есть что-то, что нуждается в улучшении, или если вы хотите использовать кобота LIMO для чего-то, не стесняйтесь высказываться, ваши ответы и лайки — наша самая большая мотивация для обновлений!