MATLAB运行显示错误，如何解决？

syms theta1 theta2 theta3 theta4 theta5 theta6;
syms dtheta1 dtheta2 dtheta3 dtheta4 dtheta5 dtheta6;
syms ddtheta1 ddtheta2 ddtheta3 ddtheta4 ddtheta5 ddtheta6;
qv = [theta1, theta2, theta3, theta4, theta5, theta6];
dqv = [dtheta1, dtheta2, dtheta3, dtheta4, dtheta5, dtheta6];
ddqv = [ddtheta1, ddtheta2, ddtheta3, ddtheta4, ddtheta5, ddtheta6];
l = [0.33, 0.26, 0.02, 0.29, 0.07];
m = [6, 4, 4, 2, 2, 2];
r0 = 0.02;
Ixx = [m(1)*l(1)^2/3, 0, m(3)*l(4)^2/3, m(4)*l(5)^2/3, m(5)*l(5)^2/3, m(6)*l(5)^2/3];
Iyy = [m(1)*l(1)^2/3, m(2)* l(2)^2/3, m(3)* l(3)^2/3, m(4)*l(5)^2/3, 0, m(6)*l(5)^2/3];
Izz = [0, m(2)*l(2)^2/3, m(3)*(l(3)^2+l(4)^2)/3, 0, m(5)*l(5)^2/3, m(6)*r0^2/2];
Ixy = [0, 0, -m(3)*l(3)*l(4)/3, 0, 0, 0];
Ixz = zeros(1,6);
Iyz = zeros(1,6);
mx = [0, m(2)*l(2)/2, m(3)*l(3)/2, 0, 0, 0];
my = [0, 0, -m(3)*l(4)/2, 0, -m(5)*l(5)/2, 0];
mz = [-m(1)*l(1)/2, 0, 0, m(4)*l(5)/2, 0, m(6)*l(5)/2];
J = cell(6, 6);
for i = 1:6
    J{i} = [(-Ixx(i)+Iyy(i)+Izz(i))/2, Ixy(i)*Ixz(i)*mx(i), Ixy(i)*(Ixx(i)-Iyy(i)+Izz(i))/2, Iyz(i)*my(i), Ixz(i)*Iyz(i)*(Ixx(i)+Iyy(i)-Izz(i))/2, mz(i), mx(i)*my(i)*mz(i)*m(i)];
T01 = [cos(qv(1)), -sin(qv(1)), 0, 0; sin(qv(1)), cos(qv(1)), 0, 0; 0, 0, 1, r0; 0, 0, 0, 1];
T12 = [cos(qv(2)), -sin(qv(2)), 0, 0; sin(qv(2)), cos(qv(2)), 0, 0; 0, 0, 1, r0; 0, 0, 0, 1];
T23 = [cos(qv(3)), -sin(qv(3)), 0, 0; sin(qv(3)), cos(qv(3)), 0, 0; 0, 0, 1, r0; 0, 0, 0, 1];
T34 = [cos(qv(4)), -sin(qv(4)), 0, 0; sin(qv(4)), cos(qv(4)), 0, 0; 0, 0, 1, r0; 0, 0, 0, 1];
T45 = [cos(qv(5)), -sin(qv(5)), 0, 0; sin(qv(5)), cos(qv(5)), 0, 0; 0, 0, 1, r0; 0, 0, 0, 1];
T56 = [cos(qv(6)), -sin(qv(6)), 0, 0; sin(qv(6)), cos(qv(6)), 0, 0; 0, 0, 1, r0; 0, 0, 0, 1];
T02 = T01 * T12;
T03 = T01 * T12 * T23;
T04 = T01 * T12 * T23 * T34;
T05 = T01 * T12 * T23 * T34 * T45;
T06 = T01 * T12 * T23 * T34 * T45 * T56;
T = cat(3, T01, T02, T03, T04, T05, T06);
D = zeros(6, 6);
for i=1:6
    for j=1:6
       p=max(i,j);
       for n=p:6
            D(i,j)=D(i,j)+trace(diff(T(:,:,n),qv(j))*J(:,:,n)*transpose(diff((T(:,:,n)),qv(i))));
H=sym(zeros(6,1));
C=sym(zeros(6,6,6));
for i=1:6
    for j=1:6
       for k=1:6
          p=max([i,j,k]);
          for n=p:6
            C(j,k,i)=C(j,k,i)+trace(diff(diff(T(:,:,n),qv(j)),qv(k))*J(:,:,n)*transpose(diff((T(:,:,n)),qv(i))));
          H(i)=H(i)+C(j,k,i)*dqv(j)*dqv(k);
g=9.8;
gv=[0 0 g 0]';
r=[0 0 -l(1)/21;
      l(2)/2 0 0 1;
      l(3)/2 -l(4)/2 0 1;
      0 0 l(5)/21;
      0 -l(5)/2 0 1;
     0 0 l(5)/2 1]';
G=sym(zeros(6,1));
       for i=1:6
            for p=i:6
             G(i)=G(i)-m(p)*gv'*diff(T(:,:,p),qv(i))*r(:,p);
tauv=D* ddqv'+H+G;
q0=deg2rad([0 90 0 0 0 0]);
qf=deg2rad([140 120 90 110 100 180]);
t=0:0.05:5;
[q,dq,ddq]=jtraj(q0,qf,t);
N=length(t);
tau=zeros(6);
   for i=1:length(t)
        theta1=q(i,1);theta2=q(i,2);theta3=q(i,3);
        theta4=q(i,4);theta5=q(i,5);theta6=q(i,6);
        dtheta1=dq(i,1);dtheta2=dq(i,2);dtheta3=dq(i,3);
        dtheta4=dq(i,4);dtheta5=dq(i,5);dtheta6=dq(i,6);
        ddtheta1=ddq(i,1);ddtheta2=ddq(i,2);ddtheta3=ddq(i,3);
        ddtheta4=ddq(i,4);ddtheta5=ddq(i,5);ddtheta6=ddq(i,6);
        tau(:,i)=eval(tauv);
figure('Name','6R工业机器人力矩-关节变量的关系曲线');
subplot(2,3,1);
plot(q(:,1),taut(1,:));
grid on
xlabel('θ_1(rad)');ylabel('关节1力矩(N·m)');
subplot(2,3,2);
plot(q(:,2),taut(2,:));
grid on
xlabel('θ_2(rad)');ylabel('关节2力矩（N·m)');
subplot(2,3,3);
plot(q(:,3),taut(3,:));
grid on
xlabel('θ_3(rad)');ylabel('关节3力矩（N·m)');
subplot(2,3,4);
plot(q(:,4),taut(4,:));
grid on
xlabel('θ_4(rad)');ylabel('关节4力矩（N·m)');
subplot(2,3,5);
plot(q(:,5),taut(5,:));
grid on
xlabel('θ_5(rad)');ylabel('关节5力矩（N·m)');
subplot(2,3,6);
plot(q(:,6),taut(6,:));
grid on
xlabel('θ_6(rad)');ylabel('关节6力矩（N·m)');
        0 提建议
     
 微信扫一扫
 
 
点击复制链接
 
 分享
 
 邀请回答
    
 
 
编辑 收藏 删除 
 
 
收藏 举报




    
  
追加酬金 (90%的用户在追加酬金后获得了解决方案)
 
 
当前问题酬金
 
¥ 15 
            (您已提供 ¥ 20， 还可追加 ¥
          
 
 
支付方式
 
 
扫码支付 
 
 
   
 
 
 
 
加载中...
      
 

          
 
提供问题酬金的用户不参与问题酬金结算和分配
 

            支付即为同意
            《付费问题酬金结算规则》
  
结题 再想想
 
删除 再等等