Выполните безопасное управление отслеживанием траектории с помощью блоков Robotics Manipulator

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

В этом примере показано, как использовать блоки алгоритма манипулятора Simulink ® с Robotics System Toolbox™ для достижения безопасного управления отслеживанием траектории для моделируемого робота, работающего в Simscape™ Multibody™.

Для запуска этого примера необходимы как Robotics System Toolbox, так и Simscape Multibody.

Введение

В этом примере вы запустите модель, которая реализует контроллер вычисленного крутящего момента с положением соединения и обратной связью скорости, используя блоки алгоритма манипулятора. Контроллер получает информацию о положении и скорости соединения от моделируемого робота (реализованная с использованием Simscape Multibody) и отправляет команды крутящего момента, чтобы привести робота в действие по заданной траектории соединения. Плоский объект расположен перед роботом так, что концевой эффектор руки робота столкнется с ним, когда выполняется управление отслеживанием траектории. Без какой-либо дополнительной настройки, увеличение крутящего момента из-за столкновения с объектом может вызвать повреждение робота или объекта. Чтобы достичь безопасного отслеживания траектории, создается блок масштабирования траектории, чтобы настроить метку времени при присвоении требуемого движения контроллеру. Можно настроить некоторые параметры и взаимодействовать с роботом во время работы модели и наблюдать эффект на моделируемого робота.

Настройте модель робота в рабочей области

Этот пример использует модель Rethink Sawyer, робота с 7 степенями свободы. Звонить importrobot чтобы сгенерировать rigidBodyTree модель из описания, хранящегося в файле Unified Robot Description Format (URDF). Установите DataFormat и Gravity свойства для согласования с Simscape.

sawyer = importrobot('sawyer.urdf');
sawyer.removeBody('head');
sawyer.DataFormat = 'column';
sawyer.Gravity = [0, 0, -9.80665];

Генерация траектории и связанный Setup

Сначала присвойте начальное время и длительность для траектории.

tStart = 0.5;
tDuration = 3;

Затем присвойте начальные и целевые строения. q0 является начальным строением и q1 - целевое строение.

q0 = [0; -1.18; 0; 2.18; 0; -1.0008; 3.3161];
q1 = zeros(7,1);

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

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

Обзор модели Simulink

Далее откройте модель Simulink. Сгенерированные выше переменные уже хранятся в рабочем пространстве модели Simulink:

open_system('robotSafeTrajectoryTracking.slx');

The robotSafeTrajectoryTracking модель реализует контроллер вычисленного крутящего момента с масштабированием траектории для безопасного отслеживания траектории. В этой модели есть три основные подсистемы:

Вычисленный контроллер крутящего момента
Масштабирование траектории и желаемое движение
Simscape Multibody с Простой Контактной Механикой

На каждом временном шаге, если переключатель масштабирования траектории включен, измененная метка времени используется для оценки желаемого положения, скорости и ускорения соединения. Затем контроллер вычисленного крутящего момента использует блоки манипулятора, сопоставленные со RigidBodyTree модель, чтобы отследить необходимое движение. Производный вход управления подается в модель Сойера в Simscape Multibody (где включен планарный объект для взаимодействия с роботом).

Создайте вычисленный контроллер крутящего момента, используя блоки Robotics Manipulator

Для манипулятора с $n$ не фиксированные соединения, динамика системы может быть выражена как:

$M (q) q_{}^{¨} + C (q, q_{}^{˙}) q_{}^{˙} + G (q) = u$

где $q$ , $q_{}^{˙}$ , $q_{}^{¨}$ , $\in R^{n}$ - положение, скорость и ускорение обобщенной координаты, $u \in R^{n}$ - вход управления (крутящий момент), $M (q)$ - большая матрица пространства соединений, $C (q, q_{}^{˙}) q_{}^{˙}$ - крутящий момент продукта скорости, $G (q)$ - коэффициент тяжести. Отслеживать по желаемой траектории соединения с желаемым положением $q_{d}$ , скорость ${q_{}^{˙}}_{d}$ и ускорение ${q_{}^{¨}}_{d}$ контроллер вычисленного крутящего момента вычисляет крутящий момент, необходимый для получения заданного строения и скорости, при условии, что переменные динамики робота $M (q)$ , $C (q, q_{}^{˙}) q_{}^{˙}$ , и $G (q)$ . В Simulink эти переменные могут быть легко выведены с помощью блоков манипуляторов робототехники из Robotics System Toolbox для разработки следующего вычисленного контроллера крутящего момента:

$u = M (q) ({q_{}^{¨}}_{d} - K_{d} {q_{}^{˙}}_{e} - K_{p} q_{e}) + C (q, q_{}^{˙}) q_{}^{˙} + G (q)$

где $q_{e} = q - q_{d}$ является позиционной ошибкой и ${q_{}^{˙}}_{e} = q_{}^{˙} - {q_{}^{˙}}_{d}$ - ошибка скорости. С помощью этого входа контроллера динамика системы становится ОДУ второго порядка:

${q_{}^{¨}}_{e} + K_{d} {q_{}^{˙}}_{e} + K_{p} q_{e} = 0$

Путем выбора $K_{d}$ и $K_{p}$ правильно, ошибка отслеживания $q_{e}$ сходится к нулю, когда время приближается к бесконечности.

Подсистема Computed Torque Controller построена с использованием трех блоков манипуляторов робототехники: Пространство соединений Большой матрицы, Крутящий момент продукта по скорости и Гравитационный крутящий момент. Обратите внимание, что связанный rigidBodyTree модель, sawyer, назначается во всех этих блоках, и строения и скорость должны быть вставлены как векторы-столбцы.

open_system('robotSafeTrajectoryTracking/Computed Torque Controller');

Внутри Computed Torque Controller есть два настраиваемых параметров (обозначенных цветными блоками):

Коэффициент усиления Kp: Пропорциональная составляющая при корректировке строения робота
Коэффициент усиления Kd: Производный коэффициент усиления при коррекции строения робота

Стандартный способ определения Kp и Kd вычислим их как:

$K_{p} = ω_{n}^{2}$

$K_{d} = 2 ω_{n} ξ$

где $ω_{n}$ и $ξ$ являются естественной частотой и коэффициентом затухания ОДУ второго порядка. В этом примере значение по умолчанию Kp и Kd получают путем установки собственной частоты и коэффициента затухания как $ω_{n} = 10$ и $ξ = 1$ сделать ОДУ второго порядка критически демпфированной системой.

Масштабирование траектории

В этой подсистеме есть два основных блока:

Масштабирование траектории
Желаемое Движение

Масштабирование траектории является основным блоком, развернутым для безопасного отслеживания траектории в этом примере. На каждом временном шаге $t_{i}$ , исходная метка времени вычисляется как $t_{i} = t_{i - 1} + Δ t$ . Однако, когда робот сталкивается с неожиданным объектом, увеличивающийся крутящий момент и отклонение от запланированной траектории могут быть разрушительными как для робота, так и для объекта. Основная идея масштабирования траектории состоит в том, чтобы вычислить метку времени как $t_{i} = t_{i - 1} + f_{s} (q_{d}, {q_{}^{˙}}_{d}, {q_{}^{¨}}_{d}, τ_{m e a}) Δ t$ путем представления $f_{s} (q_{d}, {q_{}^{˙}}_{d}, {q_{}^{¨}}_{d}, τ_{m e a}) \in [- 1, 1]$ , которая является функцией желаемого движения и измеренного крутящего момента $τ_{m e a}$ . Функция $f_{s} ()$ управляет скоростью движения робота и определяется на основе помех, ощущаемых роботом. Если измеренные крутящие моменты больше, чем ожидалось, $f_{s} ()$ уменьшается, чтобы заставить робота замедлиться или даже двигаться назад, пока не будут достигнуты требуемые крутящие моменты. Эти значения $f_{s} ()$ оказывают следующие эффекты на движение робота:

$f_{s} () > 0$ , робот движется вперед ( $f_{s} () = 1$ - нормальная скорость).
$f_{s} () = 0$ робот останавливается.
$f_{s} () < 0$ робот движется назад.

В блоке Trajectory Scaling требуется оценить внешний крутящий момент ${τ_{}^{ˆ}}_{e x t} = τ_{m e a} - τ_{}^{ˆ}$ вычислять $f_{s} ()$ , где $τ_{m e a}$ - измеренный крутящий момент из модели Simscape, и $τ_{}^{ˆ}$ - ожидаемый крутящий момент требуемого движения на предыдущей метке времени. В разделе «Внешний крутящий момент» модели, Блок Обратная динамика вычисляет ожидаемый крутящий момент, который вычитается из момента измерения. Ожидаемый крутящий момент: $τ_{}^{ˆ} = M (q_{d}) ({q_{}^{¨}}_{d}) + C (q_{d}, {q_{}^{˙}}_{d}) {q_{}^{˙}}_{d} + G (q_{d})$ .

В области Желаемое Движение выход алгоритма масштабирования траектории подается как вход в блок Полиномиальной Траектории. Этот блок вычисляет квинтическую полиномиальную траекторию, учитывая известные значения q0 и q1 и их граничные условия - нулевую скорость и ускорение. Это выводит положение, скорость и ускорение: $q_{d} (t)$ , ${q_{}^{˙}}_{d} (t)$ и ${q_{}^{¨}}_{d} (t)$ , которые подаются в подсистему Computed Torque Controller.

Модель робота Simscape Multibody и простая контактная механика

Модель робота Simscape Multibody импортируется из той же .urdf файл с использованием smimport, где добавлен набор приводов крутящего момента и датчиков крутящего момента, положения и скорости соединений. Блок контактного механизма, как показано ниже, добавлен для моделирования силы реакции между концевым эффектором и препятствием как сферой и плоскостью, где используется простая линейная модель пружины-демпфера.

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

Запуск модели

Запустите модель и наблюдайте за поведением Сойера в симуляторе робота и взаимодействуйте с ней.

Сначала откройте средство просмотра, нажав на значок возможностей, показанный ниже слева от блока модели Simscape. Он отображает сигналы, включая моменты в соединениях, силу контакта между концевым эффектором и планарным объектом и меткой времени для вычисления желаемого движения для отслеживания траектории.

Переключить переключатель масштабирования траектории на «Off».

Используйте следующую команду или кнопку «Run» в Simulink, чтобы начать симуляцию.

sim('robotSafeTrajectoryTracking.slx','StopTime','5');

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

Теперь переключатель масштабирования траектории на «On» и перезапустите модель.

sim('robotSafeTrajectoryTracking.slx','StopTime','5');

Заметьте различия в вычисленных крутящих моментах и уменьшенной силе реакции после столкновения.

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

Сводные данные

Этот пример показал, как использовать блоки манипулятора робототехники в Simulink, чтобы спроектировать контроллер вычисленного крутящего момента и интегрировать его с масштабированием траектории и динамической симуляцией в Simscape Multibody, чтобы достичь безопасного отслеживания траектории. Результирующие крутящий момент, сила реакции и метка времени также продемонстрировали способность масштабирования траектории для выполнения безопасного отслеживания траектории.

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