roboforum.ru

clc
clear
%Обратная  задача
a2=431.8; s2=149.09; a3=433.07; s6=56.25;
qi=pi/3; qi2=pi/3;
a13=sin(qi)*sin(qi2);
a23=-sin(qi)*cos(qi2);
a33=cos(qi);
wx=cs1-s6*a13;
wy=cs2-s6*a23;
wz=cs3-s6*a33;
r=sqrt(wx.^2+wy.^2+wz.^2-s2^2);
q=asin(wz./r);



% Расчет и графики q3
cq3=(a2^2-r.^2+a3^2)/(2*a2*a3);
q3=acos(cq3);
         
q3g=(180/pi).*q3;

figure
subplot(5,3,7);
plot(ti,q3g, 'b');
grid on
title('q3 от t');
xlabel('t');
ylabel('q3');
sp1=diff(q3g);
n=301;
t1=ti(1):0.01:ti(n-1);
subplot(5,3,8);
plot(t1, sp1,'b-');
grid on
xlabel('t');
ylabel('q3`');
sp2=diff(q3g,2);
t2=ti(1):0.01:ti(n-2);
subplot(5,3,9);
plot(t2, sp2,'b-');
grid on
xlabel('t');
ylabel('q3"');

%Расчет и графики q2
b=acos((a2^2+r.^2-a3^2)./(2*a2.*r));
q2=b+q;
q2g=(180/pi).*q2;
subplot(5,3,4);
plot(ti,q2g, 'b');
grid on
title('q2 от t');
xlabel('t');
ylabel('q2');
sp3=diff(q2g);
subplot(5,3,5);
plot(t1, sp3,'b-');
grid on
xlabel('t');
ylabel('q2`');
sp4=diff(q2g,2);
subplot(5,3,6);
plot(t2, sp4,'b-');
grid on
xlabel('t');
ylabel('q2"');

%Расчет и графики q1
v=-s2.*wy-r.*cos(q).*wx;
m=-s2.*wx+r.*cos(q).*wy;
q1=atan2(v,m);
q1g=(180/pi).*q1;
subplot(5,3,1);
plot(ti,q1g, 'b');
grid on
title('q1 от t');
xlabel('t');
ylabel('q1');
sp5=diff(q1g);
subplot(5,3,2);
plot(t1, sp5,'b-');
grid on
title('Первые производ.');
xlabel('t');
ylabel('q1`');
sp6=diff(q1g,2);
subplot(5,3,3);
plot(t2, sp6,'b-');
grid on
title('Вторые производ.');
xlabel('t');
ylabel('q1"');

%Расчет и графики q5
q5=acos(sin(qi).*sin(q1).*sin(q2+q3)+cos(qi).*cos(q2+q3));
q5g=(180/pi).*q5;
subplot(5,3,13);
plot(ti,q5g, 'b');
grid on
title('q5 от t');
xlabel('t');
ylabel('q5');
sp7=diff(q5g);
subplot(5,3,14);
plot(t1, sp7,'b-');
grid on
xlabel('t');
ylabel('q5`');
sp8=diff(q5g,2);
subplot(5,3,15);
plot(t2, sp8,'b-');
grid on
xlabel('t');
ylabel('q5"');

%Расчет и графики q4
q4=acos(-sin(qi2).*sin(q1)+cos(qi2)*cos(qi).*cos(q1));
q4g=(180/pi).*q4;
subplot(5,3,10);
plot(ti,q4g, 'b');
grid on
title('q4 от t');
xlabel('t');
ylabel('q4');
sp9=diff(q4g);
subplot(5,3,11);
plot(t1, sp9,'b-');
grid on
xlabel('t');
ylabel('q4`');
sp10=diff(q4g,2);
subplot(5,3,12);
plot(t2, sp10,'b-');
grid on
xlabel('t');
ylabel('q4"');


%Прямая задача для проверки
m6=cos(q4).*sin(q5);
m7=cos(q).*sin(q4).*sin(q5)-sin(q).*cos(q5);
m8=sin(q).*sin(q4).*sin(q5)+cos(q).*cos(q5);

xk=cos(q1).*(s6*m6-s2)-sin(q1).*(s6*m7+r.*cos(q));
figure
subplot(2,2,1);
plot(ti,xk,t,x,'o');
grid on
xlabel('t');
ylabel('x');

yk=cos(q1).*(s6*m7+r.*cos(q))+sin(q1).*(s6*m6-s2);
subplot(2,2,2);
plot(ti,yk,t,y,'o');
grid on
xlabel('t');
ylabel('y');

zk=s6.*m8+r.*sin(q);
subplot(2,2,3);
plot(ti,zk,t,z,'o');
grid on
xlabel('t');
ylabel('z');

subplot(2,2,4);
plot3(xk,yk,zk,'-', x,y,z,'o');
grid on
xlabel('x');
ylabel('y');
zlabel('z');