代码复制的有问题，详细见本楼，谢谢。
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;
