Receding Horizon Trajectory Optimization Through Waypoints and Path Segments

Abstract

In der klassischen Industrierobotik werden anwendungsspezifische Roboterbewegungen für statische Umgebungen geplant und wiederholt ausgeführt. Im Gegensatz dazu werden Roboter zunehmend in dynamischen Umgebungen eingesetzt, was hohe Anforderungen an die Echtzeitplanung der Robotertrajektorie stellt, um auf Änderungen in der Umgebung oder der Aufgabe reagieren zu können. Die Aufgaben ähneln oft den bisherigen Anwendungen wie beispielsweise dem Aufnehmen und Ablegen von Objekten oder der Oberflächenbearbeitung. Die Platzierung der Objekte ist jedoch nicht statisch und kann sich während der Ausführung ändern. Der Bewegungsablauf in Bezug auf die Objekte, wie beispielsweise ein Bearbeitungspfad auf der Oberfläche oder eine Einführbewegung eines Fügeprozesses, ist durch Wegpunkte und geometrische Pfade genau definiert. Die dynamische Umgebung erfordert jedoch zusätzliche Freiraumbewegungen des Roboters, um das Zielobjekt zu erreichen. Darüber hinaus spielt die Interaktion mit Menschen eine zunehmend wichtige Rolle, wobei Aufgaben wie die Übergabe von Objekten reaktive Bewegungsabläufe erfordern.Zur Lösung dieser Herausforderungen sind Ansätze erforderlich, die einerseits die Planung komplexer Roboterbewegungen ermöglichen und andererseits eine schnelle Reaktion auf Änderungen in der Umgebung oder der Aufgabe gewährleisten. Im gegenwärtigen Stand der Technik stellt die Echtzeitfähigkeit der Bewegungsplanung stets einen Kompromiss bei der Aufgabenkomplexität dar. Roboterbewegungen, die durch Wegpunkte oder geometrische Pfade eingeschränkt sind, werden typischerweise mit Offline-Planungsalgorithmen wie sampling-basierten Planern oder Trajektorienoptimierung geplant, die nicht echtzeitfähig sind. Im Gegensatz dazu können echtzeitfähige Planungsalgorithmen solche Beschränkungen nicht berücksichtigen. In dieser Arbeit werden Algorithmen auf Basis der modellprädiktiven Trajektorienoptimierung vorgestellt, die die Planung von Robotertrajektorien für komplexere Aufgaben mit zahlreichen Beschränkungen und gleichzeitig eine Reaktion auf Änderungen in der Umgebung ermöglichen. Die modellprädiktive Trajektorienoptimierung umfasst die Planung von Trajektorien über einen kurzen Planungshorizont, um eine Echtzeitplanung zu ermöglichen. Die systematische Einbeziehung von Beschränkungen in Form von Wegpunkten oder Pfadsegmenten, die sich aus der Aufgabe ergeben, stellt jedoch für kurze Planungshorizonte eine Herausforderung dar.Um diese Probleme zu lösen, wird zunächst ein neuartiges modellprädiktives Regelungskonzept (model-predictive control, MPC) mit Wegpunkten für die Echtzeitplanung von Robotertrajektorien vorgestellt, das die Planung durch Wegpunkte im Gelenkraum erlaubt. Die Hauptidee besteht darin, den Planungshorizont am Wegpunkt zu teilen, wenn dieser innerhalb des aktuellen Planungshorizonts erreichbar ist, und die Horizontlänge in Richtung der erreichbaren Wegpunkte und Zielpunkte zu reduzieren. Aufgrund der Planung über einen kurzen Horizont gewährleistet der Algorithmus Echtzeitfähigkeit und ermöglicht eine reaktive Anpassung an Veränderungen in der Umgebung des Roboters. Im Vergleich zu globalen Planungsalgorithmen wie RRT, VP-STO und dem Verfolgen zuvor optimierter Trajektorien mit MPC erfordert der vorgestellte Algorithmus deutlich weniger Planungszeit bei vergleichbaren Pfadlängen und Trajektoriendauern. Darüber hinaus wird die Fähigkeit des vorgestellten Algorithmus zur dynamischen Online-Neuplanung von Trajektorien experimentell an einem KUKA LBR iiwa 14 R820 Roboter in einem dynamischen Pick-and-Place-Szenario demonstriert.Der Planungsalgorithmus wird dann erweitert, um direkt mit Wegpunkten im Aufgabenraum des Roboters zu planen. Dabei können Freiraumbewegungen und vordefinierte Pfadsegmente beliebig kombiniert werden, auch für Sequenzen, die den Planungshorizont überschreiten. Je nach Erreichbarkeit der Wegpunkte kann der Horizont in mehrere Segmente und Wegpunkte aufgeteilt werden. Für jedes Horizontsegment können Beschränkungen für Freiraumbewegungen oder die Pfadfolge festgelegt werden. Damit lassen sich Bewegungen für die zuvor genannten Aufgaben sowohl mit fest vorgegebenen Prozessen als auch mit freien Bewegungen dazwischen beschreiben. Zusätzlich können aufgabenspezifische Toleranzen, wie beispielsweise Positions- oder Orientierungsabweichungen, für die Wegpunkte und Pfadsegmente berücksichtigt werden. Die Toleranzen ermöglichen es dem vorgestellten Planungsalgorithmus, auch für kinematisch anspruchsvolle Probleme Lösungen zu generieren. Die durchgeführten Experimente zeigen, dass die Segmentierung des Planungshorizonts zu einer signifikanten Reduktion der Trajektoriendauer im Vergleich zu herkömmlichen modellprädiktiven Punkt-zu-Punkt-Planungsmethoden führt. Im Vergleich zur traditionellen modellprädiktiven Pfadfolgeregelung ermöglicht die Integration von Freiraumbewegungen dem Planer die Bewältigung komplexerer Szenarien. Die Qualität der generierten Trajektorien wird beim Zeichnen auf einer gekrümmten Oberfläche und beim Fügen von Bauteilen in einem Assemblierszenario mit Offline-Planungsalgorithmen verglichen. Die Ergebnisse zeigen, dass eine substanzielle Verkürzung der Planungszeiten bei gleichzeitiger Aufrechterhaltung einer hohen Trajektorienqualität und Erfolgsrate erreicht wird. Die Fähigkeit des vorgestellten Algorithmus zur dynamischen Neuplanung in Echtzeit wird anhand einer Roboter-Zeichenaufgabe mit einem KUKA LBR iiwa 14 R820 Roboter veranschaulicht, bei der der Roboter einen Pfad auf einem Whiteboard zeichnet, das während des Zeichnens durch einen Menschen bewegt wird.

In classical industrial robotics applications, application-specific robot motion is planned for static environments and executed repeatedly. Conversely, robots are increasingly applied in dynamic environments that demand real-time trajectory planning, enabling the robot to respond to changes in the environment or the task. The tasks are often similar to those in classical applications, such as pick-and-place or surface processing. However, the objects’ poses in the robot’s task space are not static and can change during trajectory execution. The motion relative to the object is clearly defined by waypoints and geometrical paths, e.g., a movement along an object surface or an insertion motion of a joining application. However, the dynamic environment requires additional robot motions in free space to reach the object. Furthermore, there is an increasing demand for human-robot collaboration, where tasks such as object handovers require reactive motion planning.To address these challenges, motion planning approaches are needed that can plan complex robot motion while also quickly reacting to changes in the environment or the task. In the current state of the art, reactive real-time planning is always a compromise with respect to the task complexity. Robot motions constrained by waypoints or geometrical paths are typically planned using offline planning algorithms such as sampling-based planners or trajectory optimization, which are not real-time capable. In contrast, real-time capable planning algorithms cannot take such constraints into account. This work proposes algorithms based on model-predictive trajectory optimization that plan a robot’s trajectories for complex, highly constrained tasks while allowing fast replanning to account for changes in the environment. Model-predictive trajectory optimization is a method that plans trajectories over a short planning horizon to enable real-time planning. However, accounting for systematic constraints, such as waypoints and path segments resulting from the task description, is especially difficult for short planning horizons.A novel model-predictive control (MPC) concept with waypoints for real-time planning of robot trajectories is presented to address these difficulties, enabling joint-space planning through waypoints. The main idea is to split the planning horizon at waypoints that are reachable within the current planning horizon, and to shorten the horizon toward the reachable waypoints and the goal point. Due to planning over a short horizon, the algorithm ensures real-time capability and enables reactive adaptation to changes in the robot’s environment. Compared to global planning algorithms such as RRT and VP-STO, and to tracking previously optimized trajectories with MPC, the presented algorithm requires significantly less planning time while maintaining comparable path lengths and trajectory durations. Furthermore, the capability of the presented algorithm for dynamic online replanning of trajectories is experimentally demonstrated on a KUKA LBR iiwa 14 R820 robot in a dynamic pick-and-place scenario.The planning algorithm is extended to plan directly with waypoints in the robot’s task space. Free-space movements and predefined path segments can be arbitrarily combined, even for sequences that exceed the planning horizon. Depending on the reachability of the waypoints, the horizon can be divided into multiple path segments and waypoints. For each horizon segment, constraints on free-space movements or path-following can be defined. This allows movements for the tasks mentioned above to be described with both rigidly specified processes and free-space movements in between. Additionally, task-specific tolerances, such as position or orientation deviations, can be considered for each waypoint and path segment. The tolerances enable the presented planning algorithm to generate solutions even for kinematically demanding problems. The experiments conducted show that segmentation of the planning horizon results in a significant reduction in trajectory duration compared to conventional model-predictive point-to-point planning methods. Compared to traditional model-predictive path-following control, integrating free-space movements enables the planner to handle more complex scenarios. The quality of the generated trajectories is evaluated against offline planning algorithms, particularly for drawing on a curved surface and inserting components in an assembly scenario. The results demonstrate substantial reductions in planning time while maintaining high trajectory quality and a high success rate. The capability of the presented algorithm for dynamic replanning in real time is illustrated through a robot drawing task with a KUKA LBR iiwa 14 R820 robot, in which the robot draws a path on a whiteboard that a human moves during the drawing process.