[Algorytmy] Wyznaczanie środka ciężkości robota kroczącego

[Zuk]

Celem tego artykułu jest przedstawienie metody wyznaczania środka ciężkości robota kroczącego na podstawie aktualnych sił nacisku na podłoże. W artykule pominięto część sensoryczną i przetwarzanie sygnału. Założono, że dysponujemy informacją o położeniu wszystkich aktualnych punktów podparcia w postaci współrzędnych X i Y na płaszczyźnie oraz sile jaka działa we wszystkich tych punktach. Przedstawiony algorytm jest uogólniony i może być zastosowany w robotach o liczbie nóg od dwóch do nieskończoności – wszystko zależy od mocy obliczeniowej i pojemności pamięci maszyny, na jakiej zostaną przeprowadzone obliczenia. Z określonych powodów pominięto także dokładne wzory zastosowane w obliczeniach. Z resztą są to wszystko elementarne wzory matematyczne i fizyczne poznane w szkole podstawowej i średniej.

WPROWADZENIE

Roboty kroczące są bardzo ciekawym i stosunkowo trudnym zagadnieniem. Aby nie zaczynać od początku proponuję przeczytać artykuł: https://www.forbot.pl/forum/topics20/teoria-roboty-kroczace-teoria-i-podstawy-projektowania-vt2206.htm w dalszej części będę posługiwał się pojęciami wyjaśnionymi w linkowanym artykule.

Najprostszym sposobem chodu robotów wielonożnych (załóżmy, że mówimy o robotach co najmniej czteronożnych) jest ruch falowy czyli zapętlona sekwencja przestawiania nóg. Dużo bardziej wymagający jest ruch swobodny, w którym to układ sterowania na bieżąco wybiera, która nogę może podnieść i przestawić w następnej fazie, aby nie stracić równowagi. W obydwu przypadkach mamy ten sam problem – jaką wybrać sekwencję przestawień i zakresy ruchu robota, aby robot nie przewrócił się. W pierwszym przypadku (ruch falowy) problem ten musi rozwiązać konstruktor/programista w fazie projektowania i programowania robota, w drugim przypadku (ruch swobodny) problem ten musi na bieżąco, w trakcie ruchu, rozwiązywać układ sterowania robota. Przy czym należy sterownikowi zapewnić odpowiedni strumień danych związanych z położeniem i siłami wszystkich punktów podparcia.

Dane wejściowe i założenia upraszczające

Celem jest określenie położenia środka ciężkości. Współrzędne punktów podparcia podajemy względem geometrycznego środka robota (przy czym środek ciężkości nie musi znajdować się w środku geometrycznym robota). Najcięższymi elementami robotów kroczących jest bateria oraz napędy. O ile bateria zazwyczaj zamontowana jest na sztywno o tyle napędy najczęściej umieszczone w nogach poruszają się wywołując zmianę położenia środka ciężkości. Jest to powód, dlaczego w przypadku ruchu swobodnego musimy na bieżąco monitorować położenie środka ciężkości.

Dane wejściowe do obliczeń to siła i położenie wszystkich punktów podparcia. Zakładamy, że znamy wszystkie te wielkości. Dane te zostały pogrupowane w klastry danych (rekordy) jeden klaster zawiera wszystkie trzy parametry jednej nogi. Dalej tworzony jest wektor N-elementowy, gdzie N odpowiada liczbie nóg robota. W fazie protrakcji siła nacisku wynosi 0, w fazie retrakcji jest określona. W stanie statycznym – kiedy nie jest wykonywany żaden ruch suma sił nacisku jest równa masie robota (należy uwzględnić konwersję jednostek N i kg).

Pierwszym etapem obliczenia jest selekcja klastrów w wektorze względem siły. Taki zabieg umożliwi wstępną redukcję wektora danych przez usunięcie elementów z zerową siłą – znajdujących się na końcu. Gdy w wektorze zostaną nam tylko elementy z niezerową siłą (nogi w fazie retrakcji) przystępujemy do systematycznej redukcji ostatniego elementu wektora, aż pozostanie jeden – środek ciężkości.

Algorytm wykonywany w pętli przebiega następująco:

- wyselekcjonuj dwa ostatnie elementy wektora,
- oblicz odległość L między tymi punktami,
- oblicz odległość wybranego punktu od środka ciężkości układu tych dwóch punktów (w celu sprawdzenia poprawności algorytmu można obliczyć obydwie odległości, ich suma powinna wynosić L),
-, oblicz współrzędne środka ciężkości

- wpisz sumaryczną siłę obydwu punktów do przedostatniego elementu oraz wartość 0 do elementu ostatniego

- wpisz obliczone współrzędne środka ciężkości do przedostatniego elementu wektora

- usuń element z zerową siłą (ostatni element wektora),
-czynność powtarzaj aż zostanie tylko jeden element.

Algorytm został graficznie przedstawiony na rysunku 1.

Rys. 1.Algorytm obliczania środka ciężkości.

Gdzie N-1, N-2 – ostatni i przedostatni element wektora danych – redukowane punkty, L – długość odcinka między redukowanymi punktami, xa, ya – współrzędne środka ciężkości, r1, r2 - odległości środka ciężkości od punktów, Fsuma – suma sił w redukowanych punktach Fsuma = F(N-1) + F(N-2).

Odległości punktów od środka ciężkości oblicza się korzystając z warunku równowagi momentów sił. Innymi słowy można to zobrazować na przykładzie wagi i dwóch ciężarków. Mamy dwa ciężarki w dwóch punktach, gdzie przyłożyć punkt podparcia, aby siły zrównoważyły się. Zostało to zobrazowane na rysunku 2.

Rys. 2.Poszukiwanie punktu podparcia.

Implementacja algorytmu

Algorytm został zaimplementowany w programie LabVIEW. Rysunek 3 przedstawia kod programu. Z kodu zostały usunięte wzory. Należy pamiętać, że punkty mogą leżeć na prostej – jest to najprostszy przypadek ponieważ można zastosować wzory na obliczanie długości odcinka i położenie punktów na prostej y = ax + b. Punkty mogą znajdować się także na prostej równoległej do osi współrzędnych układu. W tym przypadku należy wspólną składową zostawić bez zmian i dodać (z uwzględnieniem znaku) odległość środka masy do współrzędnej punktu od którego ta odległość była liczona.

Rys.3.Diagram programu napisany w LabVIEW.

Zastosowanie obliczenia iteracyjnego pozwoliło uogólnić algorytm na dowolną liczbę nóg. Obliczenia przeprowadzane są o jeden mniej razy niż jest liczba obciążonych nóg (N-1 razy gdzie N to liczba nóg z niezerową wartością siły).

Jedną z metod sprawdzenia poprawności obliczeń jest podanie współrzędnych symetrycznych względem określonego punktu (np. punktu 0,0) i jednakowych siłach. W takim przypadku jako wynik obliczeń powinniśmy otrzymać punkt symetrii oraz sumaryczną siłę. Na rysunku 4 widać panel programu.

Rys.4.Widok panelu programu. Wykonanie obliczeń realizuje się za pomocą przycisku RUN.

Możliwości rozwoju projektu

Algorytm obliczeń jest gotowy do wykorzystania zarówno w fazie projektowania algorytmu chodu robota kroczącego, gotowy do implementacji w układzie sterowania robota realizującego ruch typu swobodnego. W takim wypadku wymaga modułu zbierającego i przygotowującego dane wejściowe dla algorytmu. Może zostać wykorzystany także do sprawdzenia obciążenia nóg robota w celu doboru napędów (jeżeli np. środek ciężkości przesunięty jest znacznie względem środka geometrycznego robota może być konieczne zastosowanie mocniejszych napędów w tej części).

Dalszym planowanym etapem projektu jest wizualizacja położenia środka ciężkości względem zadanego położenia nóg i sprawdzenie, czy znajduje się on we wnętrzu wielokąta podparcia.

Skompilowany plik exe dołączony jest do artykułu. Uruchomienie go (w przypadku gdy na komputerze nie ma zainstalowanego LabVIEW) wymaga pobrania bibliotek programu LabVIEW 2010 tzw. Runtime Engine: http://joule.ni.com/nidu/cds/view/p/id/2087/lang/en

Zastrzegam sobie prawo do nie publikowania plików źródłowych programu. Może którąś z następnych wersji opublikuję.

srodek_ciezkosci.zip