完善资料让更多小伙伴认识你，还能领取20积分哦， 立即完善>
3天内不再提示
9660
|
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'); 
0
|
|
相关推荐
|
|
你正在撰写答案
如果你是对答案或其他答案精选点评或询问，请使用“评论”功能。
使用simulink进行三相短路故障分析时，各参数应该如何设置
1337 浏览 1 评论
想请教一下图中是simulink的什么模块，需要这种三段斜率函数模块但没找到在哪
1451 浏览 1 评论
2367 浏览 1 评论
200圆！求助大佬给一份VSG并网和离网模式的simulink仿真
2632 浏览 0 评论
MATLAB(3)--矩阵的引用(sub2ind、ind2sub、reshape函数使用)
3462 浏览 0 评论
/6 
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 （电路图） 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证：合字B2-20210191
工商网监
湘ICP备2023018690号
评论
淘帖
5859
