matlab赋值具有的非单一,赋值具有的非单一 rhs 维度多于非单一下标数 delta2(1,i+1)=atan2(lp2-L-(lp1-L)*cos(belta),-(lp1-L)*sin(b...

clear all

deltaT=0.1;

t=0:deltaT:30;

T=30;

alpha=pi/3;

beta=2*pi/3;

belta=2*pi/3;

theta0=pi/2;

L=460;

R=15;

r=15;

fai=zeros(4,length(t));

fai(1,1)=pi/3;

fai(2,1)=0;

fai(3,1)=pi/3;

fai(4,1)=0;

dfai=zeros(4,length(t));

theta1=zeros(1,length(t));

theta1(1,1)=pi/3;

delta1=zeros(1,length(t));

delta1(1,1)=0;

theta2=zeros(1,length(t));

theta2(1,1)=pi/3;

delta2=zeros(1,length(t));

delta2(1,1)=0;

%末端期望运动轨迹

Path_desired=zeros(3,length(t));

Path_desired(1,:)=215.6916/(2*pi)*sin(2*pi/T*t-pi)+215.6916/T*t+388.3014;

Path_desired(2,:)=88.2761/(2*pi)*sin(2*pi/T*t-pi)+88.2761/T*t+101.9324;

Path_desired(3,:)=-314.738/(2*pi)*sin(2*pi/T*t-pi)-314.738/T*t+790.2592;

%末端期望速度轨迹

V_desired=zeros(3,length(t));

V_desired(1,:)=215.6916/T*cos(2*pi/T*t-pi)+215.6916/T;

V_desired(2,:)=88.2761/T*cos(2*pi/T*t-pi)+88.2761/T;

V_desired(3,:)=-314.738/T*cos(2*pi/T*t-pi)-314.738/T;

%末端实际运动轨迹

Path_actual=zeros(3,length(t));

Path_actual(1,:)=388.3014;

Path_actual(2,:)=101.9324;

Path_actual(3,:)=790.2592;

%误差

Path_errorx=zeros(1,length(t));

Path_errorx(1,:)=0;

Path_errory=zeros(1,length(t));

Path_errory(2,:)=0;

Path_errorz=zeros(1,length(t));

Path_errorz(3,:)=0;

%驱动丝

q=zeros(6,length(t));

q(1,1)=452.1460;

q(2,1)=463.9270;

q(3,1)=463.9270;

q(4,1)=912.1460;

q(5,1)=935.7080;

q(6,1)=912.1460;

dq=zeros(6,length(t));

% x=(L*(sin(theta2) - 1)*(sin(alpha - delta1 + delta2)*sin(delta1) - cos(alpha - delta1 + delta2)*cos(delta1)*sin(theta1)))/(pi/2 - theta2) - (L*cos(delta1)*(sin(theta1) - 1))/(pi/2 - theta1) + (L*cos(delta1)*cos(theta1)*cos(theta2))/(pi/2 - theta2);

% y=(L*sin(delta1)*cos(theta1)*cos(theta2))/(pi/2 - theta2) - (L*(sin(theta2) - 1)*(sin(alpha - delta1 + delta2)*cos(delta1) + cos(alpha - delta1 + delta2)*sin(delta1)*sin(theta1)))/(pi/2 - theta2) - (L*sin(delta1)*(sin(theta1) - 1))/(pi/2 - theta1);

% z=(L*cos(theta1))/(pi/2 - theta1) + (L*cos(theta2)*sin(theta1))/(pi/2 - theta2) + (L*cos(alpha - delta1 + delta2)*cos(theta1)*(sin(theta2) - 1))/(pi/2 - theta2);

for i=1:length(t)

xtheta1=-(L*cos(delta1(i))*cos(theta1(i)))/(pi/2 - theta1(i)) - (L*cos(delta1(i))*(sin(theta1(i)) - 1))/(pi/2 - theta1(i))^2 - (L*cos(delta1(i))*cos(theta2(i))*sin(theta1(i)))/(pi/2 - theta2(i)) - (L*cos(alpha - delta1(i) + delta2(i))*cos(delta1(i))*cos(theta1(i))*(sin(theta2(i)) - 1))/(pi/2 - theta2(i));

xdelta1=(L*sin(delta1(i))*(sin(theta1(i)) - 1))/(pi/2 - theta1(i)) - (L*(sin(theta2(i)) - 1)*(cos(alpha - delta1(i) + delta2(i))*sin(delta1(i)) - sin(alpha - delta1(i) + delta2(i))*cos(delta1(i)) - cos(alpha - delta1(i) + delta2(i))*sin(delta1(i))*sin(theta1(i)) + sin(alpha - delta1(i) + delta2(i))*cos(delta1(i))*sin(theta1(i))))/(pi/2 - theta2(i)) - (L*sin(delta1(i))*cos(theta1(i))*cos(theta2(i)))/(pi/2 - theta2(i));

xtheta2=(L*cos(theta2(i))*(sin(alpha - delta1(i) + delta2(i))*sin(delta1(i)) - cos(alpha - delta1(i) + delta2(i))*cos(delta1(i))*sin(theta1(i))))/(pi/2 - theta2(i)) + (L*(sin(theta2(i)) - 1)*(sin(alpha - delta1(i) + delta2(i))*sin(delta1(i)) - cos(alpha - delta1(i) + delta2(i))*cos(delta1(i))*sin(theta1(i))))/(pi/2 - theta2(i))^2 + (L*cos(delta1(i))*cos(theta1(i))*cos(theta2(i)))/(pi/2 - theta2(i))^2 - (L*cos(delta1(i))*cos(theta1(i))*sin(theta2(i)))/(pi/2 - theta2(i));

xdelta2=(L*(sin(theta2(i)) - 1)*(cos(alpha - delta1(i) + delta2(i))*sin(delta1(i)) + sin(alpha - delta1(i) + delta2(i))*cos(delta1(i))*sin(theta1(i))))/(pi/2 - theta2(i));

ytheta1=-(L*sin(delta1(i))*(sin(theta1(i)) - 1))/(pi/2 - theta1(i))^2 - (L*sin(delta1(i))*cos(theta1(i)))/(pi/2 - theta1(i)) - (L*sin(delta1(i))*cos(theta2(i))*sin(theta1(i)))/(pi/2 - theta2(i)) - (L*cos(alpha - delta1(i) + delta2(i))*sin(delta1(i))*cos(theta1(i))*(sin(theta2(i)) - 1))/(pi/2 - theta2(i));

ydelta1=(L*(sin(theta2(i)) - 1)*(cos(alpha - delta1(i) + delta2(i))*cos(delta1(i)) + sin(alpha - delta1(i) + delta2(i))*sin(delta1(i)) - cos(alpha - delta1(i) + delta2(i))*cos(delta1(i))*sin(theta1(i)) - sin(alpha - delta1(i) + delta2(i))*sin(delta1(i))*sin(theta1(i))))/(pi/2 - theta2(i)) - (L*cos(delta1(i))*(sin(theta1(i)) - 1))/(pi/2 - theta1(i)) + (L*cos(delta1(i))*cos(theta1(i))*cos(theta2(i)))/(pi/2 - theta2(i));

ytheta2=(L*sin(delta1(i))*cos(theta1(i))*cos(theta2(i)))/(pi/2 - theta2(i))^2 - (L*(sin(theta2(i)) - 1)*(sin(alpha - delta1(i) + delta2(i))*cos(delta1(i)) + cos(alpha - delta1(i) + delta2(i))*sin(delta1(i))*sin(theta1(i))))/(pi/2 - theta2(i))^2 - (L*cos(theta2(i))*(sin(alpha - delta1(i) + delta2(i))*cos(delta1(i)) + cos(alpha - delta1(i) + delta2(i))*sin(delta1(i))*sin(theta1(i))))/(pi/2 - theta2(i)) - (L*sin(delta1(i))*cos(theta1(i))*sin(theta2(i)))/(pi/2 - theta2(i));

ydelta2=-(L*(sin(theta2(i)) - 1)*(cos(alpha - delta1(i) + delta2(i))*cos(delta1(i)) - sin(alpha - delta1(i) + delta2(i))*sin(delta1(i))*sin(theta1(i))))/(pi/2 - theta2(i));

ztheta1=(L*cos(theta1(i)))/(pi/2 - theta1(i))^2 - (L*sin(theta1(i)))/(pi/2 - theta1(i)) + (L*cos(theta1(i))*cos(theta2(i)))/(pi/2 - theta2(i)) - (L*cos(alpha - delta1(i) + delta2(i))*sin(theta1(i))*(sin(theta2(i)) - 1))/(pi/2 - theta2(i));

zdelta1=(L*sin(alpha - delta1(i) + delta2(i))*cos(theta1(i))*(sin(theta2(i)) - 1))/(pi/2 - theta2(i));

ztheta2=(L*cos(theta2(i))*sin(theta1(i)))/(pi/2 - theta2(i))^2 - (L*sin(theta1(i))*sin(theta2(i)))/(pi/2 - theta2(i)) + (L*cos(alpha - delta1(i) + delta2(i))*cos(theta1(i))*(sin(theta2(i)) - 1))/(pi/2 - theta2(i))^2 + (L*cos(alpha - delta1(i) + delta2(i))*cos(theta1(i))*cos(theta2(i)))/(pi/2 - theta2(i));

zdelta2=-(L*sin(alpha - delta1(i) + delta2(i))*cos(theta1(i))*(sin(theta2(i)) - 1))/(pi/2 - theta2(i));

J_pc=[xtheta1  xdelta1  xtheta2  xdelta2;

ytheta1  ydelta1  ytheta2  ydelta2;

ztheta1  zdelta1  ztheta2  zdelta2];

J_cl=[R*cos(delta1(i))        -R*(theta1(i)-pi/2)*sin(delta1(i))          0 0;

R*cos(delta1(i)+beta)   -R*(theta1(i)-pi/2)*sin(delta1(i)+beta)    0 0;

R*cos(delta1(i)+2*beta) -R*(theta1(i)-pi/2)*sin(delta1(i)+2*beta)   0 0;

R*cos(delta1(i)+alpha)  -R*(theta1(i)-pi/2)*sin(delta1(i)+alpha)          R*cos(delta2(i)+alpha)   -R*(theta2(i)-pi/2)*sin(delta2(i)+alpha);

R*cos(delta1(i)+beta+alpha)   -R*(theta1(i)-pi/2)*sin(delta1(i)+beta+alpha)    R*cos(delta2(i)+beta+alpha)   -R*(theta2(i)-pi/2)*sin(delta2(i)+beta+alpha);

R*cos(delta1(i)+2*beta+alpha) -R*(theta1(i)-pi/2)*sin(delta1(i)+2*beta+alpha)   R*cos(delta2(i)+2*beta+alpha)  -R*(theta2(i)-pi/2)*sin(delta2(i)+2*beta+alpha)];

H=[0.04 -0.02 -0.02 0 0 0;

-0.02 0.04 -0.02 0 0 0;

-0.02 -0.02 0.04 0 0 0;

0 0 0 0.04 -0.02 -0.02;

0 0 0 -0.02 0.04 -0.02;

0 0 0 -0.02 -0.02 0.04];

f=0.1*[4*q(1,i)-2*q(2,i)-2*q(3,i);

4*q(2,i)-2*q(1,i)-2*q(3,i);

4*q(3,i)-2*q(1,i)-2*q(2,i);

4*q(4,i)-2*q(5,i)-2*q(6,i);

4*q(5,i)-2*q(4,i)-2*q(6,i);

4*q(6,i)-2*q(4,i)-2*q(5,i)];

Aeq=J_pc*pinv(J_cl);

beq=[V_desired(1,i);V_desired(2,i);V_desired(3,i)];

lb=[-1

-1;

-1;

-1;

-1;

-1];

ub=[1;

1;

1;

1;

1;

1];

[dq(:,i),fval]=quadprog(H,f,[],[],Aeq,beq,lb,ub);

q(:,i+1)=q(:,i)+dq(:,i)*deltaT;

end

for i=1:length(t)

delta1(1,i+1)=atan2(q(2,i+1)-460-(q(1,i+1)-460)*cos(2*pi/3),-(q(1,i+1)-460)*sin(2*pi/3));

theta1(1,i+1)=theta0+(q(1,i+1)-L)/(r*cos(delta1(1,i+1)));

lc1=L+r*cos(delta1+alpha)*(theta1(1,i+1)-theta0);

lc2=L+r*cos(delta1+belta+alpha)*(theta1(1,i+1)-theta0);

ls1=q(4,i+1)-lc1;

ls2=q(5,i+1)-lc2;

deltap2=atan2(ls2-L-(ls1-L)*cos(belta),-(ls1-L)*sin(belta));

thetap2=theta0+(ls1(1,i+1)-L)./(r*cos(deltap2));

lp1=L+r.*cos(deltap2-alpha).*(thetap2-theta0);

lp2=L+r.*cos(deltap2-alpha+belta).*(thetap2-theta0);

delta2(1,i+1)=atan2(lp2-L-(lp1-L)*cos(belta),-(lp1-L)*sin(belta));

theta2(1,i+1)=theta0+(lp1-L)/(r*cos(delta2(1,i+1)));

Path_actual(1,i+1)=(L*(sin(theta2(1,i+1)) - 1)*(sin(alpha - delta1(1,i+1) + delta2(1,i+1))*sin(delta1(1,i+1)) - cos(alpha - delta1(1,i+1) + delta2(1,i+1))*cos(delta1(1,i+1)).*sin(theta1(1,i+1))))/(pi/2 - theta2(1,i+1)) - (L*cos(delta1(1,i+1))*(sin(theta1(1,i+1)) - 1))/(pi/2 - theta1(1,i+1)) + (L*cos(delta1(1,i+1))*cos(theta1(1,i+1))*cos(theta2(1,i+1)))/(pi/2 - theta2(1,i+1));

Path_actual(2,i+1)=(L*sin(delta1(1,i+1))*cos(theta1(1,i+1))*cos(theta2(1,i+1)))/(pi/2 - theta2(1,i+1)) - (L*(sin(theta2(1,i+1)) - 1)*(sin(alpha - delta1(1,i+1) + delta2(1,i+1))*cos(delta1(1,i+1)) + cos(alpha - delta1(1,i+1) + delta2(1,i+1))*sin(delta1(1,i+1))*sin(theta1(1,i+1))))/(pi/2 - theta2(1,i+1)) - (L*sin(delta1(1,i+1))*(sin(theta1(1,i+1)) - 1))/(pi/2 - theta1(1,i+1));

Path_actual(3,i+1)=(L*cos(theta1(1,i+1)))/(pi/2 - theta1(1,i+1)) + (L*cos(theta2(1,i+1))*sin(theta1(1,i+1)))/(pi/2 - theta2(1,i+1)) + (L*cos(alpha - delta1(1,i+1) + delta2(1,i+1))*cos(theta1(1,i+1))*(sin(theta2(1,i+1)) - 1))/(pi/2 - theta2(1,i+1));

end

plot3(Path_desired(1,:),Path_desired(2,:),Path_desired(3,:),Path_actual(1,:),Path_actual(2,:),Path_actual(3,:)); hold on;grid on; legend('Desired Path','Actual Path');

xlabel('X/mm');ylabel('Y/mm');zlabel('Z/mm');