Модель роботизированной руки и контроллер

Этот пример использует шесть роботизированных рук степени свободы, показанных ниже. Эта рука состоит из шести соединений, помеченных от основы, чтобы снабдить подсказкой: "Проигрыватель", "Bicep", "Предплечье", "Запястье", "Рука" и "Механизм захвата". Каждое соединение приводится в движение двигателем постоянного тока за исключением соединения Bicep, которое использует два двигателя постоянного тока в тандеме.

Рисунок 1: манипулятор Роботизированной руки.

Файл "cst_robotarm.slx" содержит модель Simulink электрических и механических компонентов этой системы.

Рисунок 2: модель Simulink роботизированной руки.

Подсистема "Контроллера" состоит из шести цифровых ПИ-контроллеров (один на соединение). Каждый ПИ-контроллер реализован с помощью блока "2-DOF PID Controller" из Библиотеки Simulink (см., что ПИД Настраивается для Отслеживания Заданного значения по сравнению с примером Подавления помех для мотивации). Время контрольной выборки является Ts=0.1 (10 Гц).

Рисунок 3: структура Контроллера.

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

В этом примере рука должна переместиться в особую настройку приблизительно через 1 секунду со сглаженным угловым движением в каждом соединении. Рука запускается в полностью расширенном вертикальном положении со всех углов поворота шарнира в нуле за исключением угла Bicep в девяноста градусах. Настройка конца задана угловыми положениями: Проигрыватель = 60 градусов, Bicep = 80 градусов, Предплечье = 60 градусов, Запястье = 90 градусов, Рука = 90 градусов и Механизм захвата = 60 градусов.

Нажмите кнопку "Play" в модели Simulink, чтобы симулировать угловые траектории для значений усиления PI, заданных в модели. Можно сначала дважды кликнуть на синей кнопке, чтобы также показать 3D анимацию роботизированной руки. Угловые ответы и 3D анимация появляются ниже. Очевидно ответ слишком вял и неточен.

Рисунок 4: ненастроенный ответ.

Линеаризация объекта

Движущие силы манипулятора нелинейны. Чтобы понять, можно ли рукой управлять с одним набором усилений PI, линеаризуйте объект в различных точках (времена снимка состояния) вдоль траектории интереса. Здесь "объект" относится к динамике между управляющими сигналами (выходные параметры блоков ПИДа) и сигналы измерения (выход "6 блоков" Манипулятора степени свободы).

SnapshotTimes = 0:1:5;
% Plant is from PID outputs to Robot Arm outputs
LinIOs = [...
   linio('cst_robotarm/Controller/turntablePID',1,'openinput'),...
   linio('cst_robotarm/Controller/bicepPID',1,'openinput'),...
   linio('cst_robotarm/Controller/forearmPID',1,'openinput'),...
   linio('cst_robotarm/Controller/wristPID',1,'openinput'),...
   linio('cst_robotarm/Controller/handPID',1,'openinput'),...
   linio('cst_robotarm/Controller/gripperPID',1,'openinput'),...
   linio('cst_robotarm/6 DOF Robot Arm',1,'output')];
LinOpt = linearizeOptions('SampleTime',0);  % seek continuous-time model
G = linearize('cst_robotarm',LinIOs,SnapshotTimes,LinOpt);

size(G)

6x1 array of state-space models.
Each model has 6 outputs, 6 inputs, and 19 states.

Постройте разрыв между линеаризовавшими моделями в t=0,1,2,3,4 секунды и итоговой моделью в t=5 секунды.

G5 = G(:,:,end);  % t=5
G5.SamplingGrid = [];
sigma(G5,G(:,:,2:5)-G5,{1e-3,1e3}), grid
title('Variation of linearized dynamics along trajectory')
legend('Linearization at t=5 s','Absolute variation',...
       'location','SouthWest')

В то время как движущие силы значительно варьируются в низкой частоте и высокой частоте, спады изменения меньше чем до 10% около 10 рад/с, который является примерно желаемой пропускной способностью управления. Маленькие изменения объекта около целевой частоты среза усиления предполагают, что мы можем управлять рукой с одним набором усилений PI и не должны обращаться к табличному управлению.

Настройка ПИ-контроллеров с LOOPTUNE

С looptune, можно непосредственно настроить все шесть циклов PI, чтобы достигнуть желаемого времени отклика с минимальным взаимодействием цикла и соответствующими запасами устойчивости MIMO. Контроллер настраивается в непрерывное время и автоматически дискретизируется при обратной записи усилений PI к Simulink. Используйте slTuner интерфейс, чтобы задать, какие блоки должны быть настроены и определять местоположение контура объекта/контроллера.

% Linearize the plant at t=3s
tLinearize = 3;

% Create slTuner interface
TunedBlocks = {'turntablePID','bicepPID','forearmPID',...
               'wristPID','handPID','gripperPID'};
ST0 = slTuner('cst_robotarm',TunedBlocks,tLinearize);

% Mark outputs of PID blocks as plant inputs
addPoint(ST0,TunedBlocks)

% Mark joint angles as plant outputs
addPoint(ST0,'6 DOF Robot Arm')

% Mark reference signals
RefSignals = {...
   'ref Select/tREF',...
   'ref Select/bREF',...
   'ref Select/fREF',...
   'ref Select/wREF',...
   'ref Select/hREF',...
   'ref Select/gREF'};
addPoint(ST0,RefSignals)

В его самом простом использовании, looptune только потребности знать целевую пропускную способность управления, которая должна быть, по крайней мере, дважды обратной величиной желаемого времени отклика. Здесь желаемое время отклика составляет 1 секунду, так попробуйте целевую пропускную способность 3 рад/с (принимающий во внимание, что движущие силы объекта варьируются наименее близкие 10 рад/с).

wc = 3;  % target gain crossover frequency
Controls = TunedBlocks;      % actuator commands
Measurements = '6 DOF Robot Arm';  % joint angle measurements
ST1 = looptune(ST0,Controls,Measurements,wc);

Final: Peak gain = 0.957, Iterations = 10
Achieved target gain value TargetGain=1.

Окончательное значение рядом или ниже 1 указывает на тот looptune достигнутый требуемая пропускная способность. Сравните ответы на команду шага в угловом положении для начальных и настроенных контроллеров.

T0 = getIOTransfer(ST0,RefSignals,Measurements);
T1 = getIOTransfer(ST1,RefSignals,Measurements);

opt = timeoptions; opt.IOGrouping = 'all'; opt.Grid = 'on';
stepplot(T0,'b--',T1,'r',4,opt)
legend('Initial','Tuned','location','SouthEast')

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

Использование второй степени свободы

ПИ-контроллеры 2-DOF имеют feedforward и компонент обратной связи.

Рисунок 5: Два ПИД-регулятора степени свободы.

По умолчанию, looptune только настраивает обратную связь и не "видит" компонент feedforward. Это может быть подтверждено путем проверки, что параметры ПИ-контроллеров остаются установленными в свое начальное значение (введите showTunable(ST1) видеть настроенные значения). Чтобы использовать в своих интересах действие feedforward и уменьшать перерегулирование, замените цель пропускной способности явным требованием отслеживания шага от опорных углов до углов поворота шарнира.

TR = TuningGoal.StepTracking(RefSignals,Measurements,0.5);
ST2 = looptune(ST0,Controls,Measurements,TR);

Final: Peak gain = 0.766, Iterations = 13
Achieved target gain value TargetGain=1.

Настройка 2-DOF устраняет перерегулирование и улучшает ответ Bicep.

T2 = getIOTransfer(ST2,RefSignals,Measurements);
stepplot(T1,'r--',T2,'g',4,opt)
legend('1-DOF tuning','2-DOF tuning','location','SouthEast')

Проверка настроенного контроллера

Настроенные линейные ответы выглядят удовлетворительно, так запишите настроенные значения усилений PI назад к блокам Simulink и симулируйте полный маневр. Результаты симуляции появляются в рисунке 6.

writeBlockValue(ST2)

Рисунок 6: Настроенные угловые ответы.

Нелинейный ответ соединения Bicep заметно недостаточно поднимается. Дальнейшее расследование предлагает двух возможных преступников. Во-первых, ПИ-контроллеры слишком агрессивны и насыщают двигатели (входное напряжение ограничивается ± 5 В).

Рисунок 7: Входное напряжение к двигателям постоянного тока (управляющий сигнал).

Во-вторых, эффекты перекрестной связи между Wrist и Bicep, когда принесено, чтобы масштабироваться, оказывают значительное и длительное влияние на ответ Bicep. Чтобы видеть это, постройте переходной процесс этих трех соединений для фактических ступенчатых изменений, происходящих во время маневра (-10 градусов для соединения Bicep и 90 градусов для соединения Запястья).

H2 = T2([2 4],[2 4]) * diag([-10 90]);  % scale by step amplitude
H2.u = {'Bicep','Wrist'};
H2.y = {'Bicep','Wrist'};
step(H2,5), grid

Совершенствование проекта

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

JointDisp = [60 10 60 90 90 60];  % commanded angular displacements, in degrees
TR.InputScaling = JointDisp;

Чтобы уменьшать насыщение приводов, ограничьте усиление от ссылочных сигналов до управляющих сигналов.

UR = TuningGoal.Gain(RefSignals,Controls,6);

Повторно настройте контроллер с ними усовершенствованные настраивающиеся цели.

ST3 = looptune(ST0,Controls,Measurements,TR,UR);

Final: Peak gain = 1.13, Iterations = 200

Сравните масштабированные ответы с предыдущим проектом. Заметьте значительное сокращение связи между движением Wrist и Bicep, и в пиковом значении и в полной энергии.

T2s = diag(1./JointDisp) * T2 * diag(JointDisp);
T3s = diag(1./JointDisp) * getIOTransfer(ST3,RefSignals,Measurements) * diag(JointDisp);
stepplot(T2s,'g--',T3s,'m',4,opt)
legend('Initial 2-DOF','Refined 2-DOF','location','SouthEast')

Продвиньте повторно настроенные значения к Simulink для дальнейшей валидации.

writeBlockValue(ST3)

Результаты симуляции появляются в рисунке 8. Ответ Bicep теперь на одном уровне с другими соединениями в терминах времени урегулирования и сглаженного переходного процесса, и существует меньше насыщения привода, чем в предыдущем проекте.

Рисунок 8: Угловые положения и управляющие сигналы с усовершенствованным контроллером.

3D анимация подтверждает, что рука теперь перемещается быстро и точно к желаемой настройке.

Рисунок 9: подстроенный ответ.