Manutec R3机器人的建模

被考虑的机器人，Manutec r3，最初是由西门子的子公司Manutec制造的。实际上，该机器人包括六个不同的环节，三个定位工具中心，三个定位工具本身。这里我们只考虑一个机器人的建模与刀具中心的运动有关的三个自由度。机器人的部件将被建模为刚体，由一个自由度的旋转关节连接。齿轮箱中的摩擦和其他复杂现象以及电机和传感器的动力学被忽略了。即使有了这些简化，得到的模型结构，正如我们将看到的，还是相当复杂的。

下面进行的识别实验所使用的模型结构在文档中有详细的描述:

奥特先生和特克先生。Manutec r3机器人的DFVLR模型1和2。德国航空航天研究所机器人和系统动力学研究所，Oberpfaffenhofen, 1988年5月。

和基于简化的Manutec r3机器人的参数估计在书中早有考虑:

k . Schittkowski。动力系统的数值数据拟合。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 - q (t), (t)) ^ 2 dt

其中列向量q(t) = [q_1(t)， q_2(t)， q_3(t)]'描述了当I = 1,2,3时臂I -1与臂I之间的相对角度，臂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₁(p) + c₂(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)) *罪(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)米(3,3)= c_14 (p)

其中c_1(p)，…，c_14(p) are 14 functions of the robot parameters 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) are three functions of the parameters p. The second force, h(d/dt q(t), q(t)), is caused by coriolis and centrifugal forces, which are computed via the so-called Christoffel symbols

d d d g_ijk = -0.5 *(-------- M (i, j ) + -------- M (i, k ) - -------- M (j, k) d q_k q_j (t) d (t) d q_k (t)

作为

d 3 3 d d H_i (q - q (t), (t)) =总和(总和(g_ijk *——q_k (t)) *——q_j (t) dt j = 1 k = 1 dt dt

对于I = 1 2 3。

质量矩阵M(q(t))是可逆的(对于物理上有趣的角度)，这意味着机器人的动力学可以被写出来

1 ^ 2 d - q (t) = M (q (t)) (F (u (t)) + G (q (t)) + H (q - q (t), (t))) ^ 2 dt

通过介绍各州

X_1 (t) = q_1(t)，基础与臂1的相对夹角。

X_2 (t) = q_2(t)，第一臂和第二臂的相对夹角。

x3 (t) = q_3(t)，臂2和臂3的相对夹角。

X_4 (t) = d/dt q_1(t)，基本臂和第1臂之间的相对速度。

X_5 (t) = d/dt q_2(t)第一臂和第二臂之间的相对速度。

X_6 (t) = d/dt q_3(t)臂2和臂3之间的相对速度。

最后得到了适合IDNLGREY建模的状态空间模型结构。总之，该模型包含3个输入、6个状态、3个输出和28个不同的模型参数或常数。

IDNLGREY Manutec R3机器人模型对象

为这个应用程序开发的C-MEX模型文件非常复杂。我们将省略它的许多细节，只提供它的一个大纲;感兴趣的读者可以参考robot_c.c获得完整的图片。上述方程得到了一个具有28个(= Np)不同参数或常数的机器人模型，由于逻辑原因，将其放入10个(= Npo)唯一参数对象中:3个标量，5个向量，2个矩阵。状态更新函数compute_dx有以下简化的输入参数列表:

Void compute_dx(双*dx，双*x，双*u，双*p)

其中p包含10个参数对象:

标量通常被引用为p[0](在MATLAB文件中p(1))，第i个向量元素被引用为p[i-1](在MATLAB文件中p(i))。传递给C-MEX模型文件的矩阵是不同的，因为列以明显的顺序相互堆叠。因此，com(1,1)被称为com[0]， com(2,1)被称为com[1]， com(1,2)被称为com[3]，而com(2,2)被称为com[3]。类似地，对于i = 0,1，…，通过Ia[i]得到Ia的八个元素7。

这样，compute_dx就涉及到以下计算步骤(注意这里省略了许多赋值操作)。步骤A-E的目的是重新构造方程，以便能够显式地计算状态。

void compute_dx(double *dx, double *x, double *u, double *p){/*模型参数和中间变量的声明。* /双g *, * Fc, * r *我,*,* pl, * L * com, * Ia1, * Ia;双M [3] [3];/*质量矩阵。* /…

/*检索模型参数。* /…

/* a对称正定质量矩阵M(x, p)的分量，一个3x3矩阵。* / M [0] [0] = Ia1 [0] + [0] * r Im [0] [0] * + com [2] * com M[1][2] *……M [2] [2] = Ia [4] + [2] * r * Im [2] [2] [3] * com + com [3] * M [1] + [1] * L [1] * pl [0];

/* B.输入。*/ F[0] = Fc[0]*u[0];.．.

/* C.重力G. */ G[0] = 0;.．.

/* D.科氏力和离心力分量Gamma和力H. */ Gamma[1] = (Ia[6] - Ia[5] - com[3]*com[3]*m[1]…

/ * e .计算逆M . * /侦破= M [0] [0] * M [1] [1] * M [2] [2] + 2 * M [0] [1] * M [1] [2] * M[0][2]……

/*状态方程。*/ /* x[0]:基础与臂1之间的相对角度。*/ /* x[1]: 1号臂与2号臂的相对角度。*/ /* x[2]:臂2和臂3的相对角度。*/ /* x[3]:基础和手臂之间的相对速度1。*/ /* x[4]:臂1和臂2之间的相对速度。*/ /* x[5]:臂2和臂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要简单得多:

/*输出方程。*/ void compute_y(double y[]， double x[]) {/* y[0]:基与臂1的相对角度。*/ /* y[1]: 1号臂和2号臂的相对角度。*/ /* y[2]:臂2和臂3之间的相对角度。*/ y[0] = x[0];Y [1] = x[1];Y [2] = x[2];｝

参考“创建IDNLGREY模型文件”示例，了解更多关于C-MEX模型文件的详细信息。

现在我们有足够的知识来创建一个IDNLGREY对象，反映简化的Manutec r3机器人的运动。我们从描述输入开始:

InputName = {电机动臂施加的电压1；.．.电机动臂施加的电压2；.．.电机动臂施加的电压3};InputUnit = {“V”；“V”；“V”};

接下来，我们定义六种状态，前三种是输出:

StateName = {“\ Theta_1”；.．.基础与手臂的相对角度“\ Theta_2”；.．.手臂1和手臂2之间的相对角度“\ Theta_3”；.．.手臂2和手臂3之间的相对角度“Vel_1”；.．.基础与手臂之间的相对速度“Vel_2”；.．.手臂1和手臂2之间的相对速度“Vel_3”.．.手臂2和手臂3之间的相对速度};StateUnit = {rad的；rad的；rad的；“rad / s”；“rad / s”；“rad / s”};OutputName = StateName(1:3);OutputUnit = StateUnit(1:3);

如前所述，该模型涉及28个不同的参数或常量，它们被集中到10个不同的参数对象中，如下所示。注意，通过物理推理，有些参数指定为具有不同的最小值。这些最小参数值在单元格数组中定义，其元素的大小与用于指定参数值的元素相同。

ParName = {“引力常数”；.．.% g，标量。“电机电压-力常数”；.．.% Fc, 3 × 1矢量，用于电机1,2,3。“电动机传动比”；.．.% r, 3 × 1矢量，对于电机1,2,3。“电动机转动惯量”；.．.% Im, 3 × 1向量，对于电机1,2,3。臂2和臂3的质量(包括工具)；.．.% m, 2 × 1向量，用于手臂2和3。“有效载荷点质量”；.．.% pl，标量。“手臂2和3的长度(包括工具)”；.．.% L, 2 × 1向量，用于手臂2和3。手臂2和3的质心坐标；.．.% com, 2 × 2矩阵，1:st列用于手臂2 (x-，z-coord)， 2:nd用于手臂3。转动惯量臂1，单元(3,3)；.．.% i1，标量。转动惯量臂2和3.．.% Ia, 4 × 2矩阵。1:st列为臂2,2:nd为臂3;.．.%列元素:1:(1,1);2: (2, 2);3: (3);4:(1,3)和(3,1)。};ParUnit = {“米/秒^ 2”；“N * m / V”；”；“公斤* m ^ 2”；“公斤”；“公斤”；“米”；“米”；“公斤* m ^ 2”；“公斤* m ^ 2”};ParValue = {9.81;(-126;252;72);(-105;210;60);[1.3 e - 3;1.3 e - 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);从(3,1);从(3,1);每股收益(0)* 1 (3,1);[40; 40];.．.每股收益(0);每股收益(0)* 1 (2,1);每股收益(0)* 1 (2);负无穷;无穷(4,2)};ParMax = Inf;%无最大限制。

在指定了模型文件、初始状态等之后，一个Manutec r3 IDNLGREY模型对象如下所示创建:

文件名=“robot_c”；描述模型结构的文件。Order = [3 3 6];模型订单[ny nu nx]。参数= struct(“名字”ParName,“单位”ParUnit,“价值”ParValue,.．.“最低”ParMin,“最大”ParMax,“固定”、假);InitialStates = struct(“名字”StateName,“单位”StateUnit,“价值”0,.．.“最低”负无穷,“最大”正,“固定”,真正的);Ts = 0;时间连续系统。nlgr = idnlgrey(FileName, Order, Parameters, InitialStates, Ts，.．.“名字”，“Manutec r3机器人”，“InputName”InputName,.．.“InputUnit”InputUnit,“OutputName”OutputName,.．.“OutputUnit”OutputUnit,“TimeUnit”，“年代”）;

输入输出数据

接下来，加载可用的输入输出数据。利用上述IDNLGREY模型结构对输出进行了仿真。在存储之前，输出会受到一些附加噪声的破坏。

负载(fullfile (matlabroot“工具箱”，“识别”，“iddemos”，“数据”，“robotdata”));

该文件包含来自两个不同(模拟)实验的数据，每个实验都包含101个使用0.02秒的采样率(Ts)生成的输入输出样本。从初始状态为零开始，实验中使用的三个电机[V]的输入都保持恒定:

u (t) = [u_1 (t) u_2 (t) u_3 (t)) ^ t = (0.20 0.05 0.03) ^ t %实验1。u (t) = [u_1 (t) u_2 (t) u_3 (t)) ^ t = -(0.20 0.05 0.03) ^ %实验2。

生成的输出根据上述描述保存数据。出于建模的目的，我们创建了一个IDDATA对象z，其中包含来自两个实验的数据:

Z = merge(iddata(y1, u1, 0.02)， iddata(y2, u2, 0.02));z.Name =“Manutec r3机器人”；z.InputName = nlgr.InputName;z.InputUnit = nlgr.InputUnit;z.OutputName = nlgr.OutputName;z.OutputUnit = nlgr.OutputUnit;z.Tstart = 0;z.TimeUnit =“年代”；

最初的Manutec R3机器人模型的性能

在进行参数估计之前，我们使用初始参数值对模型进行模拟。我们使用默认的微分方程求解器(ode45)，对相对精度的要求比默认使用的稍高。模拟和真实的输出显示在一个绘图窗口中，如所示，现在的适合度已经很不错了(可能除了手臂2和手臂3之间的真实和模拟相对角度之间的适合度，即第三个输出)。

nlgr.SimulationOptions.RelTol = 1e-5;比较(z, nlgr);

图2:Manutec r3初始机器人模型的真实输出与模拟输出的比较。

参数估计

识别Manutec r3参数的要求相当高，部分原因是可用的数据在激励方面相当有限，部分原因是机器人动力学的高度非线性性质。为了简化任务，我们只估计后四个参数，即与臂3和工具相关的转动惯量:

为K = 1:size(nlgr，“禁食”) 1修复前9个参数对象的所有参数。nlgr.Parameters (k)。固定= true;结束nlgr.Parameters(结束)。Fixed(:， 1) = true;固定臂2的转动惯量参数。。

这一次我们使用Levenberg-Marquardt搜索算法。

opt = nlgreyestOptions(“SearchMethod”，“lm”，“显示”，“上”）;NLGR = nlgreyest(z, NLGR, opt);

估计的Manutec R3机器人模型的性能

接下来通过仿真研究估计的Manutec r3机器人的性能。

比较(z, nlgr);

图3:估计的Manutec r3机器人模型的真实输出与模拟输出的比较。

从图中可以看出，模拟输出和真实输出之间的拟合有了很大的提高，特别是第三个输出(手臂2和手臂3之间的相对角度)，真实参数和估计参数非常接近:

disp (' True估计参数向量'）;

估计参数向量

Ptrue = [9.81;-126;252;72;-105;210;60;1.3 e - 3;1.3 e - 3;1.3 e - 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);流(' %1.3f %1.3f\n'， ptrue(25)， nlgr.Parameters(end)。值(1、2));

5.410 - 5.414

流(' %1.3f %1.3f\n'， ptrue(26)， nlgr.Parameters(end)。值(2,2));

5.600 - 5.609

流(' %1.3f %1.3f\n'， ptrue(27)， nlgr.Parameters(end)。值(3 2));

0.390 - 0.390

流(' %1.3f %1.3f\n'， ptrue(28)， nlgr.Parameters(end)。值(4,2));

0.330 - 0.331

让我们通过PRESENT命令进一步研究估计的Manutec r3机器人模型:

礼物(nlgr);

nlgr =由'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. Model Properties

一些识别说明

在这种情况下，我们得到的参数非常接近真实的参数。然而，一般来说，有一些原因可能导致找不到真正的参数:

结论

本教程的主要目的是演示如何在IDNLGREY C-MEX建模框架中包含向量或矩阵的参数。此外，我们还为一个机器人建模示例这样做了，在该示例中，为了适应所需的显式状态空间形式，必须对方程进行操作。