InŜynieria Rolnicza 6/005 Katera Postaw Techniki Akaemia Rolnicza w Lublinie BADANIA SYMULACYJNE AGROROBOTA W ASPEKCIE DOKŁADNOŚCI POZYCJONOWANIA Streszczenie W pracy przestawiono sposób moelowania oraz wyniki komputerowej symulacji ruchu agrorobota, którego zespół pozycjonowania posiaa trzy stopnie swoboy. Analizę kinematyki przeprowazono metoą macierzową. Równania ruchu wyprowazono na postawie równań Lagrange a II rozaju. Baania symulacyjne, la trzech wybranych prękości chwytaka, przeprowazono w programie Matlab (Simulink). Wyniki przestawiono w postaci czasowych przebiegów błęów realizacji zamierzonego toru ruchu. Słowa kluczowe: agrorobot, kinematyka, ynamika, symulacja, okłaność pozycjonowania Wykaz oznaczeń λ, l, l ługości poszczególnych ogniw, [m] θ i wzglęne przemieszczenie kątowe ogniw, [ra] A i macierz przekształcenia ukłaów współrzęnych, s i, c i, s ij, c ij oznaczenia funkcji trygonometrycznych sinus i cosinus kąta θ i oraz sumy kątów θ i i θ j, m i masy poszczególnych ogniw, [kg] J Z, J Z, J Y, J Y masowe momenty bezwłaności ogniw, [kgm ] ijk współczynniki bezwłaności i symbole Christoffela, [kgm ] M ist moment statyczny obciąŝający ogniwo i, [Nm] M i (t) uogólnione momenty obrotowe, [Nm] p X, p Y, p Z współrzęne połoŝenia chwytaka w ukłazie związanym z postawą agrorobota, [m] arctg(y,x) wuargumentowa funkcja arctangens, która wyznacza wartość funkcji i uwzglęnia w której ćwiartce ukłau współrzęnych leŝy wyznaczony kąt, 05
a, b, c, współczynniki występujące w rozwiązaniu owrotnego zaania kinematyki, a P maksymalne przyspieszenie punktu P la zaanego toru ruchu, [m/s ] V xp, V yp, V zp skłaowe prękości punktu P la zaanego toru ruchu, [m/s] Wprowazenie Zautomatyzowanie czynności manipulacyjnych przy zbiorze owoców wymaga o maszyny manipulacyjnej wykonywania nie tylko złoŝonych, precyzyjnych i szybkich ruchów w przestrzeni roboczej, ale równieŝ rozpoznawania wybranych obiektów w tej przestrzeni i opowieniej orientacji chwytaka wzglęem ojrzałych owoców. ZłoŜoność czynności agrorobota i zbliŝenie ich o czynności luzkiej ręki stawiają wysokie wymagania w zakresie sterowania agrorobotem, które nie moŝe być zeterminowane i nie moŝe obywać się niejako na pamięć. W związku z tym agrorobot musi okonywać analizy sytuacji przestrzennej i wyboru toru ruchu chwytaka w czasie rzeczywistym [Morecki i in. 00]. Wymaga to zaprojektowania manipulatora mechatronicznego zbuowanego z posystemów: sensorycznego zaaniem którego byłaby ientyfikacja i wyznaczenie wzajemnego połoŝenia w przestrzeni chwytaka i ojrzałego owocu, sterującego zaaniem którego byłoby sterowanie ruchem silników napęowych ukłau jeznego, ogniw zespołu pozycjonowania oraz chwytaka, wykonawczego w skła którego wchozą ukła jezny, zespół pozycjonowania oraz chwytak, łączącego stanowiącego wszelkie połączenia pomięzy poszczególnymi posystemami. W przypaku zbioru owoców, gzie wymagana jest barzo uŝa okłaność pozycjonowania chwytaka, ze wzglęu na ich elikatność, konieczna jest analiza błęów pozycjonowania la róŝnych wartości prękości roboczych. Cel i zakres pracy Celem pracy jest baanie i ocena wpływu kinematycznych charakterystyk ruchu na okłaność owzorowania zamierzonego toru końca łańcucha pozycjonowania (w alszej części pracy oznaczanego P). W szczególności rozpatrzony został ruch po prostej tak połoŝonej w przestrzeni roboczej, aby wystąpił ruch we wszystkich parach kinematycznych łańcucha pozycjonowania. Dla zaanego toru ruchu wyznaczono błęy pozycjonowania la trzech wariantów prękości liniowych punktu P. W celu oceny okłaności pozycjonowania zbuowano matema- 06
Baania symulacyjne agrorobota... tyczny moel ynamiki zespołu pozycjonowania agrorobota. Buowa tego moelu wymagała: rozwiązania prostego zaania kinematyki, rozwiązania owrotnego zaania kinematyki (wyznaczenia połoŝeń, prękości i przyspieszeń kątowych ogniw w zaleŝności o połoŝenia, prękości i przyspieszenia punktu P), wyznaczenia energii kinetycznej i potencjalnej poszczególnych ogniw, a następnie wykorzystania równań Lagrange a II rozaju o wyprowazenia róŝniczkowych równań ruchu ogniw. Baania symulacyjne przeprowazono w programie Matlab (Simulink). Wyniki przestawiono w postaci czasowych przebiegów błęów realizacji zamierzonego toru ruchu. Metoyka Obiektem baań był zespół pozycjonowania agrorobota o trzech stopniach swoboy umoŝliwiającym realizację ruchów przestrzennych (rys. ). Rys.. Fig.. Schemat zespołu pozycjonowania agrorobota z wprowazonymi ukłaami współrzęnych Denavita-Hartenberga Agro-robot positioning system iagram with Denavit-Hartenberg systems of coorinates introuce Korzystając z notacji Denavita-Hartenberga (Craig 989) wprowazono ukłay współrzęnych związane ogniwami oraz postawą łańcucha kinematycznego (rys. ). Następnie wyznaczono: 07
macierze przekształceń pomięzy poszczególnymi ukłaami współrzęnych, wektory połoŝenia śroków mas poszczególnych ogniw łańcucha kinematycznego w ukłazie współrzęnych postawy, współrzęne wektorów prękości śroków mas ogniw, w ukłazie współrzęnych związanym z postawą, prękości kątowe ogniw wzglęem nieruchomego ukłau współrzęnych, prękości kątowe ogniw wzglęem chwilowo unieruchomionych ukłaów współrzęnych, których początki leŝą w śrokach mas ogniw, a osie są równoległe o opowienich osi ukłaów współrzęnych związanych z ogniwami. Wykorzystując uzyskane zaleŝności oraz równania Lagrange a rugiego rozaju wyprowazono równania ruchu zespołu pozycjonowania agrorobota: θ t θ t θ θ θ θ = M (t) () t t t t θ + t θ t θ θ θ + + M st = M (t) () t t t + gzie: θ t θ θ + + Mst = M (t) () t t = J Y + J Yc + J Yc + m lc + m lc + llcc + lc 4 4 ll J Ys c + J Ys c + m ls c + m ls c + (sc + cs ) + ls 4 4 ll = J Ys c + m c c + ls c 4 l l = J + J + m + m l + l l c + Z Z 4 4 = c 08
Baania symulacyjne agrorobota... ll c l + 4 = J Z + m, m s l l l =, = m + J Z 4 l l l = m c + m c + lc g, M st = m c g M st Przeprowazenie symulacji ruchu punktu P po zaanym torze, wymagało rozwiązania owrotnego zaania kinematyki. Dla baanego łańcucha kinematycznego wyznaczono połoŝenie chwytaka w kartezjańskim ukłazie współrzęnych związanym z postawą: p = c c l + c c l (4) X Y p = s c l + s c l (5) Z p = l s + l s + λ (6) W wyniku rozwiązania ukłau równań (wzory nr 4, 5 i 6) ze wzglęu na współrzęne konfiguracyjne (θ, θ, θ ), otrzymano (Graboś 00): gzie: a = l (p Xc + p Ys) = l (p λ ) θ b Z c = p X p = X + p Y + p Y θ = arctg(p Y,p Z ) (7) = arctg(b, a) + arctg( a + b c,c) (8) + p Z + λ + l + (pz λ) l l θ l l = arctg( ) (9) p l Z Prękości i przyspieszenia konfiguracyjne ogniw otrzymano przez róŝniczkowanie wzglęem czasu zaleŝności (wzór nr 7, 8 i 9). Komputerowa symulacja ruchu agrorobota została przeprowazona w programie Matlab (Simulink). Program pozielono na sześć współpracujących moułów [Graboś 00]: λ 09
tor ruchu chwytaka w którym zaawane są początkowe i końcowe połoŝenia punktu P oraz prękości i przyspieszenia la zaanej trajektorii ruchu, zaanie owrotne kinematyki w którym wyznaczane są współrzęne konfiguracyjne ogniw w funkcji połoŝenia punktu P (wzór nr 7, 8 i 9.), prękości i przyspieszenia ogniw w którym wyznaczane są prękości i przyśpieszenia kątowe ogniw w funkcji połoŝenia punktu P, charakterystyki rzeczywiste w którym, poprzez całkowanie równań ruchu (wzór nr, i ), obliczane są rzeczywiste wartości charakterystyk ruchu ogniw z uwzglęnieniem poatności i tłumienia w ukłaach napęowych, zaanie proste kinematyki w którym, na postawie rzeczywistych charakterystyk ruchu ogniw wyznaczane są realizowane (rzeczywiste) połoŝenia punktu P (wzór nr 4, 5 i 6), błęy pozycjonowania obliczane jako wartość róŝnicy pomięzy zaanymi i realizowanymi współrzęnymi punktu P. Wyniki baań i ich analiza W celu określenia wpływu kinematycznych charakterystyk ruchu na okłaność owzorowania zamierzonego toru końca łańcucha pozycjonowania (punktu P) zaplanowano trzy warianty obliczeń la których wartości charakterystyczne zestawiono w tabeli. Tabela. Wartości charakterystyczne zamierzonego toru ruchu chwytaka la poszczególnych wariantów Table. Characteristic values of gripping evice trajectory for iniviual variants Parametr Oznaczenie Wariant I Wariant II Wariant III Początkowe wartości współrzęnych punktu P Końcowe wartości współrzęnych punktu P Max. przyspieszenie punktu P p x [m] p y [m] p z [m] p x [m] p y [m] p z [m] 0,500 0,500 0-0,00,50,000 a p [m/s ],855 9,76 8,55 V xp [m/s] 0,00,000,000 Max. skłaowe prękości V liniowej punktu P yp [m/s] 0,88 0,98,875 V zp [m/s] 0,50,50,500 Max. prękość liniowa V max [m/s] 0,7,855,70 0
Baania symulacyjne agrorobota... W przeprowazonej komputerowej symulacji ruchu agrorobota przyjęto następujące ane i załoŝenia: przyjęte ługości ogniw wynoszą λ =l =l = m, zakres wzglęnych prękości kątowych w parach kinematycznych przyjęto w przeziale o <0,π>, współczynniki spręŝystości i tłumienia moelu Kelvina-Voigta, który wprowazono w celu uwzglęnienia poatności i tłumienia jenostek napęowych wynosiły opowienio k=5000 Nm/ra oraz c=00 Nms/ra, masy poszczegółnych ogniw wynoszą m =m =m =5, kg, momenty bezwłaności J Y = J Y = J Y = J Z = J Z = J Z = 0,44 kgm natomiast wartości J X,J X, J X jako istotnie mniejsze o pozostałych przyjęto równe zero. W wyniku przeprowazonej symulacji ruchu ukłau pozycjonowania agrorobota uzyskano przebiegi błęów połoŝenia punktu P w czasie ruchu po zaanej trajektorii. Rys.. Fig.. Czasowe przebiegi błęów pozycjonowania punktu P la wariantu I Time runs of positioning errors of P point for variant I
Rys.. Fig.. Czasowe przebiegi błęów pozycjonowania punktu P la wariantu II Time runs of positioning errors of P point for variant II Rys. 4. Fig. 4. Czasowe przebiegi błęów pozycjonowania punktu P la wariantu III Time runs of positioning errors of P point for variant III Wnioski We wszystkich wariantach obliczeń, niezaleŝnie o przyjętej prękości roboczej, największą wartość osiąga błą w kierunku osi z 0 - p z. Spowoowane jest to ominującą rolą ugięć statycznych ogniw wynikających z uwzglęnienia poatności jenostek napęowych.
Baania symulacyjne agrorobota... Wraz ze wzrostem prękości roboczej istotnie wzrasta błą pozycjonowania. Największą wartość p z =0,045 m osiąga la III wariantu (la prękości v=,7 m/s). Mniejsze wartości błęów zaobserwowano la wariantu II - p z =0,0 m oraz I - p z =5,86 0 - m. Wynika to głównie z większych sił bezwłaności występujących przy większych prękościach roboczych. Zmiany błęów realizacji zamierzonego toru ruchu la fazy ruchu ustalonego wynikają głównie ze zmian konfiguracji przestrzennej ogniw (połoŝenie łańcucha kinematycznego w przestrzeni), natomiast w fazach ruchu nieustalonego (rozruch i hamowanie) wioczne są oatkowo zmiany wynikające z zaburzeń stanów równowagi ynamicznej ujawniające się w postaci rgań własnych tłumionych. Dla przyjętej geometrii łańcucha pozycjonowania agrorobota niezbęne jest ograniczanie prękości roboczych lub wprowazenie ukłaów sterowania, które umoŝliwiłyby zmniejszenie błęów pozycjonowania. Bibliografia Craig J.J. 989. Introuction to Robotics. Mechanics an Control. Aison- Wesley Publ. Comp. Graboś A. 00. Analiza osobliwych konfiguracji maszyn manipulacyjnych. Rozprawa oktorska. Płock. Morecki A., Knapczyk J., Kęzior K. 00. Teoria mechanizmów i manipulatorów, WNT, Warszawa.
SIMULATIONS OF AGRO-ROBOT IN TERMS OF POSITIONING ACCURACY Summary The paper presents moelling methos an output of computer aie simulation of agro-robot movement, the positioning system of which has three egrees of free motion. The kinematics analysis was carrie out with the matrix metho. Motion equations were erive from the Lagrange equations of the secon type. Simulations, for three selecte spees of the gripping evice, were mae using the Matlab (Simuling) application. The results were presente in a form of time runs of trajectory following errors. Key wors: agro-robot, kinematics, ynamics, simulation, positioning accuracy 4