В этом примере показано, как создавать файлы моделей C-MEX, включающие скалярные, векторные и матричные параметры. В качестве основы моделирования мы будем использовать несколько идеализированный промышленный робот, где левые стороны производных уравнений пространства состояний явно не даны. Чтобы сделать его немного более иллюстративным, мы также будем использовать множественные экспериментальные данные в части идентификации.
Рассматриваемый робот, Manutec r3, был первоначально изготовлен компанией Manutec, дочерней компанией Siemens. В действительности робот содержит шесть различных звеньев, три для позиционирования центра инструмента и три для ориентации самого инструмента. Здесь мы просто рассмотрим моделирование робота с тремя степенями свободы, связанными с движением центра инструмента. Компоненты робота будут смоделированы как жесткие тела, соединенные вращательными соединениями с одной степенью свободы. Трением и другими сложными явлениями в коробках передач, а также динамикой двигателей и датчиков пренебрегают. Даже при таких упрощениях полученная структура модели, как мы увидим, довольно сложна.
Структура модели, использованная для экспериментов по идентификации, проведенных ниже, была подробно описана в документе:
М. Оттер и С. Турк. Модели DFVLR 1 и 2 робота Manutec r3. Институт робототехники и системной динамики, Немецкое аэрокосмическое научно-исследовательское учреждение (ДЛР), Оберпфаффенхофен, май 1988 года.
и оценка параметров на основе упрощенного робота Manutec r3 ранее рассматривалась в книге:
К. Шиттковски. Подбор числовых данных в динамических системах. Академические издательства Клювера, страницы 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 функций параметров р робота.
На робота также влияют две дополнительные силы. Первая, 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) - три функции параметров р. Вторая сила 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], 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: th - как p [i-1] (p (i) в файле MATLAB). Матрицы, передаваемые в файл модели C-MEX, отличаются в том смысле, что столбцы расположены друг на друге в очевидном порядке. Следовательно, ком (1, 1) упоминается как ком [0], ком (2, 1) как ком [1], ком (1, 2) как ком [3] и ком (2, 2) как ком [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.После указания файла модели, начального состояния и т. д. объект модели IDNLGREY Manutec r3 создается следующим образом:
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.
На этот раз мы используем алгоритм поиска Левенберга-Марквардта.
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');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. Кроме того, мы сделали это для примера моделирования робота, где уравнения необходимо было манипулировать, чтобы вписаться в требуемую явную форму пространства состояния.