Задайте робота и начальное состояние

Загрузите IRB-120T ABB из библиотеки роботов с помощью loadrobot функция.

robot = loadrobot("abbIrb120T","DataFormat","column","Gravity",[0 0 -9.81]);
numJoints = numel(homeConfiguration(robot));

Задайте параметры симуляции, включая временную область значений, в течение которого моделируется траектория, начальное состояние как [joint configuration; jointVelocity], и заданную точку пространства соединений.

% Set up simulation parameters
tSpan = 0:0.01:0.5;
q0 = zeros(numJoints,1);
q0(2) = pi/4; % Something off center
qd0 = zeros(numJoints,1);
initialState = [q0; qd0];

% Set up joint control targets
targetJointPosition = [pi/2 pi/3 pi/6 2*pi/3 -pi/2 -pi/3]';
targetJointVelocity = zeros(numJoints,1);
targetJointAcceleration = zeros(numJoints,1);

Визуализируйте положение цели.

show(robot,targetJointPosition)

ans = 
  Axes (Primary) with properties:

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

  Show all properties

Моделирование поведения с управлением пространством соединений

Использование jointSpaceMotionModel объект, симулируйте движение модели с обратной связью под множеством контроллеров. Этот пример сравнивает несколько из них. Каждый образец использует derivative функция для вычисления производной состояния. Здесь состояние является 2n-элементом вектора [joint configuration; joint velocity], где n - количество соединений в связанной rigidBodyTree объект.

Управление вычисляемым крутящим моментом

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

Создайте jointSpaceMotionModel и укажите модель робота. Установите "MotionType" на "ComputedTorqueControl". Обновите динамику ошибок, используя updateErrorDynamicsFromStep и укажите требуемое время урегулирования и перерегулирование соответственно. Также можно задать коэффициент затухания и естественную частоту непосредственно в объекте.

computedTorqueMotion = jointSpaceMotionModel("RigidBodyTree",robot,"MotionType","ComputedTorqueControl");
updateErrorDynamicsFromStep(computedTorqueMotion,0.2,0.1);

Эта модель движения требует, чтобы было обеспечено положение, скорость и ускорение.

qDesComputedTorque = [targetJointPosition; targetJointVelocity; targetJointAcceleration];

Чтобы просмотреть пример этого контроллера на практике в Simulink, смотрите пример выполнения безопасного управления отслеживанием траектории с использованием блоков Robotics Manipulator.

Независимое управление соединениями

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

Создайте другую joinSpaceMotionModel использование "IndependentJointMotion" тип движения.

IndepJointMotion = jointSpaceMotionModel("RigidBodyTree",robot,"MotionType","IndependentJointMotion");
updateErrorDynamicsFromStep(IndepJointMotion,0.2,0.1);

Эта модель движения требует, чтобы было обеспечено положение, скорость и ускорение.

qDesIndepJoint = [targetJointPosition; targetJointVelocity; targetJointAcceleration];

Пропорционально-производное управление

Пропорционально-Производное Управление, или PD-Управление, объединяет гравитационную компенсацию с пропорциональным и производным усилениями. Несмотря на более простую природу относительно других моделей закрытой формы, PD Controller может быть стабильным для всех положительных значений усиления, что делает его желательной опцией. Здесь коэффициент усиления PD заданы как n-на-n матриц, где n - количество соединений в связанной rigidBodyTree объект. Для этого робота n = 6. Кроме того, PD Control не требует профиля ускорения, поэтому его вектор состояния является всего лишь 2n-элементным вектором строений соединений и скоростей соединений.

pdMotion = jointSpaceMotionModel("RigidBodyTree",robot,"MotionType","PDControl");
pdMotion.Kp = diag(300*ones(1,6));
pdMotion.Kd = diag(10*ones(1,6));

Эта модель движения требует, чтобы были обеспечены положение и скорость.

qDesPD = [targetJointPosition; targetJointVelocity];

Симулируйте с использованием решателя ОДУ

The derivative функция выводит производную состояния, которая может быть интегрирована с помощью решателя для обыкновенных дифференциальных уравнений (ОДУ), такого как ode45. Для каждой модели движения решатель ОДУ выводит вектор-столбец m-элемента, который покрывает tspan и матрицу 2 на m вектора состояния 2n-элемента в каждый момент времени.

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

[tComputedTorque,yComputedTorque] = ode45(@(t,y)derivative(computedTorqueMotion,y,qDesComputedTorque),tSpan,initialState);
[tIndepJoint,yIndepJoint] = ode45(@(t,y)derivative(IndepJointMotion,y,qDesIndepJoint),tSpan,initialState);
[tPD,yPD] = ode15s(@(t,y)derivative(pdMotion,y,qDesPD),tSpan,initialState);

Графическое изображение результатов

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

% Computed Torque Control
figure
subplot(2,1,1)
plot(tComputedTorque,yComputedTorque(:,1:numJoints)) % Joint position
hold all
plot(tComputedTorque,targetJointPosition*ones(1,length(tComputedTorque)),'--') % Joint setpoint
title('Computed Torque Motion: Joint Position')
xlabel('Time (s)')
ylabel('Position (rad)')
subplot(2,1,2)
plot(tComputedTorque,yComputedTorque(:,numJoints+1:end)) % Joint velocity
title('Joint Velocity')
xlabel('Time (s)')
ylabel('Velocity (rad/s)')

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

% Independent Joint Motion
figure
subplot(2,1,1)
plot(tIndepJoint,yIndepJoint(:,1:numJoints))
hold all
plot(tIndepJoint,targetJointPosition*ones(1,length(tIndepJoint)),'--')
title('Independent Joint Motion: Position')
xlabel('Time (s)')
ylabel('Position (rad)')
subplot(2,1,2);
plot(tIndepJoint,yIndepJoint(:,numJoints+1:end))
title('Joint Velocity')
xlabel('Time (s)')
ylabel('Velocity (rad/s)')

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

% PD with Gravity Compensation
figure
subplot(2,1,1)
plot(tPD,yPD(:,1:numJoints))
hold all
plot(tPD,targetJointPosition*ones(1,length(tPD)),'--')
title('PD Controlled Joint Motion: Position')
xlabel('Time (s)')
ylabel('Position (rad)')
subplot(2,1,2)
plot(tPD,yPD(:,numJoints+1:end))
title('Joint Velocity')
xlabel('Time (s)')
ylabel('Velocity (rad/s)')

Визуализация траекторий как анимации

Чтобы увидеть, как это поведение выглядит в 3-D, в следующем примере помощник строит график движения робота во времени. Третий вход является количеством систем координат между каждой выборкой.

exampleHelperRigidBodyTreeAnimation(robot,yComputedTorque,1);

exampleHelperRigidBodyTreeAnimation(robot,yIndepJoint,1);

exampleHelperRigidBodyTreeAnimation(robot,yPD,1);