Спроектируйте и симулируйте склад приложение Pick-Place Используя мобильный манипулятор в Simulink и Gazebo

Этот пример использует:

Скрипт Open Live Script

В этом примере показано, как настроить сквозное, pick-place рабочий процесс для мобильного манипулятора как KINOVA® Gen3 на Husky® мобильный робот, в Simulink®.

Обзор

Этот пример симулирует мобильную идентификацию манипулятора и переработку объектов в два инструмента использования интервалов от этих тулбоксов:

  • Robotics System Toolbox™ — Модели и симулируют манипулятор.

  • Stateflow® — Планирует высокоуровневые задачи в примере и шаге от задачи до задачи.

  • ROS Toolbox™ — Connects MATLAB® и Simulink к Gazebo®.

  • Computer Vision Toolbox™ и Deep Learning Toolbox™ — Выполните обнаружение объектов с помощью симулированной камеры в Gazebo.

Этот пример основывается на ключевых концепциях из следующих связанных примеров:

Симуляция робота и управление в Gazebo

Этот пример использует Simulink, чтобы управлять двумя роботами в Gazebo. Используя ROS как первичный коммуникационный механизм, позволяет вам использовать разрешение официального Эскимоса и пакетов КИНОВОЙ ROS для низкоуровневого управления движением и обнаружения. Это упрощает прямой переход с симуляции на оборудование, когда команды ROS остаются такими же.

Предоставленный мир Gazebo также использует плагин co-симуляции Gazebo, который включает прямую коммуникацию MATLAB для инструментов, таких как запрос мира Gazebo или непосредственно установка состояний ссылок в Gazebo.

Симулированные датчики

Gazebo симулирует эти датчики:

  • Камера RGB-D присоединила в-конец-исполнительный-элемент манипулятора робота. Модель использует этот канал камеры, чтобы обнаружить объекты взять.

  • 2D датчик лидара впереди мобильной основы робота. Модель использует этот лазерный канал, чтобы локализовать робота в предварительно вычисленной карте.

Рабочий процесс Pick-Place Используя Simulink и Stateflow

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

  • Симулированное средство переработки

  • Симулированный мобильный манипулятор, который является манипулятором Киновой Gen3 на Хриплом мобильном роботе

Откройте модель Simulink, чтобы исследовать pick-place компоненты. 

open_system('PickPlaceWorkflowSimulinkMobileArm.slx');
Warning: Undefined function 'vipscopes.VideoViewerScopeCfg' for input arguments of type 'extmgr.ConfigurationSet'.

Warning: Undefined function 'vipscopes.VideoViewerScopeCfg' for input arguments of type 'extmgr.ConfigurationSet'.

Модель Simulink состоит из этих компонентов:

  1. Main Task Scheduler — Эта диаграмма Stateflow планирует задачи для мобильного манипулятора, чтобы завершить pick-place задание. Это активирует задачи для манипулятора робота или мобильного робота согласно необходимому рабочему процессу.

  2. Robot Manipulator — Задачи реализаций, присвоенные манипулятору робота Main Task Scheduler. Этот компонент состоит из этих основных модулей: Планировщик Манипулятора, подсистема Планирования Движения и подсистема Восприятия.

  3. Mobile Robot — Задачи реализаций, присвоенные мобильному роботу основным планировщиком. Этот компонент состоит из этих модулей: Мобильный Планировщик Робота, подсистема Планирования пути и подсистема Следования траектории.

1. Основной планировщик задач

Модель реализует основной рабочий процесс для мобильного манипулятора при помощи центральной диаграммы Stateflow, которая выполняет эти шаги:

  1. Мобильный манипулятор перешел к конвейерной ленте, чтобы взять объект переработать.

  2. Камера RGB-D на манипуляторе обнаруживает положения и типы объектов с помощью глубокого обучения.

  3. Манипулятор берет обнаруженный объект.

  4. Мобильный манипулятор перешел к соответствующей корзине для обнаруженного типа объекта (бутылка, или может) и помещать его.

  5. Робот возвращается к конвейерной ленте и повторяет этот рабочий процесс, пока больше объектов не обнаруживается.

2. Манипулятор робота

A. Планировщик манипулятора

Планировщик манипулятора содержит Idle, MoveToHome, Detect, PickObject, и PlaceObject состояния. В любом моменте времени манипулятор находится в одном из этих состояний, согласно задаче, упорядоченной Main Task Scheduler. Избегать конфликта, если манипулятор или мобильный робот находятся в несостоянии ожидания, то другой должен быть в их состоянии ожидания.

B. Планирование движения

Подсистема планирования движения является активированной подсистемой, которая, когда-то включенный сигналом из планировщика манипулятора, планирует траекторию и использует пакет управления ROS, чтобы отправить команду за отслеживанием той траектории объединенному контроллеру траектории, запускающемуся в ROS.

Планировщик манипулятора отправляет сигнал включить эту подсистему, когда манипулятор робота должен переместиться во время pick-place рабочего процесса. Подсистема остается активированной, пока робот не достигает желаемого целевого положения. Для получения дополнительной информации в активированных подсистемах, смотрите Используя Enabled Подсистемы (Simulink).

C. Восприятие

Подсистема восприятия является триггируемой подсистемой, которая применяет предварительно обученную модель глубокого обучения к симулированному каналу камеры исполнительного элемента конца от робота, чтобы обнаружить годные для повторного использования части. Модель глубокого обучения берет систему координат камеры, как введено и выводит 2D местоположение объекта (положение пикселя) и тип переработки, это требует (синий или зеленый интервал). 2D местоположение в изображении сопоставлено с базовой системой координат робота с помощью информации о свойствах камеры (фокусное расстояние и поле зрения), вход от датчика глубины и робота прямая кинематика.

Планировщик задач манипулятора робота отправляет сигнал инициировать эту подсистему, когда робот должен обнаружить следующий объект во время pick-place workflow.For больше информации о триггируемых подсистемах, смотрите Используя Триггируемые подсистемы (Simulink).

3. Мобильный робот

Mobile Robot рабочий процесс состоит из диаграммы Stateflow, которая управляет полным поведением мобильного робота, триггируемая подсистема, которая планирует путь с помощью функции MATLAB, и подсистема следования траектории, которая использует управляющую логику, чтобы следовать за ссылочным путем.

Создание карты складской площади

Для получения карту рабочей области, необходимо сначала отсканировать среду. Смонтируйте датчик лидара на Эскимосе, чтобы перейти вокруг среды и использовать buildmap функция, чтобы создать карту сетки заполнения. Для получения дополнительной информации смотрите Одновременную локализацию и картографию (SLAM) Реализации со Сканами лидара (Navigation Toolbox).

A. Мобильный планировщик робота

Мобильный планировщик робота содержит Idle, PlanPath, и FollowPath состояния. По умолчанию мобильный робот находится в Idle состояние, означая, что системы планирования пути и следования траектории неактивны. Мобильный планировщик робота получает команды задачи от Main Task Scheduler. Whenthe мобильный планировщик робота получает Tasks.Robot_Navigate задача, это переключается от Idle утвердите к PlanPath состояние. Планировщик, чем продолжает продвигаться, с помощью обратной связи от планирования пути и следования траектории subystems, чтобы совершенствоваться через этапы, в конечном счете возвращаясь к Состоянию ожидания, которое представляет неактивную задачу и указывает к Main Task Scheduler то, что это может идти дальше с высокоуровневыми задачами. Процесс затем повторяет данные новые входные параметры от Main Task Scheduler.

B. Планирование пути

Во время PlanPath состояние, мобильный планировщик робота устанавливает taskActive к true, который инициировал подсистему планирования пути. Эта подсистема содержит блок MATLAB function, который планирует путь на предоставленной бинарной карте сетки заполнения с помощью plannerRRTStar объект. Подсистема возвращает набор пути waypoints в SE (2), а также логический флаг, isPath это указывает, был ли путь успешно найден.

Если мобильный планировщик робота получает значение true для isPath, это переходит к FollowPath состояние.

C. Следование траектории

В FollowPath состояние, мобильный планировщик робота устанавливает requestFollowPath отметьте к true, который инициировал подсистему следования траектории.

Подсистема имеет эти основные части, перечисленные в порядке выполнения:

  1. Движение управления с блоком Pure Pursuit — The Pure Pursuit является первичным контроллером движения. Учитывая текущее положение робота и предстоящего waypoint, это присваивает скорость, чтобы переместить робота в waypoint.

  2. Обновите поведения, данные расстояние до цели — Check Distance to Goal и Zero-Velocity at Goal подсистемы гарантируют, что робот останавливается, когда он достиг цели. Модель также использует выход от Check Distance to Goal подсистема, чтобы проверить завершение задачи.

  3. Отправьте Команды в Gazebo через ROS — Blank Message и Publish блоки отправляют линейную команду и скорости вращения от Zero-Velocity at Goal подсистема соответствующему скоростному контроллеру на роботе в Gazebo.

Вместе, эти компоненты управляют роботом через мир Gazebo. Если робот достигает цели, подсистема возвращает значение true для atGoal, который указывает к мобильному планировщику робота, что это состояние завершено.

Присвойте переменные для использования в модели

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

Эта симуляция использует манипулятор КИНОВОЙ Gen3, присоединенный к Хриплому мобильному роботу. Для манипулятора модель КИНОВОЙ Gen3 с присоединенным механизмом захвата хранится в файле MAT. Загрузите дерево твердого тела манипулятора робота.

load("helperKINOVAGen3MobileArmPickPlace.mat");
show(robot)

ans = 
  Axes (Primary) with properties:

             XLim: [-1.5000 1.5000]
             YLim: [-1.5000 1.5000]
           XScale: 'linear'
           YScale: 'linear'
    GridLineStyle: '-'
         Position: [0.1300 0.1100 0.7750 0.8150]
            Units: 'normalized'

  Show all properties

Инициализируйте приложение выбора и места

Установите начальную настройку манипулятора и имя корпуса исполнительного элемента конца.

initialRobotJConfig = [1.583 0.086 - 0.092 1.528 0.008 1.528 -0.08];
endEffectorFrame = "gripper";

Задайте домашнюю настройку манипулятора и два положения для отбрасывания объектов двух различных типов. Первое положение соответствует синему интервалу для объектов типа 1, и второе положение соответствует зеленому интервалу для объектов типа 2. 

homeArmPose = trvec2tform([0.0 -0.35 0.4])*axang2tform([0 0 1 -pi/2])*axang2tform([0 1 0 pi]);
detectionArmPose = trvec2tform([0.4 0.0 0.56])*axang2tform([0 0 1 -pi/2])*axang2tform([0 1 0 pi]);
placingArmPose = trvec2tform([0.6 0.0 0.57])*axang2tform([0 0 1 -pi/2])*axang2tform([0 1 0 pi/2])*axang2tform([1 0 0 -1.4]);

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

homeRobotPose = [7.95, 10.6, 0]; % x, y, theta
placingRobotPose2 =[13.16, 7.94, -0.92];
placingRobotPose1 = [15.15, 3.8, -1.752];

Установите размер шага для симуляции.

Ts = 0.1;

Запустите рабочий процесс Pick-Place

Запустите ОСНОВАННОЕ НА ROS средство моделирования для робота КИНОВОЙ Gen3 и сконфигурировать связи MATLAB со средством моделирования робота.

Запустите симуляцию Gazebo

Этот пример использует виртуальную машину (VM) доступную для скачивания в Начало работы с Gazebo и Симулированным TurtleBot (ROS Toolbox).

  • Запустите рабочий стол виртуальной машины Ubuntu®.

  • В рабочем столе Ubuntu кликните по Мобильному значку Мира Манипулятора, чтобы запустить мир Gazebo, созданный для этого примера или запустить эти команды:

source /opt/ros/melodic/setup.bash; source ~/catkin_ws/devel/setup.bash 
export SVGA_VGPU10=0 
export GAZEBO_PLUGIN_PATH=/home/user/src/GazeboPlugin/export:$GAZEBO_PLUGIN_PATH  
roslaunch kortex_gazebo_depth mobilemanipulator.launch

Запустите ведущее устройство ROS и связь интерфейса Gazebo

Задайте IP-адрес и номер порта ведущего устройства ROS в Gazebo так, чтобы MATLAB мог связаться со средством моделирования робота. В данном примере ведущее устройство ROS в Gazebo включено 192.168.203.128:11311, и вашим адресом хоста - компьютера является 192.168.31.1. Замените их на соответствующие значения, соответствующие вашей настройке устройства ROS. Запустите сеть ROS 1 с помощью rosinit.

rosIP = '192.168.116.166'; % IP address of ROS enabled machine  
rosshutdown % shut down existing connection to ROS
Shutting down global node /matlab_global_node_61346 with NodeURI http://192.168.116.1:54092/

rosinit(rosIP,11311);
Initializing global node /matlab_global_node_46991 with NodeURI http://192.168.116.1:49908/

Инициализируйте Интерфейс Gazebo использование gzinit.

gzinit(rosIP);

Инициализируйте модель

Установите начальное положение руки в Gazebo с помощью ROS.

configResp = helperSetCurrentRobotJConfig(initialRobotJConfig);

Не приостановите физику Gazebo.

physicsClient = rossvcclient("gazebo/unpause_physics");
physicsResp = call(physicsClient,"Timeout",3);

Загрузите предварительно вычисленную карту складского средства переработки. 

load("helperRecyclingWarehouseMap.mat");

Запустите рабочий процесс. Симулируйте модель путем выбора, работает на вкладке Simulation.

Модель симулирует, пока не остановлено.

Если оба элемента были перемещены с моделью, все еще запускающейся, используйте Интерфейс MATLAB Gazebo, чтобы сбросить положения красной бутылки и зеленой банки:

gzlink('set','Green Can','link','Position',[7.80007 11.2462 0.736121],'Orientation',[1 0 0 0]);
STATUS:  Succeed


MESSAGE:  Parameter set successfully.

gzlink('set','Red Bottle','link','Position',[7.932210479374374  11.287119941912177 0.783803767011424],'Orientation',[1 0 0 0]);
STATUS:  Succeed


MESSAGE:  Parameter set successfully.

Можно также изменить значения положения, чтобы испытать контроллер на различных положениях. Интерфейс MATLAB позволяет вам управлять миром Gazebo непосредственно из MATLAB через команды при помощи функций, таких как gzlink и gzmodel.

Перезапускать модель:

  • Закройте и повторно выполните скрипт на VM Ubuntu. Или при использовании терминала и списка команд, просто закройте (Ctrl+C) и вызовите roslaunch управляйте снова:

roslaunch kortex_gazebo_depth mobilemanipulator.launch

  • Запустите команды настройки, начинающие с раздела Initialize Model в стороне MATLAB и в Simulink, выберите Run.

Документация Robotics System Toolbox

Поддержка

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