Промышленный робот с тремя степенями свободы: C Файл MEX на C Моделирование системы MIMO с использованием параметров вектора/матрицы
Этот пример показывает, как проектировать файлы модели C-MEX, которые включают скалярные, векторные, а также матричные параметры. В качестве базиса моделирования мы будем использовать несколько идеализированного промышленного робота, где левые стороны производных уравнений пространства состояний явным образом не заданы. Чтобы сделать его немного бита более иллюстративным, мы также будем использовать несколько экспериментальных данных в части идентификации.
Моделирование робота- R3 Манутека
Рассматриваемый робот, Manutec r3, был первоначально произведен компанией Manutec, дочерней компанией Siemens. В действительности робот содержит шесть различных ссылок, три для позиционирования центра инструмента и три для ориентации самого инструмента. Здесь мы как раз рассмотрим моделирование робота с тремя степенями свободы, связанными с движением инструмента центра. Компоненты робота будут смоделированы как твердые тела, соединенные вращательными шарнирами с одной степенью свободы. Трением и другими сложными явлениями в коробках передач, а также динамикой двигателей и датчиков пренебрегают. Даже при этих упрощениях получившаяся структура модели, как мы увидим, довольно сложна.
Структура модели, используемая для экспериментов по идентификации, проведенных ниже, была подробно описана в документе:
М. Оттер и С. Тюрк. Модели DFVLR 1 и 2 робота Manutec r3. Институт робототехники и системной динамики, Немецкое учреждение аэрокосмических исследований (DLR), Оберпфаффенхофен, май 1988 года.
и оценка параметра на основе упрощенного робота Manutec r3 ранее рассматривалась в книге:
К. Щиттовски. Численный подбор данных в динамических системах. Kluwer Academic Publishers, страницы 239-242, 2002.
Фиг.1 показывает схематическое изображение робота Manutec r3.
Фигура 1: Схематический вид робота Manutec r3.
Динамика упрощенного робота Manutec r3 задается вектором уравнением
d^2 d
M(q(t)) ---- q(t) = F(u(t)) + G(q(t)) + H(-- q(t), q(t))
dt^2 dt
где вектор-столбец q (t) = [q_1 (t), q_2 (t), q_3 (t)] 'описывает относительный угол между рычагом i-1 и рычагом i для i = 1, 2, 3, с рычагом 0, соответствующим координате дна. Крутящий момент u (t) = [u_1 (t) u_2 (t) u_3 (t)] ^ T, приложенный к трем двигателям, представляет собой экзогенную силу, перемещающую робота. Эти сигналы масштабируются индивидуально (через коэффициенты силы Fc (1), Fc (2) и Fc (3)), чтобы обеспечить движущую силу:
F(u(t)) = [Fc(1)*u_1(t) Fc(2)*u_2(t) Fc(3)*u_3(t)]'
Большая матрица M (q (t)) является довольно сложной симметричной и положительно определенной матрицей 3 на 3 с элементами следующим образом.
M(1, 1) = c_1(p) + c_2(p)*cos(q_2(t))^2 + c_3(p)*sin(q_2(t))^2 +
c_4(p)*cos(q_2(t)+q_3(t)) + c_5(p)*sin(q_2(t)+q_3(t)) +
c_6(p)*sin(q_2(t))*sin(q_2(t)+q_3(t))
M(1, 2) = c_7(p)*cos(q_2(t)) + c_8(p)*cos(q_2(t)+q_3(t))
M(1, 3) = c_9(p)*cos(q_2(t)+q_3(t))
M(2, 1) = M(1, 2)
M(2, 2) = c_10(p) + c_11(p)*cos(q_3(t))
M(2, 3) = c_12(p) + c_13(p)*cos(q_3(t))
M(3, 1) = M(1, 3)
M(3, 2) = M(2, 3)
M(3, 3) = c_14(p)
где c_1 (p),..., c_14 (p) - 14 функций параметров робота p.
На робота также влияют две дополнительные силы. Первый, G (q (t)), вызван гравитацией и имеет элементы
G_1(p) = 0
G(p) = G_2(p) = b_1(p)*sin(q_2(t)) + b_2(p)*sin(q_2(t)+q_3(t))
G_3(p) = b_3(p)*sin(q_2(t)+q_3(t))
где b_1 (p),..., b_3 (p) - три функции параметров p. Вторая сила, h (d/dt q (t), q (t)), вызвана кориолисом и центробежными силами, которые вычисляются через так называемые символы Кристоффеля
d d d
g_ijk = -0.5*(-------- M(i, j) + -------- M(i, k) - -------- M(j, k))
d q_k(t) d q_j(t) d q_k(t)
как
d 3 3 d d
H_i(-- q(t), q(t)) = sum(sum(g_ijk*-- q_k(t))*-- q_j(t))
dt j=1 k=1 dt dt
для i = 1, 2, 3.
Большая матрица M (q (t)) обратима (для физически интересных углов), что означает, что динамику робота можно записать
d^2 -1 d ---- q(t) = M(q(t)) (F(u(t)) + G(q(t)) + H(-- q(t), q(t))) dt^2 dt
Путем введения состояний
x_1 (t) = q_1 (t), относительный угол между дном и плечом 1.
x_2 (t) = q_2 (t), относительный угол между рычагом 1 и рычагом 2.
x_3 (t) = q_3 (t), относительный угол между рычагом 2 и рычагом 3.
x_4 (t) = d/dt q_1 (t), относительная скорость между дном и плечом 1.
x_5 (t) = d/dt q_2 (t), относительная скорость между рычагом 1 и рычагом 2.
x_6 (t) = d/dt q_3 (t), относительная скорость между рычагом 2 и рычагом 3.
мы заканчиваем со структурой модели пространства состояния, подходящей для моделирования IDNLGREY. Сводные данные, эта модель включает 3 входа, 6 состояний, 3 выхода и 28 различных параметров модели или констант.
Объект модели робота- R3 IDNLGREY Manutec
Файл модели C-MEX, разработанный для этого приложения, довольно замысловат. Мы оставим без внимания многие его детали и представим лишь краткую информацию о них; заинтересованный читатель упоминается robot_c.c для полного изображения. Вышеприведенные уравнения результата в модели робота с 28 (= Np) различными параметрами или константами, что по логическим причинам помещаются в 10 (= Npo) уникальных объектов параметра: 3 скалярных таковые, 5 векторов и 2 матрицы. Функция обновления состояния, compute_dx, имеет следующий сокращенный список входных параметров:
void compute_dx(double *dx, double *x, double *u, double **p)
где p содержит 10 объектов параметра:
g = p [0], pl = p [5] и Ia1 = p [8] являются скалярами.
Fc = p [1], r = p [2], Im = p [3], m = p [4] и L = p [6] являются векторами-столбцами с двумя или тремя записями.
com = p [7] является матрицей 2 на 2, а Ia = p [9] матрицей 4 на 2.
Скаляры так же обычно ссылаются как p [0] (p (1) в файле MATLAB) и i: й векторный элемент как p [i-1] (p (i) в файле MATLAB). Матрицы, переданные в файл модели C-MEX, отличаются в том смысле, что столбцы сложены друг на друга в очевидном порядке. Следовательно, com (1, 1) называется com [0], com (2, 1) - com [1], com (1, 2) - com [3], а com (2, 2) - com [3]. Точно так же восемь элементов Ia получают через Ia [i] для i = 0, 1,..., 7.
При этом compute_dx включает следующие вычислительные шаги (обратите внимание, что многие назначения здесь не заданы). Цель шагов A-E состоит в том, чтобы перестроить уравнения так, чтобы состояния могли быть явно вычислены.
void compute_dx(double *dx, double *x, double *u, double **p)
{
/* Declaration of model parameters and intermediate variables. */
double *g, *Fc, *r, *Im, *m, *pl, *L, *com, *Ia1, *Ia;
double M[3][3]; /* Mass matrix. */
...
/* Retrieve model parameters. */
...
/* A. Components of the symmetric and positive definite mass matrix M(x, p), a 3x3 matrix. */
M[0][0] = Ia1[0] + r[0]*r[0]*Im[0] + com[2]*com[2]*m[1] ...
...
M[2][2] = Ia[4] + r[2]*r[2]*Im[2] + com[3]*com[3]*m[1] + L[1]*L[1]*pl[0];
/* B. Inputs. */
F[0] = Fc[0]*u[0]; ...
/* C. Gravitational forces G. */
G[0] = 0; ...
/* D. Coriolis and centrifugal force components Gamma and forces H. */
Gamma[1] = (Ia[6] - Ia[5] - com[3]*com[3]*m[1] ...
/* E. Compute inverse of M. */
Det = M[0][0]*M[1][1]*M[2][2] + 2*M[0][1]*M[1][2]*M[0][2] ...
/* State equations. */
/* x[0]: Relative angle between fundament and arm 1. */
/* x[1]: Relative angle between arm 1 and arm 2. */
/* x[2]: Relative angle between arm 2 and arm 3. */
/* x[3]: Relative velocity between fundament and arm 1. */
/* x[4]: Relative velocity between arm 1 and arm 2. */
/* x[5]: Relative velocity between arm 2 and arm 3. */
dx[0] = x[3];
dx[1] = x[4];
dx[2] = x[5];
dx[3] = Minv[0][0]*(F[0]+G[0]+H[0]) + Minv[0][1]*(F[1]+G[1]+H[1]) + Minv[0][2]*(F[2]+G[2]+H[2]);
dx[4] = Minv[0][1]*(F[0]+G[0]+H[0]) + Minv[1][1]*(F[1]+G[1]+H[1]) + Minv[1][2]*(F[2]+G[2]+H[2]);
dx[5] = Minv[0][2]*(F[0]+G[0]+H[0]) + Minv[1][2]*(F[1]+G[1]+H[1]) + Minv[2][2]*(F[2]+G[2]+H[2]);
}
Функция обновления выхода, compute_y, заметно проще:
/* Output equations. */
void compute_y(double y[], double x[])
{
/* y[0]: Relative angle between fundament and arm 1. */
/* y[1]: Relative angle between arm 1 and arm 2. */
/* y[2]: Relative angle between arm 2 and arm 3. */
y[0] = x[0];
y[1] = x[1];
y[2] = x[2];
}
Для получения дополнительной информации о файлах модели C-MEX см. пример «Создание файлов модели IDNLGREY».
Теперь мы обладаем достаточными знаниями, чтобы создать объект IDNLGREY, отражающий движение упрощенного робота Manutec r3. Начнем с описания входов:
InputName = {'Voltage applied to motor moving arm 1'; ...
'Voltage applied to motor moving arm 2'; ...
'Voltage applied to motor moving arm 3'};
InputUnit = {'V'; 'V'; 'V'};Далее мы определяем шесть состояний, первые три являются выходами:
StateName = {'\Theta_1'; ... % Relative angle between fundament and arm 1
'\Theta_2'; ... % Relative angle between arm 1 and arm 2
'\Theta_3'; ... % Relative angle between arm 2 and arm 3
'Vel_1'; ... % Relative velocity between fundament and arm 1
'Vel_2'; ... % Relative velocity between arm 1 and arm 2
'Vel_3'... % Relative velocity between arm 2 and arm 3
};
StateUnit = {'rad'; 'rad'; 'rad'; 'rad/s'; 'rad/s'; 'rad/s'};
OutputName = StateName(1:3);
OutputUnit = StateUnit(1:3);Как упоминалось ранее, модель включает 28 различных параметров или констант, которые сгруппированы в 10 различных объектов параметра следующим образом. Заметьте, что некоторые параметры, по физическим соображениям, заданы так, чтобы иметь отдельные минимальные значения. Эти минимальные значения параметров заданы в массиве ячеек с элементами того же размера, которые используются для определения значений параметров.
ParName = {'Gravity constant'; ... % g, scalar.
'Voltage-force constant of motor'; ... % Fc, 3-by-1 vector, for motor 1, 2, 3.
'Gear ratio of motor'; ... % r, 3-by-1 vector, for motor 1, 2, 3.
'Moment of inertia of motor'; ... % Im, 3-by-1 vector, for motor 1, 2, 3.
'Mass of arm 2 and 3 (incl. tool)'; ... % m, 2-by-1 vector, for arm 2 and 3.
'Point mass of payload'; ... % pl, scalar.
'Length of arm 2 and 3 (incl. tool)'; ... % L, 2-by-1 vector, for arm 2 and 3.
'Center of mass coordinates of arm 2 and 3'; ... % com, 2-by-2 matrix, 1:st column for arm 2 (x-,z-coord), 2:nd for arm 3.
'Moment of inertia arm 1, element (3,3)'; ... % Ia1, scalar.
'Moment of inertia arm 2 and 3' ... % Ia, 4-by-2 matrix. 1:st column for arm 2, 2:nd for arm 3;
... % column elements: 1: (1,1); 2: (2,2); 3: (3,3); 4: (1,3) and (3,1).
};
ParUnit = {'m/s^2'; 'N*m/V'; ''; 'kg*m^2'; 'kg'; 'kg'; 'm'; 'm'; 'kg*m^2'; 'kg*m^2'};
ParValue = {9.81; [-126; 252; 72]; [-105; 210; 60]; [1.3e-3; 1.3e-3; 1.3e-3]; ...
[56.5; 60.3]; 10; [0.5; 0.98]; [0.172 0.028; 0.205 0.202]; ...
1.16; [2.58 11.0; 2.73 8.0; 0.64 0.80; -0.46 0.50]};
ParMin = {eps(0); -Inf(3, 1); -Inf(3, 1); eps(0)*ones(3, 1); [40; 40]; ...
eps(0); eps(0)*ones(2, 1); eps(0)*ones(2); -Inf; -Inf(4, 2)};
ParMax = Inf; % No maximum constraint.После определения файла модели, начального состояния и так далее, объект модели Manutec r3 IDNLGREY создается следующим образом:
FileName = 'robot_c'; % File describing the model structure. Order = [3 3 6]; % Model orders [ny nu nx]. Parameters = struct('Name', ParName, 'Unit', ParUnit, 'Value', ParValue, ... 'Minimum', ParMin, 'Maximum', ParMax, 'Fixed', false); InitialStates = struct('Name', StateName, 'Unit', StateUnit, 'Value', 0, ... 'Minimum', -Inf, 'Maximum', Inf, 'Fixed', true); Ts = 0; % Time-continuous system. nlgr = idnlgrey(FileName, Order, Parameters, InitialStates, Ts, ... 'Name', 'Manutec r3 robot', 'InputName', InputName, ... 'InputUnit', InputUnit, 'OutputName', OutputName, ... 'OutputUnit', OutputUnit, 'TimeUnit', 's');
Входные-выходные данные
Далее загружаем доступные данные ввода-вывода. Выходы были моделированы с использованием вышеуказанной структуры модели IDNLGREY. Перед хранением выходы были повреждены некоторым аддитивным шумом.
load(fullfile(matlabroot, 'toolbox', 'ident', 'iddemos', 'data', 'robotdata'));
Файл содержит данные двух различных (симуляционных) экспериментов, каждый из которых содержит 101 входно-выходную выборку, сгенерированную с использованием частоты дискретизации (Ts) 0,02 секунды. Начиная с нулевого начального состояния, входы для трех двигателей [V], используемых в экспериментах, были постоянными:
u(t) = [u_1(t) u_2(t) u_3(t)]^T = [0.20 0.05 0.03]^T % Experiment 1. u(t) = [u_1(t) u_2(t) u_3(t)]^T = -[0.20 0.05 0.03]^T % Experiment 2.
Сгенерированные выходы сохраняют данные в соответствии с приведенным выше описанием. В наших целях моделирования мы создадим один объект IDDATA z, содержащий данные двух экспериментов:
z = merge(iddata(y1, u1, 0.02), iddata(y2, u2, 0.02)); z.Name = 'Manutec r3 robot'; z.InputName = nlgr.InputName; z.InputUnit = nlgr.InputUnit; z.OutputName = nlgr.OutputName; z.OutputUnit = nlgr.OutputUnit; z.Tstart = 0; z.TimeUnit = 's';
Эффективность начальной модели R3 робота Манутека
Прежде чем продолжить оценку параметра, мы симулируем модель с помощью начальных значений параметров. Мы используем решатель для дифференциальных уравнений по умолчанию (ode45) с несколько более высоким требованием к относительной точности, чем то, что используется по умолчанию. Моделируемые и истинные выходы показаны в окне построения графика, и, как показано, подгонка уже сейчас достойна (возможно, за исключением соответствия между истинным и моделируемым относительным углом между рычагом 2 и рычагом 3, то есть третьим выходом).
nlgr.SimulationOptions.RelTol = 1e-5; compare(z, nlgr);
Фигура 2: Сравнение истинных выходов и моделируемых выходов исходной модели робота Manutec r3.
Оценка параметра
Идентификация параметров Manutec r3 довольно требовательна, частично потому, что доступные данные довольно ограничены с точки зрения возбуждения, и частично из-за сильно нелинейной природы динамики робота. В порядок упростить задачу, мы оцениваем только последние четыре параметра, то есть моменты инерции, связанные с рукой 3 и инструментом:
for k = 1:size(nlgr, 'Npo')-1 % Fix all parameters of the first 9 parameter objects. nlgr.Parameters(k).Fixed = true; end nlgr.Parameters(end).Fixed(:, 1) = true; % Fix the moment of inertia parameters for arm 2.
На этот раз мы используем алгоритм поиска Левенберга-Марквардта.
opt = nlgreyestOptions('SearchMethod', 'lm', 'Display', 'on'); nlgr = nlgreyest(z, nlgr, opt);
Эффективность предполагаемой модели R3 робота Манутека
Затем эффективность предполагаемого робота Manutec r3 исследуется с помощью симуляции.
compare(z, nlgr);
Фигура 3: Сравнение истинных выходов и моделируемых выходов предполагаемой модели робота Manutec r3.
Как видно на рисунке, подгонка между моделируемым и истинным выходами значительно улучшилось, особенно когда речь идет о третьем выходе (относительном угле между рычагом 2 и рычагом 3). Истинный и предполагаемый параметры довольно близки друг к другу:
disp(' True Estimated parameter vector');True Estimated parameter vector
ptrue = [9.81; -126; 252; 72; -105; 210; 60; 1.3e-3; 1.3e-3; 1.3e-3; ... 56.5; 60.3; 10; 0.5; 0.98; 0.172; 0.205; 0.028; 0.202; 1.16; ... 2.58; 2.73; 0.64; -0.46; 5.41; 5.60; 0.39; 0.33]; fprintf(' %1.3f %1.3f\n', ptrue(25), nlgr.Parameters(end).Value(1, 2));
5.410 5.414
fprintf(' %1.3f %1.3f\n', ptrue(26), nlgr.Parameters(end).Value(2, 2));5.600 5.609
fprintf(' %1.3f %1.3f\n', ptrue(27), nlgr.Parameters(end).Value(3, 2));0.390 0.390
fprintf(' %1.3f %1.3f\n', ptrue(28), nlgr.Parameters(end).Value(4, 2));0.330 0.331
Давайте далее исследуем предполагаемую модель робота Manutec r3 с помощью команды PRESENT:
present(nlgr);
nlgr =
Continuous-time nonlinear grey-box model defined by 'robot_c' (MEX-file):
dx/dt = F(t, u(t), x(t), p1, ..., p10)
y(t) = H(t, u(t), x(t), p1, ..., p10) + e(t)
with 3 input(s), 6 state(s), 3 output(s), and 4 free parameter(s) (out of 28).
Inputs:
u(1) Voltage applied to motor moving arm 1(t) [V]
u(2) Voltage applied to motor moving arm 2(t) [V]
u(3) Voltage applied to motor moving arm 3(t) [V]
States: Initial value
x(1) \Theta_1(t) [rad] xinit@exp1 0 (fixed) in [-Inf, Inf]
xinit@exp2 0 (fixed) in [-Inf, Inf]
x(2) \Theta_2(t) [rad] xinit@exp1 0 (fixed) in [-Inf, Inf]
xinit@exp2 0 (fixed) in [-Inf, Inf]
x(3) \Theta_3(t) [rad] xinit@exp1 0 (fixed) in [-Inf, Inf]
xinit@exp2 0 (fixed) in [-Inf, Inf]
x(4) Vel_1(t) [rad/s] xinit@exp1 0 (fixed) in [-Inf, Inf]
xinit@exp2 0 (fixed) in [-Inf, Inf]
x(5) Vel_2(t) [rad/s] xinit@exp1 0 (fixed) in [-Inf, Inf]
xinit@exp2 0 (fixed) in [-Inf, Inf]
x(6) Vel_3(t) [rad/s] xinit@exp1 0 (fixed) in [-Inf, Inf]
xinit@exp2 0 (fixed) in [-Inf, Inf]
Outputs:
y(1) \Theta_1(t) [rad]
y(2) \Theta_2(t) [rad]
y(3) \Theta_3(t) [rad]
Parameters: Value Standard Deviation
p1 Gravity constant [m/s^2] 9.81 0 (fixed) in ]0, Inf]
p2(1) Voltage-force constant of motor [N*m/V] -126 0 (fixed) in [-Inf, Inf]
p2(2) 252 0 (fixed) in [-Inf, Inf]
p2(3) 72 0 (fixed) in [-Inf, Inf]
p3(1) Gear ratio of motor -105 0 (fixed) in [-Inf, Inf]
p3(2) 210 0 (fixed) in [-Inf, Inf]
p3(3) 60 0 (fixed) in [-Inf, Inf]
p4(1) Moment of inertia of motor [kg*m^2] 0.0013 0 (fixed) in ]0, Inf]
p4(2) 0.0013 0 (fixed) in ]0, Inf]
p4(3) 0.0013 0 (fixed) in ]0, Inf]
p5(1) Mass of arm 2 and 3 (incl. tool) [kg] 56.5 0 (fixed) in [40, Inf]
p5(2) 60.3 0 (fixed) in [40, Inf]
p6 Point mass of payload [kg] 10 0 (fixed) in ]0, Inf]
p7(1) Length of arm 2 and 3 (incl. tool) [m] 0.5 0 (fixed) in ]0, Inf]
p7(2) 0.98 0 (fixed) in ]0, Inf]
p8(1,1) Center of mass coordinates of arm 2 and 3 [m] 0.172 0 (fixed) in ]0, Inf]
p8(2,1) 0.205 0 (fixed) in ]0, Inf]
p8(1,2) 0.028 0 (fixed) in ]0, Inf]
p8(2,2) 0.202 0 (fixed) in ]0, Inf]
p9 Moment of inertia arm 1, element (3,3) [kg*m^2] 1.16 0 (fixed) in [-Inf, Inf]
p10(1,1) Moment of inertia arm 2 and 3 [kg*m^2] 2.58 0 (fixed) in [-Inf, Inf]
p10(2,1) 2.73 0 (fixed) in [-Inf, Inf]
p10(3,1) 0.64 0 (fixed) in [-Inf, Inf]
p10(4,1) -0.46 0 (fixed) in [-Inf, Inf]
p10(1,2) 5.41371 1.80292e-11 (estimated) in [-Inf, Inf]
p10(2,2) 5.60905 7.67168e-11 (estimated) in [-Inf, Inf]
p10(3,2) 0.389978 1.58707e-12 (estimated) in [-Inf, Inf]
p10(4,2) 0.330506 2.25628e-12 (estimated) in [-Inf, Inf]
Name: Manutec r3 robot
Status:
Termination condition: Near (local) minimum, (norm(g) < tol)..
Number of iterations: 10, Number of function evaluations: 49
Estimated using Solver: ode45; Search: lm on time domain data "Manutec r3 robot".
Fit to estimation data: [99.99 99.99;99.92 99.93;99.97 99.97]%
FPE: 9.788e-25, MSE: [2.821e-08 3.086e-08]
More information in model's "Report" property.
Некоторые идентификационные замечания
В этом случае мы получаем параметры, очень близкие к истинным таковым. Однако, вообще говоря, существует ряд причин, по которым истинные параметры могут не быть найдены:
Доступные данные не являются «достаточно информативными» для идентификации параметров (данные не являются постоянно захватывающими).
Данные повреждены таким количеством шума, что найти истинные параметры практически невозможно.
Найден локальный, а не глобальный минимум поиска. Этот риск всегда присутствует при использовании итерационных алгоритмов поиска.
Параметры структуры модели не однозначно идентифицируются. Вообще говоря, этот риск становится больше, когда оценивается больше параметров. (Оценка всех 28 параметров робота Manutec r3, например, обычно приводит к некоторым физически невозможным значениям параметров.)
Структура модели просто «слишком сложна» или содержит нелинейности, которые не «достаточно гладки».
Заключения
Основной целью этого руководства было проиллюстрировать, как включить параметры, которые являются векторами или матрицами в среде моделирования IDNLGREY C-MEX. В сложение мы сделали это для примера моделирования робота, где уравнения должны были манипулировать в порядок, чтобы соответствовать необходимой явной форме пространства состояний.