Warum nicht auch Themen in einem Blogpost verarbeiten, die in der Uni Beachtung fanden und das Interesse auf eine eigene Implementierung wecken? In diesem ersten von zwei Teilen geht es darum, eine Simulationsumgebung für ein Auto zu schaffen, das später diverse Wege und Einparkroutinen findet. Das ganze Funktioniert über Rapidly-Exploring Random Trees, indem man einfach losfährt und dadurch einen Suchbaum aufspannt. Keine Wegfindung, keine Heuristik oder Gradientenfelder, aber vieeele Knoten und Versuche.

Langweilig denkt Ihr? Wenn ihr kein Interesse an dem eigenlichen Thema habt, so lernt ihr doch einiges über C# und objektorientierte Programmierung, welche uns das Leben wieder mal einfacher macht. Den aktuellen Sourcecode findet ihr unten im Anhang, da doch einige “unnötige” Sachen wie Properties und Hilfsklassen nicht gepostet werden. Hier schonmal eine kleine Vorschau, was die Simulation am Ende vom Post mit wenigen Codezeilen kann:

Los ans Coden, beginnen wir mit der abstrakten Enviroment-Klasse. Von einer abstrakten Klasse kann keine Instanz (= kein Objekt) erstellt werden, und das ist auch gut so. Denn die Environment-Klasse dient nur als grobes Schema für alle weiteren Szenarien, daher halten wir sie sehr allgemein. Zunächst reicht es zu überlegen, was eine Simulationsumgebung braucht: Eine Höhe und Breite (ohne Einheit, aber rechnen wir in Metern) , natürlich ein sich bewegendes Objekt (Auto, Lastwagen mit Anhänger, Flugzeug) und eine Liste von Hindernissen, die es zu vermeiden gilt. In Code ausgedrückt sieht es sehr ähnlich aus:

public class Environment
{
    protected Int32 _height;
    protected Int32 _width;
 
    protected List _obstacles;
 
    protected MovingEntity _movingEntity;
 
    public Int32 Height { get { return _height; } }
    public Int32 Width { get { return _width; } }
    public MovingEntity MovingEntity {  get { return _movingEntity; }  }
 
    public Environment(int width, int height, MovingEntity entity)
    {
        _width = width;
        _height = height;
 
        _movingEntity = entity;
        _obstacles = new List();
    }

Damit sind wir fast fertig, fehlt noch nur die generelle Methode zur Kollisionserkennung, welche aber einfach ist: Wir prüfen, ob alle Hindernisse nicht mit der aktuellen MovingEntity kollidieren und liefern dementsprechend true oder false zurück:

public bool IsMovingEntityCollisionFree()
{
        foreach (var obstacle in _obstacles)
                if (obstacle.CollidesWith(_movingEntity))
                    return false;
        return true;
}

Die Klassen MovingEntity und Obstacle sind noch nicht definiert. Zum Glück haben beide Klassen Gemeinsamkeiten als man zunächst annimmt! Beide haben eine Position, eine Größe und eine Rotation. Position und Größe ist ein 2D Vektor, die Rotation in Radians ist ein float. All das packen wir in die abtrakte Klasse Entity. Das Thema Kollisionserkennung ist später noch ein Thema, das ich an dieser Stelle ausklammern möchte. Nur soviel: Die Entity kann auch einen rotierten Polygonzug zurückgeben, mit dem wir später auf Kollisionen prüfen können.

Was unterscheidet jetzt die Entity von der MovingEntity? Natürlich dass sie sich bewegen kann. Daher bekommt die MovingEntity, die von Entity erbt, eine abtrakte Methode Move spendiert. Da wir alles allgemein halten wollen ist das ein Array aus Eingängen (Geschwindigkeit, Einlenkwinkel …) die später mit der Simulation verrechnet werden. Des weiteren haben wir eine StepSize im Parameter, da später eine Schrittgröße zum Simulieren gebraucht wird.

public abstract class MovingEntity : Entity
{
    public abstract void Move(float[] u, float stepSize);
}

Was unterscheidet jetzt ein Obstacle von einer Entity? Nunja, eigentlich nur dass ein Obstacle mit einer Entity Kollidieren kann. Daher bekommt diese Klasse eine abstrakte CollidesWith-Methode eingebaut.

public abstract class Obstacle : Entity
{
    public Obstacle(Vector  position, Vector  size, float rotation) : base(position, size, rotation)
    {
    }
 
    public abstract bool CollidesWith(Entity entity);
}

Damit hätten wir den allgemeinen Fall erschlagen, werden wir ein wenig Konkreter. Zu Beginn wollen wir nur boxförmige Hindernisse haben (einfache Kollisionserkennung, man kann daraus Korridore aufbauen etc.). Daher gibts nun die Klasse BoxObstacle, die natürlich von Obstacle erbt.

public class BoxObstacle : Obstacle
{
    public BoxObstacle(Vector  position, Vector  size, float rotation) : base(position, size, rotation)
    {
    }
 
    public override bool CollidesWith(Entity entity)
    {
        if (Helper.ArePolygonsIntersecting(this.PolygonRotated, entity.PolygonRotated))
            return true;
 
        return false;
    }
}

Während im Konstruktor einfach die Werte weitergereicht werden, überschreiben wir die CollidesWith Methode. Diese prüft, ob sich die rotierten Polygonzüge des eigenen Obstacles und der zu prüfenden Entity überschneiden.

Was fehlt nun? Natürlich die MovingEntity, in diesem Fall ein Auto. Bevor ich euch die Bewegungsgleichungen um den Kopf haue, erstmal etwas Theorie: Bei einem Auto handelt es sich um ein nicht holonomes System. Ein Auto hat im 2D Fall eine X und Y Position, sowie eine Rotation. Allerdings können wir die Position und Rotation nur über die Änderung der Steuerung erreichen und die Werte nicht direkt setzen. Ein Auto kann ja schließlich nicht seitwärts fahren, es ist beschränkt in der Bewegung und in den Freiheitsgraden. Damit haben wir nur zwei Eingänge (Geschwindigkeit und Lenkwinkel), aber drei Variablen die das System beschreiben. Wenn wir weniger Eingäge als Variablen haben, die das System beschreiben, so heißt das System nicht holonom.

Der mathematische Teil ist bald vorrüber, wir müssen die Bewegung nur noch so implementieren. Was dahintersteckt muss nicht zwangsweise verstanden werden Die obigen Gleichungen beschreiben das eingeschränke Fahrverhalten des Autos. Es bewegt sich in X-Richtung mit der Geschwindigkeit v und dem Cosinus von der aktuellen Autorotation Theta, ähnlich wie in Y-Richtung mit dem Sinusanteil. Die aktuelle Rotation ändert sich über den Lenkwinkel Phi, wobei noch die Länge des Autos mit einfließt. In C# sehen diese Bewegungsgleichungen recht ähnlich aus:

public override void Move(float[] u, float stepSize)
{
    float v = u[0];
    float phi = u[1];
 
    _lastPosition = _position;
    _lastRotation = _rotation;
 
    this._position.X += (float)(stepSize * Math.Cos(this.Rotation) * v);
    this._position.Y += (float)(stepSize * Math.Sin(this.Rotation) * v);
    this._rotation += (float)(stepSize * Math.Tan(phi) * (v / this.Size.X));
}

Als Eingänge haben wir die Geschwindigkeit und den Lenkwinkel und ändern somit die aktuelle Autoposition. Multipliziert mit der aktuellen Schrittweite der Simulation entspricht das einem expliziten Euler-Verfahren zur numerischen Integration.

Damit ist die Mathematik abgehakt und die Simulation kann laufen. Ich habe dazu eine weitere Klasse ParkEnvironment erstellt, die einfach ein Obstacle in der Mitte hinstellt. Lassen wir doch das Auto im Kreis fahren und dabei immer mit einer Box kollidieren, wie im obigen Gif gezeigt. Wir stellen den Lenkwinkel auf 30 Grad und sagen, es sollte mit konstanter Geschwindigkeit 10 px = 2 m/s fahren.

var car = new Car(200, 200);
var env = new ParkEnvironment(car);
 
for (int i = 0; i < 10000; i++)
{
    // Velocity = 10 (= 2 m/s), Angle = 30°
    car.Move(new float[] {10f, (float)Helper.DegreeToRadian(30)},0.1f);
    System.Threading.Thread.Sleep(10);
}

Wir lassen die Simulation 10000 Schritte laufen. Der Code zum Zeichnen der Obstacles und des Autos wurde rausgenommen, seht ihr dann im eigentlichen Projekt. Damit haben wir alle Vorraussetzungen, um das Auto durch enge Wege zu leiten und in die engsten Parklücken zu lotsen!

Binaries: RandomTree_Car (22 Downloads)

Sourcecode + Binaries: RandomTree_Car_src.rar (26 Downloads)

P.S. Nachwort zur Kollisionserkennung: Im Prinzip nutze ich einen Code von Codeproject zur 2D Polygon Kollisionserkennung. Dabei erstelle ich die 4 Ecken des Autos bzw. des Obstacles als Polygonzug und rotiere die Punkte über die von .NET bereitgestellten Matrixklassen um die Z-Achse. Mehr Infos dazu gibts u.a. hier auf Stackoverflow oder direkt in der Helperklasse

6 people like this post.

Like

Unlike