1. 引言
前面的一系列文章已经介绍了机器人动力学的基础，动力学的牛顿欧拉推导以及在二连杆平面臂上的应用。这篇文章着重于实践，主要是用matlab进行符号运算推导一下二连杆平面臂的动力学方程并在simulink下使用Simscape/Multibody进行推导结果的验证。
2. 二连杆动力学符号推导
2.1 连杆坐标系与DH参数
这里我们还是先建立二连杆平面臂的连杆坐标系，写出它的DH参数，因为前面的文章已经反复强调了坐标系是一切的开始。二连杆坐标系建立如下图1所示（为了简化DH参数，基坐标系的定义和上一篇文章不同）。
图1 二连杆平面臂改进DH坐标系
根据以上建立的改进DH坐标系可以写出DH参数表如下表1所示。
表1 改进DH参数
2.2 动力学方程的符号推导
我们再罗嗦一下牛顿欧拉法，其实不管是几个连杆的机器人，每一个连杆均是在自身的质心坐标系下列写牛顿方程和欧拉方程的。前向递归是为了求这个连杆的角速度，角加速度以及线加速度在质心坐标系下的表达。反向递归是为了求当前连杆所受到的下一级连杆施加给它的力和力矩。
二连杆平面臂动力学符号推导代码如下所示。推导过程用到了rvctools机器人工具箱10.1，你可以去Peter Corke个人主页下载，也可以在我的github下载。
syms L_1xx L_1xy L_1xz L_1yy L_1yz L_1zz real;
syms l_1x l_1y l_1z real;
syms m_1 real;
syms fv_1 fc_1 real;
syms L_2xx L_2xy L_2xz L_2yy L_2yz L_2zz real;
syms l_2x l_2y l_2z real;
syms m_2 real;
syms fv_2 fc_2 real;
syms q1 dq1 ddq1 q2 dq2 ddq2 l1 l2 real;
l_1x = l1 / 2;
l_1y = 0;
l_1z = 0;
l_2x = l2 / 2;
l_2y = 0;
l_2z = 0;
% angular velocity of base link
w_0_0 = [0;0;0];
% linear velocity of base link
v_0_0 = [0;0;0];
% angular acceleration of base link
alpha_0_0 = [0; 0; 0];
% linear acceleration of base link
a_0_0 = [0; 9.81; 0];
dh = [0, 0, 0, q1;
0, l1, 0, q2];
% acceleration of CoM p_ci_i
w = cell(1, size(dh, 1));
v = cell(1, size(dh, 1));
alpha = cell(1, size(dh, 1));
a = cell(1, size(dh, 1));
ac = cell(1, size(dh, 1));
l = cell(1, size(dh, 1));
T = cell(1, size(dh, 1) + 1);
R = cell(1, size(dh, 1) + 1);
r = cell(1, size(dh, 1) + 1);
I = cell(1, size(dh, 1));
T_e = cell(1, size(dh, 1) + 1);
t_e_pre = eye(4);
for i = 1:size(dh, 1)
T{i} = trotx(dh(i, 1))*transl([dh(i, 2), 0, dh(i, 3)])*trotz(dh(i, 4));
R{i} = T{i}(1:3, 1:3);
r{i} = T{i}(1:3, 4);
T_e{i} = t_e_pre * T{i};
t_e_pre = T_e{i};
end
T{i+1} = eye(3);
R{i+1} = eye(3);
r{i+1} = [l2, 0, 0];
T_e{i+1} = T_e{i} * transl([l2, 0, 0]);
q = [q1 q2];
q_dot = [dq1 dq2];
q_ddot = [ddq1 ddq2];
l{1} = [l_1x; l_1y; l_1z];
l{2} = [l_2x; l_2y; l_2z];
pre_w = w_0_0;
pre_v = v_0_0;
pre_alpha = alpha_0_0;
pre_a = a_0_0;
% forward iteration
for i = 1:2
t = R{i}';
w_i_i = t * pre_w + q_dot(i) * [0;0;1];
v_i_i = t * (pre_v + cross(pre_w, r{i}));
alpha_i_i = t * pre_alpha + cross((t * pre_w), q_dot(i) * [0;0;1]) + q_ddot(i) * [0;0;1];
a_i_i = t * (pre_a + cross(pre_alpha, r{i}) + cross(pre_w, cross(pre_w, r{i})));
ac_i_i = a_i_i + cross(alpha_i_i, l{i}) + cross(w_i_i, cross(w_i_i, l{i}));
w{i} = w_i_i;
v{i} = v_i_i;
alpha{i} = alpha_i_i;
a{i} = a_i_i;
ac{i} = ac_i_i;
pre_w = w_i_i;
pre_v = v_i_i;
pre_alpha = alpha_i_i;
pre_a = a_i_i;
end
m = [m_1; m_2];
% backward iteration
f_3_3 = [0;0;0];
n_3_3 = [0;0;0];
next_f_i_i = f_3_3;
next_n_i_i = n_3_3;
I{1} = [L_1xx L_1xy L_1xz; L_1xy L_1yy L_1yz; L_1xz L_1yz L_1zz];
I{2} = [L_2xx L_2xy L_2xz; L_2xy L_2yy L_2yz; L_2xz L_2yz L_2zz];
tau = cell(1, size(dh, 1));
for i = 2:-1:1
r = R{i+1};
f_i_i = m(i)*ac{i} + r * next_f_i_i;
z_i = [0;0;1];
TT = T_e{i};
TT(1:3,1:3) = (T_e{i}(1:3,1:3))';
TT(1:3, 4) = -(T_e{i}(1:3,1:3))' * T_e{i}(1:3,4);
xx = TT * T_e{i+1}(1:4, 4);
r_ip1_ci = l{i} - xx(1:3);
torque_i = I{i}*alpha{i} + cross(w{i}, I{i}*w{i}) + r * next_n_i_i + cross(l{i}, f_i_i) - cross(r_ip1_ci, r * next_f_i_i);
tau{i} = simplify(torque_i(3));
next_f_i_i = f_i_i;
next_n_i_i = torque_i;
end
我们大概解释一下这个代码，1-18行是在作动力学及相关符号的定义。值得注意的是l_1x, l_1y, l_1z代表的是连杆1质心在连杆1坐标系的坐标，从图中容易看出，只有x轴方向分量为l1/2，另外两个方向都是0。
20-27行，w_0_0代表基座角速度在基坐标系下的表示，这个显然是0；v_0_0是基座线速度在基坐标系下的表示，显然是0。alpha_0_0代表的是基座角加速度在基坐标系下的表示，显然是0；a_0_0代表的是基座线加速度在基坐标系下的表示。前面我们提过为了简化重力表达我们认为没有重力场，取而代之的是我们把机器人放在一个与重力加速度大小相等，方向相反的加速度场中。由于重力加速度在基坐标系下表达为g=[0; -9.81; 0]，因此a_0_0=[0; 9.81; 0]。
31-43又是一些变量定义，w存储连杆角速度，v存储连杆线速度，alpha存储连杆角加速度，a存储连杆线加速度，ac存储连杆质心线加速度，l存储连杆质心，T存储相邻连杆的齐次变换，R存储相邻连杆的旋转矩阵，r存储相邻连杆的平移向量，I存储每个连杆的惯性张量，T_e存储每个连杆和基座之间的齐次变换。
65-83行对应的是前向递推代码，95-109对应的是反向递推代码，可以参考前面两篇文章推导的公式。
总的来说并不复杂，但是里面涉及的变量以及他们分别在哪个坐标系下表示的还是会让人觉得凌乱（代码变量定义也不是特别直观），因此要完全读懂这些代码还是需要一些时间的。
3. Multibody搭建二连杆平面臂仿真环境
前面我们完成了二连杆的动力学符号推导，那么如何验证我们的推导是否正确呢？这个验证的工作可以利用Simscape/Multibody搭建二连杆平面臂的仿真模型，可以参考这篇文章搭建仿真模型。为了验证二连杆平面臂的动力学需要添加一个Matlab Function模块用于实时计算机器人的关节力矩并与Revolute Joint模块的力矩反馈进行对比，如果两者完全一致说明我们计算的动力学方程是正确的。仿真模型如下图2所示。
图2 二连杆平面臂动力学验证仿真框图
图中的fcn就是所谓的Matlab Function模块，这个模块非常强大，它可以把一个matlab的函数绑到simulink上，fcn中就是我们实现的二连杆平面臂动力学计算函数，函数的输入就是关节角，关节角速度，关节角加速度。函数的输出是两个关节的当前力矩。我们可以将fcn计算得到的力矩与两个Revolute Joint模块力矩反馈值进行比较，如果二者能完全一致就说明我们的动力学计算没有问题。fcn模块函数实现如下。
function [t1,t2] = fcn(q1,q2,dq1,dq2,ddq1,ddq2)
L_1xx = 4.71239e-06;
L_1xy = 0;
L_1xz = 0;
L_1yy = 0.000709215;
L_1yz = 0;
L_1zz = 0.000709215;
m_1 = 0.0942478;
L_2xx = 6.28319e-06;
L_2xy = 0;
L_2xz = 0;
L_2yy = 0.000422021;
L_2yz = 0;
L_2zz = 0.000422021;
m_2 = 0.125664;
l1 = 0.3;
l2 = 0.2;
l_1x = l1 / 2;
l_1y = 0;
l_1z = 0;
l_2x = l2 / 2;
l_2y = 0;
l_2z = 0;
% angular velocity of base link
w_0_0 = [0;0;0];
% linear velocity of base link
v_0_0 = [0;0;0];
% angular acceleration of base link
alpha_0_0 = [0; 0; 0];
% linear acceleration of base link
a_0_0 = [0; 9.81; 0];
dh = [0, 0, 0, q1;
0, l1, 0, q2];
% acceleration of CoM p_ci_i
w = cell(1, size(dh, 1));
v = cell(1, size(dh, 1));
alpha = cell(1, size(dh, 1));
a = cell(1, size(dh, 1));
ac = cell(1, size(dh, 1));
l = cell(1, size(dh, 1));
T = cell(1, size(dh, 1) + 1);
R = cell(1, size(dh, 1) + 1);
r = cell(1, size(dh, 1) + 1);
I = cell(1, size(dh, 1));
T_e = cell(1, size(dh, 1) + 1);
t_e_pre = eye(4);
for i = 1:size(dh, 1)
trx = [1 0 0 0;
0 cos(dh(i, 1)) -sin(dh(i, 1)) 0;
0 sin(dh(i, 1)) cos(dh(i, 1)) 0;
0 0 0 1];
tl = [1 0 0 dh(i, 2);
0 1 0 0;
0 0 1 dh(i, 3);
0 0 0 1];
trz = [cos(dh(i, 4)) -sin(dh(i, 4)) 0 0;
sin(dh(i, 4)) cos(dh(i, 4)) 0 0;
0 0 1 0;
0 0 0 1];
T{i} = trx * tl * trz;%trotx(dh(i, 1))*transl([dh(i, 2), 0, dh(i, 3)])*trotz(dh(i, 4));
R{i} = T{i}(1:3, 1:3);
r{i} = T{i}(1:3, 4);
T_e{i} = t_e_pre * T{i};
t_e_pre = T_e{i};
end
T{i+1} = eye(3);
R{i+1} = eye(3);
r{i+1} = [l2, 0, 0];
T_e{i+1} = T_e{i} * [1 0 0 l2; 0 1 0 0; 0 0 1 0; 0 0 0 1];%T_e{i} * transl([l2, 0, 0]);
q = [q1 q2];
dq = [dq1 dq2];
ddq = [ddq1 ddq2];
l{1} = [l_1x; l_1y; l_1z];
l{2} = [l_2x; l_2y; l_2z];
pre_w = w_0_0;
pre_v = v_0_0;
pre_alpha = alpha_0_0;
pre_a = a_0_0;
% forward iteration
for i = 1:2
t = R{i}';
w_i_i = t * pre_w + dq(i) * [0;0;1];
v_i_i = t * (pre_v + cross(pre_w, r{i}));
alpha_i_i = t * pre_alpha + cross((t * pre_w), dq(i) * [0;0;1]) + ddq(i) * [0;0;1];
a_i_i = t * (pre_a + cross(pre_alpha, r{i}) + cross(pre_w, cross(pre_w, r{i})));
ac_i_i = a_i_i + cross(alpha_i_i, l{i}) + cross(w_i_i, cross(w_i_i, l{i}));
w{i} = w_i_i;
v{i} = v_i_i;
alpha{i} = alpha_i_i;
a{i} = a_i_i;
ac{i} = ac_i_i;
pre_w = w_i_i;
pre_v = v_i_i;
pre_alpha = alpha_i_i;
pre_a = a_i_i;
end
m = [m_1; m_2];
% backward iteration
f_3_3 = [0;0;0];
n_3_3 = [0;0;0];
next_f_i_i = f_3_3;
next_n_i_i = n_3_3;
I{1} = [L_1xx L_1xy L_1xz; L_1xy L_1yy L_1yz; L_1xz L_1yz L_1zz];
I{2} = [L_2xx L_2xy L_2xz; L_2xy L_2yy L_2yz; L_2xz L_2yz L_2zz];
tau = cell(1, size(dh, 1));
for i = 2:-1:1
r = R{i+1};
f_i_i = m(i)*ac{i} + r * next_f_i_i;
TT = T_e{i};
TT(1:3,1:3) = (T_e{i}(1:3,1:3))';
TT(1:3, 4) = -(T_e{i}(1:3,1:3))' * T_e{i}(1:3,4);
xx = TT * T_e{i+1}(1:4, 4);
r_ip1_ci = l{i} - xx(1:3);
torque_i = I{i}*alpha{i} + cross(w{i}, I{i}*w{i}) + r * next_n_i_i + cross(l{i}, f_i_i) - cross(r_ip1_ci, r * next_f_i_i);
tau{i} = torque_i(3);
next_f_i_i = f_i_i;
next_n_i_i = torque_i;
end
t1 = tau{1};
t2 = tau{2};
你可以和前面符号推导对比一下，这里其实就是把符号变量换成了常数（去掉了关于rvctools工具箱的调用，否则simulink将报错）。
有一个问题，我们是如何得到各个连杆的动力学参数信息的呢？连杆长度比较容易，这个是我们自己设置的，质心在连杆坐标系中的位置也是十分简单的，就是连杆的几何中心。那么惯性张量要怎么得到呢？其实很简单，可以打开连杆对应的Solid模块，在属性窗口中打开Inertia，你会看到Derived Values即推导值，如下图3所示。也就是matlab能够帮助我们依据连杆形状，尺寸以及密度信息计算出质心坐标系下的转动惯量。点击Derived Values右侧Update即可得到这个连杆的动力学参数。值得注意的是matlab对质心坐标系的定义与我们定义的改进DH参数各个轴的朝向不同。matlab中连杆的z轴是沿着连杆方向的，而我们定义的改进DH坐标系x轴是沿着连杆方向的，因此惯性张量这一块要小心对应。
图3 连杆惯性张量的获得
二连杆平面臂动力学符号推导以及multibody动力学仿真验证相关内容已上传至github：https://github.com/hitgavin/simmechanics/tree/master/two_link_planner_robot，感兴趣的读者可以下载看一下。
4. 动力学仿真
仿真环境搭建完成了可以运行并查看示波器的显示，可以发现Revolute Joint力矩测量值和我们的动力学函数计算的结果几乎重合，这也验证了动力学结果的正确性。
5. 总结
这篇文章主要介绍了二连杆平面臂动力学牛顿欧拉推导的matlab实现以及动力学仿真验证，接下来我们将尝试把二连杆模型动力学推导推广到任意的串联机器人。
由于个人能力有限，所述内容难免存在疏漏，欢迎指出，欢迎讨论。
评论（3）
您还未登录，请登录后发表或查看评论