Quelltext der Seite ArduMower: Simulation der Kinematik eines Fahrzeugs mit drei Rädern

[[Kategorie:ArduMower]]
[[Datei:Fahrzeugmodell_kinematisch_Skizze_3Rad.JPG|rechts|mini|400px|Skizze eines 3-rädrigen Fahrzeugs mit Geschwindigkeiten, Ortsvektoren und Koordinatensystemen zur Herleitung des kinematischen Fahrzeugmodells. ]]
'''Autor:''' [[Benutzer:Mirekgoebel|Prof. Dr. Mirek Göbel]] 


== Einleitung ==
Dieser Artikel beschreibt den Aufbau eines Simulationsmodells für ein dreirädriges Fahrzeug, um dieses für die Entwicklung von Steuerungs- und Regelungsalgorithmen zum autonomen Fahren zu verwenden. Damit stellt es die Basis dar, um ohne Hardware die Algorithmen in einer closed-loop-Simulationsumgebung entwickeln und vor allem testen zu können. Der Anstoß zur Entwicklung dieses Modell lieferte das Projekt "ArduMower", in dem der Kollege Prof. Schneider und der Autor dieses Artikels mit Studierenden gemeinsam einen autonomen Rasenmäher entwickeln, siehe [[Projekt_ArduMower]].

Das systematische Vorgehen bei der Entwicklung des Modells orientiert sich am V-Prozessmodell.

== Anforderungen==
{| class="mw-datatable"
! style="font-weight: bold;" | ID
! style="font-weight: bold;" | Inhalt
! style="font-weight: bold;" | Ersteller
! style="font-weight: bold;" | Datum
! style="font-weight: bold;" | Geprüft von
! style="font-weight: bold;" | Datum
|-
| 1
| Das Fahrzeugmodell muss die Kinematik des als starr angenommenen Fahrzeugkörpers beschreiben.
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
| 2
| Das Fahrverhalten muss unter der Annahme korrekt abgebildet werden, dass die Räder schlupffrei abrollen.
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
| 3
| Am Eingang werden die Längsgeschwindigkeiten entlang der x-Achse des Fahrzeugkoordinatensystems K der Räder rechts (R) und links (L) vorgegeben.
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
| 4
| Am Ausgang müssen 
# die Position und Geschwindigkeit des Mittelpunktes M und des frei definierbaren Punktes D in x- und y-Richtung des Inertialsystems I 
# der Gierwinkel
# und die Gierrate 
zur Verfügung stehen.
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
| 5
| Das Modell muss in Matlab/Simulink erstellt werden.
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
| 6
| Die Rechnung des Modells erfolgt mit diskreten Zeitschritten (es sind diskrete Integratoren zu verwenden).
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
| 7
| Die Dokumentation muss auf Basis der Mehrkörpersystemeberechnung leicht nachvollziehbar erfolgen.
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
| 8
| Bei der Simulation muss eine graphische Ausgabe der Position und Richtung des Fahrzeugs in x- und y-Koordinaten des I-Systems erfolgen.
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
| 9
| Die Signalnamen müssen gemäß nebenstehender Abbildung gewählt werden.
| Prof. Göbel
| 07.06.2017
| Prof. Schneider
| 08.06.2017
|-
|}
[[Datei:Fahrzeugmodell_kinematisch_3Rad_AnforderungenSchnittstellen.JPG|rechts|mini|300px|Schnittstellen für die Modellierung eines 3-rädrigen Fahrzeugs mit Geschwindigkeiten, Ortsvektoren und Koordinatensystemen.]]


== Funktionaler Systementwurf / Technischer Systementwurf ==
Bei der Größe des zu erstellenden Modells werden die Schritte Funktionaler Systementwurf / Technischer Systementwurf des V-Modells zusammen gelegt und bereits Schnittstellen zwischen den einzelnen Blöcken definiert.

Das Fahrzeugmodell wird unterteilt in 
* ein Block "Gieren und Geschwindigkeit in Achsmitte", in dem die Position und Geschwindikeit der Punkte M und D in Fahrzeugkoordinaten K bestimmt werden.
* und zwei Blöcke "Transformation und Integration Punkt M/D", in denen Position und Geschwindikeit der Punkte M und D in Inertialkoordinaten I bestimmt werden.

[[Datei:Fahrzeugmodell_kinematisch_3Rad_FunktionalerSystementwurf.JPG|rechts|mini|800px|Funktionaler Systementwurf für die Modellierung eines 3-rädrigen Fahrzeugs mit Geschwindigkeiten, Ortsvektoren und Koordinatensystemen.]]

== Komponentenspezifikation ==
Das Modell wird insgesamt als Komponente aufgefasst, d. h. die einzelnen Blöcke aus dem Systementwurf werden als Bestandteil der Komponente "Fahrzeugmodell" definiert. An dieser Stelle wäre es selbstverständlich möglich, die Komponente weiter aufzuteilen (damit würde die Komponente in Teilsystem umbenannt) und auch beim Testen diese einzelnen Komponenten dann zu berücksichtigen.

Die Spezifikation besteht aus den theoretischen Grundlagen zur Beschreibung von der ebenen Fahrzeugbewegung.

=== Ansatz ===
Bei der Bestimmung der Geschwindigkeit in M wird mit dem Satz "räumliche Bewegung" die bekannte Geschwindigkeit in R, der Relativdrehvektor der Koordinatensysteme K gegenüber I sowie der Ortsvektor zwischen R und M verwendet (siehe <ref> M. Göbel: Formelsammlung Mehrkörpersysteme und Robotik, HSHL, Version 29.06.2017</ref>):

<math>\vec{v}_M = \vec{v}_R + ^{IK} \vec{\omega} \times \vec{r}_{RM}</math>

Im körperfesten Koordinatensystem K beschrieben folgt eine Beschreibung, in der jeder Term selbst 3 Komponenten (x, y, z) enthält und mit denen jetzt im jeweiligen Koordinatensystem gerechnet werden kann (das ging bei der vektoriellen Schreibweise oben noch nicht!).

<math>_K \underline{v}_M = _K \underline{v}_R + ^{IK} _K \underline{\omega} \times _K \underline{r}_{RM}</math>


=== x-y-z-Komponenten einsetzen und Geschwindigkeit in M bestimmen ===
Mit den eingetragenen Komponenten sieht die Gleichung wie folgt aus:

<math>_K \underline{v}_M = 
\begin{bmatrix}
v_{Rx}\\
0\\
0
\end{bmatrix} +
\begin{bmatrix}
0\\
0\\
\dot{\psi}
\end{bmatrix} \times
\begin{bmatrix}
-l/2\\
b/2\\
0
\end{bmatrix} =
\begin{bmatrix}
v_R - b/2 \cdot \dot{\psi}\\
-l/2 \cdot \dot{\psi}\\
0
\end{bmatrix}
</math>

=== Bestimmung der Gierrate===
Zur Bestimmung der Gierrate wird obiger Ansatz erneut verwendet, um von der bekannten Geschwindigkeit in R auf die ebenfalls bekannte Geschwindikeit in L "zu schließen", sodass der Relativdrehvektor der Koordinatensysteme K gegenüber I in K-Koordinaten bestimmt werden kann (jetzt mit Komponenten besser Spaltenmatrix genannt: <math>^{IK} _K \underline{\omega}</math>). Es folgt:

<math>_K \underline{v}_L = _K \underline{v}_R + ^{IK} _K \underline{\omega} \times _K \underline{r}_{RL}</math>.

Mit eingesetzten Komponenten ergibt sich im körperfesten System K:

<math>
\begin{bmatrix}
v_{Lx}\\
0\\
0
\end{bmatrix}
= 
\begin{bmatrix}
v_{Rx}\\
0\\
0
\end{bmatrix} 
+
\begin{bmatrix}
0\\
0\\
\dot{\psi}
\end{bmatrix} 
\times
\begin{bmatrix}
0\\
-l\\
0
\end{bmatrix} 
=
\begin{bmatrix}
v_{Rx} + b \cdot \dot{\psi}\\
0\\
0
\end{bmatrix}
</math>.

Aus Zeile 1 der obigen Gleichung kann die Gierrate (selbstverständlich im körperfesten System K) mit nachstehendem Zusammenhang ermittelt werden.

<math>v_{Lx} = v_{Rx} + b \cdot \dot{\psi}</math>

Umgestellt folgt daraus:

<math>\dot{\psi} = \frac{v_{Rx} - v_{Lx}}{b}</math>

=== Position der Punkte M und D in Inertialkoordinaten ===
Um die Position des Punktes M in Inertialkoordinaten zu berechnen, wird seine Geschwindigkeit in Intertialkoordinaten I benötigt, da diese dann durch eine einfache Integration in die Position überführt werden kann. Im körperfesten System ist dies nicht erlaubt bzw. möglich, da dieses sich dreht! Mit Hilfe einer Transformationsmatrix kann diese Umrechnung in einem Schritt erfolgen.

<math>_I \underline{v}_M = ^{IK}\textbf{A} \cdot _K \underline{v}_M =
\begin{bmatrix}
\cos{\psi} & -\sin{\psi} & 0\\
\sin{\psi} & \cos{\psi} & 0\\
0 & 0 & 1
\end{bmatrix}
\cdot _K \underline{v}_M
 </math>


Der über den Ortsvektor <math>\vec{r}_{RD}</math> von R nach D beliebig wählbare Punkt D kann genauso wie oben der Mittelpunkt M behandelt werden. Die Gleichungen in Kurzform dazu sind wie folgt.

<math>\vec{v}_D = \vec{v}_R + ^{IK} \vec{\omega} \times \vec{r}_{RD}</math>

Im körperfesten Koordinatensystem K beschrieben folgt eine Beschreibung, in der jeder Term selbst 3 Komponenten (x, y, z) enthält und mit denen jetzt im jeweiligen Koordinatensystem gerechnet werden kann (das ging bei der vektoriellen Schreibweise oben noch nicht!).

<math>_K \underline{v}_D = _K \underline{v}_R + ^{IK} _K \underline{\omega} \times _K \underline{r}_{RD}</math>

Das Ergebnis für D lautet:

<math>_K \underline{v}_D =
\begin{bmatrix}
v_{Rx} - r_{RDy} \cdot \dot{\psi}\\
r_{RDx} \cdot \dot{\psi}\\
0
\end{bmatrix}
\quad
</math>
mit dieser zuvor ausgemessenen Spaltenmatrix
<math>
\quad
_K \underline{r}_{RD} =
\begin{bmatrix}
r_{RDx}\\
r_{RDy}\\
0
\end{bmatrix}
</math>.


== Programmierung ==
Die Programmierung ist nun auf Basis der oben ermittelten Gleichungen möglich und erfolgt in Matlab/Simulink. 


=== Matlab ===
Über ein Start-Skript werden alle relevanten Parameter gesetzt und das Modell geöffnet.
<source line lang="matlab" style="font-size:tiny">
%%%
%%% Simulation des Ardumower-Fahrzeugs
%%%
%%% Prof. Mirek Göbel, 08.06.2017

%% Initialisierung
clc; clear; close all;


%% Einstellungen
PAR_Darstellung_Schalter_EIN_bit = 1;
Simulinkmodus = 1;
addpath ../Hauptprogramm/funktionen/;
addpath ../Hauptprogramm/libraries/;
addpath ../Hauptprogramm/parameter/;


%% Parameter
PAR_FZG_spurweite_f64 = 0.4; % in m
PAR_FZG_radstand_f64 = 0.4; % in m
PAR_FZG_abstand_sensor_VA_f64 = 0.23; % Abstand des Perimeter-Sensors von der Vorderachse in m
T = 0.01; % Schrittweite für die Simulation (gilt für das ganze Modell)
PAR_VIS_Anzahl_Schritte_n = 10; % Angabe, alle wieviel Schritte etwas dargestellt werden soll

% Startwinkel
PAR_FZG_psi0_I_f64 = 0; % Start-Gierwinkel in rad
cospsi =  cos(PAR_FZG_psi0_I_f64); % zur Abkürzung / Vermeidung von Schreibarbeit (s. u.)
sinpsi =  sin(PAR_FZG_psi0_I_f64); % zur Abkürzung / Vermeidung von Schreibarbeit (s. u.)

% Startweg des Mittelpunktes M
PAR_FZG_x0_I_f64 = 0; % Start-x-Weg in m
PAR_FZG_y0_I_f64 = 0.5; % Start-y-Weg in m


% Startweg des Punktes D
r_MDx_K = PAR_FZG_radstand_f64/2 + PAR_FZG_abstand_sensor_VA_f64;
r_MDy_K = PAR_FZG_spurweite_f64/2 + 0;
PAR_FZG_xD0_I_f64 = PAR_FZG_x0_I_f64 + cospsi*r_MDx_K + sinpsi*r_MDy_K; % Start-x-Weg des Punktes D in m
PAR_FZG_yD0_I_f64 = PAR_FZG_y0_I_f64 - sinpsi*r_MDx_K + cospsi*r_MDy_K; % Start-y-Weg des Punktes D in m


%% Modell auf!
open('fahrzeugmodell2017a.slx');
</source>

=== Simulink === 
In Simulink werden die o. g. Gleichungen verwendet, um die in der Funktionsstruktur genannten Blöcke mit Leben zu füllen. Das Simulink liegt hier: [[Datei:Fahrzeugmodell2017a.zip]]. 



Abbildung [Programmierung 1] zeigt die Subkomponente zur Berechnung der Gierrate, aus der mittels Integration (1) der Gierwinkel berechnet wird. [[Datei:Fahrzeugmodell_kinematisch_psip_v_K_bestimmen.JPG|rechts|mini|600px|[Programmierung 1] Simulink-Modell zur Bestimmung der Gierrate und körperfesten Geschwindigkeit in den Punkten M und D]]
Dies ist der Winkel zwischen den x-Achsen der Koordinatensysteme I und K. Die Parameter in blau (4) werden den beiden Blöcken (3) und (4) zur Verfügung gestellt, worin die Geschwindigkeit in K-Koordinaten in den Punkten M und D bestimmt wird.



Diese beiden körperfesten Geschwindigkeiten werden in der nächsten Subkomponente aufgegriffen, siehe Abbildung [Programmierung 2]. 
[[Datei:Fahrzeugmodell_kinematisch_Bestimmung_v_I.JPG|rechts|mini|600px|[Programmierung 2] Simulink-Modell zur Bestimmung der Inertial-Geschwindigkeit in den Punkten M und D und Integration zur Positionsbestimmung]]
Hier wird mittels des Gierwinkels die aktuelle Transformationsmatrix zwischen den Koordinatensystemen I und K ermittelt (Transformation von K nach I, Block (1)). Damit ist nun die Transformation der körperfesten Geschwindigkeiten in das Intertialsystem mölglich (2). Die reine Integration ohne Berücksichtigung von Drehtermen ist nur im I-System erlaubt, sodass nun diese Inertialgeschwindigkeiten durch Integration (3) und (4) in absolute Positionen in I-Koordinaten umgerechnet werden können!

== Komponententest ==
Da es sich bei dieser Entwicklung um die einer einzelnen Komponente handelt, schließt der Komponententest mit dem Testbericht die Entwicklung ab.
{| class="mw-datatable"
! style="font-weight: bold;" | ID
! style="font-weight: bold;" | Testfallbeschreibung
! style="font-weight: bold;" | Eingang <math>_Kv_{Lx} [\mbox{m/s}]</math>
! style="font-weight: bold;" | Eingang <math>_Kv_{Rx}[\mbox{m/s}] </math> 
! style="font-weight: bold;" | Erwartetes Ergebnis
! style="font-weight: bold;" | Testergebnis
! style="font-weight: bold;" | Testperson
! style="font-weight: bold;" | Datum
|-
| 1
| Das Fahrzeugmodell steht.
| 0
| 0
| Alle Ausgänge sind Null.
| OK
| Prof. Göbel
| 10.06.2017
|-
| 2
| Das Fahrzeugmodell fährt eine Rechtskurve.
| 1
| 0
| Rechtskurve: Negative Gierrate, negativer Gierwinkel.
| OK
| Prof. Göbel
| 10.06.2017
|-
| 3
| Das Fahrzeugmodell fährt eine Linkskurve.
| 0
| 1
| Linkskurve: Positive Gierrate, positiver Gierwinkel.
| OK
| Prof. Göbel
| 10.06.2017
|-
| 4
| Das Fahrzeugmodell fährt geradeaus.
| 1
| 1
| Keine Gierrate und Gierwinkel, <math>_Kv_M = _Kv_L = _Kv_R</math>.
| OK
| Prof. Göbel
| 10.06.2017
|-
|}

Als Abschluss zeigt die letzte Abbildung dieses Artikels eine Ergebnisdarstellung der Fahrzeugbewegung in I-Koordinaten (Draufsicht).
[[Datei:Fahrzeugmodell_kinematisch_3Rad_Ergebnis.pdf|rechts|mini|400px|Ergebnisdarstellung für die Modellierung eines 3-rädrigen Fahrzeugs in einer x-y-Draufsicht]]

== Fazit ==
Das Modell ist fertig und funktioniert wie gewünscht! Somit ist die Entwicklung von Algorithmen möglich, ohne ein Fahrzeug in Hardware zur Verfügung zu haben. Dadurch eröffnen sich ernorme Möglichkeiten wie z. B. simultanes Entwickeln, automatisiertes Testen, simulative Auslegung von Reglern u. s. w.!

== Literaturverzeichnis ==
<references />



----
→ zurück zum Hauptartikel: [[Projekt_ArduMower|Projekt ArduMower]]