ROBOTYKA Odwrotne zadanie kinematyki - projekt
Zawartość. Wstęp...... Proste zadanie kinematyki cel...... Odwrotne zadanie kinematyki cel..... Analiza statyczna robota..... Proste zadanie kinematyki.... 4.. Projekt manipulatora.... 4.. Notacja Denavita-Hartenberga.... 5.. Obliczenia trajektorii w programie MATAB.... 5.4. Kolejne pozycje manipulatora dla zmian zmiennych konfiguracyjnych..... Odwrotne zadanie kinematyki.... 9.. Ogólna postać macierzy transformacji.... 9.. Macierze transformacji kolejnych układów.... 9.. Obliczanie trajektorii w programie MATAB.... 4 4. Analiza statyczna manipulatora.... 8
. Wstęp. Przedstawiony tutaj projekt z robotyki jest kompletnym rozwiązaniem odwrotnego zadania kinematyki. Przedmiotem projektu jest przykładowy manipulator(robot). Aby była możliwość obliczenia odwrotnego zadania kinematyki konieczne jest rozwiązanie prostego zadania kinematyki robota zgodnie z notacją Denavita-Hartenberga. Dodatkowo poza rozwiązanym prostym zadaniem kinematyki oraz odwrotnym zadaniem kinematyki wykreślone są trajektorie ruchu manipulatora. Obliczenie trajektorii zostało wykonane w programie MATAB. W projekcie jest kompletny kod źródłowy za pomocą którego można wykonać obliczenia a następnie wykreślić wyniki symulacji... Proste zadanie kinematyki cel. Proste zadanie kinematyki można w skrócie opisać jako przypadek gdy znane są kąty i przesunięcia kolejnych członów manipulatora a w wyniku uzyskać chcemy współrzędne ostatniego układu współrzędnych w układzie bazowym. Dane: zukane:.. Odwrotne zadanie kinematyki cel. W odwrotnym zadaniu kinematyki danymi wejściowymi jest pozycja jaką chcemy aby uzyskał ostatni układ współrzędnych w układzie bazowym. Znając pozycję zadaną ostatniego układu współrzędnych jako wynik chcemy uzyskać kąty ugięć lub długości przesunięć kolejnych napędów manipulatora. Dane: zukane:. Analiza statyczna robota. W projekcie tym przeprowadzona będzie analiza statyczna robota. Przeprowadzenie analizy statycznej robota da odpowiedź jakie siły lub momenty muszą być przyłożone do kolejnych napędów robota jak znajduje się on w stanie spoczynku. Ponadto w wyniku uzyskamy wzory na wartości sił i momentów jakie będzie musiała wytrzymać konstrukcja. Zaznaczam że jest to tylko analiza statyczna. W trakcie normalnej pracy robota dochodzą jeszcze obciążenia dynamiczne. Analiza obciążeń dynamicznych jest przedstawiona w innym projekcie, który traktuje tylko i wyłącznie o dynamice robota. Projekt dotyczący dynamiki robota można znaleźć na stronie w dziale dotyczącym robotyki.
. Proste zadanie kinematyki... Projekt manipulatora. O "" ) (t O "" O "" )) (t ) (t (t) 4 O "5" P "4" ) (t 5 Rysunek. Model robota. 4
.. Notacja Denavita-Hartenberga. Tabela. Notacja Denavita-Hartenberga tabelka. i α i- a i- d i θ i ⁰ θ (t) ⁰ l θ (t) ⁰ l θ (t) 4 9⁰ d 4 (t) 8⁰ 5 9⁰ l 4 θ 5 (t).. Obliczenia trajektorii w programie MATAB. function[t]=ma_trans(alfa,a,d,theta) ca=cos(alfa); sa=sin(alfa); cq=cos(theta); sq=sin(theta); T=[ cq -sq a ; sq*ca cq*ca -sa -sa*d ; sq*sa cq*sa ca ca*d ; ] ; >> [T]=ma_trans(,,,) T = 5
>> [T]=ma_trans(,8,,) T = 8 >> [T]=ma_trans(,8,,) T = 8 >> [T4]=ma_trans(pi/,,,pi) T4 = -. -.. -. -.. -... >> [T54]=ma_trans(pi/,7,,) T54 =. 7.. -.... 6
>> P5=[4 ]' P5 = 4 >> P=TTT*T4*T54*P5 P = -4... >> plot (T, 'DisplayName','T', 'YDataource', 'T'); figure(gcf) >> P5=[4 ]' P5 = 4 >> P=TTT*T4*T54*P5 P = 5.... >> t=:.:5; [m n]=size(t); 7
for i=::n if t(i)<5 theta(i)=; theta(i)=; theta(i)=; d4(i)=t(i); theta5(i)=; elseif t(i)< theta(i)=; theta(i)=pi/t(i)-pi/; theta(i)=; d4(i)=5; theta5(i)=; elseif t(i)<5 theta(i)=; theta(i)=pi/; theta(i)=; d4(i)=5; theta5(i)=-pi/t(i)+pi; elseif t(i)< theta(i)=; theta(i)=pi/; theta(i)=pi/5*t(i)-*pi; d4(i)=5; theta5(i)=-pi/; else theta(i)=-pi/t(i)+*pi; theta(i)=pi/; theta(i)=pi; d4(i)=5; theta5(i)=-pi/; end% if end%i subplot(5,,),plot(t,theta8/pi) subplot(5,,),plot(t,theta*8/pi) subplot(5,,),plot(t,theta*8/pi) subplot(5,,4),plot(t,d4) subplot(5,,5),plot(t,theta5*8/pi); 8
q(t) d4(t) q(t) q(t) q(t) -5-5 5 5 5 5 5 5 5 5 5 5 5 5 5-5 - 5 5 5 Rysunek. Trajektorie zmian zmiennych konfiguracyjnych dla prostego zadania kinematyki. Osie rzędnych przedstawionych powyżej wykresów opisane są innymi symbolami, odpowiadają one następującym zmiennym: 9
%zadajemy parametry manipulatora =8; =8; =; 4=5; 5=4; P5=[5,,,]'; %pozycja końcówki w ostatnim układzie %powiązanie poszczególnych parametrów z czasem i wcześniejszymi wyliczeniami for j = :n alfa = [ pi/ pi/]; wcześniejszych wyliczeń %tabelka D-H zmienne zależne od czasu (j) brane od a = [ 4 ]; d = [ d4(j) ]; theta = [theta(j) theta(j) theta(j) pi theta5(j)]; Tpom = eye(4); %tworzenie końcowej macierzy for i = :5 %wywołanie kolejnych wartości zadanych sq = sin(theta(i)); cq = cos(theta(i)); sa = sin(alfa(i)); ca = cos(alfa(i)); %wyliczenia T = [ cq -sq a(i) ; sq*ca cq*ca -sa -sa*d(i); sq*sa cq*sa ca ca*d(i); ];
Tpom = Tpom * T; end % i P = Tpom * P5; %wyliczenie końcowej pozycji xx(j) = P(); zz(j) = P(); end % j figure; plot(zz,xx) %no i w końcu rysujemy Rysunek. Ruch narzędzia manipulatora w płaszczyźnie XY dla zadanych zmiennych konfiguracyjnych.
.4. Kolejne pozycje manipulatora dla zmian zmiennych konfiguracyjnych. t t 4 t t t 5 Rysunek 4. Rzut trajektorii i pozycji członów manipulatora w płaszczyźnie XY.
Ustawienie początkowe dla t=. i α i- a i- d i θ i ⁰ ⁰ ⁰ l ⁰ ⁰ l ⁰ 4 9⁰ 8⁰ 5 9⁰ l 4 ⁰
Ustawienie po czasie t d 4 m 7 6 5 4 t t t t t4 t5 4
Ustawienie po czasie t 8 5 9 45-45 t t t t t4 t5-9 -5-8 5
Ustawienie po czasie t 8 5 9 45 5-45 t t t t t4 t5-9 -5-8 Ustawienie po czasie t 4 6
8 5 9 45-45 t t t t t4 t5-9 -5-8 7
Ustawienie po czasie t 5 8 5 9 45-45 t t t t t4 t5-9 -5-8 8
. Odwrotne zadanie kinematyki. W opracowaniu tym odwrotne zadanie kinematyki zostanie wyznaczone tylko do układu współrzędnych nr. Ograniczenie to spowodowane jest złożonością zadania. W oparciu o uzyskane w tym projekcie obliczenia czytelnik może w własnym zakresie wykonać odwrotne zadanie kinematyki uwzględniając wszystkie układy współrzędnych. elem odwrotnego zadania jest znaleźć w oparciu o zadaną pozycję kartezjańską względem układu bazowego, kąty ugięć lub przesunięcia kolejnych członów manipulatora. Jak wyżej wspomniano ostatnim układem będzie tutaj układ współrzędnych nr... Ogólna postać macierzy transformacji. Macierz opisuje transformację z układu i do układu i- [ ] W macierzy przyjęto skrócone zapisy funkcji trygonometrycznych.. Macierze transformacji kolejnych układów. Macierz transformacji z układu współrzędnych do [ ] Gdzie: Macierz transformacji z układu współrzędnych do [ ] Gdzie: 9
Macierz transformacji z układu współrzędnych do [ ] Gdzie: Znając kolejne macierze transformacji można wyznaczyć macierz transformacji z układu współrzędnych nr do układu współrzędnych nr. Po kolejnych operacjach mnożenia macierzy otrzymamy: * * * * * * * * * * * T Otrzymana macierz w zapisie symbolicznym opisuje złożenie rotacji i przesunięcia układu współrzędnych nr względem układu współrzędnych nr [ ] Gdzie: [ ] W modelu manipulatora będącego przedmiotem tego opracowania ruch odbywa się w dwóch wymiarach.
Macierz w przypadku ogólnym dla ruchu w dwóch wymiarach wygląda następująco: [ ] Macierz nazywana jest też macierzą zadaną. Aby możliwe było wyrażenie kątów lub przesunięć kolejnych członów manipulatora w funkcjach współrzędnych kartezjańskich względem układu bazowego porównać trzeba macierz z otrzymaną wcześniej macierzą. Poprzez porównanie rozumiemy porównanie indeksów tych macierzy a następnie przyrównaniem ich do siebie. W efekcie otrzymujemy następujący układ równań: * * * * * * Y Y X x T Y X T D { Po skorzystaniu z trygonometrycznych wzorów redukcyjnych dwa pierwsze równania uproszczą się, w wyniku otrzymamy: { Gdzie:, Następnie zmienimy zapis równań na i na następujący: { Podnosimy teraz powyższe równania do kwadratu i dodajemy stronami: {
Wymnażamy teraz kolejne wyrazy w równaniach: korzystamy z zależności: Zastosujemy podstawienia: Po uwzględnieniu podstawień można pierwsze równanie zapisać: ( ) Wobec tego: Gdzie:
Po wstawieniu do wzoru ogólnego otrzymujemy: ( ) ( ) Analogicznie postępujemy z pozostałymi zmiennymi: ( ) ( ) ( ) Na tym etapie znamy już zależności zmiennych konfiguracyjnych w funkcji zmiennych kartezjańskich. Konieczne jest teraz wyznaczenie dziedzin funkcji. Bez kłopotu możemy stwierdzić, że na pewno wartości długości ramion muszą być większe od zero. ; Oczywiście nie jest to koniec wyznaczania dziedziny. Dalsze wyznaczenie dziedziny pozostawiam czytelnikowi. Podczas wyznaczania dziedziny(przedziału wartości kątów) należy zwrócić uwagę na kolizje jakie mogą nastąpić przy różnych pozycjach manipulatora.
.. Obliczanie trajektorii w programie MATAB. clear clc clear =8; =8; t=:.:; [m n]=size(t); for i=::n if t(i)<5 x(i)=; y(i)=-*t(i)+5; elseif t(i)< x(i)=t(i)-4; y(i)=-5; elseif t(i)<5 x(i)=6; y(i)=*t(i)-5; else x(i)=-t(i)+; y(i)=5; end% if end% i figure() subplot(,,),plot(t,x) subplot(,,),plot(t,y) subplot(,,),plot(y,x) 4
X Y X 6 4 4 6 8 4 6 8 t 5-5 4 6 8 4 6 8 t 4 6 5 4 - - - -4-5 Y Rysunek 5. Zadana trajektoria w zmiennych kartezjańskich. fi=; t=:.:;% vector of time [m n]=size(t); for j=::n th(j)=*atan( ( y(j)+sqrt( y(j)^-x(j)^- ( (x(j)^+y(j)^-^+^)/(*) )^ ) )/ ( x(j)+( (x(j)^+y(j)^-^+^)/(*) ) ) ); th(j)=*atan((cos(th(j))+sqrt((cos(th(j)))^-(sin(th(j)))^-((y(j)- sin(th(j))*)/())^))/ (sin(th(j))+((y(j)-sin(th(j))*)/()))); th(j)=*atan( (-cos(th(j))*sin(th(j))-sin(th(j))*cos(th(j))+sqrt((-cos(th(j))*sin(th(j))- sin(th(j))*cos(th(j)))^-(cos(th(j))*cos(th(j))-sin(th(j))*sin(th(j)))^-cos(fi)^))/ (cos(th(j))*cos(th(j))-sin(th(j))*sin(th(j))+cos(fi)) ); end % j 5
figure() subplot(,,),plot(t,th) xlabel('t[s]'); ylabel('theta[rad]'); subplot(,,),plot(t,th) xlabel('t[s]'); ylabel('theta[rad]'); subplot(,,),plot(t,th) xlabel('t[s]'); ylabel('theta[rad]'); Rysunek 6. Kąty ugięcia ramion dla zadanego ruchu w układzie bazowym. 6
P = [5 ] ; for k=::n c=cos(th(k)); s=sin(th(k)); c=cos(th(k)); s=sin(th(k)); c=cos(th(k)); s=sin(th(k)); T= [ (cc-ss)*c+(-cs-sc)*s (cc-ss)*(-s)+(-cs-sc)*c (cc- ss)*+c ; (sc+cs)*c+(-ss+cc)*s (sc+cs)*(-s)+(-ss+cc)*c (sc+cs)*+s ; ; ]; P = T*P; xx(k)=p(); yy(k)=p(); end %k plot(yy,xx) 7
4. Analiza statyczna manipulatora. elem analizy statycznej jest uzyskanie informacji o siłach jakie będą działać na jego konstrukcję oraz napędy podczas spoczynku robota. iła działająca w narzędzie robota przedstawiona w postaci ogólnej: [ ] Przejście z siłą do układu Ponieważ wirtualny układ nr 6 jest identyczny jak układ nr 5 więc Obliczenie momentu w układzie Ponieważ Gdzie to wersory jednostkowe układu Wyznaczenie wartości momentu napędowego dla układu [ ] [ ] Wyznaczanie obciążeń w układzie Obciążenia wyznaczone w układzie będą występowały również w układzie, wobec tego będą uwzględnione w wzorach ogólnych. 8
Rotacja pomiędzy układami oraz jest opisana następującą macierzą rotacji: [ ] [ ] [ ] [ ] [ ] [ ] [ ] Wyznaczenie wartości siły napędowej dla układu [ ] [ ] Wyznaczanie obciążeń w układzie Obciążenia wyznaczone w układzie będą występowały również w układzie, wobec tego będą uwzględnione w wzorach ogólnych. Rotacja pomiędzy układami oraz jest opisana następującą macierzą rotacji: [ ] [ ] [ ] [ ] [ ] [ ] [ ] 9
Wyznaczenie wartości momentu napędowego dla układu [ ] [ ] Wyznaczanie obciążeń w układzie Obciążenia wyznaczone w układzie będą występowały również w układzie, wobec tego będą uwzględnione w wzorach ogólnych. Rotacja pomiędzy układami oraz jest opisana następującą macierzą rotacji: [ ] [ ] [ ] [ ] [ ] [ ] [ ] Wyznaczenie wartości siły napędowej dla układu [ ] [ ]
Wyznaczanie obciążeń w układzie Obciążenia wyznaczone w układzie będą występowały również w układzie, wobec tego będą uwzględnione w wzorach ogólnych. Rotacja pomiędzy układami oraz jest opisana następującą macierzą rotacji: [ ] [ ] [ ] [ ] [ ] [ ] [ ] Wyznaczenie wartości momentu napędowego dla układu [ ] [ ]