Этот пример показывает, как разработать файлы модели C-MEX, которые включают скаляр, векторные, а также параметры матрицы. Как основание моделирования, мы будем использовать несколько идеализированный промышленный робот, где левые стороны выведенных уравнений пространства состояний не явно заданы. Чтобы сделать его иллюстративным, мы будем также использовать несколько данных об эксперименте в идентификационной части.
Продуманный робот, Manutec r3, был первоначально произведен Manutec, дочерней компанией Siemens. В действительности робот включает шесть различных ссылок, три для расположения центра инструмента и три для ориентации самого инструмента. Здесь мы будем только считать моделирование робота с этими тремя степенями свободы связанным с перемещением центра инструмента. Компоненты робота будут смоделированы как твердые тела, соединенные вращательными соединениями с одной степенью свободы. Трением и другими комплексными явлениями в коробках передач, а также динамике двигателей и датчиков пропускают. Даже с этими упрощениями, получившаяся образцовая структура, как мы будем видеть, довольно сложный.
Образцовая структура, используемая для идентификационных экспериментов, проводимых ниже, была описана подробно в документе:
M. Выдра и S. Турок. Модели 1 и 2 DFVLR робота Manutec r3. Институт Динамики Робототехники и Системы, немецкая Космическая Научно-исследовательская организация (ДОЛЛАР), Оберпфаффенхофен, май 1988.
и оценка параметра на основе упрощенного робота Manutec r3 была ранее рассмотрена в книге:
К. Щиттковский. Числовые Данные, Помещающиеся в Динамические Системы. Kluwer Академические Издатели, страницы 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, применился к этим трем двигателям, представляют внешнюю силу, перемещающую робота. Эти сигналы индивидуально масштабируются (через коэффициенты силы ФК (1), ФК (2) и ФК (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 различных параметров модели или константы.
Файл модели 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], мн = p[5] и Ia1 = p[8] скаляры.
ФК = p[1], r = p[2], я = p[3], m = p[4] и L =, p[6] является вектор-столбцами с двумя или тремя записями.
com = p[7] является матрицей 2 на 2 и Ia = p[9] 4 2 матрица.
На скаляры, как обычно, ссылаются как p [0] (p (1) в файле MATLAB) и i:th векторный элемент как 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];
}Консультируйтесь с "Созданием пример" Файлов Модели IDNLGREY для получения дальнейшей информации о файлах модели C-MEX.
У нас теперь есть достаточное знание, чтобы создать объект 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';
Перед продолжением оценки параметра мы моделируем модель с помощью начальных значений параметров. Мы используем решатель для дифференциальных уравнений по умолчанию (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.
На этот раз мы используем алгоритм поиска Levenberg-Marquardt.
opt = nlgreyestOptions('SearchMethod', 'lm', 'Display', 'on'); nlgr = nlgreyest(z, nlgr, opt);
Производительность предполагаемого робота Manutec r3 затем исследована посредством симуляции.
compare(z, nlgr);
Рисунок 3: Сравнение между истинными выходными параметрами и моделируемыми выходными параметрами предполагаемой модели робота Manutec r3.
Как видно в фигуре, подгонка между моделируемыми и истинными выходными параметрами значительно улучшилась, особенно когда дело доходит до третьего вывода (относительный угол между рукой 2 и рукой 3). Истина и предполагаемые параметры вполне друг близко к другу:
disp(' 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)); fprintf(' %1.3f %1.3f\n', ptrue(26), nlgr.Parameters(end).Value(2, 2)); fprintf(' %1.3f %1.3f\n', ptrue(27), nlgr.Parameters(end).Value(3, 2)); fprintf(' %1.3f %1.3f\n', ptrue(28), nlgr.Parameters(end).Value(4, 2));
True Estimated parameter vector 5.410 5.414 5.600 5.609 0.390 0.390 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 моделирование среды. Кроме того, мы сделали это для примера моделирования робота, где уравнениями нужно было управлять в порядке поместиться в необходимую явную форму пространства состояний.