Optimisation based motion planning for non-holonomic vehicles in narrow environments

Abstract

Fahrzeuge auf engem Raum zu manövrieren stellt selbst für erfahrene Fahrer eine Herausforderung dar. Durch die Nähe zu den Hindernissen sind präzise Lenkvorgänge und gefühlvolles Beschleunigen beziehungsweise Bremsen notwendig. Abhilfe kann ein Fahrassistenzsystem schaffen, das das Fahrzeug autonom von der aktuellen Position zu einer gewünschten Zielposition fährt. Dabei muss natürlich sichergestellt werden, dass keine Kollisionen mit Hindernissen oder anderen Fahrzeugen auftreten. Außerdem sollte der gefahrene Weg ähnlich dem eines menschlichen Fahrers sein und keine unnötigen Umwege oder Richtungswechsel aufweisen. Eine mögliche Anwendung für so ein Fahrassistenzsystem ist eine automatische Einparkhilfe. Die Schwierigkeit dabei entsteht durch die nichtholonomen Beschränkungen des Autos: Das Auto kann weder seitwärts fahren noch sich im Stand drehen. Außerdem muss das Fahrzeug sehr nah an die Hindernisse heranfahren und benötigt meist mehrere Richtungswechsel. Diese Eigenschaften erschweren die Planung eines geometrischen Pfades, so dass eine echtzeitfähige Berechnung mit klassischen Verfahren der Pfadplanung aus der Robotik nicht möglich ist. Deshalb wird in dieser Arbeit ein neues Pfadplanungskonzept, das auf Einparkvorgänge zugeschnitten ist, vorgestellt. Dafür werden die Differentialgleichungen des Autos bezüglich des Pfades diskretisiert und für die resultierenden diskreten Pfadsegmente ein statisches Optimierungsproblem formuliert. Durch geeignete Wahl der Kostenfunktion kann unabhängig vom Parkszenario ein Pfad gefunden werden. Dieses Optimierungsproblem kann numerisch sehr schnell gelöst werden. Durch das eingeprägte lokale Verhalten müssen zusätzlich heuristische Regeln für eventuell benötigte Richtungswechsel formuliert werden, die so gewählt wurden, dass sie das Verhalten eines menschlichen Fahrers imitieren. Aufgrund des lokalen Verhaltens dieser Methode können komplexe Szenarien nicht bewältigt werden. Um auch in diesen Fällen einen realisierbaren Pfad planen zu können, werden zwei globale Erweiterungen vorgestellt. In der ersten Erweiterung wird die analytische Lösung der Differentialgleichungen des Autos benutzt um eine für die Pfadplanung verbesserte Position zu erreichen. Von dieser neuen Position aus ist die Wahrscheinlichkeit, dass der lokale Pfadplaner einen Pfad zur Parkposition findet, deutlich erhöht. In der zweiten Erweiterung wird ein Baum bestehend aus Wegpunkten aufgebaut, welche implizit durch den lokalen Pfadplaner erstellt werden. Dadurch wird der freie Raum effektiv durchsucht und Pfade selbst für komplexe Umgebungen wie beispielsweise in einer Parkgarage gefunden. Die Konvergenz des Pfadplaners sowie der beiden Erweiterungen wird mittels Monte-Carlo Simulationen verifiziert und mit drei auf Fahrzeuge spezialisierte Pfadplaner der Literatur, darunter der für autonomes Fahren berühmte Hybrid A* Algorithmus, verglichen. Um eine realisierbare Trajektorie zu erhalten, muss der geometrische Pfad in der Zeit parametriert werden. Dafür wird auf Basis der Eigenschaft der differentiellen Flachheit des mathematischen Modells des Autos ein lineares Eingrößensystem formuliert. Eine Stellgröße für dieses System, die die gegebenen Randbedingungen berücksichtigt, wird mit Hilfe eines Optimalsteuerungsproblems berechnet wird. Dadurch wird der geometrische Pfad in kurzer Zeit unter Berücksichtigung der physikalischen Beschränkungen des Autos durchlaufen. Um der Trajektorie unter realen Bedingungen folgen zu können wird ein Regler basierend auf der Lyapunovtheorie entworfen. Die asymptotische Konvergenz des Trajektorienfolgesystems wird mit Hilfe von Barbalats Lemma gezeigt. Anhand zahlreicher Simulationen wird die Zeitparametrierung und das Verhalten des Reglers getestet. Im zweiten Teil der Arbeit wird die Pfadplanung für einen Lastwagen mit Anhänger auf engem Raum betrachtet. Dieses System ist nichtholonom, hat keinen flachen Ausgang und ist instabil bei Rückwärtsfahrt. Daher gestaltet sich die Pfadplanung besonders aufwändig. Um dieses Problem zu lösen wird zuerst ein initialer Pfad von der Parkposition zu der Umgebung der Startposition geplant. Anschließend wird mit Hilfe eines Optimalsteuerungsproblems dieser initiale Pfad beginnend von der exakten Startposition verfolgt. Anstatt das Optimalsteuerungsproblem für den gesamten Pfad auf einmal zu lösen, wird nur ein kurzes Teilstück des Pfades betrachtet und der ganze Pfad iterativ berechnet. Das dadurch entstehende lokale Verhalten wird wiederum verwendet um heuristische Regeln für Richtungswechsel einzuführen. Die Effektivität des Pfadplaners wird mittels Monte-Carlo Simulationen für das Beispiel einer Ladebucht demonstriert.

Manoeuvring vehicles in narrow environments is a challenging task even for experienced drivers. The short distance to the obstacles necessitates precise steering and velocity control. Thus, a driving assistance system which autonomously manoeuvres the vehicle from the current position to a desired target position is a helpful tool for the driver. Obviously, this assistance system has to avoid collisions. Furthermore, the designed path should be similar to the path typically chosen by a human driver by avoiding an unnecessary large number of driving direction changes. One popular example for a driving assistance system in narrow environments is a car parking system. In this context, challenges arise from the non-holonomic characteristics of the car, i.e. a pure sidewards movement and turning without moving are not possible. Additionally, short driving distances in close vicinity to obstacles have to be performed. Therefore, calculating a geometric path which connects the starting with the parking position cannot be done under reasonable computing costs in a straightforward way. In this thesis, a path planning concept tailored to car parking is proposed. The presented approach discretises the differential equations of the car with respect to the path parameter. For the resulting discrete path segments, a static optimisation problem is formulated. The cost function of this optimisation problem is designed in a way that all common parking scenarios can be handled. The optimisation problem can be solved numerically in a very efficient way. Due to the local nature of the algorithm, heuristic rules for driving direction changes are formulated which intend to imitate the behaviour of a human driver. The drawback of the local nature is the lack of global information to handle more complex scenarios. To overcome this problem, two guidance schemes using global information are proposed. In the first scheme, the analytic solution of the differential equations is used to improve the position and orientation of the car along a street. From this improved position, the likelihood of finding a feasible path into the parking spot with the local path planner is increased. The second guidance scheme builds up a tree of landmarks. These landmarks are implicitly created by the local path planner. Thus, the free space is explored in an efficient way also in the vicinity of obstacles. This enables the path planner to handle more complex scenarios like parking at a parking deck or in a basement garage. The performance of the local path planner and the two guidance schemes is evaluated utilising extensive Monte-Carlo simulations. The results are compared to three different path planners for car and car-like robots known from literature. To obtain a feasible trajectory into the parking spot, the geometric path has to be parametrised in time. Utilising the flatness property of the car, the time parametrisation problem is simplified to find a suitable control input for a linear single input single output system. An optimal control problem is formulated to obtain a time optimal solution which respects the physical constraints of the car. To account for model uncertainties and disturbances, a Lyapunov-based feedback controller is designed for the trajectory error system. Utilising Barbalat's lemma, asymptotic convergence of the trajectory error dynamics is proven. Simulation studies show the applicability and efficiency of the proposed time parametrisation and feedback control law. A second key topic of this thesis is path planning for a truck with attached trailer in narrow environment. This system is non-holonomic, has no flat output and is not stable in backward driving direction. These properties pose a major challenge for path planning. In contrast to path planning for a car, a purely local approach does not yield feasible paths. Thus, a two-step approach is presented. First, an initial path is planned from the target position to the neighbourhood of the starting position using a fast tree-based path planner. This initial path is used as global information in the second step. Thereby, the initial path is followed beginning from the starting position. To this end, an optimal control problem is solved for a small distance of the path. This process is repeated along the path until the desired target position is reached. The induced local behaviour is again used to formulate heuristic rules for driving direction changes. The performance of the proposed path planner is evaluated in Monte-Carlo simulation studies at a loading bay.