roboforum.ru

Я в ужасе, вы знаете что такое кубические сплайны, но не можете с помощью IF'ов и arccos выстроить функцию определения угла по 2 координатам...

Может всё-таки моя мысль про IF'ы вас спасёт?

КАК? Как можно не придумать условие?

Есть 4 вариантов знаков:
x>0,y>0
x<0,y>0
x>0,y<0
x<0,y<0

Что тут думать??? Это же блин 9-й класс средней школы, какой еще "у меня мало опыта, всего 4-й курс"

Расстреляйте своих преподавателей по математике

Я вас не пойму - вы задачу инверсной кинематики что ли решаете? так она через 1 arccos и не решается, надо строить модель и разворачивать всё от исходной точки, к тому же у вас может быть неоднозначность решения.

Неоднозначность разрешать добавляя условия. Желательно гладкие

Неопределенность с arccos может быть только в процедуре перевода (x,y) в угол и расстояние.

Остальное уже сами

Ну нужно придумать какое-то условие, чтобы не было неоднозначности, например, добавить виртуальную пружину.

А гладкость нужна чтобы при небольшой смене положения манипулятора не произошло резкого изменения углов, т.е. виртуальную пружину можно поставить так, что при переходе положения через какой-то рубеж она резко утянет манипулятор куда-нибудь.

Но это я из общих рассуждений, тут вроде есть спецы кто это по учебе проходил, должны сказать как это называется и где читать

код в студию + алгоритм или теорию по которой делался код...

Duhas писал(а):код в студию + алгоритм или теорию по которой делался код...

Код написан в matlabe. Суть в том, что у меня есть 4 точки в пространстве (x,y,z). Через них я провожу кубический сплайн (это гладкая траектория). Далее для этой траектории я считаю углы поворотов сочленений робота. По алгоритму отмеченному красным в прилагаемой методичке. После чего снова считаю прямую задачу кинематики, из полученных углов получаю координаты. Траектория должна быть такой же, но она отличается, и отличается она тогда, когда высчитываемые углы выходят за пределы области значений arccos(он начинает неправильно вычислять четверти углов). Ссылка на методичку http://files.mail.ru/S64ZJS
Как я понимаю было бы лучше если бы углы рассчитывались через arctg, т.к. там есть скачки значений углов при переходе области значений, которые можно устранить в правильную сторону, у arccos этого нет, поэтому определить четверть сложнее. А может я и не права...

Код: Выделить всё • Развернуть

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');

Добавлено спустя 1 минуту 7 секунд:

Angel71 писал(а):есть очень большое подозрение, что вы изначально не правильно управление делаете.
если уж совсем тяжко, могу на мыло скинуть "Introduction to Mechanics and Control 3th ed John Craig" или на форум выложить, если разрешат или на трекер.

Буду благодарна. Почта Homamilae123@mail.ru