kramann.info
© Guido Kramann

Login: Passwort:










Robuste Systemintegration
1 Grundlagen
..1.1 Newton
....1.1.1 LinearSchwinger
....1.1.2 Daempfung
....1.1.4 ODE
....1.1.5 Saaluebung
..1.2 NewtonEuler
....1.2.1 Traegheitsmomente
....1.2.2 Modellgleichungen
....1.2.3 Einfachpendel
..1.3 Scilab
....1.3.1 Erste_Schritte
....1.3.2 Skripte
....1.3.3 Funktionen
..1.4 Laplace
....1.4.1 Eigenwerte
....1.4.2 PT1
..1.5 Regleroptimierung
....1.5.1 Guetefunktion
....1.5.2 Heuristiken
....1.5.3 Scilab
..1.6 Einstellregeln
....1.6.1 Totzeit
....1.6.2 Methode1
....1.6.3 Methode2
....1.6.4 Scilab
..1.7 Zustandsregler
..1.8 Polvorgabe
..1.8 Polvorgabe_alt
..1.9 Beobachter
....1.9.1 Haengependel
..1.10 Daempfungsgrad
..1.11 Processing
....1.11.1 Installation
....1.11.2 Erste_Schritte
....1.11.3 Mechatronik
....1.11.4 Bibliotheken
....1.11.5 Uebung
....1.11.6 Snippets
......1.11.6.1 Dateioperationen
......1.11.6.2 Bilder
......1.11.6.3 GUI
......1.11.6.4 Text
......1.11.6.5 PDF
......1.11.6.8 Maus
......1.11.6.10 Zeit
......1.11.6.13 Animation
......1.11.6.15 Simulation
....1.11.7 Referenzen
..1.12 Breakout
2 Beispiel
3 Beispielloesung
4 Praxis
5 javasci
6 Fehlertoleranz1
7 Reglerentwurf
..7.1 Sprungantwort
..7.2 Messdaten
..7.3 Systemidentifikation
..7.4 Polvorgabe
..7.5 Beobachter
..7.6 Robuster_Entwurf
..7.7 SIL
8 Systementwicklung
9 Arduino
..9.1 Lauflicht
..9.2 Taster
..9.3 Sensor
..9.12 Motor_PWM1
..9.13 Motor_PWM2_seriell
..9.14 Motor_PWM3_analogWrite
..9.15 Scheduler
..9.20 AV
..9.21 Mikrofon
..9.22 Universal
....9.22.1 Laborplatine
....9.22.2 LED_Leiste
....9.22.3 Motortreiber
....9.22.4 Sensoreingaenge
....9.22.5 Taster
....9.22.6 Tests
....9.22.7 Mikrofon
....9.22.8 Lautsprecher
....9.22.9 Fahrgestell
..9.23 Zauberkiste
..9.24 OOP
....9.24.1 Uebungen
..9.25 AVneu
....9.25.1 Tests
..9.26 DA_Wandler
..9.27 CompBoard
....9.27.1 Tastenmatrix
....9.27.2 ASCIIDisplay
..9.28 CTC
..9.29 Tonerzeugung
10 EvoFuzzy
..10.1 Fuzzy
....10.1.1 Fuzzylogik
....10.1.2 FuzzyRegler
....10.1.3 Uebung9
....10.1.5 Softwareentwicklung
......10.1.5.1 AgileSoftwareentwicklung
......10.1.5.2 FuzzyRegler
......10.1.5.3 Uebung
....10.1.6 Umsetzung
......10.1.6.1 FuzzyRegler
......10.1.6.2 Simulation
......10.1.6.3 Optimierung
......10.1.6.4 Uebung
....10.1.7 Haengependel
......10.1.7.1 Haengependel
......10.1.7.2 Simulation
......10.1.7.3 FuzzyRegler
......10.1.7.4 Optimierer
......10.1.7.5 Genetisch
....10.1.8 Information
....10.1.9 Energie
..10.2 Optimierung
....10.2.1 Gradientenverfahren
....10.2.2 Heuristiken
....10.2.3 ModifizierteG
....10.2.4 optim
..10.3 Genalgorithmus
..10.4 NeuronaleNetze
....10.4.1 Neuron
....10.4.2 Backpropagation
....10.4.3 Umsetzung
....10.4.4 Winkelerkennung
..10.5 RiccatiRegler
11 Agentensysteme
12 Simulation
20 Massnahmen
21 Kalmanfilter
..21.1 Vorarbeit
..21.2 Minimalversion
..21.3 Beispiel
30 Dreirad
31 Gleiter
..31.1 Fehlertoleranz
80 Vorlesung_2014_10_01
81 Vorlesung_2014_10_08
82 Vorlesung_2014_10_15
83 Vorlesung_2014_10_22
84 Vorlesung_2014_10_29
kramann.info
© Guido Kramann

Login: Passwort:




Modellbildung und Simulation eines Autonomen Vehikels in Form eines Dreirads

(EN google-translate)

(PL google-translate)

dreirad.zip - alle nachfolgend aufgeführten Scilab-Skripte.
HINWEIS: Die eingefügten Formeln werden durch ein Javascript-Plugin gerendert. Dies funktioniert wahrscheinlich nur mit einem Firefox-Webbrowser.

Im weiteren Verlauf der Vorlesung sollen auch Fehlertolerante Programmiertechniken erlernt werden.

Als Anwendungsfall, der weiterhin auch mit dem Thema der robusten Regelung verlinkt bleibt, soll ein Autonomes Vehikel dienen.

Dieses soll allerdings nur als möglichst einfaches Simulationsmodell bereit gestellt werden.

Als Form des AV wird von einem Dreirad ausgegangen, das seinen Antrieb und seine Lenkung im Vorderrad hat und bei dem vorausgesetzt wird, dass die Räder keinen Schlupf haben, also ideal abrollen. Nachfolgend ist ein physikalisches Beispiel eines solchen Systems zu sehen:

Beispiel eines kleinen, Mikrocontroller geregelten Autonomen Vehikels, das als Dreirad ausgeführt ist.

Bild 0-1: Beispiel eines kleinen, Mikrocontroller geregelten Autonomen Vehikels, das als Dreirad ausgeführt ist.

Vorüberlegungen

Die Räder werden nicht mit modelliert. Lediglich die Zwangsbedingungen, die sich aus den Radeigenschaften ergeben werden mit berücksichtigt.

Außerdem können die Räder der Hinterachse zu einem zentralen Rad zusammengefaßt werden.

Schwerpunkt des Vehikels ist der Punkt S, Mittelpunkt der Hinterachse H, Mittelpunkt des vorderen Lenk- und Antriebsrades sei V.

Der Abstand zwischen Schwerpunkt S und Hinterachspunkt H sei q.

Der Abstand zwischen Schwerpunkt S und Vorderachspunkt V sei r.

Das Chassis Fahrzeug wird als homogener Quader angenähert, Masseträgheitsmoment J und Masse m.

Aufgrund des Lenkwinkels β ergibt sich ein bestimmter Momentanpol P.

Der Übergang zu β=0 rad und +/-π/2 rad muß vermieden, oder gesondert behandelt werden.

Die Lage des Fahrzeugs in der Ebene ist bestimmt durch seine absolute Verdrehung φ und den Schwerpunktskoordinaten [x,y], bzw. [xS,yS].

Abstraktion der Geometrie des AVs.

Bild 0-2: Abstraktion der Geometrie des AVs.

Die Modellierung geschieht in Scilab in mehreren Stufen. Die ersten vier sind:

  1. Modell passend zu: Fahrzeug fährt mit festem Lenkwinkel und konstanter Antriebskraft FA im Kreis.
  2. Modell passend zu: Wie erstens, aber mit Dämpfungsanteil (s.u.) und damit kombinierter Antriebskraft FAD.
  3. Modell passend zu: Lenkwinkel kann mit bestimmtem Übertragungsverhalten vorgegeben werden.
  4. Modell passend zu: Fahrzeug wird auf Kreisbahn geregelt.
  5. Modell passend zu: Fahrzeug erledigt irgend eine mehrstufige Aufgabe und wird Störungen ausgesetzt.
  6. Modell passend zu: Interaktiver Vehikelschwarm.

Dynamik

Senkrecht zur Rollrichtung vermeiden die Räder ein Ausbrechen des Fahrzeugs. Die dabei entstehenden Zwangskräfte für Hinter- und Vorderachse werden mit FH und FV bezeichnet.

Außerdem ist das Vorderrad mit einem Antrieb versehen. Dies resultiert in einer Antriebskraft FA, welche immer in die Lenkrichtung wirkt.

Rollreibung uns sonstiger Fahrwiderstand werden in einer Dämpfungskraft zusammengefaßt, die stets in die entgegengesetzte Richtung wie die Geschwindigkeitsrichtung des Antriebsrades wirkt. Diese Dämpfungskraft FD kann mit der Antribeskraft FA zu der kombinierten Kraft FAD zusammengefasst werden, weil beide die gleiche Richtung haben. In der ersten Ausbaustufe des Modells wird die Dämpfung vernachlässigt.

Auf das Fahrzeug wirkende Kräfte.

Bild 0-3: Auf das Fahrzeug wirkende Kräfte.

Die beiden Newton- und die Eulergleichung für das Fahrzeug in der Ebene ergeben sich dann wie folgt:

$ m \ddot x = F_{Hx} + F_{Vx} + F_{Ax} $

Formel 0-1: Newton-Gleichung in x-Richtung.


$ m \ddot y = F_{Hy} + F_{Vy} + F_{Ay} $

Formel 0-2: Newton-Gleichung in y-Richtung.


$ J \ddot \phi = \vec r_{SH} x \vec F_H + \vec r_{SV} x \left( \vec F_V + \vec F_A \right) $

Formel 0-3: Euler-Gleichung \left(x soll Kreuzprodukt bezeichnen\right).


Die Kräfte FH und FV sind zuächst unbekannt. Dagegen wird FA als gegeben vorausgesetzt.

Die Wirkrichtungen der Kräfte hängen direkt von φ und β ab. Darum kann jede der Kräfte auf ein Skalar reduziert werden, das mit einem Richtungsvektor multipliziert wird:

$ \vec F_H = F_H \cdot \left[\begin{array}{cc}- \sin \left( \phi \right) \\ \cos \left( \phi \right)\end{array}\right] $

Formel 0-4: Berücksichtigung der Richtung von F<sub>H</sub>.


$ \vec F_V = F_V \cdot \left[\begin{array}{cc}- \sin \left( \phi + \beta \right) \\ \cos \left( \phi + \beta \right)\end{array}\right] $

Formel 0-5: Berücksichtigung der Richtung von F<sub>V</sub>.


$ \vec F_A = F_A \cdot \left[\begin{array}{cc} \cos \left( \phi + \beta \right) \\ \sin \left( \phi + \beta \right)\end{array}\right] $

Formel 0-6: Berücksichtigung der Richtung von F<sub>A</sub>.


Zwangsbedingungen und die Bestimmung der Zwangskräfte

Das System dreht sich um den Momentanpol mit der Winkelgeschwindigkeit ω.

Diese Winkelgeschwindigkeit ist identisch mit der Winkelgeschwindigkeit des Schwerpunktes (und jedes anderen Punktes auf dem starren Körper, den das Fahrzeug darstellt).

Allgemein gilt folgender Zusammenhang zwischen der aktuellen Geschwindigkeit v in einem Punkt Q eines Körpers und der Winkelgeschwindigkeit ω der Drehung um das Zentrum Z (Momentanpol):

$ \vec v = \vec \omega x \vec r_{ZQ} $

Formel 0-7: Zusammenhang zwischen der aktuellen Geschwindigkeit in einem Punkt und der Winkelgeschwindigkeit.


Zusammenhang zwischen der aktuellen Geschwindigkeit in einem Punkt und der Winkelgeschwindigkeit.

Bild 0-4: Zusammenhang zwischen der aktuellen Geschwindigkeit in einem Punkt und der Winkelgeschwindigkeit.

Ausgeführt ergibt sich in der Ebene:

$ \left[\begin{array}{cc}v_x \\ v_y \\ 0\end{array}\right] = \left[\begin{array}{cc}0 \\ 0 \\ \omega \end{array}\right] x \left[\begin{array}{cc}r_x \\ r_y \\ 0\end{array}\right] = \left[\begin{array}{cc}- \omega r_y \\ \omega r_x \\ 0\end{array}\right] $

Formel 0-8: Geschwindigkeit aus Winkelgeschwindigkeit und Momen \tan pol.


Herleitung der Zweangsbedingungen

Für den Punkt H gilt entsprechend:

$ \left[\begin{array}{cc}\dot x_H \\ \dot y_H \\ 0\end{array}\right] = \left[\begin{array}{cc}0 \\ 0 \\ \omega \end{array}\right] x \left[\begin{array}{cc}r_{PHx} \\ r_{PHy} \\ 0\end{array}\right] = \left[\begin{array}{cc}- \omega \cdot \left(y_H-y_P\right) \\ \omega \cdot \left(x_H-x_P\right) \\ 0\end{array}\right] $

Formel 0-9: Geschwindigkeit aus Winkelgeschwindigkeit und Momen \tan pol im Punkt H.


Ort und Geschwindigkeit im Punkt H können durch die Minimalkoordinaten des Systems ausgedrückt werden:

$ \left[\begin{array}{cc}x_H \\ y_H\end{array}\right] = \left[\begin{array}{cc}x_S \\ y_S\end{array}\right]+q \cdot \left[\begin{array}{cc}- \cos \left( \phi \right) \\ - \sin \left( \phi \right)\end{array}\right] $

Formel 0-10: Zusammenhang zwischen H und den Minimalkoordinaten.


$ \left[\begin{array}{cc}\dot x_H \\ \dot y_H\end{array}\right] = \left[\begin{array}{cc}\dot x_S \\ \dot y_S\end{array}\right]+q \cdot \left[\begin{array}{cc}\dot \phi \cdot \sin \left( \phi \right) \\ -\dot \phi \cdot \cos \left( \phi \right)\end{array}\right] $

Formel 0-11: Zusammenhang zwischen der Geschwindigkeit in H und den Minimalkoordinaten.


Oben eingesetzt ergibt (ω ist auf jedem Punkt des Körpers (Chassis) gleich und entspricht deshalb dφ/dt):

$ \left[\begin{array}{cc}\dot x_S \\ \dot y_S\end{array}\right]+q \cdot \left[\begin{array}{cc}\dot \phi \cdot \sin \left( \phi \right) \\ -\dot \phi \cdot \cos \left( \phi \right)\end{array}\right] = \left[\begin{array}{cc}-\dot \phi \cdot \left(y_S-q \cdot \sin \left( \phi \right)-y_P\right) \\ \dot \phi \cdot \left(x_S-q \cdot \cos \left( \phi \right)-x_P\right) \\ 0\end{array}\right] $

Formel 0-12: Geschwindigkeit aus Winkelgeschwindigkeit und Momen \tan pol im Punkt H in Minimalkoordinaten dargestellt.


...ist äquivalent zu (und hätte für S auch gleich so hingeschrieben werden können):

$ \dot x_S = - \dot \phi \cdot \left(y_S-y_P\right) $

Formel 0-13: Geschwindigkeit aus Winkelgeschwindigkeit und Momen \tan pol im Punkt H in Minimalkoordinaten dargestellt.


$ \dot y_S = \dot \phi \cdot \left(x_S-x_P\right) $

Formel 0-14: Geschwindigkeit aus Winkelgeschwindigkeit und Momen \tan pol im Punkt H in Minimalkoordinaten dargestellt.


Zur Bestimmung der Zwangskräfte kann die zeitliche Ableitung dieser Beziehungen gebildet und zunächst genutzt werden, um in den Newton-Gleichungen die translatorischen Beschleunigungen durch Terme mit der Winkelbeschleunigung zu ersetzen. Da sich auch die Punkte S und P zeitlich ändern, müssen diese auch mit nach der Zeit abgeleitet werden:

$ \ddot x_S = - \ddot \phi \cdot \left(y_S-y_P\right) - \dot \phi \cdot \left(\dot y_S- \dot y_P\right) $

Formel 0-15: Zeitliche Ableitung der ersten Beziehung.


$ \ddot y_S = \ddot \phi \cdot \left(x_S-x_P\right) + \dot \phi \cdot \left(\dot x_S- \dot x_P\right) $

Formel 0-16: Zeitliche Ableitung der zweiten Beziehung.


Danach ist es noch notwendig die Winkelbeschleunigung in beiden Gleichungen durch die rechte Seite der Euler-Gleichung zu ersetzen und man erhält so ein LGS zur Bestimmung von FH und FV.

Doch zunächst muß noch die Position des Momentanpols berechnet werden:

Dieser läßt sich als Schnittpunkt der Geraden h: entlang der Wirkrichtung der Kraft FH und der Geraden v: entlang der Wirkrichtung der Kraft FV bestimmen:

$ h: \left[\begin{array}{cc}x_h \\ y_h\end{array}\right] = \left[\begin{array}{cc}x_S \\ y_S\end{array}\right] + q \cdot \left[\begin{array}{cc}- \cos \left( \phi \right) \\ - \sin \left( \phi \right)\end{array}\right] + a \cdot \left[\begin{array}{cc}- \sin \left( \phi \right) \\ \cos \left( \phi \right)\end{array}\right] $

Formel 0-17: Gerade h: a ist variabel & der Rest ist zu jedem Integrationszeitpunkt bekannt.


$ v: \left[\begin{array}{cc}x_v \\ y_v\end{array}\right] = \left[\begin{array}{cc}x_S \\ y_S\end{array}\right] + r \cdot \left[\begin{array}{cc} \cos \left( \phi \right) \\ \sin \left( \phi \right)\end{array}\right] + b \cdot \left[\begin{array}{cc}- \sin \left( \phi + \beta \right) \\ \cos \left( \phi + \beta \right)\end{array}\right] $

Formel 0-18: Gerade v: b ist variabel & der Rest ist zu jedem Integrationszeitpunkt bekannt.


Durch Gleichsetzen beider Geraden läßt sich ein LGS zur Bestimmung von a und b herstellen und damit zur Bestimmung des Momentanpols P. Es ergibt sich:

$ b = \frac {r+q}{ \sin \left( \phi + \beta \right) \cdot \cos \left( \phi \right)- \cos \left( \phi + \beta \right) \cdot \sin \left( \phi \right)} $

Formel 0-19: b bei P.


$ \dot b = - \left(r+q\right) \cdot \left( \sin \left( \phi + \beta \right) \cdot \cos \left( \phi \right)- \cos \left( \phi + \beta \right) \cdot \sin \left( \phi \right)\right)^{-2} \cdot \left(QQ+RR\right) $

Formel 0-20: Zeitliche Ableitung von b - weiter unten benötigt.


$ QQ = \left(\dot \phi + \dot \beta \right) \cdot \cos \left( \phi + \beta \right) \cdot \cos \left( \phi \right)-\dot \phi \cdot \sin \left( \phi + \beta \right) \cdot \sin \left( \phi \right) $

Formel 0-21: Fortsetzung zeitliche Ableitung von b.


$ RR = \left(\dot \phi + \dot \beta \right) \cdot \sin \left( \phi + \beta \right) \cdot \sin \left( \phi \right) - \dot \phi \cdot \cos \left( \phi + \beta \right) \cdot \cos \left( \phi \right) $

Formel 0-22: Fortsetzung zeitliche Ableitung von b.


Dies kann in folgende Bestimmung der Ableitung von P eingesetzt werden:

$ \dot x_P = \dot x_S - r \cdot \dot \phi \cdot \sin \left( \phi \right) - \dot b \cdot \sin \left( \phi + \beta \right) - b \cdot \left(\dot \phi + \dot \beta \right) \cdot \cos \left( \phi + \beta \right) $

Formel 0-23: Zeitliche Ableitung des x-Wertes der zweiten Geradengleichung nach Einsetzen von b.


$ \dot y_P = \dot y_S + r \cdot \dot \phi \cdot \cos \left( \phi \right) + \dot b \cdot \cos \left( \phi + \beta \right) - b \cdot \left(\dot \phi + \dot \beta \right) \cdot \sin \left( \phi + \beta \right) $

Formel 0-24: Zeitliche Ableitung des y-Wertes der zweiten Geradengleichung nach Einsetzen von b.


Diese zeitliche Ableitung der Lösung kann dann in die Formeln 30-10 und 30-11 eingesetzt werden, um einen Zusammenhang zwischen den translatorischen Beschleunigungen in den Newton-Gleichungen und der Winkelbeschleunigung der Eulergleichung herzustellen.

Wie oben bereits erwähnt: Durch Ersetzen der Winkelbeschleunigung in den Newton-Gleichungen durch die rechte Seite der Euler-Gleichung kann schließlich ein LGS zur Bestimmung der Zwangskräfte FH und FV gewonnen werden, das dann vor jedem Integrationsschritt gelöst werden muß.

Konkret ergibt sich aus den Newton-Gleichungen und 30-15 und 30-16:

$ m \cdot \left(- \ddot \phi \cdot \left(y_S-y_P\right) - \dot \phi \cdot \left(\dot y_S- \dot y_P\right)\right)= F_{Hx} + F_{Vx} + F_{Ax} $

Formel 0-25: Newton-Gleichung in x-Richtung und Zwangsbedingung eingesetzt.


$ m \cdot \left(\ddot \phi \cdot \left(x_S-x_P\right) + \dot \phi \cdot \left(\dot x_S- \dot x_P\right)\right)= F_{Hy} + F_{Vy} + F_{Ay} $

Formel 0-26: Newton-Gleichung in y-Richtung und Zwangsbedingung eingesetzt.


Ergänzend ergibt sich nach Auflösung der rechten Seite der zum Einsetzen benötigten Eulergleichung:

$ J \ddot \phi = -q \cdot F_H + r \cdot F_V \cdot \left( \cos \left( \phi \right) \cdot \cos \left( \phi + \beta \right) + \sin \left( \phi \right) \cdot \sin \left( \phi + \beta \right)\right) + r \cdot F_A \cdot \left( \cos \left( \phi \right) \cdot \sin \left( \phi + \beta \right)- \sin \left( \phi \right) \cdot \cos \left( \phi + \beta \right)\right) $

Formel 0-27: Euler-Gleichung aufgelöst.


Aufgaben

Damit sind nun alle Formeln hergeleitet, die benötigt werden, das erste Simulationsprogramm zum AV (konstante Antriebskraft und konstanter Lenkwinkel) in Scilab zu schreiben.

Versuchen Sie es.

HINWEIS: Bei der Umsetzung mit Scilab können lange zu einem Integrationsschritt konstante Terme in Hilfsvariablen gespeichert und dann in die Formeln eingesetzt werden, um die Ausdrücke übersichtlicher zu gestalten (Substitution).

Lesen Sie außerdem den Text zu fehlertoleranter Softwareentwicklung.

Umsetzung mit Scilab

clear();

function f = rechteSeite(t,y)
    q = 0.1;
    r = 0.1;
    m = 1.0;
    quad_lang = 0.2;
    quad_breit = 0.1;
    J = m*(quad_breit*quad_breit + quad_lang*quad_lang)/12;

    bbeta  = 0.1*%pi;
    Dbbeta = 0.0;
    FA   = 10;

    xs   = y(1,1);
    ys   = y(2,1);
    phi  = y(3,1);

    vx  = y(4,1);
    vy  = y(5,1);
    om  = y(6,1);

    SP = sin(phi);
    CP = cos(phi);
    SB = sin(bbeta);
    CB = cos(bbeta);
    SPB = sin(phi+bbeta);
    CPB = cos(phi+bbeta);

    //Zwangskräfte berechnen

        //Momentanpol:
        b = (r+q)/(SPB*CP-CPB*SP);
        xP = xs + r*CP - b*SPB;
        yP = ys + r*SP + b*CPB;
        QQ = (om+Dbeta)*CPB*CP-om*SPB*SP;
        RR = (om+Dbeta)*SPB*SP - om*CPB*CP;
        Db = -(r+q)* ( (SPB*CP-CPB*SP)^(-2) ) * (QQ+RR);//Zeitliche Ableitung: vorangestelltes D!
        DxP = vx - r*om*SP-Db*SPB-b*(om+Dbeta)*CPB;
        DyP = vy + r*om*CP+Db*CPB-b*(om+Dbeta)*SPB;


        //Herleitung:
        //  DDx = (FHx + FVx + FAx)/m;
        //  DDy = (FHy + FVy + FAy)/m;
        //  -DDphi*(y-yP)*m-om*(vy-DyP)*m = -FH*SP - FV*SPB + FA*CPB;
        //   DDphi*(x-xP)*m+om*(vx-DxP)*m =  FH*CP + FV*CPB + FA*SPB;

        //   DDphi = (-q*FH+r*FV*(CP*CPB+SP*SPB)+r*FA*(CP*SPB-SP*CPB))/J
        EEE = q/J;
        FFF = r*(CP*CPB+SP*SPB)/J;
        GGG = r*(CP*SPB-SP*CPB)/J;
        //   DDphi = -EEE*FH + FFF*FV + GGG*FA
        HHH =  om*(vy-DyP)*m;
        III =  om*(vx-DxP)*m;
        JJJ =  (ys - yP)*m;
        KKK =  (xs - xP)*m;
        //  -DDphi*JJJ-HHH = -FH*SP - FV*SPB + FA*CPB;
        //   DDphi*KKK+III =  FH*CP + FV*CPB + FA*SPB;
        //
        //  -(-EEE*FH + FFF*FV + GGG*FA)*JJJ-HHH = -FH*SP - FV*SPB + FA*CPB;
        //   (-EEE*FH + FFF*FV + GGG*FA)*KKK+III =  FH*CP + FV*CPB + FA*SPB;
        //
        //     EEE*JJJ*FH - FFF*JJJ*FV - GGG*JJJ*FA - HHH = -FH*SP - FV*SPB + FA*CPB;
        //    -EEE*KKK*FH + FFF*KKK*FV + GGG*KKK*FA + III =  FH*CP + FV*CPB + FA*SPB;
        //     EEE*JJJ*FH + FH*SP - FFF*JJJ*FV + FV*SPB   =   + FA*CPB + GGG*JJJ*FA + HHH;
        //    -EEE*KKK*FH - FH*CP + FFF*KKK*FV - FV*CPB   =   + FA*SPB - GGG*KKK*FA - III;

        // AA * [FH;FV] = bb

        AA = [ EEE*JJJ + SP , - FFF*JJJ + SPB;
              -EEE*KKK - CP ,   FFF*KKK - CPB ];

        bb = [FA*CPB + GGG*JJJ*FA + HHH ; FA*SPB - GGG*KKK*FA - III ];

        FHFV = inv(AA)*bb;

        FH   = FHFV(1);
        FV   = FHFV(2);

    //Ende Zwangskräfte berechnen

    FAx = FA*CPB;
    FAy = FA*SPB;
    FHx = -FH*SP;
    FHy =  FH*CP;
    FVx = -FV*SPB;
    FVy =  FV*CPB;

    f(1,1) = vx;
    f(2,1) = vy;
    f(3,1) = om;

    f(4,1) = (FHx + FVx + FAx)/m;
    f(5,1) = (FHy + FVy + FAy)/m;
    f(6,1) = (-q*FH+r*FV*(CP*CPB+SP*SPB)+r*FA*(CP*SPB-SP*CPB))/J;
endfunction

t = linspace(0,3,3000);
y0 = [0,0,0,0,0,0]';
t0 = 0;
y  = ode(y0,t0,t,rechteSeite);

plot(y(1,:)',y(2,:)');

Code 0-1: Einfache Umsetzung des Dreirads: Lenkwinkel und Antriebskraft sind zunächst konstant.

Trajektorie der Schwerpunktbewegung bei obigem Simulationsmodell.

Bild 0-5: Trajektorie der Schwerpunktbewegung bei obigem Simulationsmodell.

Umschaltung zwischen Kreis- und Geradeausfahrt

clear();

function f = rechteSeite(t,y)
    MINBETA = 0.00001;  //Grenze zum Umschalten des Modells für Geradeausfahrt
    
    //Tests:
    //MINBETA = 0.0001;  //O.K.
    //MINBETA = 0.001;  //O.K.
    //MINBETA = 0.01;  //NICHT O.K.


    q = 0.1;
    r = 0.1;
    m = 1.0;
    quad_lang = 0.2;
    quad_breit = 0.1;
    J = m*(quad_breit*quad_breit + quad_lang*quad_lang)/12;

    //bbeta  = 0.1*%pi;
    //Dbbeta = 0.0;

    //Testweise Zeitabhängig
    bbeta  =  0.2*cos(t);
    Dbbeta = -0.2*sin(t);

    FA   = 10;

    xs   = y(1,1);
    ys   = y(2,1);
    phi  = y(3,1);

    vx  = y(4,1);
    vy  = y(5,1);
    om  = y(6,1);

    SP = sin(phi);
    CP = cos(phi);
    SB = sin(bbeta);
    CB = cos(bbeta);
    SPB = sin(phi+bbeta);
    CPB = cos(phi+bbeta);

    //Zwangskräfte berechnen
    if abs(bbeta)>MINBETA then
        //Momentanpol:
        b = (r+q)/(SPB*CP-CPB*SP);
        xP = xs + r*CP - b*SPB;
        yP = ys + r*SP + b*CPB;
        QQ = (om+Dbbeta)*CPB*CP-om*SPB*SP;
        RR = (om+Dbbeta)*SPB*SP - om*CPB*CP;
        Db = -(r+q)* ( (SPB*CP-CPB*SP)^(-2) ) * (QQ+RR);//Zeitliche Ableitung: vorangestelltes D!
        DxP = vx - r*om*SP-Db*SPB-b*(om+Dbbeta)*CPB;
        DyP = vy + r*om*CP+Db*CPB-b*(om+Dbbeta)*SPB;


        //Herleitung:
        //  DDx = (FHx + FVx + FAx)/m;
        //  DDy = (FHy + FVy + FAy)/m;
        //  -DDphi*(y-yP)*m-om*(vy-DyP)*m = -FH*SP - FV*SPB + FA*CPB;
        //   DDphi*(x-xP)*m+om*(vx-DxP)*m =  FH*CP + FV*CPB + FA*SPB;

        //   DDphi = (-q*FH+r*FV*(CP*CPB+SP*SPB)+r*FA*(CP*SPB-SP*CPB))/J
        EEE = q/J;
        FFF = r*(CP*CPB+SP*SPB)/J;
        GGG = r*(CP*SPB-SP*CPB)/J;
        //   DDphi = -EEE*FH + FFF*FV + GGG*FA
        HHH =  om*(vy-DyP)*m;
        III =  om*(vx-DxP)*m;
        JJJ =  (ys - yP)*m;
        KKK =  (xs - xP)*m;
        //  -DDphi*JJJ-HHH = -FH*SP - FV*SPB + FA*CPB;
        //   DDphi*KKK+III =  FH*CP + FV*CPB + FA*SPB;
        //
        //  -(-EEE*FH + FFF*FV + GGG*FA)*JJJ-HHH = -FH*SP - FV*SPB + FA*CPB;
        //   (-EEE*FH + FFF*FV + GGG*FA)*KKK+III =  FH*CP + FV*CPB + FA*SPB;
        //
        //     EEE*JJJ*FH - FFF*JJJ*FV - GGG*JJJ*FA - HHH = -FH*SP - FV*SPB + FA*CPB;
        //    -EEE*KKK*FH + FFF*KKK*FV + GGG*KKK*FA + III =  FH*CP + FV*CPB + FA*SPB;
        //     EEE*JJJ*FH + FH*SP - FFF*JJJ*FV + FV*SPB   =   + FA*CPB + GGG*JJJ*FA + HHH;
        //    -EEE*KKK*FH - FH*CP + FFF*KKK*FV - FV*CPB   =   + FA*SPB - GGG*KKK*FA - III;

        // AA * [FH;FV] = bb

        AA = [ EEE*JJJ + SP , - FFF*JJJ + SPB;
              -EEE*KKK - CP ,   FFF*KKK - CPB ];

        bb = [FA*CPB + GGG*JJJ*FA + HHH ; FA*SPB - GGG*KKK*FA - III ];

        FHFV = inv(AA)*bb;

        FH   = FHFV(1);
        FV   = FHFV(2);

        FHx = -FH*SP;
        FHy =  FH*CP;
        FVx = -FV*SPB;
        FVy =  FV*CPB;
    else
        //Wenn Geradeausfahrt
        FH   = 0;
        FV   = 0;

        FHx = 0;
        FHy = 0;
        FVx = 0;
        FVy = 0;
    end
    //Ende Zwangskräfte berechnen

    FAx = FA*CPB;
    FAy = FA*SPB;

    f(1,1) = vx;
    f(2,1) = vy;
    f(3,1) = om;

    f(4,1) = (FHx + FVx + FAx)/m;
    f(5,1) = (FHy + FVy + FAy)/m;
    f(6,1) = (-q*FH+r*FV*(CP*CPB+SP*SPB)+r*FA*(CP*SPB-SP*CPB))/J;
endfunction

t = linspace(0,3,3000);
y0 = [0,0,0,0,0,0]';
t0 = 0;
y  = ode(y0,t0,t,rechteSeite);

plot(y(1,:)',y(2,:)');

Code 0-2: Erweitertes Modell für Kreis- und Geradeausfahrt

Erweitertes Modell mit Berücksichtigung von dämpfenden Anteilen

clear();

function f = rechteSeite(t,y)
    //003:
    MINBETA = 0.00001;  //Grenze zum Umschalten des Modells für Geradeausfahrt
    
    //Tests:
    //MINBETA = 0.0001;  //O.K.
    //MINBETA = 0.001;  //O.K.
    //MINBETA = 0.01;  //NICHT O.K.

    //004:
    //Geschwindigkeitsabhängige Dämpfungskraft FD einbauen.
    Drad = 1.0; //Dämpfung ... Rollreibung, Luftwiderstand usw.


    q = 0.1;
    r = 0.1;
    m = 1.0;
    quad_lang = 0.2;
    quad_breit = 0.1;
    J = m*(quad_breit*quad_breit + quad_lang*quad_lang)/12;

    //bbeta  = 0.1*%pi;
    //Dbbeta = 0.0;

    bbeta  = 0.0;
    Dbbeta = 0.0;

    //Testweise Zeitabhängig
    //bbeta  =  0.2*cos(t);
    //Dbbeta = -0.2*sin(t);

    FA   = 1;

    xs   = y(1,1);
    ys   = y(2,1);
    phi  = y(3,1);

    vx  = y(4,1);
    vy  = y(5,1);
    om  = y(6,1);   

    SP = sin(phi);
    CP = cos(phi);
    SB = sin(bbeta);
    CB = cos(bbeta);
    SPB = sin(phi+bbeta);
    CPB = cos(phi+bbeta);

    //Richtung und Geschwindigkeit des Rades berechnen, um FD bilden zu können:

    //1. Position des Radmittelpunktes:
    x_rad = xs + CP*r;
    y_rad = ys + SP*r;

    //2. Richtung des Rades (ex)
    dir_x_rad = CPB;
    dir_y_rad = SPB;

    //3. Aktuelle Geschwindigkeit des Radmittelpunktes:
    vx_rad = vx - om*SP*r;
    vy_rad = vy + om*CP*r;

    //4. Skalar der Radgeschwindigkeit in Radrichtung ex*vrad
    v_rad = dir_x_rad*vx_rad + dir_y_rad*vy_rad;

    //5. Berechnung der Dämpfungskraft und Hinzunahme zur Antriebskraft:
    FD = -Drad*v_rad;
    FA = FA+FD;

    //Zwangskräfte berechnen
    if abs(bbeta)>MINBETA then
        //Momentanpol:
        b = (r+q)/(SPB*CP-CPB*SP);
        xP = xs + r*CP - b*SPB;
        yP = ys + r*SP + b*CPB;
        QQ = (om+Dbbeta)*CPB*CP-om*SPB*SP;
        RR = (om+Dbbeta)*SPB*SP - om*CPB*CP;
        Db = -(r+q)* ( (SPB*CP-CPB*SP)^(-2) ) * (QQ+RR);//Zeitliche Ableitung: vorangestelltes D!
        DxP = vx - r*om*SP-Db*SPB-b*(om+Dbbeta)*CPB;
        DyP = vy + r*om*CP+Db*CPB-b*(om+Dbbeta)*SPB;


        //Herleitung:
        //  DDx = (FHx + FVx + FAx)/m;
        //  DDy = (FHy + FVy + FAy)/m;
        //  -DDphi*(y-yP)*m-om*(vy-DyP)*m = -FH*SP - FV*SPB + FA*CPB;
        //   DDphi*(x-xP)*m+om*(vx-DxP)*m =  FH*CP + FV*CPB + FA*SPB;

        //   DDphi = (-q*FH+r*FV*(CP*CPB+SP*SPB)+r*FA*(CP*SPB-SP*CPB))/J
        EEE = q/J;
        FFF = r*(CP*CPB+SP*SPB)/J;
        GGG = r*(CP*SPB-SP*CPB)/J;
        //   DDphi = -EEE*FH + FFF*FV + GGG*FA
        HHH =  om*(vy-DyP)*m;
        III =  om*(vx-DxP)*m;
        JJJ =  (ys - yP)*m;
        KKK =  (xs - xP)*m;
        //  -DDphi*JJJ-HHH = -FH*SP - FV*SPB + FA*CPB;
        //   DDphi*KKK+III =  FH*CP + FV*CPB + FA*SPB;
        //
        //  -(-EEE*FH + FFF*FV + GGG*FA)*JJJ-HHH = -FH*SP - FV*SPB + FA*CPB;
        //   (-EEE*FH + FFF*FV + GGG*FA)*KKK+III =  FH*CP + FV*CPB + FA*SPB;
        //
        //     EEE*JJJ*FH - FFF*JJJ*FV - GGG*JJJ*FA - HHH = -FH*SP - FV*SPB + FA*CPB;
        //    -EEE*KKK*FH + FFF*KKK*FV + GGG*KKK*FA + III =  FH*CP + FV*CPB + FA*SPB;
        //     EEE*JJJ*FH + FH*SP - FFF*JJJ*FV + FV*SPB   =   + FA*CPB + GGG*JJJ*FA + HHH;
        //    -EEE*KKK*FH - FH*CP + FFF*KKK*FV - FV*CPB   =   + FA*SPB - GGG*KKK*FA - III;

        // AA * [FH;FV] = bb

        AA = [ EEE*JJJ + SP , - FFF*JJJ + SPB;
              -EEE*KKK - CP ,   FFF*KKK - CPB ];

        bb = [FA*CPB + GGG*JJJ*FA + HHH ; FA*SPB - GGG*KKK*FA - III ];

        FHFV = inv(AA)*bb;

        FH   = FHFV(1);
        FV   = FHFV(2);

        FHx = -FH*SP;
        FHy =  FH*CP;
        FVx = -FV*SPB;
        FVy =  FV*CPB;
    else
        //Wenn Geradeausfahrt
        FH   = 0;
        FV   = 0;

        FHx = 0;
        FHy = 0;
        FVx = 0;
        FVy = 0;
    end
    //Ende Zwangskräfte berechnen

    FAx = FA*CPB;
    FAy = FA*SPB;

    f(1,1) = vx;
    f(2,1) = vy;
    f(3,1) = om;

    f(4,1) = (FHx + FVx + FAx)/m;
    f(5,1) = (FHy + FVy + FAy)/m;
    f(6,1) = (-q*FH+r*FV*(CP*CPB+SP*SPB)+r*FA*(CP*SPB-SP*CPB))/J;
endfunction

t = linspace(0,10,3000);
y0 = [0,0,0,0,0,0]';
t0 = 0;
y  = ode(y0,t0,t,rechteSeite);

//plot(y(1,:)',y(2,:)');

//Geschwindigkeit in x-Richtung anzeigen:
plot(t,y(4,:)');

a = gca();
a.x_label.text = 't/s';
a.y_label.text = 'vx/ m/s';
a.title.text = 'Geschwindigkeitsverlauf Geradeausfahrt mit Dämpfung';

Code 0-3: Modell mit Dämpfungskraft im Rad (FD zu FA zugeschlagen)

Geschwindigkeitsverlauf bei konstanter Antriebskraft FA von 1N und Geradeausfahrt.

Bild 0-6: Geschwindigkeitsverlauf bei konstanter Antriebskraft FA von 1N und Geradeausfahrt.

Übung
Mit

Bild 0-7: Mit "Bodensensor" geregelte Fahrt.

Offensichtlich funktioniert die Regelung mit einem Bodensensor noch nicht in befriedigender Weise.

Überlegen Sie sich ein Konzept auf der Grundlage von "Pattern for Fault Tolerant Software", um die Situation zu verbessern.

Setzen Sie das Konzept um und benennen Sie das/die angewendeten Pattern.

clear();

KANTE = 1;


function f = rechteSeite(t,y)
    //003:
    MINBETA = 0.00001;  //Grenze zum Umschalten des Modells für Geradeausfahrt
    
    //Tests:
    //MINBETA = 0.0001;  //O.K.
    //MINBETA = 0.001;  //O.K.
    //MINBETA = 0.01;  //NICHT O.K.

    //004:
    //Geschwindigkeitsabhängige Dämpfungskraft FD einbauen.
    Drad = 1.0; //Dämpfung ... Rollreibung, Luftwiderstand usw.


    q = 0.1;
    r = 0.1;
    m = 1.0;
    quad_lang = 0.2;
    quad_breit = 0.1;
    J = m*(quad_breit*quad_breit + quad_lang*quad_lang)/12;




    FA   = 0.1;

    xs   = y(1,1);
    ys   = y(2,1);
    phi  = y(3,1);

    vx  = y(4,1);
    vy  = y(5,1);
    om  = y(6,1);   

    //wird durch Integration als 7. Zustandsgröße gewonnen:
    bbeta = y(7,1);

    SP = sin(phi);
    CP = cos(phi);
    SB = sin(bbeta);
    CB = cos(bbeta);
    SPB = sin(phi+bbeta);
    CPB = cos(phi+bbeta);

    //  ******************** BODENSENSOR  *****************************
    //Bodensensor:
    //Es wird angenommen, dass am Boden ein schwarzes Quadrat 
    //der Kantenlänge 2m liegt, Ursprung linke untere Ecke 0/0.

    //KANTE = 2;  //global s.o.

    //Der Sensor ist ein Strich parallel zur Hinterachse durch den Schwerpunkt.
    //Der Sensorwert ist der Anteil, den der Strich im schwarzen Quadrat liegt.

    //1) Wie weit liegt der Schnittpunkt der "Sensorgeraden" mit dem Rand des 
    //   schwarzen Quadrates vom Schwerpunkt weg?

    STRICHRADIUS = 0.3;

    BODENSENSOR = 1000; //Default kein Wert    
    Bunten  = 1000;  //aufgrund der Schnittpunkte mit den einzelnen Randgeraden des Quadrates
    Boben   = 1000;
    Blinks  = 1000;
    Brechts = 1000;

    phiphi = phi;
    while(phiphi>2*%pi)  //Begrenzen auf 0..2pi
        phiphi = phiphi-2*%pi;
    end
    while(phiphi<0)  //Begrenzen auf 0..2pi
        phiphi = phiphi+2*%pi;
    end

    //Mitte: 0
    //if (  (phiphi>1.5*%pi) | (phiphi<%pi/2) ) then 
    if (  abs(CP)>0 & xs<KANTE ) then 
        //[xs;ys]+Bunten*[-sin phi;cos phi] = [x;0]
        // ys + Bunten*cos phi = 0
        Bunten = -ys/CP;
    end


    //Mitte: 1.5pi
    //if (  (phiphi<2.0*%pi) & (phiphi>%pi) ) then 
    if (  abs(SP)>0 & ys<KANTE ) then 
        //[xs;ys]+Blinks*[-sin phi;cos phi] = [0;y]
        // xs - Blinks*sin phi = 0
        Blinks = xs/SP;
    end

    //Mitte: pi
    //if (  (phiphi>0.5*%pi) | (phiphi<1.5*%pi) ) then 
    if (  abs(CP)>0 & xs>0 ) then 
        //[xs;ys]+Boben*[-sin phi;cos phi] = [x;KANTE]
        // ys + Boben*cos phi = KANTE
        Boben = (KANTE-ys)/CP;
    end

    //Mitte: 0.5pi
    //if (  (phiphi<%pi) & (phiphi>0) ) then 
    if (  abs(SP)>0 & ys>0 ) then 
        //[xs;ys]+Brechts*[-sin phi;cos phi] = [KANTE;y]
        // xs - Brechts*sin phi = KANTE
        Brechts = (xs-KANTE)/SP;
    end

    if(  abs(Bunten)<abs(BODENSENSOR) ) then 
        BODENSENSOR = Bunten;
    end
    if(  abs(Boben)<abs(BODENSENSOR) ) then 
        BODENSENSOR = Boben;
    end
    if(  abs(Blinks)<abs(BODENSENSOR) ) then 
        BODENSENSOR = Blinks;
    end
    if(  abs(Brechts)<abs(BODENSENSOR) ) then 
        BODENSENSOR = Brechts;
    end


    if (abs(BODENSENSOR)>STRICHRADIUS) then //wenn ausserhalb Sensorbereich, einfach gerade stellen
        Dbbeta = -0.001*bbeta;
    else
        Dbbeta = 15*BODENSENSOR;
    end



    if (  (bbeta>0.3*%pi) & (Dbbeta>0) ) then
         Dbbeta = 0.0;
    end
    if (  (bbeta<-0.3*%pi) & (Dbbeta<0) ) then
         Dbbeta = 0.0;
    end

    //  ******************** ENDE BODENSENSOR  *****************************



    //********************** DÄMPFUNG ****************************
    //Richtung und Geschwindigkeit des Rades berechnen, um FD bilden zu können:

    //1. Position des Radmittelpunktes:
    x_rad = xs + CP*r;
    y_rad = ys + SP*r;

    //2. Richtung des Rades (ex)
    dir_x_rad = CPB;
    dir_y_rad = SPB;

    //3. Aktuelle Geschwindigkeit des Radmittelpunktes:
    vx_rad = vx - om*SP*r;
    vy_rad = vy + om*CP*r;

    //4. Skalar der Radgeschwindigkeit in Radrichtung ex*vrad
    v_rad = dir_x_rad*vx_rad + dir_y_rad*vy_rad;

    //5. Berechnung der Dämpfungskraft und Hinzunahme zur Antriebskraft:
    FD = -Drad*v_rad;
    FA = FA+FD;
    //********************** ENDE DÄMPFUNG ****************************

    //Zwangskräfte berechnen
    if abs(bbeta)>MINBETA then
        //Momentanpol:
        b = (r+q)/(SPB*CP-CPB*SP);
        xP = xs + r*CP - b*SPB;
        yP = ys + r*SP + b*CPB;
        QQ = (om+Dbbeta)*CPB*CP-om*SPB*SP;
        RR = (om+Dbbeta)*SPB*SP - om*CPB*CP;
        Db = -(r+q)* ( (SPB*CP-CPB*SP)^(-2) ) * (QQ+RR);//Zeitliche Ableitung: vorangestelltes D!
        DxP = vx - r*om*SP-Db*SPB-b*(om+Dbbeta)*CPB;
        DyP = vy + r*om*CP+Db*CPB-b*(om+Dbbeta)*SPB;


        //Herleitung:
        //  DDx = (FHx + FVx + FAx)/m;
        //  DDy = (FHy + FVy + FAy)/m;
        //  -DDphi*(y-yP)*m-om*(vy-DyP)*m = -FH*SP - FV*SPB + FA*CPB;
        //   DDphi*(x-xP)*m+om*(vx-DxP)*m =  FH*CP + FV*CPB + FA*SPB;

        //   DDphi = (-q*FH+r*FV*(CP*CPB+SP*SPB)+r*FA*(CP*SPB-SP*CPB))/J
        EEE = q/J;
        FFF = r*(CP*CPB+SP*SPB)/J;
        GGG = r*(CP*SPB-SP*CPB)/J;
        //   DDphi = -EEE*FH + FFF*FV + GGG*FA
        HHH =  om*(vy-DyP)*m;
        III =  om*(vx-DxP)*m;
        JJJ =  (ys - yP)*m;
        KKK =  (xs - xP)*m;
        //  -DDphi*JJJ-HHH = -FH*SP - FV*SPB + FA*CPB;
        //   DDphi*KKK+III =  FH*CP + FV*CPB + FA*SPB;
        //
        //  -(-EEE*FH + FFF*FV + GGG*FA)*JJJ-HHH = -FH*SP - FV*SPB + FA*CPB;
        //   (-EEE*FH + FFF*FV + GGG*FA)*KKK+III =  FH*CP + FV*CPB + FA*SPB;
        //
        //     EEE*JJJ*FH - FFF*JJJ*FV - GGG*JJJ*FA - HHH = -FH*SP - FV*SPB + FA*CPB;
        //    -EEE*KKK*FH + FFF*KKK*FV + GGG*KKK*FA + III =  FH*CP + FV*CPB + FA*SPB;
        //     EEE*JJJ*FH + FH*SP - FFF*JJJ*FV + FV*SPB   =   + FA*CPB + GGG*JJJ*FA + HHH;
        //    -EEE*KKK*FH - FH*CP + FFF*KKK*FV - FV*CPB   =   + FA*SPB - GGG*KKK*FA - III;

        // AA * [FH;FV] = bb

        AA = [ EEE*JJJ + SP , - FFF*JJJ + SPB;
              -EEE*KKK - CP ,   FFF*KKK - CPB ];

        bb = [FA*CPB + GGG*JJJ*FA + HHH ; FA*SPB - GGG*KKK*FA - III ];

        FHFV = inv(AA)*bb;

        FH   = FHFV(1);
        FV   = FHFV(2);

        FHx = -FH*SP;
        FHy =  FH*CP;
        FVx = -FV*SPB;
        FVy =  FV*CPB;
    else
        //Wenn Geradeausfahrt
        FH   = 0;
        FV   = 0;

        FHx = 0;
        FHy = 0;
        FVx = 0;
        FVy = 0;
    end
    //Ende Zwangskräfte berechnen

    FAx = FA*CPB;
    FAy = FA*SPB;

    f(1,1) = vx;
    f(2,1) = vy;
    f(3,1) = om;

    f(4,1) = (FHx + FVx + FAx)/m;
    f(5,1) = (FHy + FVy + FAy)/m;
    f(6,1) = (-q*FH+r*FV*(CP*CPB+SP*SPB)+r*FA*(CP*SPB-SP*CPB))/J;
    f(7,1) = Dbbeta; //so wird aus der Winkelgeschw. des Lenkwinkels dieser integriert.
endfunction

t = linspace(0,100,10000);
y0 = [0,-0.05,0,0,0,0,0]'; //7. Zustand: Lenkwinkel
//y0 = [KANTE+0.05,0,0.5*%pi,0,0,0,0]'; //7. Zustand: Lenkwinkel
t0 = 0;
y  = ode("rkf",y0,t0,t,rechteSeite);

//Quadrat einzeichnen:
plot([0,KANTE],[0,0],'gre');
plot([0,KANTE],[KANTE,KANTE],'gre');
plot([0,0.0001],[0,KANTE],'gre');
plot([KANTE,KANTE+0.0001],[0,KANTE],'gre');

plot(y(1,:)',y(2,:)');
a = gca();
a.x_label.text = 'xs/m';
a.y_label.text = 'ys/m';
a.title.text = 'Fahrtverlauf ys gegen xs';



//Geschwindigkeit in x-Richtung anzeigen:
//plot(t,y(4,:)');
//a = gca();
//a.x_label.text = 't/s';
//a.y_label.text = 'vx/ m/s';
//a.title.text = 'Geschwindigkeitsverlauf Geradeausfahrt mit Dämpfung';

Code 0-4: Fahrzeug durch Bodensensor geregelt.