Выполните безопасную траекторию, отслеживающую управление Используя блоки манипулятора робототехники

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

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

И Robotics System Toolbox и Simscape Multibody требуются запустить этот пример.

Введение

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

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

Этот пример использует модель Лесоруба Переосмысления, 7 манипуляторов робота степени свободы. Вызвать importrobot сгенерировать a rigidBodyTree модель из описания сохранена в файле Объединенного формата описания робота (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);

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

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

p = exampleHelperPolynomialTrajectory(q0,q1,tDuration);

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

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

open_system('robotSafeTrajectoryTracking.slx');

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

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

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

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

Для манипулятора с $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}$ будет сходиться, чтобы обнулить, когда время приблизится к бесконечности.

Вычисленная Подсистема контроллера Крутящего момента создается с помощью трех блоков манипулятора робототехники: Матрица масс пространства соединений, Скоростной крутящий момент продукта, и Крутящий момент силы тяжести. Обратите внимание на то, что связанное rigidBodyTree модель, sawyer, присвоен во всех тех блоках, и настройка и скорость должны быть вставлены как вектор-столбцы.

open_system('robotSafeTrajectoryTracking/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, и $τ_{}^{ˆ}$ ожидаемый крутящий момент желаемого движения на предыдущей метке времени. В разделе External Torque Observer модели, Обратный блок Dynamics вычисляет ожидаемый крутящий момент, который вычтен из крутящего момента меры. Ожидаемый крутящий момент: $τ_{}^{ˆ} = M (q_{d}) ({q_{}^{¨}}_{d}) + C (q_{d}, {q_{}^{˙}}_{d}) {q_{}^{˙}}_{d} + G (q_{d})$ .

В блоке Desired Motion полиномиальные коэффициенты даны, чтобы сгенерировать желаемую траекторию с данным timeStamp введите, который может быть настроен на основе алгоритма масштабирования траектории. $q_{d} (t)$ , ${q_{}^{˙}}_{d} (t)$ и ${q_{}^{¨}}_{d} (t)$ выходные параметры на основе полинома траектории и питаются Вычисленную Подсистему контроллера Крутящего момента.

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

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

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

Запустите модель

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

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

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

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

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

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

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

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

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