Forum: Compiler & IDEs Inverse Kinematik auf dem ATMEGA8-16


von Heinz G. (das-gespenst)


Lesenswert?

hallo liebe avr-freaks ;)
ich habe hier mal in matlab eine kleine inverse kinematic für ein 
einzelnes 3dof bein zusammen genalgelt.  diese funktion soll jetzt auf 
einem atmega8 im takt von 20mSec ausgeführt werden.  dabei soll der 
quarz so langsam wie möglich bleiben.  die werte x, y und z bekommt er 
per i2c (float).  als ansteuerung der gelenke kommt entweder pwm 
(modellbau-servos) oder uart (rs485) in frage.

so leute, meine frage an euch ist, wie kann ich den code auf 
geschwindigkeit tunen?  da gibts sicher ein paar gurus unter euch die 
nix lieber machen, als den armen atmel mit float zahlen zu knechten ;)

danke schonmal, bin auf eure antworten gespannt :)

ps: die vektoroperationen werd ich schon selbst nachbilden...
für die, die villeicht verwirrt sind:
"%" ist das kommentar-zeichen in matlab, %{ %} macht nen block-kommentar 
wie /* */ in C
ein vektor (array) wird als vektorname[element1;element2;element3;] 
angelegt und so: "vektorname(1)" wird das erste element ausgewertet. 
(nicht null-indiziert!) z.B. bei p01 und p13 im Beispiel unten.

es folgt der matlab-code:

function [q1,q2,q3] = fcn(x,y,z,param)
%{
* inverse kinematics for one leg
* does not check valid workspace (acos(x) -> imaginary)
* does not conform to denavit hartenberg!
* standard right hand rule for xyz
* all variables are float values

            L01         L12          L23
        |<------->|<----------->|<---------->|

                  ^ z1          ^ z2         ^ z3
                  |             |            |
 -      ---------(*-->---------(*-->---------*-->
 ^      |        q2  x1        q3  x2           x3
 |      ^        K1            K2            K3
 |      - q1
 |      V
 |H01   |
 |      ^z0
 V   K0 |
------- *-->x0 ----------------------------------
/////////////////////////////////////////////////
%}

%% import parameters
H01 = param(1);
L01 = param(2);
L12 = param(3);
L23 = param(4);

%% do the IK
% q1
q1 = atan2(y,x);    % tan(x) function in AVR-GCC? atan2(y,x) ?

% q3
p01 = [L01*cos(q1); L01*sin(q1); H01;]; % cos(x) sin(x) in AVR-GCC?
p03 = [x; y; z;];
p13 = p03-p01;  % do this manually element-wise in C
p13sq = p13(1)^2+p13(2)^2+p13(3)^2; % use pow(x,2) or x*x ? AVR-GCC?

q3 = -acos((L12^2+L23^2-p13sq)/(2*L23*L12))+pi; % acos(x) in AVR-GCC?

% q2
b1 = atan2(p13(3),sqrt(p13(1)^2+p13(2)^2));
b2 = acos((L12^2+p13sq-L23^2)/(2*L12*sqrt(p13sq)));
q2 = -(b1+b2);
% EOF

von Heinz G. (das-gespenst)


Lesenswert?

EDIT:
ich hab net kapiert wie ich den beitrag noch editieren kann...

also ein vektor wird nicht
vektorname[element1;element2;element3;]
angelegt, sondern natürlich
vektorname=[element1;element2;element3;]

schö,
heinz

von Roland P. (pram)


Lesenswert?

ich weiß jetzt nicht genau was inverse Kinematik ist, aber mit Floats 
rechnet man meiner Meinung nach im AVR am besten in dem man sie nicht 
verwendet :-)

Also den Float möglichst bald in einen Integer/Long (fixed point) 
umwandeln und aufpassen dass es keine Overflows gibt

Einfaches Beispiel:

Fläche eines Kreises mit r = 2.5

A = 2.5*2.5*3.14 = 19.625

Nun mit fixed-point:
int r = 2.5*10
int pi = 3.14*100

A' = 25*25*314 = 196250 // Achtung bei 16bit Int
A = 196250 / 10000 = 19,625

Als Faktoren verwende ich aber dann meist 2er Potenzen, also

int r = 2.5 * 16 = 40
int pi = 3.14 *256 = 804

A'  = 40*40*804 = 1286400
A = 1286400 / 65536 = 19,629

Bei längeren Formeln steigt leider die Komplexität sehr schnell an und 
man muss sich an fast jeder Stelle überlegen, ob kein Overflow auftreten 
kann.
(dafür wirds denk ich um den Faktor 5-10 schneller  und du kannst dir 
ggf sogar die ganze FPU-Library sparen, so dass du mehr Flash hast.)

Für sin/cos kann man je nach Genauigkeit auch Tabellen verwenden.

Gruß
Roland

von Karl H. (kbuchegg)


Lesenswert?

> dabei soll der quarz so langsam wie möglich bleiben.

Warum? Stromsparen?
Vergiss es, das bischen Strom, das du damit sparen kannst, brauchen die 
Servos locker in 2/10 Sekunden auf.

sin und cos, asin, acos könnte man durch Tabellen ersetzen. Eine 
Auflösung von 1° sollte eigentlich reichen. Würde mich wundern, wenn die 
Servos genauer sind.

Ansonsten: Ja, Fixedpoint ist natürlich immer eine Überlegung wert. 
Allerdings dürfte das bei deinen Berechnungen nicht so einfach sein. Das 
Problem besteht einfach darin, dass du Overflows mitten in den 
Berechnungen berücksichtigen musst. Und mit dem ganzen drum und dran 
wird das dann letztendlich ein unübersichtlicher Wust an Code.

Hast du schon ausprobiert, ob du mit float hinkommen wirst? Das würd ich 
mal als erstes ausprobieren.

> p13sq = p13(1)^2+p13(2)^2+p13(3)^2; % use pow(x,2) or x*x ? AVR-GCC?
pow ist eine eierlegende Wollmilchsau. Gut möglich, dass in der Library 
der pow über logarithmen gerechnet wird. x*x ist allemal schneller.

von yalu (Gast)


Lesenswert?

Eine ganz, ganz grobe Abschätzung, ob so etwas gehen kann:

Ich habe etwa 24 einfache (+, -, *), 4 mittelschwere (/, sqrt) und 5
schwere Operationen (Winkelfunktionen) gezählt. Wenn man annimmt, dass
eine mittelschwere Operation 8 und eine schwere 20 einfachen entspricht,
kommt man auf etwa 156 einfache Operationen. Bei 16MHz entsprechen 20ms
320000 Zyklen, so dass für eine einfache Operation etwa 2000 Zyklen
bleiben, was eigentlich locker reichen sollte. Ich würde also einfach
mal einen Test ohne großartige Optimierung mit Tabelle u.ä. wagen.

Was die reduzierte Taktfrequenz betrifft, schließe ich mich dem von Karl
Heinz Geschriebenen an und füge folgendes hinzu: Lass den Controller
lieber mit der vollen Geschwindigkeit laufen und lege ihn, falls die
Rechnung in weniger als 20ms ausgeführt wird, für den Rest der Zeit
schlafen. Damit erreichst du einen ähnlich niedrigen Stromverbrauch wie
mit einer geeignet reduzierten Taktfrequenz, hast aber genügend
Rechenzeitreserven übrig für den Fall, dass die Rechnung doch einmal
etwas länger dauert.

von ichbins (Gast)


Lesenswert?

Heinz,

wolltest Du nicht Urlaub machen?????? :-)

Marcus Gpunkt

von Kai G. (runtimeterror)


Lesenswert?

Ich kann zwar kein Matlab, aber das hier sieht irgendwie unsinnig aus:

q1 = atan2(y,x);
p01 = [L01*cos(q1); L01*sin(q1); H01;];

Sollte das nicht dasselbe sein wie (Pseudocode)
p01 = q1 / |q1| * L01 (und dann noch H01 hinten dran)
mit |q1| == Länge des Vektors

Hoffe, irgendwer versteht, was ich meine ;)

Das sollte nochmal einiges an Rechenzeit einsparen.

EDIT: Ich sehe gerade, dass q1 auch nach außen gereicht wird - kann 
sein, dass mein Vorschlag dann keinen Sinn macht.

von Heinz G. (das-gespenst)


Lesenswert?

tach leute,
also erstmal herzlichen dank für die guten ernstgemeinten antworten.

@ yalu: damit machst du mir mut!  das ist mal eine interessante 
abschätzung.

ich werde in der tat einfach mal die funktion in C umsetzen und mal 
schauen wo ich zeitlich lande.  um bei der kommunikation noch chansen zu 
haben werde ich wohl mit dem 14.7456MHz oder dem 18.4320MHz quarz 
arbeiten.  Hat da jemand erfahrung mit quarzen größer 16MHz?  gibts da 
gefahren was die stabilität angeht?

@ Marcus: solltest du nicht am arbeiten sein?  jetzt hab ich schon 
urlaub und werd dich trotzdem net los ;)

@ Kai: q1 ist ein skalar der so gebraucht wird (gelenkwinkel erste 
achse), muss also stehen bleiben. p01 ist ein vektor mit drei elementen 
(zeigt von dem K0-koordinatensystem zum K1-koordinatensystem); was du da 
vorschlägst würde ja einen skalar draus machen.

danke nochmal an alle,
gruss,
heinz

von Simon K. (simon) Benutzerseite


Lesenswert?

Heinz Geist wrote:
> ich werde in der tat einfach mal die funktion in C umsetzen und mal
> schauen wo ich zeitlich lande.  um bei der kommunikation noch chansen zu
> haben werde ich wohl mit dem 14.7456MHz oder dem 18.4320MHz quarz
> arbeiten.  Hat da jemand erfahrung mit quarzen größer 16MHz?  gibts da
> gefahren was die stabilität angeht?

Statt Mega8 nimmste einfach einen Mega88, Mega168 oder Mega328. 
Pinkompatibel und bis 20MHz bei 5V.

von Karl H. (kbuchegg)


Lesenswert?

Heinz Geist wrote:

> @ Kai: q1 ist ein skalar der so gebraucht wird (gelenkwinkel erste
> achse), muss also stehen bleiben. p01 ist ein vektor mit drei elementen
> (zeigt von dem K0-koordinatensystem zum K1-koordinatensystem); was du da
> vorschlägst würde ja einen skalar draus machen.

Aber so unrecht hat er nicht, denke ich mal.

Wenn du aus x und y einen Vektor bildest, den auf Einheitslänge 
normierst und mit L01 multiplizierst, müsste das eigentlich die 
Komponenten L01*cos(q1) bzw L01*sin(q1) ergeben.

Zum Normieren brauchst du zwei Quadrate und eine Wurzel. Und das dürfte 
in Summe schneller gehen als cos und sin.

Also in Matlab Syntax (ist das erste mal das ich sowas mache, also haut 
mich nicht, wenn ich was falsch mache)

q1 = atan2(y,x);    % tan(x) function in AVR-GCC? atan2(y,x) ?
p011 = [x;y]
p011sq = p01(1)^2+p01(2)^2;
p011 = p011/p011sq*L01

% q3
p01 = [p011(1); p011(2); H01;]; % cos(x) sin(x) in AVR-GCC?

von Kai G. (runtimeterror)


Lesenswert?

Danke für die Übersetzung :)

>p011sq = p01(1)^2+p01(2)^2;
>p011 = p011/p011sq*L01
Da fehlt noch die von dir erwähnte Wurzel ;)

Ich weiß halt nicht, was wirklich schneller ist - eine Wurzel oder 
einmal sin_cos - hängt stark von der Implementierung ab. Als ich den 
Post angefangen hatte, hatte ich noch die Hoffnung, dass das atan2 
rausfliegen kann.

Bitte melde dich an um einen Beitrag zu schreiben. Anmeldung ist kostenlos und dauert nur eine Minute.
Bestehender Account
Schon ein Account bei Google/GoogleMail? Keine Anmeldung erforderlich!
Mit Google-Account einloggen
Noch kein Account? Hier anmelden.