В этом примере показано, как симулировать движение объединенного пробела автоматизированного манипулятора под управлением с обратной связью.
Загрузите 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.5000 1.5000] YLim: [-1.5000 1.5000] 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, смотрите Движение Манипулятора LBR Управления Через пример Задания крутящего момента в сочленении.
С независимым объединенным управлением, модель каждое соединение как отдельная система, которая имеет ответ отслеживания второго порядка. Этот тип модели является идеализированным поведением и лучше всего используется, когда ответ медленный, или когда динамика не окажет значительное влияние на результирующую траекторию. В тех случаях это будет вести себя то же самое как управление вычисленного крутящего момента, но с меньшим количеством вычислительных издержек.
Создайте другой 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);