[C] Kinematyka odwrotna - implementacja w AVR

[AE]

Cześć,

Od jakiegoś czasu siedzę nad implementacją kinematyki odwrotnej dla czteronożnego robota kroczącego. Gdy wpiszę tylko jeden wektor ze współrzędnymi zewnętrznymi (np. jako wartość początkowa) to wszystko ładnie chodzi. Współrzędne wewnętrzne są obliczane przez mikrokontroler i wysterowuje odpowiednie serwa. Gdy w kodzie chcę wpisać sekwencje ruchu dla nogi (zmienają się początkowe wartości współrzędnych zewnętrznych) to serwa zaczynają wariować i nie ma to nic wspólnego z prawidlowym wysterowaniem, a z istnym rodeo 🙂

Jestem świadomy, że takie obliczenia są dla AVR-ka dużym obciążeniem dlatego starałem się zoptymalizować fragment, gdzie owa kinematyka jest obliczana. Zmiana wartości współrzędnych zewnętrznych nie następuje zbyt szybko. Nawet przy zmianach następujących co kilka sekund serwa również świrują. Poniżej zamieszczam kod. Czytalem, że kilku osobom udało się zaimplementować kinematykę odwrotną w swoich projektach, także proszę o jakieś wskazówki. Używam ATmega32 z 16 MHz kwarcem.

Pozdrawiam

#define F_CPU 16000000L
#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>

#define Rad_Deg 57.2957 //używane, aby przejść z radianów na stopnie 

volatile unsigned int ServoPosition = 24500;           //wartość wskazuje wychylenie orczyka, obliczana na samym końcu kodu, wartość ta przypisywana jest do OCR1A
volatile unsigned char ServoNr = 0, ServoAdress[] = {PD0,PD1,PD2,PD3,PD4,PD5};        //ServoNr - nr serwa, jak na razie mam ich 6; ServoAdress - tablica z wyjściami dla programowego PWM`a

ISR(TIMER0_COMP_vect)           //przerwanie od Timer`a 0, odmierza 3,33 ms (20ms/6serw=3,33ms na serwo)
{
OCR1A = ServoPosition;             //ustawienie długości impulsu (wychylenia) orczyka
PORTD |= (1<<ServoAdress[ServoNr]);            //ustawienie nr serwa, które ma się wychylić
TCCR1B |= (1<<CS10);            //start timera 1, który odmierza zadany impuls
}

ISR(TIMER1_COMPA_vect)               // przerwanie timera 1
{
PORTD &= ~(1<<ServoAdress[ServoNr]); 
TCCR1B &= ~(1<<CS10);
ServoNr++;
}

int main(void)
{
char czas;               //zmienna używana do generowania sekwencji ruchu
int ServoAngle[6], TCP[] = {130,-15,-35};      //TCP - Tool Center Point - współ. zewnętrzne końcówki nogi (stopy) [x,y,z]; ServoAngle - zmienna pomocnicza
float q[6], m1, m2, m3, m4, m5;       //q - obliczane współ. wewnętrzne dla poszczególnych serw; reszta to zmienne pomocnicze przy obl. kinematyki odwrotnej

DDRD |= (1<<PD0) | (1<<PD1) | (1<<PD2) | (1<<PD3) | (1<<PD4) | (1<<PD5);	//wyjścia do sterowania serwami

DDRC &= ~(1<<PC0) & ~(1<<PC1);	//przyciski START i STOP
PORTC |= (1<<PC0) | (1<<PC1);

TCCR0 |= (1<<WGM01);	//Timer 0 -> CTC, prescaler 256
TCNT0 = 0;
OCR0 = 103;		//3,33 ms
TIMSK |= (1<<OCIE0);	//przerwania T0

TCCR1B |= (1<<WGM12);	//Timer 1 -> CTC, prescaler 1
TCNT1 = 0;
TIMSK |= (1<<OCIE1A);	//przerwania T1

sei();

while(1)
{
	if(!(PINC & 0x01))	//STOP
	{
		TCCR1B &= ~(1<<CS10);
		TCCR0 &= ~(1<<CS02);
		ServoNr = 0;
	}

	if(!(PINC & 0x02))	//START
	{
		TCCR0 |= (1<<CS02);
	}

	if(ServoNr > 5)
	{
		ServoNr = 0;
                       czas++;             //zwiększenie zmiennej czas
	}
//w tym miejscu chciałem wstawić sekwencje ruchu.
	//---------------------Kinematyka odwrotna-------------------- //obliczenia są tylko dla jednej nogi (serwa 1, 2, 3), dla drugiej nogi (serwa 4, 5, 6) w najbliższej przyszłości

	if(TCP[1] == 0){TCP[1] = 1;}

	q[0] = atan2(TCP[1],TCP[0]);

	m1 = (TCP[1] * pow(sin(q[0]),-1)) - 62.0;

	q[0] = q[0]  * Rad_Deg;                 // dla pierwszego serwa

	m2 = pow(m1,2) + pow(TCP[2],2);
	m3 = TCP[2] * pow(m2,-0.5);
	m4 = (m2-7500.0) * pow(m2,-0.5) * 0.01;
	m5 = (m2-12500.0) * 0.0001;

	q[1] = (asin(m3) + acos(m4)) * Rad_Deg;               //drugiego serwa

	q[2] = -acos(m5) * Rad_Deg; // trzeciego serwa

	if(q[0] > 45){q[0] = 45;}		//ograniczenia dla serw
	if(q[0] < -25){q[0] = -25;}

	if(q[1] > 90){q[1] = 90;}
	if(q[1] < -90){q[1] = -90;}

	if(q[2] > 5){q[2] = 5;}
	if(q[2] < -180){q[2] = -180;}

	//---------------------Kinematyka odwrotna--------------------

	if(ServoNr == 0) {ServoAngle[0] = 50 + q[0];}	//konwersja z q  na parametry zrozumiałe dla serw 1, 2, 3, q jest w stopniach
	if(ServoNr == 1) {ServoAngle[1] = 105 - q[1];}
	if(ServoNr == 2) {ServoAngle[2] = 30 - q[2];}

	if(ServoNr == 3) {ServoAngle[3] = 50 + q[3];}	//konwersja z q[st] na parametry zrozumiałe dla serw 4,5,6, na razie nie wykorzystywane
	if(ServoNr == 4) {ServoAngle[4] = 105 - q[4];}
	if(ServoNr == 5) {ServoAngle[5] = 30 - q[5];}

	ServoPosition = (172 * ServoAngle[ServoNr]) + 9000;           //obliczanie wartości dla OCR1A;
}
}

[mactro]

Serwa świrują, ale jeśli każesz robotowi przejść z jednej pozycji do drugiej, to osiągają w końcu zadane położenie, czy nie?

Nie wczytywałem się bardzo dokładnie w kod, ale pierwsze co mi przychodzi do głowy, to to, że warto by wpisywać nowe położenia dla wszystkich serw jednocześnie, a nie tak jak u Ciebie, liczysz dla jednego serwa, zadajesz mu położenie, liczysz dla drugiego... itd. Poza tym, czy jesteś pewien, że położenia, które zadajesz są osiągalne?

I jeszcze uwaga kosmetyczna: stosuj #define zamiast "magicznych liczb" w kodzie. Jak w ostatniej linii masz

ServoPosition = (172 * ServoAngle[ServoNr]) + 9000; 

to jedyne co mi przychodzi na myśl to: WTF? 😋

[AE]

Edytowałem kod w pierwszym poście, aby go ciągle nie kopiować. Opisy powinny rozjaśnić to i owo. Wartości dla serw są obliczane jednocześnie (w ramach jednej nogi - 3 serwa). Podawane jest TCP czyli x,y,z stopy, a kinematyka powinna obliczyć q w stopniach dla 1,2,3 serwa. Następnie q jest konwertowane na wartości zrozumiałem dla ATmegi. Czyli dla OCR1A, dlatego potrzebuje zmienne ServoAngle i ServoPosition. Pójdę za radą i jak już zacznę optymalizować kod to użyje #define.

Serwa świrują i nie osiągają zadanej wartości. Po prostu każde wychyla się jak mu się chcę i cały czas są w ruchu. W ogóle się nie zatrzymują. Można to porównać do kowboja siedzącego na byku 🙂 Cały czas rzuca się w różne strony.

Położenia są osiągalne dla nogi. Przykładowo: gdy wpiszę TCP[] = {130,-15,-35}; na samym początku przy deklaracji to noga osiąga te położenie bez najmniejszych problemów.

int main(void) 
{ 
   int TCP[] = {130,-15,-35};

Inny przykład: gdy zadeklaruje TCP[3] na początku funkcji main() i podam wartości x,y,z w miejscu gdzieś w kodzie (oczywiście przed obliczaniem kinematyki odwrotnej) to również nogą osiąga te położenie.

int main(void) 
{ 
   int TCP[3];

//kod programu

TCP[0]=130;
TCP[1]=-15;
TCP[2]=-30;

//obliczanie kinematyki odwrotnej

Problem się zaczyna, gdy podam "gdzieś" w programie osiągnij położenie A, a następnie położenie B. Użyje do tego zmiennej "czas", która jest edytowana po każdych 20ms (co 20ms czas++, zatem 20ms*50=1s). Instrukcje warunkowe 'if" są po to, aby wraz ze wzrostem wartości "czas" raz było osiągnięte położenie A, a później B, następnie znowu A itp.

int main(void) 
{ 
   int TCP[3];
   char czas;

//kod programu
if(czas <= 50) //osiągnij położenie A przez 1s
{
TCP[0]=130; //położenie A
TCP[1]=-15;
TCP[2]=-30;
}
if(czas > 50) //osiągnij położenie B przez 1s
{
TCP[0]=130; //położenie B
TCP[1]=0;
TCP[2]=-20;
}
if(czas >= 100){czas = 0;} //wracamy do położenia A
//obliczanie kinematyki odwrotnej

[airgrzybek]

Z jakiego algorytmu wyznaczania kinematyki odwrotnej skorzystałeś? Patrzę na Twój kod i nie potrafię nic z niego wywnioskować. Ja w swoim hexapodzie korzystałem z metody algebraicznej i dla 6ciu nóg działania to naprawdę szybko (mniej niż 20ms).

Błędy, które mogłeś popełnić to konwersja z radianów na stopnie w złych miejscach. Zauważ że atan2 również zwraca wartość w rad. Sprawdzenie kodu zaczął bym od tego. Drug sprawa to zakres ruchu nogi w przestrzeni zadaniowej i jej skala? W czym podajesz współrzędne? Metry? Milimetry? Poza tym mieszasz liczby całkowite i zmiennoprzecinkowe bez rzutowania. Czasem to może zadziałać nie tak jak chciałeś.

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[davidpi]

Przede wszystkim powiedz czy samo generowanie PWM działa dobrze?? Mówisz, że serwa wariuja więc nie wiadomo czy generowanie PWM jest OK.

Jeżeli tamto jest ok to napisz w kilku zdaniach jak działa ten algorytm kinematyki odwrotnej, bo patrzę na ten kod i nic nie rozumiem niestety 😋

[AE]

Obliczyłem to metodą geometryczną (tw. sinusów i cosinusów itp.). Wiem, że atan2 zwraca radiany. Sinus potrzebuje również radianów dlatego konwersje na stopnie odbywają się po obliczeniu już tych wartości. Z rzutowaniem również próbowałem, ale efekt ten sam. Opisany w poście wcześniej (jego mogłeś nie przeczytać, bo akurat w tym samym czasie pisaliśmy). Wartości TCP czyli x,y,z stopy podawane są w [mm]. Skala również moim zdaniem jest ok (znowu odsyłam do postu wcześniej.) PWM jest ok, bo dla "nie zmieniających" się współrzędnych stopy (TCP) zadane położenie jest osiągane bez najmniejszych problemów.

[airgrzybek]

A co jeśli wartości kątów będą niecałkowite? Jaki jest Twój sposób konwersji takich wartości do wartości podawanych na serwa? Jaka jest rozdzielczość PWM?

[AE]

Co do samej kinematyki odwrotnej.

Podaje współrzędne stopy (TCP) w [mm]. Jak już wspomniałem kinematyka jest wyznaczona metodą geometryczną. Obliczane są wartości współrzędnych wewnętrznych dla każdego serwa czyli q[0] dla 1. serwa, q[1] dla 2. serwa, q[2] dla 3. serwa. Najpierw obliczane jest q[0] za pomocą atan2. Następnie m1, gdzie jest wykorzystywane q[0] (jeszcze w radianach) do obliczenia sinusa. Konwersja q[0] na stopnie. Obliczanie zmiennych pomocniczych m2, m3, m4, m5, które są potrzebne do obliczenia q[1] i q[2]. Konwersja na q[1] i q[2] na stopnie. Następnie dla każdego serwa (ServoNr) jest obliczana ServoAngle, a później ServoPosition, które jest przypisywane do OCR1A.

[ Dodano: 17-09-2012, 18:58 ]

ServoAngle jest int`em, więc tutaj od razu wartości po przecinku są odcinane. Nawet jak korzystałem z rzutowania na int`a, zaokrąglania w góre ceil() i w dół floor() to nic nie dawało.

[ Dodano: 17-09-2012, 19:04 ]

Do programowego PWM korzystam Timera1 - CTC, preskaler 1. Położenie neutralne OCR1A = 24500, dla OCR1A = 9000 - max w lewo, dla OCR1A = 41000 - max w prawo

[davidpi]

Hmm nie do końca rozumiem tą Twoją kinematykę.

Przede wszystkim sprawdź czy wyliczane wartości sa poprawne. Wrzuć to w jakiegoś matlaba czy coś. Zrób kinematykę prostą i odwrotna dla takich samych wartości i zobacz czy się zgadza.

Natomiast co do algorytmu to mogę się nie znać, ale nie wiem jak ty to wyliczasz za pomocą atan??

Noga Twojego robota to manipulator o otwartym łańcuchu kinematycznym o 3 stopniach swobody.

Aby obliczyć kinematykę odwrotną dla takiego manipulatora trzeba rozwiązać układ trzech równań nieliniowych z trzema niewiadomymi. Czy Twój algorytm robi coś takiego??

Taki układ będzie miał trzy różne rozwiązania np:

q[1],q[2],q[3]

x[1],x[2],x[3]

w[1],w[2],w[3]

i trzeba wybrać to które jest fizycznie możliwe do zrealizowania.

[AE]

Sprawdziłem poprawność obliczeń. PWM również jest ok. Jak w kodzie podam tylko raz współrzędne x,y,z to wszystkie q są obliczane i stopa ustawia się dokładnie tam, gdzie powinna. Z kolei, gdy chcę, aby noga się poruszała (wykonała jakąś sekwencję ruchu) np. do przodu to podaję dwa pkt. w przestrzeni. Załóżmy, że pierwszy to x1,y1,z1, a drugi to x2,y2,z2. Wtedy w kodzie piszę, aby np. przez 1s stopa osiągnęła pkt.1, a 1s osiągnęła pkt.2, później z powrotem do pkt.1 i tak w kółko. Wtedy wszystko się sypie. Nie osiąga ani tego położenia ani drugiego. Noga lata we wszystkie strony w każdym możliwym kierunku.

Gdyby PWM nie działał prawidłowo to noga nie ustawiałaby się prawidłowo, gdy podaję tylko jeden pkt w przestrzeni. Tak samo z wzorami na kinematykę odwrotną - dla jednego pkt. działa, ale kilku już nie.

Jutro zamieszczę skany z wyznaczonymi wzorami dla kinematyki i rysunek samej nogi. Zamieszczę jeszcze filmy jak zachowuje się noga w obu przypadkach.

Pozdrawiam

[mactro]

Spróbuj, zmieniać położenie nogi np. po wciśnięciu przycisku, to wykluczymy problem ze zliczaniem czasu.

[AE]

Dodałem przycisk. Zauważyłem coś ciekawego. Poniżej zamieściłem tylko co najważniejsze. Zbędne rzeczy usunąłem, aby nie zaciemniać kodu.

int main(void)
{
int TCP[] = {130,15,-35};

//deklaracje we/wy, ustawianie timerów, przycisków itp.

while(1)
{
	//jakiś kod

	if(!(PINC & 0x04))	//położenie A
	{
	*******
	}

//reszta kodu

Jeśli w miejscu gwizdek (*******) zostawię to miejsce puste to wszystko gra, bo x,y,z stopy pobierane jest podczas deklarowania TCP, ale jeśli wpiszę tutaj np. TCP[1] = 1; już wszystko się sypie zanim jeszcze wcisnę przycisk. Dziwne... Przecież zanim wcisnę przycisk to teoretycznie to miejsce powinno być omijane. A tu zonk... A jak już wcisnę przycisk to i tak efekt jest ten sam.🙂

Poniżej skan samej nogi. Noga ma takie położenie, gdy q[0] = q[1] = q[2] = 0.

Tutaj jest wyprowadzenie wzorów na poszczególne kąty wychyleń serw (współrzędne wewnętrzne). Oczywiście owe kąty są zależne od x, y, z stopy.

W załączniku są filmy jak zachowuje się noga.

nok.avi

ok.avi

[airgrzybek]

Ja bym na Twoim miejscu zmienił kod na taki, w którym w przerwaniu ustawiana była jakaś flaga, a następnie w głównym kodzie gdy flaga będzie spełniona wpisujesz odpowiednie wartości do rejestrów timera. Mogę później napisać Ci jakiś przykładowy kod.

[mactro]

Twój przycisk zwiera do plusa, czy do masy? Jeśli do masy, to czy masz włączonego pull-up'a? Możesz też dać przed pętlą główną ze 100ms opóźnienia, bo w trakcie startu mikrokontrolera mogą się dziać rzeczy dziwne.

[davidpi]

Sprawdź czy dla różnych wartości TCP jest ok gdy podajesz tylko raz. Sprawdź też czy gdy wprowadzasz nową wartość TCP to czy przypadkiem któreś zmienne pomocnicze nie przechowują jeszcze starych wartości. Może trzeba je wyzerować przed rozpoczęciem kolejnych obliczeń.

Jak masz wyświetlacz to spróbuj po każdej kolejnej linijce kodu obliczeń wprowadzić dwu sekundowe opóźnienie i wyświetlić wyliczone wartości. Będziesz mógł skontrolować w którym momencie się sypie.

Jeżeli możesz to wrzuć filmy n YT bo tych jakoś nie mogę obejrzeć.