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');
扫一扫
6万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
