roboforum.ru

Технический форум по робототехнике.

 

Расчет траектории движения робота РМ-01(Puma 560)

Раздел создан специально для людей которым интересна робототехника, но в силу разных причин они не знают с чего начать.
Задавайте ваши вопросы, какими бы простыми они не казались, постоянные посетители форума постараются ответить на них по мере своих сил.
Робот своими руками. Самодельный робот.

Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Homamilae » 09 апр 2010, 10:11

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

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение =DeaD= » 09 апр 2010, 10:20

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

Может всё-таки моя мысль про IF'ы вас спасёт? :)
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Homamilae » 09 апр 2010, 11:56

Может действительно я чего-то торможу... (просто у меня нет опыта в программировании). Я понимаю, что при помощи условных операторов (IF) я могу выбрать нужную четверть угла. Но я не могу придумать условие :( Я просто еще на 4-ом курсе института, у меня мало опыта :( Это мой первый проект :)
Аватара пользователя
Homamilae
 
Сообщения: 23
Зарегистрирован: 09 апр 2010, 10:01
Откуда: Москва
прог. языки: C/C++; Matlab
ФИО: Петина Наталья

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение =DeaD= » 09 апр 2010, 11:59

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

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

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

Расстреляйте своих преподавателей по математике :P
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Homamilae » 09 апр 2010, 12:07

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

Мда... преподов на мыло :)
Последний раз редактировалось Homamilae 12 апр 2010, 17:06, всего редактировалось 2 раз(а).
Аватара пользователя
Homamilae
 
Сообщения: 23
Зарегистрирован: 09 апр 2010, 10:01
Откуда: Москва
прог. языки: C/C++; Matlab
ФИО: Петина Наталья

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение =DeaD= » 09 апр 2010, 12:08

Я вас не пойму - вы задачу инверсной кинематики что ли решаете? так она через 1 arccos и не решается, надо строить модель и разворачивать всё от исходной точки, к тому же у вас может быть неоднозначность решения.
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Homamilae » 09 апр 2010, 12:13

Да-да. Это у нас называется обратная задача кинематики. У меня есть алгоритм ее вычисления (правда я считаю ее в matlabе). (Могу для понятности сбросить код.). и что же делать с этой неоднозначностью? :)
Аватара пользователя
Homamilae
 
Сообщения: 23
Зарегистрирован: 09 апр 2010, 10:01
Откуда: Москва
прог. языки: C/C++; Matlab
ФИО: Петина Наталья

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение =DeaD= » 09 апр 2010, 12:15

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

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

Остальное уже сами :)
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Homamilae » 09 апр 2010, 12:22

Уфф... Вот я и не могу понять какие это такие гладкие условия :) Может есть где почитать об этом?
Аватара пользователя
Homamilae
 
Сообщения: 23
Зарегистрирован: 09 апр 2010, 10:01
Откуда: Москва
прог. языки: C/C++; Matlab
ФИО: Петина Наталья

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение =DeaD= » 09 апр 2010, 12:41

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

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

Но это я из общих рассуждений, тут вроде есть спецы кто это по учебе проходил, должны сказать как это называется и где читать :pardon:
Проект [[Open Robotics]] - Универсальные модули для построения роботов
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Homamilae » 09 апр 2010, 12:50

Спасибо большое :) Итак, вопрос по прежнему актуален :RTFM:
Аватара пользователя
Homamilae
 
Сообщения: 23
Зарегистрирован: 09 апр 2010, 10:01
Откуда: Москва
прог. языки: C/C++; Matlab
ФИО: Петина Наталья

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Duhas » 09 апр 2010, 14:53

код в студию + алгоритм или теорию по которой делался код...
«Как сердцу выразить себя? … Мысль изреченная есть ложь!»
В этом мире меня подводит доброта и порядочность...
"двое смотрят в лужу, один видит лужу, другой отраженные в ней звезды"
Аватара пользователя
Duhas
 
Сообщения: 6341
Зарегистрирован: 15 сен 2007, 13:03
Откуда: Красноярск
прог. языки: ASM(МК), C(PC)
ФИО: Гагарский Андрей Александрович

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Angel71 » 09 апр 2010, 14:57

есть очень большое подозрение, что вы изначально не правильно управление делаете.
если уж совсем тяжко, могу на мыло скинуть "Introduction to Mechanics and Control 3th ed John Craig" или на форум выложить, если разрешат или на трекер.
Аватара пользователя
Angel71
 
Сообщения: 9232
Зарегистрирован: 18 апр 2009, 22:18
Предупреждения: -1

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Homamilae » 09 апр 2010, 23:03

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
Последний раз редактировалось Homamilae 23 фев 2011, 13:17, всего редактировалось 1 раз.
Аватара пользователя
Homamilae
 
Сообщения: 23
Зарегистрирован: 09 апр 2010, 10:01
Откуда: Москва
прог. языки: C/C++; Matlab
ФИО: Петина Наталья

Re: Расчет траектории движения робота РМ-01(Puma 560)

Сообщение Duhas » 10 апр 2010, 11:50

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


ПС мы обратную считали поисковыми методами...
«Как сердцу выразить себя? … Мысль изреченная есть ложь!»
В этом мире меня подводит доброта и порядочность...
"двое смотрят в лужу, один видит лужу, другой отраженные в ней звезды"
Аватара пользователя
Duhas
 
Сообщения: 6341
Зарегистрирован: 15 сен 2007, 13:03
Откуда: Красноярск
прог. языки: ASM(МК), C(PC)
ФИО: Гагарский Андрей Александрович

След.

Вернуться в Новичкам или основы основ роботостроения.

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 3

Mail.ru counter