工业三自由度机器人:基于矢量/矩阵参数的MIMO系统C mex -文件建模

打开实时脚本

这个例子展示了如何设计包含标量、矢量和矩阵参数的C-MEX模型文件。作为建模的基础，我们将使用一个理想化的工业机器人，其中所导出的状态空间方程的左边没有明确给出。为了更加说明问题，我们还将在识别部分使用多个实验数据。

曼努特克R3机器人的建模

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

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

奥特和土耳其。曼努特克r3机器人的DFVLR模型1和2。德国航空航天研究所机器人与系统动力学研究所，奥伯芬霍夫，1988年5月。

和基于简化的Manutec r3机器人的参数估计已经在书中考虑过了:

k . Schittkowski。动力学系统的数值数据拟合。Kluwer学术出版社，第239-242页，2002。

图1显示了Manutec r3机器人的示意图。

图1:曼努泰克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 (23) = 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）是机器人参数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) 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 H_i（--q（t），q（t））=sum（sum（g_ijk*--q_k（t））*--q_j（t））dt j=1k=1dt

对于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)臂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个不同的模型参数或常数。

IDNLGREY Manutec R3机器人模型对象

为该应用程序开发的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](在MATLAB文件中的p(1))和第i:th向量元素作为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]。同样，Ia的8个元素是通过Ia[i] for i = 0,1，…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];

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

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

/* / 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 = x;y [1] = x [1];y [2] = x [2];｝

有关C-MEX模型文件的详细信息，请参阅“创建IDNLGREY模型文件”示例。

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

InputName = {'施加在电机移动臂1上的电压'；．..“施加在电机运动臂2上的电压”；．..“施加于电机运动臂3”的电压};InputUnit = {“V”；“V”；“V”};

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

StateName = {“\Theta_1”；．..%基座与臂1的相对夹角“\Theta_2”；．..臂1和臂2之间的相对角度“\ Theta_3”；．..%臂2和臂3之间的相对角度“Vel_1”；．..%基本面和臂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)’；．..%Ia1，标量。转动惯量臂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”}平均值={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）*one（3，1）；[40；40]；．..每股收益(0);每股收益(0)* 1 (2,1);每股收益(0)* 1 (2);负无穷;无穷(4,2)};ParMax =正;%没有最大限制。

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

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

输入输出数据

接下来，我们加载可用的输入-输出数据。本文使用上述IDNLGREY模型结构对输出进行了仿真。在存储之前，输出会受到一些附加噪声的影响。

加载（完整文件）（matlabroot，“工具箱”，“识别”，“iddemos”，“数据”，“robotdata”)）；

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

实验1.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]^t%实验2。

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

Z = sum (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 = 1 e-5;比较(z, nlgr);

$图中包含3个轴对象。坐标轴对象1包含2个类型为line的对象。这些对象代表Manutec r3机器人:Exp1 (\Theta_1)， Manutec r3机器人:97.7%。axis对象2包含2个类型为line的对象。这些对象代表Manutec r3机器人:Exp1 (\Theta_2)， Manutec r3机器人:81%。坐标轴对象3包含2个类型为line的对象。这些对象代表Manutec r3 robot:Exp1 (\Theta_3)， Manutec r3 robot: 28.54%。$

图2:曼努泰克r3机器人模型的真实输出和模拟输出的比较。

参数估计

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

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

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

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

估计的曼努特克R3机器人模型的性能

接下来通过仿真研究了估计的曼努特克r3机器人的性能。

比较(z, nlgr);

$图中包含3个轴对象。坐标轴对象1包含2个类型为line的对象。这些对象代表Manutec r3机器人:Exp1 (\Theta_1)， Manutec r3机器人:99.99%。axis对象2包含2个类型为line的对象。这些对象代表Manutec r3机器人:Exp1 (\Theta_2)， Manutec r3机器人:99.92%。坐标轴对象3包含2个类型为line的对象。这些对象代表Manutec r3机器人:Exp1 (\Theta_3)， Manutec r3机器人:99.97%。$

图3:估算的曼努泰克r3机器人模型的真实输出和模拟输出的比较。

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

disp (“真实估计参数向量”）;

估计参数向量

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.3 f % 1.3 f \ n”ptrue (25), nlgr.Parameters(结束)。值(1、2));

5.410 - 5.414

流(“% 1.3 f % 1.3 f \ n”，ptrue（26），nlgr.参数（end）.值（2,2））；

5.600 - 5.609

流(“% 1.3 f % 1.3 f \ n”ptrue (27), nlgr.Parameters(结束)。值(3 2));

0.390 - 0.390

流(“% 1.3 f % 1.3 f \ n”ptrue (28), nlgr.Parameters(结束)。值(4,2));

0.330 - 0.331

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

礼物(nlgr);

nlgr = 'robot_c'定义的连续时间非线性灰箱模型(MEX-file): dx/dt = F(t, u(t)， x(t)， p1，…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.

识别一些话

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

可用数据“信息量”不足，无法识别参数（数据并非持续令人兴奋）。
数据被如此多的噪声所破坏，几乎不可能找到真正的参数。
找到了一个局部的而不是搜索的全局最小值。当使用迭代搜索算法时，这种风险总是存在的。
模型结构的参数不是唯一可识别的。一般来说，当估计的参数越多，这种风险就越大。(例如，估计Manutec r3机器人的所有28个参数，通常会导致一些物理上不可能的参数值。)
模型结构只是“太复杂”或包含“不够平滑”的非线性。

结论

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

系统识别工具箱文档

金宝app

尝试MATLAB, Si金宝appmulink和其他产品下载188bet金宝搏

得到审判现在