Определение робота и начального состояния

Загрузите ABB- IRB-120T из библиотеки роботов с помощью 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 см. в примере «Выполнение безопасного управления отслеживанием траектории с помощью блоков манипулятора робототехники».

Независимый совместный контроль

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

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

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

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

qDesIndepJoint = [targetJointPosition; targetJointVelocity; targetJointAcceleration];

Пропорционально-производный контроль

Управление пропорциональной производной, или управление PD, объединяет гравитационную компенсацию с пропорциональным и производным коэффициентами усиления. Несмотря на более простую природу по сравнению с другими моделями закрытой формы, контроллер PD может быть стабильным для всех положительных значений усиления, что делает его желательным вариантом. Здесь PD Gains устанавливаются как 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];

Моделирование с использованием решателя ОДУ

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

% 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);