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

В этом примере используются:

В этом примере показано, как использовать блоки алгоритмов манипуляторов Simulink ® with 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];

Создание траектории и связанная настройка

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

tStart = 0.5;
tDuration = 3;

Затем назначьте начальную и целевую конфигурацию. q0 - начальная конфигурация и q1 является целевой конфигурацией.

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

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

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

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

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

open_system('robotSafeTrajectoryTracking.slx');

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

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

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

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

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

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

где $q$ , $_{}^{q˙}$ , $q_{}^{¨}$ , $^{∈Rn}$ являются положением, скорость и ускорение обобщенной координаты, $^{u∈Rn}$ являются входом контроля (крутящий момент), $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}$ будет сходиться к нулю, когда время приближается к бесконечности.

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

open_system('robotSafeTrajectoryTracking/Computed Torque Controller');

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

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

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

$_{Kp} =_{}^{}$ startn2

$_{Kd} =_{}$ 2

где $_{startn}$ и $start$ - собственная частота и коэффициент демпфирования ОДУ второго порядка. В этом примере значение по умолчанию 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], которая является функцией от желаемого движения и $_{}$ измеренного крутящего момента startmea. $_{}$ Функция fs () управляет скоростью движения робота и определяется на основе помех, ощущаемых роботом. Если измеренные крутящие моменты больше, $_{} чем$ ожидалось, fs () уменьшается, чтобы заставить робота замедлиться или даже двигаться назад до тех пор, пока не будут достигнуты требуемые крутящие моменты. $_{} Эти$ значения fs () оказывают следующее влияние на движение робота:

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

В блоке Масштабирование траектории (Trajectory Scaling) необходимо оценить внешний крутящий момент, ${_{}^{}}_{}_{}_{}^{τˆext=τmea-τˆ}$ для вычисления $_{fs}$ (), где $_{}$ startmea - измеренный крутящий момент из модели Simscape, а $_{}^{τˆ}$ - ожидаемый крутящий момент требуемого движения на предыдущей временной метке. В разделе «Внешний крутящий момент наблюдателя» модели Блок «Обратная динамика» вычисляет ожидаемый крутящий момент, который вычитается из измеряемого крутящего момента. Ожидаемый крутящий момент составляет: $_{}^{} τˆ=M (_{} qd) {_{}^{(}}_{q} cu)_{} + {_{}^{C}}_{} ({_{}^{}}_{} qd,q˙d)_{}$ q˙d+G (qd).

В области требуемого движения выходной сигнал алгоритма масштабирования траектории подается как входной сигнал в блок полиномиальной траектории. Этот блок вычисляет квинтическую полиномиальную траекторию, учитывая известные значения q0 и q1 и их граничные условия - нулевую скорость и ускорение. Он выдает положение, скорость и ускорение: $_{qd} (t$ ), ${_{}^{}}_{q˙d} (t$ ) и ${_{}^{q (}}_{} t),$ которые подаются в подсистему контроллера вычисленного крутящего момента.

Модель многофюзеляжного робота Simscape и простая контактная механика

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

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

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

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

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

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

Используйте следующую команду или кнопку «Выполнить» в Simulink для запуска моделирования.

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

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

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

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

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

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

Резюме

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

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