Motion Planning for Nonholonomic Vehicles with Space Exploration Guided Heuristic Search

The development of the modern sensing, actuation, communication, and computation technology unfolds a promising future of intelligent mobile robots, especially for autonomous automobiles. Motion planning for mobile robots is one of the most important software components in an autonomous system, as it is responsible for a maneuver strategy or a motion trajectory while considering the prior knowledge of the world, real-time perception, and domain specific rules. Particularly, a motion planning problem for an intelligent vehicle is beyond a theoretical problem of finding an executable collision-free trajectory. An autonomous vehicle should also behave rationally in traffic, which includes following a global route from the navigation, adapting local behaviors according to its circumstances, obeying traffic rules, and considering the convenience of the human traffic participants. Furthermore, the different driving tasks are highly diverse, usually requiring a specific optimized motion planning method for each maneuver type. In this case, a motion planning agent should be able to handle the available information with domain knowledge, select efficient algorithms for specific tasks, and integrate them seamlessly. In this dissertation, a Space Exploration Guided Heuristic Search (SEHS) method is introduced as a base framework for mobile robot motion planning. Several extensions of SEHS such as Orientation-Aware Space Exploration Guided Heuristic Search (OSEHS) and Space Time Exploration Guided Heuristic Search (STEHS) are developed for specific autonomous driving scenarios. They can be integrated in a hierarchical planning architecture with a high-level symbolic task planning to process information in different layers and to achieve online motion planning for mobile robots, especially autonomous vehicles. The general idea of the Space Exploration Guided Heuristic Search approach is to boost the forward search algorithm in a continuous state space with heuristics of the space knowledge such as topology and dimension. The SEHS framework consists of two sequential procedures: (1) An exploration phase investigates a low dimensional sub-space with distance queries for a path corridor between the start and goal locations, (2) a heuristic search algorithm propagates robot states with primitive motions following the path corridor to construct a trajectory from the start state to the goal state. The space knowledge obtained in the exploration phase enables the search algorithm to adapt the step size of primitive motions and the state resolution regarding the free-space dimension. Furthermore, as a generic exploration and search framework, the SEHS method can flexibly scale the sub-space to include robot orientation for maneuvers in narrow places as Orientation-Aware Space Exploration Guided Heuristic Search, or the time domain for dynamic scenarios as Space Time Exploration Guided Heuristic Search. A Motion Planning Engine (MPE) is developed as a real-time planning entity for autonomous driving based on the hybrid planning schema. Task planning is introduced as the middle layer between route planning and motion planning. It evaluates symbolic states of the system with traffic domain knowledge for a sequence of driving tasks to accomplish the driving mission. Each task can be handled as a motion planning problem, which is constrained in an appropriate context so that it can be efficiently solved with a suitable planning algorithm in the low-level planning layer. Thus, the MPE provides interfaces to the data such as road-map, object-list, and traffic rules, produces motions with steering and acceleration commands, and guarantees safety during the whole driving process. In addition, a C++ library, named AutoDrive for autonomous driving, is developed. The library consists of packages for general math utilities, vehicle kinematic models, environment models, and various planning algorithms. A simulation environment is also provided to visualize planning procedures and results in different traffic scenarios. The goal is to provide a compact essential software base for education, research, and prototype development for mobile robots and autonomous driving applications. Die Entwicklung der modernen Sensorik, Aktorik, Kommunikations- und Rechentechnik lässt eine vielversprechende Zukunft von intelligenten mobilen Robotern erwarten, vor allem für autonome Fahrzeuge. Bewegungsplanung für mobile Roboter ist eine der wichtigsten Software-Komponenten in einem autonomen System, da es für die Manöverstrategie oder die Bewegungstrajektorie verantwortlich ist. Die gilt vor allem in Bezug auf Vorwissen der Welt, Echtzeit-Wahrnehmung und domänenspezifische Vorschriften. Insbesondere ist ein Bewegungsplanungsproblem für ein intelligentes Fahrzeug mehr als das theoretische Problem, eine ausführbare kollisionsfreie Trajektorie zu finden. Ein autonomes Fahrzeug sollte sich auch im Straßenverkehr rational verhalten. Das umfasst das Folgen eines globalen Pfads einer übergeordneten Navigationskomponente, eine Anpassung des lokalen Verhaltens an äußere Umstände, sowie die Befolgung von Verkehrsregeln. Dies alles, unter Berücksichtigung des Komforts menschlicher Verkehrsteilnehmer. Darüber hinaus sind verschiedene Fahraufgaben sehr vielfältig, was in der Regel eine spezifisch optimierte Bewegungsplanungsmethode für den einzelnen Manövertyp erfordert. In diesem Fall sollte ein Bewegungsplanungsagent in der Lage sein, die verfügbaren Informationen mit Domänenkenntnissen zu behandeln, effiziente Algorithmen für bestimmte Aufgaben zu wählen und alle Funktionen nahtlos zu integrieren. In dieser Dissertation wird eine Space Exploration Guided Heuristic Search (SEHS) Methode als Basis für die Bewegungsplanung mobiler Roboter eingeführt. Mehrere Erweiterungen von SEHS wie Orientation-Aware Space Exploration Guided Heuristic Search (OSEHS) und Space Time Exploration Guided Heuristic Search (STEHS) sind für bestimmte autonome Fahrszenarien entwickelt worden. Sie können in einer hierarchischen Planungsarchitektur mit einer symbolischen High-Level-Aufgabenplanung integriert werden, um Daten in verschiedenen Schichten zu verarbeiten zur Realisierung einer Online-Bewegungsplanung für mobile Roboter, insbesondere für autonome Fahrzeuge. Die Grundidee des Space Exploration Guided Heuristic Search Ansatzes ist es, den Vorwärtssuchalgorithmus in einem kontinuierlichen Zustandsraum mit raumbasierter Heuristiken zu unterstützen, wie etwa dem Wissen über Topologie und Abmessung des Raumes. Das SEHS Framework besteht aus zwei aufeinanderfolgenden Verfahren: (1) Eine Explorationsphase untersucht einen niedrigdimensionalen Unterraum mit Abfragen über Abstände um einen Pfadkorridor zwischen Start- und Zielpose zu finden, (2) ein heuristischer Suchalgorithmus propagiert Zustände des Roboters mit Bewegungsprimitiven zum Korridorweg um eine Trajektorie vom Startzustand zum Zielzustand zu konstruieren. Die Kenntnisse über den Raum, die in der Explorationsphase erhalten werden, ermöglichen es dem Suchalgorithmus, die Schrittweite der Bewegungsprimitive und die Auflösung der Zustände zur Freiraumdimension anzupassen. Ferner lässt sich mit dem SEHS Verfahren als generisches Explorations- und Such-Framework der Unterraum flexibel skalieren, um etwa die Ausrichtung des Roboters für Manöver in engen Stellen bei Orientation-Aware Space Exploration Guided Heuristic Search, oder die Zeitdomäne für dynamische Szenarien bei Space Time Exploration Guided Heuristic Search. Eine Motion Planning Engine (MPE) wird als Echtzeit-Planungsinstanz für autonomes Fahren auf Basis eines Hybridplanungsschemas entwickelt. Aufgabenplanung wird als eine Mittelschicht zwischen Routenplanung und Bewegungsplanung eingeführt. Diese evaluiert die symbolischen Zustände des Systems mit domänenspezifischem Wissen über den Verkehr für eine Folge von Fahraufgaben um die Fahrmission auszuführen. Jede Aufgabe kann als ein Bewegungsplanungsproblem dargestellt werden, das in einem geeigneten Kontext beschränkt wird, so dass es mit einem geeigneten Planungsalgorithmus in der Low-Level Planung gelöst werden kann. Somit bietet die MPE Schnittstellen zu den Daten wie Fahrplan, Objekt-Liste und Verkehrsregeln, produziert Bewegungen mit Lenk- und Beschleunigungsbefehlen und gewährleistet die Sicherheit während des gesamten Fahrvorgangs. Darüber hinaus wurde eine C++ Bibliothek mit dem Namen AutoDrive für autonomes Fahren entwickelt. Die Bibliothek besteht aus Paketen mit Funktionen für allgemeine Mathematik, Fahrzeugkinematik, Umwelt-Modelle und verschiedenen Planungsalgorithmen. Eine Simulationsumgebung wurde ebenfalls bereitgestellt, um Planungsverfahren und Ergebnisse in verschiedenen Verkehrsszenarien zu visualisieren. Das Ziel ist es, eine kompakte essenzielle Software-Basis für Bildung, Forschung und die Entwicklung von Prototypen von mobilen Robotern oder autonomen Fahranwendungen zu bieten.