三自由度连杆动力学Newton-Euler方法

Crig的机器人学采用的是MDH建模，采用NE方法建立三连杆动力学。

Matlab源程序：

%Method:MDH
%Goal:compute 3R Dynamics
%Author:easyR
%Date:2019/08/08
% theta:du；
% dtheta:rad/s;
% ddtheta:rad/s^2;

clc;clear all;

%% 
tic;
t4R=dynamics([0;0;90],[1;2;3],[0.5;1;1.5])
toc;

%% RBT verification20190807
% modifyied
d1=0;d4=0.792;a2=0.708;alpha1=pi/2; alpha3=pi/2;  %set base equal to 0

th(1) = 0; d(1) = d1; a(1) = 0;  alp(1) = 0;
th(2) = 0; d(2) = 0;  a(2) = 0;  alp(2) = alpha1;
th(3) = 0; d(3) = 0;  a(3) = a2; alp(3) = 0;

% MDH      th     d     a     alpha
L1 = Link([th(1), d(1), a(1), alp(1)], 'modified');
L2 = Link([th(2), d(2), a(2), alp(2)], 'modified');
L3 = Link([th(3), d(3), a(3), alp(3)], 'modified');

% Links mass
L1.m = 20; L2.m = 15; L3.m = 10; 
 
% Links century
L1.r = [0 0 0];
L2.r = [0.354 0 0];
L3.r = [0.396 0 0];

% Links tensor
L1.I = [0 0 0; 0 0 0; 0 0 0.5];
L2.I = [0 0 0; 0 0 0; 0 0 0.2];
L3.I = [0 0 0; 0 0 0; 0 0 0.1];

robot = SerialLink([L1, L2, L3]); 
robot.name = 'GK3R-mdh';
robot.comment = 'GK3R';
robot.display() 
% Forward Pose Kinematics
theta=[0, 0, 90]*pi/180;
% robot.teach()
robot.plot(theta); 
t0=robot.fkine(theta)    %末端执行器位姿
% TAU = R.rne(Q, QD, QDD, GRAV)
tau = robot.rne(theta, [1, 2, 3,], [0.5, 1, 1.5], [0 9.8 0])

%----------------------------------------------------------------------------------------------------%
%% 

function dh=myfunMDHTable(q,d,a,alpha)
%Method:MDH
%Goal:compute transform matrix
%Author:easyR
%Date:2019/08/07
%main function is assamble the MDH table for computing
%Variable:q
%q：
%d:
%a:
%alpha:
q_1=q(1);q_2=q(2);q_3=q(3);
d_1=d(1);d_2=d(2);d_3=d(3);
a_1=a(1);a_2=a(2);a_3=a(3);
alpha_1=alpha(1);alpha_2=alpha(2);alpha_3=alpha(3);

dh=[q_1     q_2     q_3     ;
    d_1     d_2     d_3     ;
    a_1     a_2     a_3     ;
    alpha_1 alpha_2 alpha_3 
    ]';
end
%----------------------------------------------------------------------------------------------------%

function  [T01,T12,T23,T02,T03] = MDHTrans(dh)
%Method:MDH
%Goal:compute transform matrix
%Author:easyR
%Date:2019/8/07
%dh：MDH Table
%T:Transform Matrix from endpoint to basement
for k=1:3    %k is the dof of Manipulator
    for i=1:k
        T(:,:,k)=myfunMatrixTrans( dh(i,1),dh(i,2),dh(i,3),dh(i,4));
    end
end
% disp('display each transform matrix Tn:');
%%
% transform matrix
Tb=eye(4);T01=(T(:,:,1));T12=(T(:,:,2));T23=(T(:,:,3));
T01=Tb*T01;T02=T01*T12;T03=T02*T23;

end

%% 
function tau =dynamics(theta, theta_d, theta_dd)
% 改进D-H参数
th(1) = theta(1)*pi/180; d(1) = 0;     a(1) = 0;     alp(1) = 0;
th(2) = theta(2)*pi/180; d(2) = 0;     a(2) = 0;     alp(2) = pi/2;
th(3) = theta(3)*pi/180; d(3) = 0;     a(3) = 0.708; alp(3) = 0;

% base_link initial
w00  = [0; 0; 0];  v00 = [0; 0; 0]; 
w00d = [0; 0; 0]; v00d = [0; 9.8; 0];

%distance:ri->ri+1
p10 = [0; 0; 0];  p21 = [0; 0; 0]; p32 = [0.708; 0; 0]; p43 = [0.792; 0; 0]; 
%distance:ri->rc
pc11 = [0; 0; 0]; pc22 = [0.354; 0; 0]; pc33 = [0.396; 0; 0];   
z = [0; 0; 1];

%Links mass
m1 = 20; m2 = 15; m3 = 10; m4 = 8;
% Links tensor
I1 = [0 0 0; 0 0 0; 0 0 0.5]; I2 = [0 0 0; 0 0 0; 0 0 0.2]; 
I3 = [0 0 0; 0 0 0; 0 0 0.1];
% trans matrix
dh=myfunMDHTable(th,d,a,alp);
[T01,T12,T23] = MDHTrans(dh);

R01 = T01(1:3, 1:3); R12 = T12(1:3, 1:3); R23 = T23(1:3, 1:3);
R34 =  [1 0 0; 0 1 0; 0 0 1];
R10 = R01'; R21 = R12'; R32 = R23'; 
R43 = R34';
%% 

%% Outward iterations: i: 0->2
% i = 0 to 1
w11 = R10*w00 + theta_d(1)*z;
w11d = R10*w00d + cross(R10*w00, z*theta_d(1)) + theta_dd(1)*z;
v11d = R10*(cross(w00d, p10) + cross(w00, cross(w00, p10)) + v00d);
vc11d = cross(w11d, pc11) + cross(w11, cross(w11, pc11)) + v11d;
F11 = m1*vc11d;
N11 = I1*w11d + cross(w11, I1*w11);
% i = 1 to 2
w22 = R21*w11 + theta_d(2)*z;
w22d = R21*w11d + cross(R21*w11, z*theta_d(2)) + theta_dd(2)*z;
v22d = R21*(cross(w11d, p21) + cross(w11, cross(w11, p21)) + v11d);
vc22d = cross(w22d, pc22) + cross(w22, cross(w22, pc22)) + v22d;
F22 = m2*vc22d;
N22 = I2*w22d + cross(w22, I2*w22);
% i = 2 to 3
w33 = R32*w22 + theta_d(3)*z;
w33d = R32*w22d + cross(R32*w22, z*theta_d(3)) + theta_dd(3)*z;
v33d = R32*(cross(w22d, p32) + cross(w22, cross(w22, p32)) + v22d);
vc33d = cross(w33d, pc33) + cross(w33, cross(w33, pc33)) + v33d;
F33 = m3*vc33d;
N33 = I3*w33d + cross(w33, I3*w33);

%% Inward iterations: i: 3->1
f44= [0; 0; 0]; n44= [0; 0; 0];
% i = 3,3 to 2
f33 = R34*f44 + F33;
n33 = N33 + R34*n44 + cross(pc33, F33) + cross(p43, R34*f44);
tau(3) = n33'*z;
% i = 2, 2 to 1
f22 = R23*f33 + F22;
n22 = N22 + R23*n33 + cross(pc22, F22) + cross(p32, R23*f33);
tau(2) = n22'*z;
% i =1, 1 to 0
f11 = R12*f22 + F11;
n11 = N11 + R12*n22 + cross(pc11, F11) + cross(p21, R12*f22);
tau(1) = n11'*z;

end

%————————————————————————————————————————————————

程序结果：

t4R =

   97.0814  -44.8108   18.1888

时间已过 0.015419 秒。
 
robot = 
 
GK3R-mdh (3 axis, RRR, modDH, fastRNE)                           
 GK3R;                                                           
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|          0|          0|
|  2|         q2|          0|          0|      1.571|          0|
|  3|         q3|          0|      0.708|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
                                                                 
grav =    0  base = 1  0  0  0   tool =  1  0  0  0              
          0         0  1  0  0           0  1  0  0              
       9.81         0  0  1  0           0  0  1  0              
                    0  0  0  1           0  0  0  1              
 

t0 =

    0.0000   -1.0000         0    0.7080
    0.0000    0.0000   -1.0000         0
    1.0000    0.0000    0.0000         0
         0         0         0    1.0000

Fast RNE: (c) Peter Corke 2002-2012

tau =

   97.0814  -44.8108   18.1888

>>