代码复制的有问题，详细见本楼，谢谢。

clc;clear;

DR=pi/180;

%time

j = 1;

for i = 0 : 0.1 : 2

%input

theta= 45 * DR *(1+6*exp((-1)*i/0.3)-8*exp((-1)*i/0.4));

theta_d= (-9*pi)/20*exp(-0.3*i)+20*exp(-2.5*i);

theta_dd= (27*pi/200)*exp(-0.3*i) - 50*exp(-2.5*i);

% DH parameters

the(1) = theta*DR; d(1)=2; a(1)=0; alp(1)=0;

the(2) = theta*DR; d(2)=0; a(2)=0; alp(2)=90*DR;

the(3) = theta*DR; d(3)=0; a(3)=3; alp(3)=0;

the(4) = 0; d(4)=0; a(4)=4; alp(4)=0;

%base link

w00=[0;0;0]; v00=[0;0;0]; w00d=[0;0;0];v00d=[0;9.8;0];

% rotation matrix

T01=[cos(the(1)), (-1)*sin(the(1)), 0, a(1); sin(the(1))*cos(alp(1)), cos(the(1))*cos(alp(1)), -sin(alp(1)), -sin(alp(1))*d(1); sin(the(1))*sin(alp(1)), cos(the(1))*sin(alp(1)), cos(alp(1)), cos(alp(1))*d(1); 0,0,0,1];

T12=[cos(the(2)), (-1)*sin(the(2)), 0, a(2); sin(the(2))*cos(alp(2)), cos(the(2))*cos(alp(2)), -sin(alp(2)), -sin(alp(2))*d(2); sin(the(2))*sin(alp(2)), cos(the(2))*sin(alp(2)), cos(alp(2)), cos(alp(2))*d(2);0,0,0,1];

T23=[cos(the(3)), (-1)*sin(the(3)), 0, a(3); sin(the(3))*cos(alp(3)), cos(the(3))*cos(alp(3)), -sin(alp(3)), -sin(alp(3))*d(3); sin(the(3))*sin(alp(3)), cos(the(3))*sin(alp(3)), cos(alp(3)), cos(alp(3))*d(3);0,0,0,1];

T34=[cos(the(4)), (-1)*sin(the(4)), 0, a(4); sin(the(4))*cos(alp(4)), cos(the(4))*cos(alp(4)), -sin(alp(4)), -sin(alp(4))*d(4); sin(the(4))*sin(alp(4)), cos(the(4))*sin(alp(4)), cos(alp(4)), cos(alp(4))*d(4);0,0,0,1];

R01= T01(1:3,1:3); R12= T12(1:3,1:3); R23= T23(1:3,1:3); R34= T34(1:3,1:3);

R10= R01'; R21= R12'; R32= R23'; R43= R34';

T10=T01';T21=T12';T32=T23';T43=T34';

p10= T10(1:3,4);p21= T12(1:3,4); p32= T23(1:3,4); p43= T34(1:3,4);

%center distance

pc11=[2;0;0]; pc22=[3;0;0];pc33=[4;0;0];

z=[0;0;1];

% link mass

m1=4; m2=2; m3=2;

% inertia tensor

Ic1=0; Ic2=0; Ic3=0;

% outward recrusion

%link1

w11 = R10*w00 + theta_d*z;

w11d = R10*w00d + cross(R10*w00, z*theta_d) + theta_dd*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 = Ic1*w11d + cross(w11, Ic1*w11);

% link2

w22 = R21*w11 + theta_d*z;

w22d = R21*w11d + cross(R21*w11, z*theta_d) + theta_dd*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 = Ic2*w22d + cross(w22, Ic2*w22);

% link3

w33 = R32 * w22 + theta_d * z;

w33d = R32*w22d + cross(R32*w22, z*theta_d) + theta_dd*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 = Ic3*w33d + cross(w33, Ic3*w33);

% Inward iterations: i: 3->1

f44 = [0; 0; 0]; n44 = [0; 0; 0];

% i = 3

f33 = R34*f44 + F33;

n33 = N33 + R34*n44 + cross(pc33, F33) + cross(p43, R34*f44);

tau3 = n33'* z;

Tau_3(j) = tau3;

%plot(i,Tau_3,'-b')

% i = 2

f22 = R23*f33 + F22;

n22 = N22 + R23*n33 + cross(pc22, F22) + cross(p32, R23*f33);

tau2 = n22'* z;

Tau_2(j) = tau2;

%plot(i,Tau_2,'-k')

% i =1

f11 = R12*f22 + F11;

n11 = N11 + R12*n22 + cross(pc11, F11) + cross(p21, R12*f22);

tau1 = n11'* z;

Tau_1(j) = tau1;

%plot(i,Tau_1,'-r')

j = j + 1;

%tau4 = tau1+ tau2+tau3

end

i = 0: 0.1:2

plot(i, Tau_3,'-b')

hold on;

plot(i, Tau_2,'-r')

hold on;

plot(i, Tau_1,'-g')

hold on;