Atmega32: Aktueller Ausrichtung des Roboters falsch... Nur wieso ?

**Ritchie** · 28.01.2012, 19:34

Hallo Zusammen,

in meiner Motorsteuerung berechne ich den aktuellen Winkel des Roboters zur Positionsbestimmung.

Nur ändert sich der Winkel nach einer kurzen Fahrstrecke vollkommen, obwohl er geradeausfahren soll (also keine Winkeländerung). Der Roboter fährt einen Meter fast 100% geradeaus, hat aber eine massive Winkeländerung in seiner Berechnung.

Z.B. ist die Ausrichtung 3.14... beim Starten und nach einem Meter etwas über 4 rad.

Ich verwende folgende Formel und sehe den Fehler nicht:

nStepsLeft und nStepsRight sind die letzte Schrittänderungen seit des Letzen Aufruf des PID Controllers

Code:

void        CalculateOrientation(unsigned int nStepsLeft, unsigned int nStepsRight)
{
double     deltaOrientation; 

    if (m_DirectionFlagLeft == m_DirectionFlagRight )                            //Are we driving in front
        {
        if( nStepsLeft > nStepsRight)
            {
            deltaOrientation = -1.0 *(double) (nStepsLeft-nStepsRight) * RADPROIMPULS; // Calculation the change in the angle during the drive
            }
        else
            {
            deltaOrientation = (double) (nStepsRight-nStepsLeft) * RADPROIMPULS; // Calculation the change in the angle during the drive
            }
        }
    else
        {
        deltaOrientation =  ((( nStepsLeft  + nStepsRight  ) / 2.0) * RADPROIMPULS);// Add average calculation for the angle difference
        if(m_DirectionFlagLeft == 1 && m_DirectionFlagRight == 0)                // Are we turning to the right direction ?
            {
            deltaOrientation *= -1.0;                                            // negative angle
            }
        }

    m_ActualOrientation = AjustToTwoPI(m_ActualOrientation + deltaOrientation);    // Adjust the value to the full circle
}

Die Konstanten ergeben sich hierbei mit :

Code:

#define IMPULSPERFULLROTATION    ((double)250.0)       // Anzahl der Impulse vom Sensor für eine Radumdrehung
#define    RADRADIUSINMM            ((double)30.0)          // Radius des Antriebrads
#define WHEELBASE_INMM            ((double)155.0)         // Abstand der Kettenräder
#define    TWO_PI                    (M_PI * 2.0)
#define    WAYPROIMPULSINMM        ((TWO_PI * RADRADIUSINMM) / IMPULSPERFULLROTATION)    // Weg in MM für einen Impuls des Sensors
#define    WAYFULLROTATION_INMM    (TWO_PI * WHEELBASE_INMM)                                       // Weg für eine volle Drehung in MM
#define IMPULSEPERFULLROTATION    (WAYFULLROTATION_INMM / WAYPROIMPULSINMM)              //Anzahl der Impulse für eine volle Drehung
#define RADPROIMPULS            (TWO_PI / IMPULSEPERFULLROTATION)                                      // Winkeländerung in rad pro Impulse

Generell ist hier mein Ansatz, das wenn ich auf einer Kette einen Step mehr habe, als auf der anderen Kette, ich eine entsprechende Winkeländerung des Roboters bekomme, welche sich bezogen auf die Gesamtanzahl der Impulse für eine volle Umdrehung errechnet.

Der Weg für eine volle Umdehung wäre hierbei:

Code:

#define    WAYFULLROTATION_INMM    (TWO_PI * WHEELBASE_INMM)

Sind die Anzahl der Schritte auf der rechten Seiten geringer, wird eine negative Winkeländerung erkannt:
daher das "-1.0".

Code:

            deltaOrientation = -1.0 *(double) (nStepsLeft-nStepsRight) *  RADPROIMPULS; // Calculation the change in the angle during the drive

Entweder mache ich einen generellen Fehler oder ich sehe den Fehler nicht der in dem Programm steckt.

Gruss R.

**RP6conrad** · 28.01.2012, 20:46

Sie machen das fiel zu kompliziert !! .Es ist sehr einfach : die Winkelaenderung ist identisch an der Differenz von Linker und Rechter Anzahl pulsen. Die Absolute Distance ist unwichtig, ebenso die abgelegte Strecke !! Sie mussen dan auch nur diesen Differenz verwenden um ihre actuelle Winkel zu wissen. Dafur brauchen sie die Abstand zwischen beide Rader (oder Ketten) und die Auflosung ihre Encoder (mm/tic) Der Cartesiaanse Position da entgegen braucht eine Regelmassige Update und Berechnung. Eine Kettenantrieb hat eher eine Schlechte Odometrie Genauigkeit wegend den Schlupf.
Google mal nach Odometrie. Meine Odometrie Function (E-Compass ist die Differenz zwischen L/R Encoder,delta_distance ist Summe L/R encoder):

Code:

void odometrie (void){ 
static int16_t e_compass_old; //waarde e_compass bij vorige doorloop (encodertics)

delta_compass=e_compass - e_compass_old; //hoekverdraaing tov vorige doorloop (encodertics)

if((abs(delta_distance) > 200)||(abs(delta_compass) > 100)||(isMovementComplete()&(abs(delta_distance) >100)&(program>40))){ //alleen update indien voldoende verschil !!
int32_t delta_x = 0; //||((delta_distance>5)&isMovementComplete())
int32_t delta_y = 0;
double hoek= ((e_compass_old+e_compass)/2/E_COMPASS_RESOLUTION)*M_PI/180;//hoek omzetten naar radialen

delta_x = delta_distance*cos(hoek); //
delta_y = delta_distance*sin(hoek); // opgelet overflow vanaf delta_distance >125 !!
total_distance=total_distance+delta_distance; //totaal afgelegde baan-afstand
delta_distance=0; //resetten van delta_distance ; 
x_pos=x_pos+delta_x; //aktuele x_pos updaten *ENCODER_RESOLUTION/2
y_pos=y_pos+delta_y; //aktuele y-pos updaten *ENCODER_RESOLUTION/2
e_compass_old=e_compass;
if(print_data==1){
writeString("\n");
writeInteger((x_pos*ENCODER_RESOLUTION/2),DEC);writeString_P(" ");
writeInteger((y_pos*ENCODER_RESOLUTION/2),DEC);writeString_P(" ");
writeInteger((e_compass/E_COMPASS_RESOLUTION),DEC);writeString_P(" ");
writeInteger(state,DEC);writeString_P(" ");
}
}
}

**Ritchie** · 29.01.2012, 09:23

Hallo,

danke für Deine Antwort.

Ich denke aber auch, das in diesem Forum ein "du" besser angebracht ist, obwohl ich keine "16" mehr bin.

In meiner Routine will ich nur die Winkeländerung ermitteln und den absoluten Winkel im Raum. Hierzu ist muss ich den gefahrenen Kreisbogen, welcher sich durch die Differenz der gefahrenen Strecke ergibt, berechnen.

Da ich "unsigned int" als Stepzähler verwende, ist folgender Code notwendig für die Differenzberechnung:

Code:

        if( nStepsLeft > nStepsRight) 
            {
             deltaOrientation = -1.0 *(double) (nStepsLeft-nStepsRight) * RADPROIMPULS; // Calculation the change in the angle during the drive             }
         else
             {
             deltaOrientation = (double) (nStepsRight-nStepsLeft) * RADPROIMPULS; // Calculation the change in the angle during the drive
             }

Die Konstante gibt mir dann für eine Schrittdifferenz die Winkeldifferenz in Radiant an.

Das "Minuszeichen" muss eingerechnet werden, da ich mit "Unsigned" rechne und bei der Differenzberechnung die Richtung der Winkeländerung verloren habe. Da es sich ja um eine Winkeldifferenz handelt.

Die Konstante "RADPROIMPULS" ist hierbei eine Konstante, welche der Compiler ermittelt, anhand der mechanischen Anordnung.

Klar ist ja auch, das die gefahrene Strecke, bei einer Winkeländerung, größer wird, je größer der Radius ist (der Kreisbogen ist länger).

Google mal nach Odometrie. Meine Odometrie Function (E-Compass ist die Differenz zwischen L/R Encoder,delta_distance ist Summe L/R encoder):

Was bitte sollen dann diese Zeilen sein ? In meiner Routine wird nur die Winkeländerung berechnet.

Code:

(nStepsLeft-nStepsRight)

Beide Variablen haben bei Aufruf der Routine die Schrittänderung seit dem letzten Aufruf der "Encoderroutine", welche hier nicht aufgeführt ist, aber richtig arbeitet.

Die Zeilen addieren die Winkeländerung zur aktuellen Position:

m_ActualOrientation = AjustToTwoPI(m_ActualOrientation + deltaOrientation);

Die Funktion "AjustToTwoPI" achtet hierbei nur darauf, das der Winkel weiterhin im Bereich eines Kreises bleibt (also 2+PI).
Damit die sin/cos Funktionen weiterhin korrekt arbeiten.

Bei mir wird ja nur der Winkel falsch ermittelt, nicht die gefahrene Strecke/Weg zurückgerechnet.
Es wird die korrekt Position angefahren, welche er bei dem ersten Kommando bekommt.

Folgepositionen sind aber bedingt durch den falschen Winkel komplett falsch.

Gruss R.

**oberallgeier** · 29.01.2012, 10:33

Zitat von Ritchie

... Nur ändert sich der Winkel nach einer kurzen Fahrstrecke vollkommen, obwohl er geradeausfahren soll ...

Odometrie ist Koppelnavigation mit Bezug des aktuellen Schrittes auf die jeweils letzte Position. Schon dieser Satz weckt doch das Gefühl, dass sich Fehler aufsummieren könnten. Hatte RP6Conrad ja schon angedeutet. Dazu die Ketten - die KEINEN eindeutig definierten AuflagePUNKT haben für den abgearbeiteten Weg. Ich habe nicht über die Rechnung geguckt, denn ich würde den Fehler zu allererst im Schlupf der Kette suchen, evtl. test-/behelfsweise das Fahrzeug mal auf Noträdern fahren lassen und schauen ob dann der Fehler noch ebenso groß ist.

Aber eine Erkenntnis ist ja schon richtig: Alles scheint auf den ersten Blick so einfach. Ist es aber nicht.

**Ritchie** · 29.01.2012, 15:03

Ich habe nicht über die Rechnung geguckt, denn ich würde den Fehler zu allererst im Schlupf der Kette suchen,

Danke für diesen Hinweis.

"Ich habe das schon verstanden, wer Ketten verwendet ist selber schuld".

Gruss R.

**oberallgeier** · 29.01.2012, 15:51

Zitat von Ritchie

... "Ich habe das schon verstanden, wer Ketten verwendet ist selber schuld" ...

Ich bin sicher manchmal recht spitzzüngig - hier hatte ich das keinesfalls so gemeint. Wenns so rausgekommen ist, dann entschuldige das bitte.

Die Odometrie lebt eben davon, dass ein eindeutig definierbarer Wälzkörper sich auf einer eindeutig definierbaren Linie - und das möglichst schlupflos - über die befahrene Fläche bewegt. Und die befahrene Fläche sollte eine Ebene sein, sonst wirds richtig kompliziert. Bei der Kette ist eben die abwälzende Linie nicht eindeutig definierbar.

Möglicherweise gäbe es eine Lösung durch einen Nach-/Mitläufer, ein Rad, das - ähnlich wie ein Messrad bei Autotests - neben der Kette (z.B. "unsichtbar" innen neben jeder der beiden Ketten) angeordnet ist.

**Ritchie** · 29.01.2012, 16:10

Hallo,
danke für die Entschuldigung.

Ich Suche gerade weiter nach evtl. Fehler in der Mathe.

Klar ist mir auch, das Ketten nicht so optimal sind, wie es sich bei normalen Gummireifen zeigt.

Ich will nur derzeit die Mathefehler ausschliessen, bevor ich mit dem "Gyro" anfange, die Probleme einer Kette weiter einzuschrenken. Ich habe mich halt für eine Kettenfahrzeug entschieden und muss damit leben.

Gruss R.

**RP6conrad** · 29.01.2012, 17:25

Wird diese CalculateOrientation() nur einmal pro PID Durchlauf aufgerufen, oder mehrmal ? Wird "deltaOrientation" zuruck auf 0 gesetzt oder aufaddiert ? Werden nStepsLeft und nStepsRight jeden mal auf 0 gesetzt ? Ich sollte in diese Richtung suchen. Eigenlich soll ihre Winkelaenderung einfach "nStepsLeft - nStepsRight" sein, naturlich dan mit signed int gerechtnet. Mit unsigned mussen sie dan erst prufen welche die grosste Wert ist, und dan auch das differenz bilden. Da ist keine notwendigkeit um das jeden PID Durchlauf zu machen, diese Winkelaenderung ist theoretisch unabhangig von gefahrene Strecke und soll immer proportional sein mit das Differenz L/R Pulsen
Hass du kwadratur encoder, oder muss du die Drehrichtung schatzen an Hand von die ansteurung von H-Brucken ? Da kan naturlich auch etwas schief gehen. Es scheint muir einfacher nstepsRight und nStepsLeft signed zu definieren, und an Hand von der Ansteurung von die H-Brucke dan diese nSteps HOch oder Runter zahlen. Dan passt auch das Differenz immer.

**Ritchie** · 29.01.2012, 17:45

Hallo,

ich muss wohl nochmals die Hardware prüfen, ob mir der Sensor IC-MA (Hall Sensor), welchen ich hier das erste mal einsetze, nicht ein paar Streiche spielt.

http://www.ichaus.de/product/iC-MA

Hierbei sind folgende Fehlerquellen eine mögliche Quelle des Verhaltens:

- Schwankende Spanungsversorgung durch Motorlast während der fahrt könnte zu falschen Impulsen führen

- Magnetfelder erzeugt durch die Motoren (laut Datenblatt soll der Sensor das aber ausregeln, nur wer weiss ..), könnte auch falsche Impulsmeldungen erzeugen

Bis jetzt konnte ich keine Fehler in meiner Software finden! Die Berechnungen erscheinen mir korrekt.

@RP6conrad
ja, diese Routine wird nur einmalig aufgerufen, nachdem der PID Regler die Motorenansteuerung angepasst hat.
Die Ansteuerung und Auswertung der Richtung erfolgt anhand der Software. Daher die Auswertung von "m_DirectionFlagLeft == m_DirectionFlagRight". Sind beide Variablen gleich, erfolgt keine Rotation, sondern nur eine vorwärts oder rückwärtsfahrt.

"double deltaOrientation" ist nur in der Funktion "CalculateOrientation" gültig. Sie wird nur für eine Zwischenrechnung verwendet und addiert den ermittelten Wert auf die globale Variable "m_ActualOrientation". Diese Variable kann ich durch eine Visualisierung auf meinem Rechner während der Fahrt sichtbar machen. Hier sehe, das der Wert nicht korrekt ist.

Ich habe nicht die Möglichkeiten des Encoder ausgenutzt, das er mir die entsprechende Richtung angibt.
Ich gehe gehe davon aus, das wenn ich beide Motoren in der gleichen Richtung über das IC "vnh2sp30" ansteuere, das hier auch die korrekte Richtung durch den Motor erfolgt.

Gruss R.