MODELOWANIE INŻYNIERSKIE 2016 nr 58, ISSN 1896-771X MODELOWANIE KINEMATYKI PROSTEJ I ODWROTNEJ ŻURAWIA SAMOCHODOWEGO O STRUKTURZE REDUNDANTNEJ Z WYKORZYSTANIEM ŚRODOWISKA MATLAB Paweł Herbin 1a,, Mirosław Pajor 1b 1 Instytut Technologii Mechanicznej, Zachodniopomorski Uniwersytet Technologiczny w Szczecinie a pawel.herbin@zut.edu.pl, b miroslaw.pajor@zut.edu.pl Streszczenie Sterowanie żurawiem samochodowym polega na zadawaniu ruchu w poszczególnych parach kinematycznych konstrukcji nośnej (w tzw. współrzędnych konfiguracyjnych lub napędowych), co wymaga dużej wprawy i doświadczenia operatora, ponieważ zadane przemieszczenie w wybranej parze kinematycznej powoduje zwykle złożony przestrzenny ruch końcówki roboczej dźwigu. Budowa modelu kinematyki prostej oraz odwrotnej umożliwia opracowanie algorytmów sterowania żurawiem samochodowym w sposób znacznie bardziej intuicyjny. Opracowane modele można zastosować do sterowania żurawiem we współrzędnych kartezjańskich bądź cylindrycznych. Manipulacja ładunkiem w przestrzeni kartezjańskiej lub cylindrycznej dla operatora jest znacznie łatwiejsza, jednakże wymaga jednoczesnego zadawania ruchu w kilku parach kinematycznych. Z uwagi na występowanie redundantnych stopni swobody konieczne jest zastosowanie algorytmów tymczasowego ograniczenia ruchu określonych par kinematycznych. Blokowanie określonych stopni swobody zapewnia jednoznaczność rozwiązania zagadnienia odwrotnego kinematyki, a tym samym efektywne sterowanie dźwigiem. Omawiane modele matematyczne oraz badania symulacyjne opracowanych algorytmów sterowania żurawiem zaimplementowano i zrealizowano w środowisku Matlab Simulink. Słowa kluczowe: robot, żuraw przeładunkowy, kinematyka MODELING DIRECT AND INVERSE KINEMATICS OF LOADING CRANE WITH REDUNDANT DEGREES OF FREEDOM STRUCTURE USING MATLAB Summary Control of hydraulic car crane consist on inflicting motion of each kinematic pair supporting structure (configuration coordinates or driving coordinates), which requires a lot of the operator s practice and experience, because the movement of the selected kinematics pair usually results in a complex movement of hydraulic crane s working tip. Development of simple kinematic and inverse kinematics allows to elaborate the loading crane s operating algorithms in a much more intuitive way. Developed models can be used to perform the movement in Cartesian or cylindrical coordinates. Handling of cargo in Cartesian or cylindrical coordinates is much easier for the operator, however it requires moving in a number of kinematic pairs. Due to the presence of redundant degrees of freedom it is necessary to use algorithms temporarily limiting the movement of specified of kinematic pairs. Selective blocking degrees of freedom provides to unique solution of the inverse kinematics problem, and hence the effective control of crane. These mathematical models and simulation studies of designed crane control algorithms were implemented and realized in Matlab Simulink. Keywords: robot, loading crane, kinematics 44
PAWEŁ HERBIN, MIROSŁAW PAJOR 1. WSTĘP Żurawie przeładunkowe, popularnie nazywane HDS (hydrauliczny dźwig samochodowy), stanowią dużą gałąź przemysłu dźwigowego. Bardzo często spotykana jest integracja pojazdu transportowego z żurawiem przeładunkowym. Kierowca ciężarówki winien posiadać stosowne uprawnienia oraz umiejętności obsługi urządzenia dźwigowego. Sprawne sterowanie samochodowym żurawiem samochodowym wymaga posiadania dużej wprawy i doświadczenia operatora. Klasyczny układ sterowania, który znajduje zastosowanie przy żurawiach przeładunkowych, to sterowanie za pomocą ruchu poszczególnych przegubów ramienia przy użyciu oddzielnych manetek (sterowanie we współrzędnych złączowych) [1]. Żurawie przeładunkowe w większości są strukturami szeregowymi o redundantnej liczbie stopni swobody [2]. Na rys. 1 przedstawiono przykładowy żuraw przeładunkowy. Analizowany HDS posiada dziewięć stopni swobody, trzy rotacyjne oraz sześć translacyjnych wynikających z zastosowania w konstrukcji żurawia ramienia teleskopowego. Sterowanie wysuwem teleskopowego ramienia realizowane jest za pomocą sprzężonych siłowników. Ruch teleskopowego ramienia realizowany jest według jednej z trzech strategii: Praca według strategii a. polega na wysuwie wszystkich członów teleskopowych jednocześnie, b. podczas pracy w danej chwili czasowej wysuwany jest tylko jeden człon w sekwencyjnej kolejności, c. zależnie od konfiguracji, sił tarcia wysuwa się losowo wybrany człon. Dla potrzeb prowadzonych prac rozpatrywano żuraw o układzie wysuwu sekwencyjnego. Sterowanie żurawiem przeładunkowym musi uwzględniać konieczność przeniesienia ładunku ponad przeszkodą[2][3], m.in. ścianami budynków. Jednym z wielu problemów spotykanych podczas manipulacji ładunkiem z wykorzystaniem HDS-ów jest możliwość przekroczenia strefy bezpiecznej dla manipulacji ładunkiem o danym ciężarze. Zaprojektowanie systemu zabezpieczeń wymaga opracowania systemu sterowania umożliwiającego obliczenie położenia zawiesia haka względem ciężarówki [3][4][5]. W niniejszym artykule przedstawiono opis matematyczny kinematyki prostej oraz sposób rozwiązania zadania odwrotnego kinematyki. Dla rozpatrywanej konstrukcji żurawia przeładunkowego zaprezentowano także wyniki badań symulacyjnych wraz z wizualizacją opracowaną w programie Matlab. 2. MODEL MATEMATYCZNY ŻURAWIA PRZEŁADUNKOWEGO 2.1 MODEL KINEMATYKI PROSTEJ ŻURAWIA Model matematyczny kinematyki prostej opracowano w oparciu o notacje Denavita Harteneberga [6]. Zbudowano model o dziewięciu stopniach swobody. Na rys. 2 zaprezentowano lokalizację układów współrzędnych żurawia przeładunkowego w pozycji zerowej. a. ruch synchroniczny, b. ruch sekwencyjny, c. ruch dowolny (losowy). Rys. 2. Lokalizacja układów współrzędnych według notacji Denavita-Hartenberga dla żurawia w pozycji zerowej Dla przedstawionego żurawia zapisano jego parametry geometryczne zgodnie z notacją D-H ( długość członu, kąt skręcenia członu, odsuniecie członu, kąt obrotu członu) przedstawione zostały w tabeli 1. Rys. 1. Żuraw przeładunkowy Hiab HS 111 Tabela 1. Parametry Denavita-Hartenberga dla żurawia Hiab XS 111 (mm) (deg) (mm) (deg) 1 0 0 2089 180 2 263 270 0 180 3 2140 0-290 270 4 225 0 0 0 5 0 0 0 0 6 0 0 0 0 7 0 0 0 0 8 0 0 0 0 9 0 0 0 0 Na podstawie tabeli 1 opracowano zależność (1) opisującą położenie końcówki żurawia względem jego podstawy: = (1) 45
MODELOWANIE KINEMATYKI PROSTEJ I ODWROTNEJ ŻURAWIA SAMOCHODOWEGO ( ) macierz przekształceń jednorodnych pomiędzy poszczególnymi członami. 2.2 MODEL KINEMATYKI ODWROTNEJ ŻURAWIA Żuraw przeładunkowy jest układem redundantnym o dziewięciu stopniach swobody, który można uprościć do urządzenia o czterech stopniach swobody (zastąpienie sześciu stopni wysuwnych jednym stopniem swobody na podstawie zależności 25). Mimo przeprowadzonego zabiegu redukcji stopni swobody dla manipulatora o zadanej strukturze kinematycznej nie otrzymuje się jednoznacznego rozwiązania. Aby otrzymać jednoznaczne rozwiązanie, należy blokować jeden z trzech stopni swobody (θ,θ, ). Dla żurawia otrzymano trzy modele kinematyki odwrotnej. a. model 1 zablokowany wysuw osi 4 ( =), b. model 2 zablokowany obrót osi 3 (θ =) c. model 3 zablokowany obrót osi 2 (θ =const). Wobec każdego z trzech modeli kąt obrotu kolumny obliczany jest wg następującego wzoru (2): θ =arctg ( ) ( ) +, (11) Wyniki symulacji ruchu członów dźwigu podczas przemieszczania końcówki z punktu (X=3000 mm, =2000 mm, =500 mm) do punktu (X=6000 mm, Y=-2000 mm Z=4000 mm) po linii prostej przedstawiono na rysunkach 3 4. Rys. 3. Wizualizacja ruchu żurawia przy stałej wartości wysuwu. θ =arctg arctg, (2), współrzędne końcówki żurawia, θ kąt konfiguracyjny 1 członu. Położenie końcówki dźwigu względem punktu A (rys. 2) opisano równaniami (3 5): =cos(θ )+sin(θ )+, (3) = sin( )+cos( ), (4) =, (5) współrzędna końcówki roboczej żurawia. Obliczenie kolejnych wartości współrzędnych konfiguracyjnych dotyczące każdego modelu wykonywane jest za pomocą odrębnego algorytmu. Współrzędne konfiguracyjne dla modelu 1 kinematyki ( =) odwrotnej zostały opisane następującymi wzorami (6 11): = +, (6) = +, (7) θ =, (8) θ =arctg, (9) θ =θ θ, (10) Rys. 4. Wartości zmiennych konfiguracyjnych podczas poruszania się z punktu do dla modelu z zablokowanym wysuwem osi 4 ( =) Współrzędne konfiguracyjne dla modelu 2 (θ =) kinematyki odwrotnej zostały opisane następującymi wzorami (12 19): = + +, (12) = +, (13) =2 (θ ), (14) = + +2 (θ ), (15) = 4, (16) =, (17) θ =θ +, (18) θ =arctg ( ) ( ) +, (19) 46
PAWEŁ HERBIN, MIROSŁAW PAJOR Wyniki symulacji ruchu członów dźwigu podczas przemieszczania końcówki z punktu do punktu po linii prostej przedstawiono na rysunkach 5 7. = + +, (23) θ = θ arctg, (24) Wyniki symulacji ruchu członów dźwigu podczas przemieszczania końcówki z punktu do punktu po linii prostej przedstawiono na rysunkach 8 10. Rys. 5. Wizualizacja ruchu żurawia przy stałej wartości kąta θ Rys. 8. Wizualizacja ruchu żurawia przy stałej wartości kąta θ Rys. 6. Wartości zmiennych konfiguracyjnych podczas poruszania się z punktu do dla modelu z zablokowanym obrotem osi 3 (θ =) Rys. 9. Wartości zmiennych konfiguracyjnych podczas poruszania się z punktu do dla modelu z zablokowanym obrotem osi 3 (θ =) Rys. 7. Wartości wysuwu ramienia podczas poruszania się z punktu do dla modelu z zablokowanym obrotem osi 3 (θ =) Dla modelu 3 kinematyki (θ =) odwrotnej wyznaczono położenie punktu B (rys. 2) (20 22): = cos(θ ), (20) =, (21) = sin (θ ), (22) Następnie przystąpiono do wyznaczenia współrzędnych konfiguracyjnych (23 24): Rys. 10. Wartości wysuwu ramienia podczas poruszania się z punktu do dla modelu z zablokowanym obrotem osi 3 (θ =) Implementacja opisanych modeli kinematyki odwrotnej umożliwia przyjmowanie różnych wartości współrzędnej konfiguracyjnej blokowanej. Prowadzi to do możliwości realizacji określonej trajektorii ruchu na wiele sposobów. Rys. 11 ilustruje zmiany konfiguracji żurawia dla róż- 47
MODELOWANIE KINEMATYKI PROSTEJ I ODWROTNEJ ŻURAWIA SAMOCHODOWEGO ( ) nych wartości zmiennej konfiguracyjnej, która jest blokowana w modelu 2. kinematyki odwrotnej. Z (mm) Rys. 11. Zmiana konfiguracji żurawia podczas zmiany kąta konfiguracyjnego θ przy stałej pozycji końcówki 2.3 MODEL WYSUWU RAMIENIA Model wysuwu ramienia został opisany według konwencji ruchu sekwencyjnego. Wysuw kolejnych elementów opisano w sposób iteracyjny równaniem (25): 0 = < <,=1 6, (25) wysuw i-tego członu, parametry wykorzystane w modelu zaprezentowane w tabeli 2. 3. ALGORYTM STEROWANIA Sterowanie żurawiem przeładunkowym za pomocą współrzędnych kartezjańskich jest zagadnieniem niejednoznacznym z uwagi na redundantne stopnie swobody, aczkolwiek zastosowanie tymczasowego blokowania jednej ze współrzędnych konfiguracyjnych umożliwia efektywne sterowanie żurawiem. W zaproponowanym podejściu, polegającym na wykorzystaniu naprzemiennie trzech modeli kinematyki odwrotnej żurawia, wybór modelu kinematyki odwrotnej urządzenia następuje na podstawie ograniczeń przestrzeni roboczej oraz zakresów ruchu poszczególnych przegubów. Przełączenie między trybami pracy może być realizowane automatycznie lub wymuszane ręcznie. Równocześnie podczas sterowania położeniem końcówki roboczej XYZ możliwa jest zmiana położenia zablokowanego przegubu. Uzyskuje się zatem możliwość sterowania za pomocą czterech współrzędnych, tj. XYZ, oraz pozycją ograniczonego przegubu. Podejście takie jest wymagane, aby osiągnąć konfigurację umożliwiającą ominiecie przeszkody. Na rys. 13 zaprezentowano algorytm programu kinematyki odwrotnej. W zaprezentowanym algorytmie część A odpowiada za pracę w trybie ręcznego przełączania modeli kinematyki odwrotnej, a część B za pracę w trybie automatycznym. Wybór modelu w trybie automatycznym jest dokonywany również wtedy, gdy w wyniku wybranego przez użytkownika algorytmu kinematyki odwrotnej urządzenie znalazłoby się poza zakresem ruchu przegubów. Opracowany algorytm wykorzystuje funkcje (Θ) odpowiedzialną za sprawdzenie poprawności danego rozwiązania pod kątem zasięgów żurawia przeładunkowego. Rys. 12. Ilustracja parametrów teleskopowego ramienia Tabela 1. Parametry ramienia teleskopowego i ó () () 0 0 2706 1 1650 4356 2 1900 6256 3 2000 8256 4 1200 9456 5 2100 11556 6 2100 13656 Rys. 13. Algorytm przełączania modeli kinematyki odwrotnej 48
PAWEŁ HERBIN, MIROSŁAW PAJOR 4. SYMULACJA Z uwagi na specyfikę pracy żurawia symulator jego pracy powinien zawierać: W wyniku przeprowadzonych prac otrzymano algorytm przełączania trybów kinematyki odwrotnej oraz ograniczeń ruchu poszczególnych przegubów żurawia (zakresy ruchu). Na rys. 15 przedstawiono żurawia przeładunkowego w 3 pozycjach osiągniętych podczas ruchu zadanego przez manipulator 3d. a) model kinematyki prostej, b) model kinematyki odwrotnej, c) jakobian prędkości żurawia samochodowego, d) model dynamiki, e) układ sterowania żurawiem, f) interfejs zadawania przemieszczenia, g) wizualizację W ramach niniejszego artykułu przedstawiono opracowane elementy a, b, e, f, g. Symulator opracowano w programie Matlab Simulink. Na podstawie modelu CAD wykonano wizualizację z wykorzystaniem VRML. Opracowanie wizualizacji w języku VRML uproszczono dzięki możliwości eksportu modelu CAD do VRML. W ramach eksportu nie zostają jednak przeniesione więzy pomiędzy kolejnymi członami. Ustawiono zatem kolejne człony w modelu CAD zgodnie z przyporządkowanymi im układami współrzędnych. Do animacji modelu geometrycznego wykorzystano macierze przekształceń jednorodnych [6] stosowane również w modelu kinematyki prostej. W opracowanym symulatorze zaimplementowano modele kinematyki prostej, odwrotnej, wizualizację oraz sterowanie ruchem żurawia na podstawie sygnałów pochodzących z manipulatora 3D SpacePilot Pro. Proces budowy symulatora zaprezentowano formie schematu blokowego na rys. 14. Rys. 15. Przebieg symulacji ruchu wg zadanej trajektorii Dla zaprezentowanego ruchu przebiegi zmiennych kątowych oraz wysuw osi czwartej zostały przedstawione na rysunkach 16 oraz 17. d4 (mm) 5500 5000 4500 4000 3500 3000 2500 0 5 10 15 20 25 30 35 czas (s) Rys. 16. Zmienna konfiguracyjna podczas ruchu zaprezentowanego na rys. 14 Rys. 17. Zmienne konfiguracyjne θ,θ,θ podczas ruchu zaprezentowanego na rys. 14 5. PODSUMOWANIE Rys. 14. Proces tworzenia symulatora żurawia przeładunkowego W artykule opisano metodykę modelowania kinematyki prostej i odwrotnej żurawia przeładunkowego o redundantnej strukturze. Otrzymane zależności umożliwiają sterowanie w układzie kartezjańskim końcówką roboczą żurawia niezależnie od konfiguracji. Zastosowanie kinematyki odwrotnej do sterowania żurawiem przeładun- 49
MODELOWANIE KINEMATYK PROSTEJ I ODWROTNEJ ŻURAWIA SAMOCHODOWEGO ( ) kowym dopuszcza ograniczenie ruchu żurawia w okre- Staje się to ślonym kierunku względem ciężarówki. niezwykle istotne, ponieważ żurawie przeładunkowe muszą spełniać odpowiednie normy bezpieczeństwa [1]. W pracy pokazano, jak można zamodelować kinematykę prostą i odwrotną wraz z wizualizacją wykorzystując zaawansowane metody wizualizacji w programie Matlab. Dzięki zaprezentowanemu podejściu możliwe jest łatwe modyfikowanie układu oraz rozbudowa modelu o kolejne moduły takie jak model dynamiki i modele hydrauliczne siłowników żurawia. Opracowane modele mogą posłużyć konstruktorom do opracowania odpowiedniego układu sterowania uwzględniającego możliwość ruchu we współ- Prace realizowane były w ramach projektu rzędnych kartezjańskich lub cylindrycznych. PBS3/A6/28/2015 finansowanegoo przez NCBiR. Literatura 1. Skrzymowski W.: Żurawie przeładunkowe: budowa i eksploatacja. Krosno: Kabe, 2006. ISBN 83 8938 72 63 2. Mettin U., La Hera P. M., Morales D. O., Shiriaev A. S., Freidovich L. B., Westerberg S..: Trajectory planning and time-independent motion control for a kinematically redundant hydraulic manipulator. In: Advanced Robot- ics 2009, p. 1-6.. 3. Westerberg S., Manchester I. R., Mettin U., La Hera P. M., Shiriaev A.: Virtual environment teleoperation of a hydraulic forestry crane. In: Robotics and Automation 2008, p. 4049-4054. 4. La Hera P. M., Morales D. O.: Modeling dynamics of an electro-hydraulic servo actuated manipulator: a case study of a forestry forwarder crane. In: World Automation Congress (WAC) 2012, p. 1-6. 5. Morales D. O., Westerberg S., La Hera P. X., Mettin U., Freidovich L., Shiriaev A.: Increasing the level of automation in the forestry logging process with crane trajectory planning and control. Journal of Field Robotics 2014, No. 31, p. 343-363. 6. Craig J. J.: Introduction to robotics: mechanics and control. Pearson: Prentice Hall, 2005. ISBN 02-015-4361-3. Ten artykuł dostępny jest na licencji Creative Commons Uznanie autorstwa 3.0 Polska. Pewne prawa zastrzeżone na rzecz autorów. Treść licencji jest dostępna na stronie http://creativecommons.org/licenses/by/3.0/ /pl/ 50