Neigungsmessung

Die Berechnung eines Winkels mit hoher Auflösung und Genauigkeit sollte auf einem PC stattfinden. Die Genauigkeit der verwendeten Sensoren erlauben eine Genauigkeit von < 0,1°.

Da die Winkelwerte aus den Beschleunigungswerten abgeleitet werden und diese mit einem bestimmten Rauschen beaufschlagt sind, muss der Winkelwert mittels geeigneter Algorithmen gefiltert werden.

Dies kann im einfachsten Fall z. B. ein gleitender Mittelwert sein.

Neigungsmessung 1:Abb.6: Winkelmessung, Prozessdaten als Beschleunigungswerte, Berechnung im PC
Neigungsmessung 2:Abb.7: Detaildarstellung Signalrauschen

Farbe

Bedeutung

Rot

Winkel gemessen mit 1024-Schritt-Encoder / 4-fach Auswertung als Referenz

Grün

trigonmetrisch im PC errechneter Winkel, ohne Rauschunterdrückung

blau

schneller Algorithmus

gelb

arithmetisches Mittel (1000 Werte gleitend)

Beispielprogramm 1: Berechnung von Roll und Pitch

Dieses Beispielprogramm zeigt die Berechnung der Lagewinkel Roll und Pitch auf Basis der Beschleunigungs-Messwerte.

PROGRAM MAIN

VAR
    (* Acceleration values *)
    fX_11 AT %I* : INT;
    fY_11 AT %I* : INT;
    fZ_11 AT %I* : INT;
    pi: LREAL;

    fRoll : LREAL; (* Angle about X-axis *)
    fPitch : LREAL; (* Angle about Y-axis *)
END_VAR
pi:=3.14159265;

IF fX_11 <> 0 AND fZ_11 <> 0 THEN
    fRoll := ATAN(fY_11/SQRT(EXPT(fX_11,2)+EXPT(fZ_11,2)));
    IF fRoll < 0 THEN
        fRoll := fRoll + 2*pi;
    END_IF
    fRoll := fRoll*180/pi;
END_IF

IF fY_11 <> 0 AND fZ_11 <> 0 THEN
    fPitch := ATAN(fX_11/SQRT(EXPT(fY_11,2)+EXPT(fZ_11,2)));
    IF fPitch < 0 THEN
        fPitch := fPitch + 2*pi;
    END_IF
    fPitch := fPitch*180/pi;
END_IF

Beispielprogramm 2: Winkelberechnung zwischen zwei Messzuständen

Dieses Beispielprogramm zeigt die Berechnung des Winkels zwischen dem aktuellen Beschleunigungsvektor und einem Referenz-Beschleunigungsvektor.

PROGRAM MAIN

VAR
    (* Acceleration values *)
    fX_11 AT %I* : INT;
    fY_11 AT %I* : INT;
    fZ_11 AT %I* : INT;

    fAbsLast: LREAL;
    bStoreInitial: BOOL;
    fAbsInit: LREAL;
    pi: LREAL;
    fXInit: LREAL;
    fYInit: LREAL;
    fZInit: LREAL;

    fAlpha: LREAL;
END_VAR
pi:=3.14159265;

IF bStoreInitial THEN
    fXInit := fX_11;
    fYInit := fY_11;
    fZInit := fZ_11;
    bStoreInitial := FALSE;
END_IF

fAbsInit := SQRT(EXPT(fXInit,2)+EXPT(fYInit,2)+EXPT(fZInit,2));
fAbsLast := SQRT(EXPT(fX_11,2)+EXPT(fY_11,2)+EXPT(fZ_11,2));

IF fAbsInit <> 0 AND fAbsLast <> 0 THEN
    fAlpha := ACOS((fXInit*fX_11 + fYInit*fY_11 + fZInit*fZ_11)/(fAbsInit*fAbsLast))*180/pi;
END_IF

Beispiel

Neigungsmessung 3:

Formel zur Berechnung des Winkels α:

Neigungsmessung 4:

Umsetzung in TwinCAT:

alpha := ATAN(a_y1 / (SQRT(a_x1 * a_x1 + a_z1 * a_z1))) * 360/(2*3.14);