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

Загрузите 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 установлены как n на n матрицы, где n является количеством соединений в связанном rigidBodyTree объект. Для этого робота, n = 6. Кроме того, Управление PD не требует ускоряющего профиля, таким образом, его вектор состояния является только вектором 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);