4-RUPaR并联机器人反解

global Xp Yp Zp Jp %命令文件 t=0: 5/200:2*5; %计算时间 x=50*cos(t)-50; %规划轨迹曲线 y=50*sin(t); z=2*t+60; j=sin(t); temp=[0,0,0,0]'; %待求解转角初始值 for n=1:401 %循环，将规划轨迹时间离散值赋给动平台中心点坐标 Xp=x(n); Yp=y(n); Zp=z(n); Jp=j(n); Q=fsolve('myfun',temp,optimset('Display','off')); %求解非线性方程，temp为给定初值 temp=Q; %改变求解初始值 THETA1(n)=Q(1);%返回转角1 THETA2(n)=Q(2);%返回转角2 THETA3(n)=Q(3);%返回转角3 THETA4(n)=Q(4);%返回转角4 end Slpine1=zeros(401,2);%二维数组Slpine1存放转角1随时间变化值 Slpine1(:,1)=t; Slpine1(:,2)=THETA1*180/pi; Slpine2=zeros(401,2);%二维数组Slpine2存放转角2随时间变化值 Slpine2(:,1)=t; Slpine2(:,2)=THETA2*180/pi; Slpine3=zeros(401,2);%二维数组Slpine3存放转角2随时间变化值 Slpine3(:,1)=t; Slpine3(:,2)=THETA3*180/pi; Slpine4=zeros(401,2);%二维数组Slpine4存放转角3随时间变化值 Slpine4(:,1)=t; Slpine5(:,2)=THETA4*180/pi; save('spline1.txt','Slpine1','-ascii','-double');%输出TXT反解数据 save('spline2.txt','Slpine2','-ascii','-double'); save('spline3.txt','Slpine3','-ascii','-double'); save('spline4.txt','Slpine4','-ascii','-double'); plot(t,THETA1*180/pi,'color','b','linewidth',1.5);%蓝色 grid on; title('转角随时间变化曲线'); xlabel('时间(S)'); ylabel('转角(degree)'); %text(0.8,-5,'蓝色为转角1,绿色为转角2,红色为转角3,青色为转角4'); hold on; plot(t,THETA2*180/pi,'color','g','linewidth',1.5);%绿色 hold on; plot(t,THETA3*180/pi,'color','r','linewidth',1.5);%红色 hold on; plot(t,THETA4*180/pi,'color','c','linewidth',1.5);%青色