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

Этот пример показывает вам, как использовать 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¨, Rn положение, скорость и ускорение обобщенной координаты, uRn вход управления (крутящий момент), M(q) объединенная большая матрица пробела, C(q,q˙)q˙ скоростной крутящий момент продукта, G(q) крутящий момент силы тяжести. Отслеживать вдоль желаемой объединенной траектории с желаемым положением qd, скорость q˙d и ускорение q¨d, вычисленный контроллер крутящего момента вычисляет, крутящий момент должен был получить данную настройку и скорость, обеспечил переменные динамики робота M(q), C(q,q˙)q˙, и G(q). В Simulink эти переменные могут быть легко выведены с помощью блоков манипулятора робототехники из Robotics System Toolbox, чтобы спроектировать следующий вычисленный контроллер крутящего момента:

u=M(q)(q¨d-Kdq˙e-Kpqe)+C(q,q˙)q˙+G(q)

где qe=q-qd ошибка положения и q˙e=q˙-q˙d ошибка скорости. С этим входом контроллера системная динамика становится ОДУ второго порядка:

q¨e+Kdq˙e+Kpqe=0

Путем выбора Kd и Kp правильно, ошибка отслеживания qe будет сходиться, чтобы обнулить, когда время приблизится к бесконечности.

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

open_system('robotSafeTrajectoryTracking/Computed Torque Controller');

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

  • Получите Kp: Пропорциональная составляющая при исправлении настройки робота

  • Получите Kd: Производное усиление при исправлении настройки робота

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

Kp=ωn2

Kd=2ωnξ

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

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

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

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

  • Желаемое движение

Масштабирование траектории является основным блоком, развернутым для безопасного отслеживания траектории в этом примере. На каждом временном шаге ti, исходная метка времени вычисляется как ti=ti-1+Δt. Однако, когда робот сталкивается с непредвиденным объектом, увеличивающийся крутящий момент и отклонение от запланированной траектории могут быть разрушительными и для робота и для объекта. Основная идея масштабирования траектории состоит в том, чтобы вычислить метку времени как ti=ti-1+fs(qd,q˙d,q¨d,τmea)Δt путем представления fs(qd,q˙d,q¨d,τmea)[-1,1], который является функцией желаемого движения и измеренного крутящего момента τmea. Функция fs() контролирует скорость движения робота и определяется на основе интерференции, которую чувствует робот. Если измеренные крутящие моменты больше, чем ожидалось, fs() уменьшен, чтобы заставить робота замедлиться или даже переместиться назад, пока желаемые крутящие моменты не достигаются. Эти значения fs() окажите следующие влияния на движение робота:

  • fs()>0, робот продвигается (fs()=1 нормальная скорость).

  • fs()=0, остановки робота.

  • fs()<0, робот перемещается назад.

В блоке Trajectory Scaling это требуется, чтобы оценивать внешний крутящий момент τˆext=τmea-τˆ вычислять fs(), где τmea измеренный крутящий момент из модели Simscape, и τˆ ожидаемый крутящий момент желаемого движения на предыдущей метке времени. В разделе External Torque Observer модели, Обратный блок Dynamics вычисляет ожидаемый крутящий момент, который вычтен из крутящего момента меры. Ожидаемый крутящий момент: τˆ=M(qd)(q¨d)+C(qd,q˙d)q˙d+G(qd).

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

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

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

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

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

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

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

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

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

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

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

  • Нажмите кнопку Stop в Simulink, чтобы остановить симуляцию.

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

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

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