工业机械臂建模
本文介绍如何在MATLAB中进行工业机械臂建模以及动力学仿真，翻译于Modeling an industrial arm
平台：MATLAB R2017a
机械臂模型由三部分组成：电机（Motor），减速机（Gear-box），臂结构（Arm structure），各部分都有各自的质量，分别绕着一个不受重力影响的轴转动。如图1所示。
图1：工业机械臂原理图
这个模型在两方面进行了简化：1.假设运动围绕一个不受重力影响的轴。一些机械臂（比如SCARA）由于结构的关系，建模时可以忽略重力项的影响，所以这种化简是合理的。2.令减速比r = 1。我们之后可以通过直接换算减速比来获得真实的物理参数，所以这种化简也是合理的。
具体的原理可以参考论文：E. Wernholt and S. Gunnarsson. Nonlinear Identification of a Physically Parameterized Robot Model. In preprints of the 14th IFAC Symposium on System Identification, pages 143-148, Newcastle, Australia, March 2006.
一． 建立状态空间方程
状态空间方程是整个现代控制理论的基础，让我们看看如何建立各个关节的状态空间方程。
状态变量：
对三个质量应用力矩平衡方程，得到状态空间方程：
其中J_m, J_g, J_a分别是电机，减速机，臂结构的惯量矩；d_g, d_a是“弹簧”阻尼系数，k_a是臂结构刚度。（弹簧代表刚体弹性）
机器人的输入为电机产生的转矩u(t)=tau(t)，电机的角速度y(t) = d/dt q_m(t)为测量输出（可以通过码盘读出）。质量被建模在齿轮箱之后和臂结构的末端，所以他们的角位置：q_g(t)和q_a(t)是不可测量的。齿轮箱的弹性由非线性弹簧建模，由弹簧扭矩tau_s(t)描述，它位于电机和第二个质量之间；而臂结构的灵活性则由线性弹簧在最后两个质量之间建模。系统的摩擦主要作用于第一个质量，这里用非线性摩擦力矩tau_f(t)来建模。
齿轮箱摩擦扭矩tau_f(t)，被建模为包括库仑摩擦、粘滞摩擦并考虑Stribeck效应（将静摩擦转换为动摩擦的过渡阶段视作自然指数增长）的模型：
其中Fv是粘滞系数，Fc是库仑摩擦系数，Fcs和是反映Stribeck效应的系数，beta是用来获得x3(t)速度从负向正平稳过渡的参数。
弹簧的扭矩tau_s(t)，用一个无平方项的三次多项式来描述：
其中k_g1和k_g3为齿轮箱弹簧刚度参数。
因为总体惯量J已知，故引入待定的比例因子a_m和a_g，进行参数重整化：
综上所述得到最终的状态空间结构，涉及13个不同的参数:Fv, Fc, Fcs, alpha, beta, J, a_m, a_g, k_g1, k_g3, d_g, k_a, d_a（其中前六个固定，后七个待求）
二． IDNLGREY机械臂模型对象
上面的模型结构被输入到一个名为robotarm_c的C（mex）文件中。(整个文件可以通过命令“type robotarm_c.c”查看)。在状态更新函数中，注意我们在这里使用了两个中间双变量，一方面是为了增强方程的可读性，另一方面是为了提高执行速度(tau在方程中出现了两次，但是只计算了一次)。
/* State equations. */
void compute_dx(double *dx, double *x, double *u, double **p)
{
/* Declaration of model parameters and intermediate variables. */
double *Fv, *Fc, *Fcs, *alpha, *beta, *J, *am, *ag, *kg1, *kg3, *dg, *ka, *da;
double tauf, taus; /* Intermediate variables. */
/* Retrieve model parameters. */
Fv = p[0]; /* Viscous friction coefficient. */
Fc = p[1]; /* Coulomb friction coefficient. */
Fcs = p[2]; /* Striebeck friction coefficient. */
alpha = p[3]; /* Striebeck smoothness coefficient. */
beta = p[4]; /* Friction smoothness coefficient. */
J = p[5]; /* Total moment of inertia. */
am = p[6]; /* Motor moment of inertia scale factor. */
ag = p[7]; /* Gear-box moment of inertia scale factor. */
kg1 = p[8]; /* Gear-box stiffness parameter 1. */
kg3 = p[9]; /* Gear-box stiffness parameter 3. */
dg = p[10]; /* Gear-box damping parameter. */
ka = p[11]; /* Arm structure stiffness parameter. */
da = p[12]; /* Arm structure damping parameter. */
/* Determine intermediate variables. */
/* tauf: Gear friction torque. (sech(x) = 1/cosh(x)! */
/* taus: Spring torque. */
tauf = Fv[0]*x[2]+(Fc[0]+Fcs[0]/(cosh(alpha[0]*x[2])))*tanh(beta[0]*x[2]);
taus = kg1[0]*x[0]+kg3[0]*pow(x[0],3);
/* x[0]: Rotational velocity difference between the motor and the gear-box. */
/* x[1]: Rotational velocity difference between the gear-box and the arm. */
/* x[2]: Rotational velocity of the motor. */
/* x[3]: Rotational velocity after the gear-box. */
/* x[4]: Rotational velocity of the robot arm. */
dx[0] = x[2]-x[3];
dx[1] = x[3]-x[4];
dx[2] = 1/(J[0]*am[0])*(-taus-dg[0]*(x[2]-x[3])-tauf+u[0]);
dx[3] = 1/(J[0]*ag[0])*(taus+dg[0]*(x[2]-x[3])-ka[0]*x[1]-da[0]*(x[3]-x[4]));
dx[4] = 1/(J[0]*(1.0-am[0]-ag[0]))*(ka[0]*x[1]+da[0]*(x[3]-x[4]));
}
/* Output equation. */
void compute_y(double y[], double x[])
{
/* y[0]: Rotational velocity of the motor. */
y[0] = x[2];
}
下一步是创建一个反映建模情况的IDNLGREY（非线性灰盒）对象。这里需要注意的是，为机械臂找到合适的初始参数值需要一些预先的知识。在Wernholt和Gunnarsson的论文中，这是在前面两个步骤中进行的，其中使用了其他模型参数辨识技术。下面使用的初始参数值是这些识别实验的结果。
FileName = 'robotarm_c'; % File describing the model structure.
Order = [1 1 5]; % Model orders [ny nu nx].
Parameters = [ 0.00986346744839 0.74302635727901 ...
3.98628540790595 3.24015074090438 ...
0.79943497008153 0.03291699877416 ...
0.17910964111956 0.61206166914114 ...
20.59269827430799 0.00000000000000 ...
0.06241814047290 20.23072060978318 ...
0.00987527995798]'; % Initial parameter vector.
InitialStates = zeros(5, 1); % Initial states.
Ts = 0; % Time-continuous system.
nlgr = idnlgrey(FileName, Order, Parameters, InitialStates, Ts, ...
'Name', 'Robot arm', ...
'InputName', 'Applied motor torque', ...
'InputUnit', 'Nm', ...
'OutputName', 'Angular velocity of motor', ...
'OutputUnit', 'rad/s', ...
'TimeUnit', 's');
为了方便记忆，状态名称需要简化
nlgr = setinit(nlgr, 'Name', {'Angular position difference between the motor and the gear-box'…
'Angular position difference between the gear-box and the arm'…
'Angular velocity of motor'…
'Angular velocity of gear-box'…
'Angular velocity of robot arm'}');
nlgr = setinit(nlgr, 'Unit', {'rad' 'rad' 'rad/s' 'rad/s' 'rad/s'});
之后详细指定参数名称。此外，所有的参数都应该是正的，即将每个参数的最小值设置为0。在Wernholt和Gunnarsson的论文中，前6个参数：Fv, Fc, Fcs, alpha, beta, J十分精确，所以不需要被估计。
nlgr = setpar(nlgr, 'Name', {'Fv : Viscous friction coefficient' ... % 1.
'Fc : Coulomb friction coefficient' ... % 2.
'Fcs : Striebeck friction coefficient' ... % 3.
'alpha: Striebeck smoothness coefficient' ... % 4.
'beta : Friction smoothness coefficient' ... % 5.
'J : Total moment of inertia' ... % 6.
'a_m : Motor moment of inertia scale factor' ... % 7.
'a_g : Gear-box moment of inertia scale factor' ... % 8.
'k_g1 : Gear-box stiffness parameter 1' ... % 9.
'k_g3 : Gear-box stiffness parameter 3' ... % 10.
'd_g : Gear-box damping parameter' ... % 11.
'k_a : Arm structure stiffness parameter' ... % 12.
'd_a : Arm structure damping parameter' ... % 13.
});
nlgr = setpar(nlgr, 'Minimum', num2cell(zeros(size(nlgr, 'np'), 1))); % All parameters >= 0!
for parno = 1:6 % Fix the first six parameters.
nlgr.Parameters(parno).Fixed = true;
end
到目前为止进行的建模步骤完成了一个初始的机械臂模型，其属性如下：
present(nlgr);
三． 输入-输出数据
实验机器人采集了大量真实世界的数据集。为了保持机器人在其工作点附近，同时也是出于安全考虑，数据收集采用了实验反馈控制安排，随后允许离线计算关节控制器的参考信号。
在这个例子中，我们将讨论限制在四个不同的数据集中，其中之一用于估计（estimation），另三个用于验证（validation）。在每种情况下，使用大约10秒的周期激励信号为控制器生成参考速度。选择的采样频率为2 kHz(采样时间Ts = 0.0005秒)。对于数据集，使用了三种不同类型的输入信号:(ue:估计数据集的输入信号;uv1、uv2、uv3:三个验证数据集的输入信号)
ue, uv1:在1- 40hz的频率范围内，峰值为16 rad/s的平坦幅度谱的复合正弦波信号。复合正弦波信号叠加在振幅为20 rad/s，截止频率为1 Hz的滤波方波上。
uv2:类似于ue和uv1，但是没有方波。
uv3:频率为0.1、0.3、0.5 Hz，峰值为40 rad/s的复合正弦波信号。
让我们加载可用数据，并将所有四个数据集放到一个IDDATA类型的对象z中：
load(fullfile(matlabroot, 'toolbox', 'ident', 'iddemos', 'data', 'robotarmdata'));
z = iddata({ye yv1 yv2 yv3}, {ue uv1 uv2 uv3}, 0.5e-3, 'Name', 'Robot arm');
z.InputName = 'Applied motor torque';
z.InputUnit = 'Nm';
z.OutputName = 'Angular velocity of motor';
z.OutputUnit = 'rad/s';
z.ExperimentName = {'Estimation' 'Validation 1' 'Validation 2' 'Validation 3'};
z.Tstart = 0;
z.TimeUnit = 's';
present(z);
输入如下代码，显示四个实验的输入-输出数据：
figure('Name', [z.Name ': input-output data'],...
'DefaultAxesTitleFontSizeMultiplier',1,...
'DefaultAxesTitleFontWeight','normal',...
'Position',[100 100 900 600]);
for i = 1:z.Ne
zi = getexp(z, i);
subplot(z.Ne, 2, 2*i-1); % Input.
plot(zi.u);
title([z.ExperimentName{i} ': ' zi.InputName{1}],'FontWeight','normal');
if (i < z.Ne)
xlabel('');
else
xlabel([z.Domain ' (' zi.TimeUnit ')']);
end
subplot(z.Ne, 2, 2*i); % Output.
plot(zi.y);
title([z.ExperimentName{i} ': ' zi.OutputName{1}],'FontWeight','normal');
if (i < z.Ne)
xlabel('');
else
xlabel([z.Domain ' (' zi.TimeUnit ')']);
end
end
图2：实验机械臂的测量输入-输出数据
四． 初始机械臂模型的性能
最初的机械臂模型有多好?让我们使用COMPARE来模拟模型输出(对于所有四个实验)，并将结果与相应的测量输出进行比较。对于所有四个实验，我们知道前两种状态（x1, x2）的值恒为0（固定），而其余三种状态（x3, x4, x5）的初值设置为开始时的测量输出（不固定）。然而，在默认情况下，COMPARE估计所有的初始状态，而z包含四个不同的实验，也就是4 * 5 = 20次初始状态估计。即使固定了前两种状态，4 * 3 = 12个初始状态仍然需要估计(如果遵循内部模型初始状态策略)。因为数据集相当大，这将导致冗长的计算。为了避免这种情况，我们使用PREDICT来预测4 * 3不固定的初始状态 (将初始状态作为初始状态传递结构),但限制估计前十个可用数据。然后我们指示COMPARE使用得到的5×4初始状态矩阵X0init，而不是执行初始状态估计。
zred = z(1:round(zi.N/10));
nlgr = setinit(nlgr, 'Fixed', {true true false false false});
X0 = nlgr.InitialStates;
[X0.Value] = deal(zeros(1, 4), zeros(1, 4), [ye(1) yv1(1) yv2(1) yv3(1)], ...
[ye(1) yv1(1) yv2(1) yv3(1)], [ye(1) yv1(1) yv2(1) yv3(1)]);
[~, X0init] = predict(zred, nlgr, [], X0);
nlgr = setinit(nlgr, 'Value', num2cell(X0init(:, 1)));
clf
compare(z, nlgr, [], compareOptions('InitialCondition', X0init));
图3：测量输出与初始机械臂模型仿真输出的比较。
从图中可以看出，初始机器人手臂模型的性能相当好。这三种类型的数据集对ye和yv1的适合度约为79%，对yv2的适合度为37%，对yv3的适合度为95%。请注意，与yv2相比，ye/yv1的拟合程度很高，这是由于初始模型能够捕获方波，而复合正弦波部分的捕获效果并不理想。我们还可以看看这四个实验的预测误差：
pe(z, nlgr, peOptions('InitialCondition',X0init));
图4:初始机械臂模型预测误差。
五． 参数估计
现在我们通过估计z的第一个实验（估计数据集）的7个待定模型参数和3个待定初始状态来提高初始机械臂模型的性能。这个估计需要一些时间(通常是几分钟)。
nlgr = nlgreyest(nlgr, getexp(z, 1), nlgreyestOptions('Display', 'on'));
六． 估计机械臂模型的表现
再次使用COMPARE来评估估计机械臂模型的性能。我们依然在这里指示COMPARE不执行任何初始状态估计。在第一个实验中，我们用NLGREYEST估计的初始状态代替了猜测的初始状态，在剩下的三个实验中，我们使用PREDICT基于简化的IDDATA对象zred估计初始状态。
X0init(:, 1) = cell2mat(getinit(nlgr, 'Value'));
X0 = nlgr.InitialStates;
[X0.Value] = deal(zeros(1, 3), zeros(1, 3), [yv1(1) yv2(1) yv3(1)], ...
[yv1(1) yv2(1) yv3(1)], [yv1(1) yv2(1) yv3(1)]);
[yp, X0init(:, 2:4)] = predict(getexp(zred, 2:4), nlgr, [], X0);
clf
compare(z, nlgr, [], compareOptions('InitialCondition', X0init));
图5:机械臂模型的实测输出与仿真输出对比。
比较图显示了在拟合方面的改进。ye和yv1的拟合度现在约为85%(之前:79%)，yv2约为63%(之前:37%)，yv3略低于95.5%(之前:也略低于95.5%)。这种改进在第二个验证数据集中表现得最为明显，在第二个验证数据集中，输入是一个没有任何方波的复合正弦波信号。然而，估计模型对ye和yv1多轴部分的跟踪能力也得到了显著提高(但这并没有反映在拟合图形中，因为这些图形更受方波拟合的影响)。预测误差图还显示，残差现在比最初的机械臂模型要小：
figure;
pe(z, nlgr, peOptions('InitialCondition',X0init));
图6:估计机械臂模型的预测误差。
我们通过对估计机械臂模型的各种特性进行文本总结来结束本例研究。
present(nlgr);
七． 结论
系统辨识技术广泛应用于机器人领域。良好的机器人模型对于现代机器人控制的实现至关重要，通常被认为是满足不断增长的速度和精度要求的必要条件。这些模型在各种机器人诊断应用中也是至关重要的组成部分，用于预测与磨损相关的问题和检测机器人故障的成因。