roboforum.ru

Помогите пожалуйста, может кому-то приходилось уже с этим сталкиваться.
У меня есть робот РМ-01. Мне нужно рассчитать к нему систему управления. В общем у меня есть координаты в пространстве (4 точки). Через них я провела траекторию методом кубических сплайнов. Теперь мне нужно посчитать обратную задачу (найти углы поворотов его сочленений). Но практически все улглы расчитываются при помощи arccos. А функция арккосинус определена от 0 до пи. Соответственно если углы у меня отрицательные, арккосинус все равно дает положительные. И соответственно траектория получается не правильная. Как можно сделать так, чтобы можно было автомотически правильно определить фазы углов (может алгоритм какой есть?).
Заранее благодарна.

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

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

Может действительно я чего-то торможу... (просто у меня нет опыта в программировании). Я понимаю, что при помощи условных операторов (IF) я могу выбрать нужную четверть угла. Но я не могу придумать условие Я просто еще на 4-ом курсе института, у меня мало опыта Это мой первый проект

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

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

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

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

А если 3 координаты и 5 углов, четыре из которых вычисляются arccos? И каждый из этих углов начинается не из начала координат?

Мда... преподов на мыло

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

Да-да. Это у нас называется обратная задача кинематики. У меня есть алгоритм ее вычисления (правда я считаю ее в matlabе). (Могу для понятности сбросить код.). и что же делать с этой неоднозначностью?

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

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

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

Уфф... Вот я и не могу понять какие это такие гладкие условия Может есть где почитать об этом?

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

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

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

Спасибо большое Итак, вопрос по прежнему актуален

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

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

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

ну тут уже правильно Dead говорил, перед вычислением арккосинуса простыми ИФ-ами проверяйте четверть и все...


ПС мы обратную считали поисковыми методами...