Unsupervised trajectory compression

We present a method for compressing trajectories in an unsupervised manner. Given a set of trajectories sampled from a space we construct a basis for compression whose elements correspond to paths in the space which are topologically distinct. This is achieved by computing a canonical representative for each element in a generating set for the first homology group and decomposing these representatives into a set of distinct paths. Trajectory compression is subsequently accomplished through representation in terms of this basis. Robustness with respect to outliers is achieved by only considering those elements of the first homology group which exist in the super-level sets of the Kernel Density Estimation (KDE) above a threshold. Robustness with respect to small scale topological artifacts is achieved by only considering those elements of the first homology group which exist for a sufficient range in the super-level sets. We demonstrate this approach to trajectory compression in the context of a large set of crowd-sourced GPS trajectories captured in the city of Chicago. On this set, the compression method achieves a mean geometrical accuracy of 108 meters with a compression ratio of over 12

CTopPRM: Clustering Topological PRM for Planning Multiple Distinct Paths in 3D Environments

In this paper, we propose a new method called Clustering Topological PRM (CTopPRM) for finding multiple homotopically distinct paths in 3D cluttered environments. Finding such distinct paths, e.g., going around an obstacle from a different side, is useful in many applications. Among others, using multiple distinct paths is necessary for optimization-based trajectory planners where found trajectories are restricted to only a single homotopy class of a given path. Distinct paths can also be used to guide sampling-based motion planning and thus increase the effectiveness of planning in environments with narrow passages. Graph-based representation called roadmap is a common representation for path planning and also for finding multiple distinct paths. However, challenging environments with multiple narrow passages require a densely sampled roadmap to capture the connectivity of the environment. Searching such a dense roadmap for multiple paths is computationally too expensive. Therefore, the majority of existing methods construct only a sparse roadmap which, however, struggles to find all distinct paths in challenging environments. To this end, we propose the CTopPRM which creates a sparse graph by clustering an initially sampled dense roadmap. Such a reduced roadmap allows fast identification of homotopically distinct paths captured in the dense roadmap. We show, that compared to the existing methods the CTopPRM improves the probability of finding all distinct paths by almost 20% in tested environments, during same run-time. The source code of our method is released as an open-source package.Comment: in IEEE Robotics and Automation Letter

Sampling-based optimal kinodynamic planning with motion primitives

This paper proposes a novel sampling-based motion planner, which integrates in RRT* (Rapidly exploring Random Tree star) a database of pre-computed motion primitives to alleviate its computational load and allow for motion planning in a dynamic or partially known environment. The database is built by considering a set of initial and final state pairs in some grid space, and determining for each pair an optimal trajectory that is compatible with the system dynamics and constraints, while minimizing a cost. Nodes are progressively added to the tree {of feasible trajectories in the RRT* by extracting at random a sample in the gridded state space and selecting the best obstacle-free motion primitive in the database that joins it to an existing node. The tree is rewired if some nodes can be reached from the new sampled state through an obstacle-free motion primitive with lower cost. The computationally more intensive part of motion planning is thus moved to the preliminary offline phase of the database construction at the price of some performance degradation due to gridding. Grid resolution can be tuned so as to compromise between (sub)optimality and size of the database. The planner is shown to be asymptotically optimal as the grid resolution goes to zero and the number of sampled states grows to infinity

Human-aware space sharing and navigation for an interactive robot

Les méthodes de planification de mouvements robotiques se sont développées à un rythme accéléré ces dernières années. L'accent a principalement été mis sur le fait de rendre les robots plus efficaces, plus sécurisés et plus rapides à réagir à des situations imprévisibles. En conséquence, nous assistons de plus en plus à l'introduction des robots de service dans notre vie quotidienne, en particulier dans les lieux publics tels que les musées, les centres commerciaux et les aéroports. Tandis qu'un robot de service mobile se déplace dans l'environnement humain, il est important de prendre en compte l'effet de son comportement sur les personnes qu'il croise ou avec lesquelles il interagit. Nous ne les voyons pas comme de simples machines, mais comme des agents sociaux et nous nous attendons à ce qu'ils se comportent de manière similaire à l'homme en suivant les normes sociétales comme des règles. Ceci a créé de nouveaux défis et a ouvert de nouvelles directions de recherche pour concevoir des algorithmes de commande de robot, qui fournissent des comportements de robot acceptables, lisibles et proactifs. Cette thèse propose une méthode coopérative basée sur l'optimisation pour la planification de trajectoire et la navigation du robot avec des contraintes sociales intégrées pour assurer des mouvements de robots prudents, conscients de la présence de l'être humain et prévisibles. La trajectoire du robot est ajustée dynamiquement et continuellement pour satisfaire ces contraintes sociales. Pour ce faire, nous traitons la trajectoire du robot comme une bande élastique (une construction mathématique représentant la trajectoire du robot comme une série de positions et une différence de temps entre ces positions) qui peut être déformée (dans l'espace et dans le temps) par le processus d'optimisation pour respecter les contraintes données. De plus, le robot prédit aussi les trajectoires humaines plausibles dans la même zone d'exploitation en traitant les chemins humains aussi comme des bandes élastiques. Ce système nous permet d'optimiser les trajectoires des robots non seulement pour le moment présent, mais aussi pour l'interaction entière qui se produit lorsque les humains et les robots se croisent les uns les autres. Nous avons réalisé un ensemble d'expériences avec des situations interactives humains-robots qui se produisent dans la vie de tous les jours telles que traverser un couloir, passer par une porte et se croiser sur de grands espaces ouverts. La méthode de planification coopérative proposée se compare favorablement à d'autres schémas de planification de la navigation à la pointe de la technique. Nous avons augmenté le comportement de navigation du robot avec un mouvement synchronisé et réactif de sa tête. Cela permet au robot de regarder où il va et occasionnellement de détourner son regard vers les personnes voisines pour montrer que le robot va éviter toute collision possible avec eux comme prévu par le planificateur. À tout moment, le robot pondère les multiples critères selon le contexte social et décide de ce vers quoi il devrait porter le regard. Grâce à une étude utilisateur en ligne, nous avons montré que ce mécanisme de regard complète efficacement le comportement de navigation ce qui améliore la lisibilité des actions du robot. Enfin, nous avons intégré notre schéma de navigation avec un système de supervision plus large qui peut générer conjointement des comportements du robot standard tel que l'approche d'une personne et l'adaptation de la vitesse du robot selon le groupe de personnes que le robot guide dans des scénarios d'aéroport ou de musée.The methods of robotic movement planning have grown at an accelerated pace in recent years. The emphasis has mainly been on making robots more efficient, safer and react faster to unpredictable situations. As a result we are witnessing more and more service robots introduced in our everyday lives, especially in public places such as museums, shopping malls and airports. While a mobile service robot moves in a human environment, it leaves an innate effect on people about its demeanor. We do not see them as mere machines but as social agents and expect them to behave humanly by following societal norms and rules. This has created new challenges and opened new research avenues for designing robot control algorithms that deliver human-acceptable, legible and proactive robot behaviors. This thesis proposes a optimization-based cooperative method for trajectoryplanning and navigation with in-built social constraints for keeping robot motions safe, human-aware and predictable. The robot trajectory is dynamically and continuously adjusted to satisfy these social constraints. To do so, we treat the robot trajectory as an elastic band (a mathematical construct representing the robot path as a series of poses and time-difference between those poses) which can be deformed (both in space and time) by the optimization process to respect given constraints. Moreover, we also predict plausible human trajectories in the same operating area by treating human paths also as elastic bands. This scheme allows us to optimize the robot trajectories not only for the current moment but for the entire interaction that happens when humans and robot cross each other's paths. We carried out a set of experiments with canonical human-robot interactive situations that happen in our everyday lives such as crossing a hallway, passing through a door and intersecting paths on wide open spaces. The proposed cooperative planning method compares favorably against other stat-of-the-art human-aware navigation planning schemes. We have augmented robot navigation behavior with synchronized and responsive movements of the robot head, making the robot look where it is going and occasionally diverting its gaze towards nearby people to acknowledge that robot will avoid any possible collision with them. At any given moment the robot weighs multiple criteria according to the social context and decides where it should turn its gaze. Through an online user study we have shown that such gazing mechanism effectively complements the navigation behavior and it improves legibility of the robot actions. Finally, we have integrated our navigation scheme with a broader supervision system which can jointly generate normative robot behaviors such as approaching a person and adapting the robot speed according to a group of people who the robot guides in airports or museums

Near-Optimal Motion Planning Algorithms Via A Topological and Geometric Perspective

Motion planning is a fundamental problem in robotics, which involves finding a path for an autonomous system, such as a robot, from a given source to a destination while avoiding collisions with obstacles. The properties of the planning space heavily influence the performance of existing motion planning algorithms, which can pose significant challenges in handling complex regions, such as narrow passages or cluttered environments, even for simple objects. The problem of motion planning becomes deterministic if the details of the space are fully known, which is often difficult to achieve in constantly changing environments. Sampling-based algorithms are widely used among motion planning paradigms because they capture the topology of space into a roadmap. These planners have successfully solved high-dimensional planning problems with a probabilistic-complete guarantee, i.e., it guarantees to find a path if one exists as the number of vertices goes to infinity. Despite their progress, these methods have failed to optimize the sub-region information of the environment for reuse by other planners. This results in re-planning overhead at each execution, affecting the performance complexity for computation time and memory space usage. In this research, we address the problem by focusing on the theoretical foundation of the algorithmic approach that leverages the strengths of sampling-based motion planners and the Topological Data Analysis methods to extract intricate properties of the environment. The work contributes a novel algorithm to overcome the performance shortcomings of existing motion planners by capturing and preserving the essential topological and geometric features to generate a homotopy-equivalent roadmap of the environment. This roadmap provides a mathematically rich representation of the environment, including an approximate measure of the collision-free space. In addition, the roadmap graph vertices sampled close to the obstacles exhibit advantages when navigating through narrow passages and cluttered environments, making obstacle-avoidance path planning significantly more efficient. The application of the proposed algorithms solves motion planning problems, such as sub-optimal planning, diverse path planning, and fault-tolerant planning, by demonstrating the improvement in computational performance and path quality. Furthermore, we explore the potential of these algorithms in solving computational biology problems, particularly in finding optimal binding positions for protein-ligand or protein-protein interactions. Overall, our work contributes a new way to classify routes in higher dimensional space and shows promising results for high-dimensional robots, such as articulated linkage robots. The findings of this research provide a comprehensive solution to motion planning problems and offer a new perspective on solving computational biology problems

Social robot navigation in urban dynamic environments

Deploying mobile robots in social environments requires novel navigation algorithms which are capable of providing valid solutions in such challenging scenarios. The main objective of the present dissertation is to develop new robot navigation approaches able to solve in an intelligent way the navigation problem in urban settings while considering at the same time the interactions with pedestrians, similar to what people easily do with little attention. Before studying in depth navigation algorithms, this thesis focuses on prediction algorithms to provide a more detailed model of the scene. Understanding human motion in outdoor and indoor scenarios is an appealing requirement to characterize correctly urban settings. Urban environments consist essentially of static obstacles and people, which are treated as dynamic and highly uncertain obstacles. Accordingly, it is mandatory to calculate people's intentions in order to successfully build a human prediction model that generates the corresponding human trajectories and considers their interactions with the environment, such as other pedestrians, static obstacles or even robots. It is of great interest that service robots can navigate successfully in typical urban environments, which are dynamic and constrained. In addition, people's behavior should not be conditioned by the presence and the maneuvering of robots. To this end, the robot navigation should seek to minimize its impact on the environment, in our case, on people. This thesis proposes new robot navigation methods that contemplate the social interactions taking place in the scene. In order to procure more intelligence to the navigation algorithm, we propose to integrate seamlessly the human motion prediction information into a new robot planning approach. Real experimentation is essential for the validation of the navigation algorithms. As there are real people involved, we must validate the results in real settings since simulation environments have limitations. In this thesis, we have implemented all the prediction and navigation algorithms in our robotic platform and we have provided plenty of evaluations and testings of our algorithms in real settings.Ubicar robots móviles en entornos sociales requiere novedosos algoritmos de navegación que sean capaces de aportar soluciones válidas en éstos exigentes escenarios. El prinicipal objetivo de la presente disertación es el de desarrollar nuevas soluciones para la navegación de robots que sean capaces de resolver, de una manera más inteligente, los problemas de navegación en emplazamientos urbanos, a la vez que se consideran las interacciones con los transeúntes de manera similar a lo que la gente hace fácilmente prestando poca atención. Antes de estudiar en profundidad los algoritmos de navegación, esta tesis se centra en los algoritmos de predicción para proporcionar un modelo más detallado de la escena. Entender el movimiento humando en entornos exteriores e interiores es un requerimiento deseable para caracterizar correctamente emplazamientos urbanos. Los entornos urbanos están consistituídos por muchos objetos dinámicos y altamente impredecibles, la gente. Por lo tanto, es obligatorio calcular las intenciones de la gente para constriur de manera exitosa un modelo de predicción humano que genere las correspondientes trayectorias humanas y considere sus interacciones con el entorno, como otros peatones, obstáculos estáticos o incluso robots. Es de gran interés que los robots de servicios puedan navegar correctamente en entornos típicamente urbanos, que son dinámicos y acotados, además de que el comportamiento de las personas no debería estar condicionado por la presencia y las maniobras de los robots. Con este fin, la navegación de robots debe buscar minimizar su impacto al entorno, en nuestro caso, a la gente. Esta tesis propone nuevos métodos para la navegación de robots que contemplen las interacciones sociales que suceden en la escena. Para proporcionar una navegación más inteligente, proponemos integrar de manera suave el algoritmo de predicción del movimiento humano con un nuevo enfoque de planificación de trayectorias. La experimentación real es esencial para la validación de los algoritmos de navegación. Ya que hay personas reales implicadas, debemos validar los resultados en emplazamientos reales porque el entorno de simulación tiene limitaciones. En esta tesis hemos implementado todos los algoritmos de predicción y de navegación en la plataforma robótica y hemos proporcionado multitud de evaluaciones y pruebas de nuestros algoritmos en entornos reales

Computational Methods for Cognitive and Cooperative Robotics

In the last decades design methods in control engineering made substantial progress in the areas of robotics and computer animation. Nowadays these methods incorporate the newest developments in machine learning and artificial intelligence. But the problems of flexible and online-adaptive combinations of motor behaviors remain challenging for human-like animations and for humanoid robotics. In this context, biologically-motivated methods for the analysis and re-synthesis of human motor programs provide new insights in and models for the anticipatory motion synthesis. This thesis presents the author’s achievements in the areas of cognitive and developmental robotics, cooperative and humanoid robotics and intelligent and machine learning methods in computer graphics. The first part of the thesis in the chapter “Goal-directed Imitation for Robots” considers imitation learning in cognitive and developmental robotics. The work presented here details the author’s progress in the development of hierarchical motion recognition and planning inspired by recent discoveries of the functions of mirror-neuron cortical circuits in primates. The overall architecture is capable of ‘learning for imitation’ and ‘learning by imitation’. The complete system includes a low-level real-time capable path planning subsystem for obstacle avoidance during arm reaching. The learning-based path planning subsystem is universal for all types of anthropomorphic robot arms, and is capable of knowledge transfer at the level of individual motor acts. Next, the problems of learning and synthesis of motor synergies, the spatial and spatio-temporal combinations of motor features in sequential multi-action behavior, and the problems of task-related action transitions are considered in the second part of the thesis “Kinematic Motion Synthesis for Computer Graphics and Robotics”. In this part, a new approach of modeling complex full-body human actions by mixtures of time-shift invariant motor primitives in presented. The online-capable full-body motion generation architecture based on dynamic movement primitives driving the time-shift invariant motor synergies was implemented as an online-reactive adaptive motion synthesis for computer graphics and robotics applications. The last chapter of the thesis entitled “Contraction Theory and Self-organized Scenarios in Computer Graphics and Robotics” is dedicated to optimal control strategies in multi-agent scenarios of large crowds of agents expressing highly nonlinear behaviors. This last part presents new mathematical tools for stability analysis and synthesis of multi-agent cooperative scenarios.In den letzten Jahrzehnten hat die Forschung in den Bereichen der Steuerung und Regelung komplexer Systeme erhebliche Fortschritte gemacht, insbesondere in den Bereichen Robotik und Computeranimation. Die Entwicklung solcher Systeme verwendet heutzutage neueste Methoden und Entwicklungen im Bereich des maschinellen Lernens und der künstlichen Intelligenz. Die flexible und echtzeitfähige Kombination von motorischen Verhaltensweisen ist eine wesentliche Herausforderung für die Generierung menschenähnlicher Animationen und in der humanoiden Robotik. In diesem Zusammenhang liefern biologisch motivierte Methoden zur Analyse und Resynthese menschlicher motorischer Programme neue Erkenntnisse und Modelle für die antizipatorische Bewegungssynthese. Diese Dissertation präsentiert die Ergebnisse der Arbeiten des Autors im Gebiet der kognitiven und Entwicklungsrobotik, kooperativer und humanoider Robotersysteme sowie intelligenter und maschineller Lernmethoden in der Computergrafik. Der erste Teil der Dissertation im Kapitel “Zielgerichtete Nachahmung für Roboter” behandelt das Imitationslernen in der kognitiven und Entwicklungsrobotik. Die vorgestellten Arbeiten beschreiben neue Methoden für die hierarchische Bewegungserkennung und -planung, die durch Erkenntnisse zur Funktion der kortikalen Spiegelneuronen-Schaltkreise bei Primaten inspiriert wurden. Die entwickelte Architektur ist in der Lage, ‘durch Imitation zu lernen’ und ‘zu lernen zu imitieren’. Das komplette entwickelte System enthält ein echtzeitfähiges Pfadplanungssubsystem zur Hindernisvermeidung während der Durchführung von Armbewegungen. Das lernbasierte Pfadplanungssubsystem ist universell und für alle Arten von anthropomorphen Roboterarmen in der Lage, Wissen auf der Ebene einzelner motorischer Handlungen zu übertragen. Im zweiten Teil der Arbeit “Kinematische Bewegungssynthese für Computergrafik und Robotik” werden die Probleme des Lernens und der Synthese motorischer Synergien, d.h. von räumlichen und räumlich-zeitlichen Kombinationen motorischer Bewegungselemente bei Bewegungssequenzen und bei aufgabenbezogenen Handlungs übergängen behandelt. Es wird ein neuer Ansatz zur Modellierung komplexer menschlicher Ganzkörperaktionen durch Mischungen von zeitverschiebungsinvarianten Motorprimitiven vorgestellt. Zudem wurde ein online-fähiger Synthesealgorithmus für Ganzköperbewegungen entwickelt, der auf dynamischen Bewegungsprimitiven basiert, die wiederum auf der Basis der gelernten verschiebungsinvarianten Primitive konstruiert werden. Dieser Algorithmus wurde für verschiedene Probleme der Bewegungssynthese für die Computergrafik- und Roboteranwendungen implementiert. Das letzte Kapitel der Dissertation mit dem Titel “Kontraktionstheorie und selbstorganisierte Szenarien in der Computergrafik und Robotik” widmet sich optimalen Kontrollstrategien in Multi-Agenten-Szenarien, wobei die Agenten durch eine hochgradig nichtlineare Kinematik gekennzeichnet sind. Dieser letzte Teil präsentiert neue mathematische Werkzeuge für die Stabilitätsanalyse und Synthese von kooperativen Multi-Agenten-Szenarien

Intention prediction for interactive navigation in distributed robotic systems

Modern applications of mobile robots require them to have the ability to safely and effectively navigate in human environments. New challenges arise when these robots must plan their motion in a human-aware fashion. Current methods addressing this problem have focused mainly on the activity forecasting aspect, aiming at improving predictions without considering the active nature of the interaction, i.e. the robot’s effect on the environment and consequent issues such as reciprocity. Furthermore, many methods rely on computationally expensive offline training of predictive models that may not be well suited to rapidly evolving dynamic environments. This thesis presents a novel approach for enabling autonomous robots to navigate socially in environments with humans. Following formulations of the inverse planning problem, agents reason about the intentions of other agents and make predictions about their future interactive motion. A technique is proposed to implement counterfactual reasoning over a parametrised set of light-weight reciprocal motion models, thus making it more tractable to maintain beliefs over the future trajectories of other agents towards plausible goals. The speed of inference and the effectiveness of the algorithms is demonstrated via physical robot experiments, where computationally constrained robots navigate amongst humans in a distributed multi-sensor setup, able to infer other agents’ intentions as fast as 100ms after the first observation. While intention inference is a key aspect of successful human-robot interaction, executing any task requires planning that takes into account the predicted goals and trajectories of other agents, e.g., pedestrians. It is well known that robots demonstrate unwanted behaviours, such as freezing or becoming sluggishly responsive, when placed in dynamic and cluttered environments, due to the way in which safety margins according to simple heuristics end up covering the entire feasible space of motion. The presented approach makes more refined predictions about future movement, which enables robots to find collision-free paths quickly and efficiently. This thesis describes a novel technique for generating "interactive costmaps", a representation of the planner’s costs and rewards across time and space, providing an autonomous robot with the information required to navigate socially given the estimate of other agents’ intentions. This multi-layered costmap deters the robot from obstructing while encouraging social navigation respectful of other agents’ activity. Results show that this approach minimises collisions and near-collisions, minimises travel times for agents, and importantly offers the same computational cost as the most common costmap alternatives for navigation. A key part of the practical deployment of such technologies is their ease of implementation and configuration. Since every use case and environment is different and distinct, the presented methods use online adaptation to learn parameters of the navigating agents during runtime. Furthermore, this thesis includes a novel technique for allocating tasks in distributed robotics systems, where a tool is provided to maximise the performance on any distributed setup by automatic parameter tuning. All of these methods are implemented in ROS and distributed as open-source. The ultimate aim is to provide an accessible and efficient framework that may be seamlessly deployed on modern robots, enabling widespread use of intention prediction for interactive navigation in distributed robotic systems

Implications of Motion Planning: Optimality and k-survivability

We study motion planning problems, finding trajectories that connect two configurations of a system, from two different perspectives: optimality and survivability. For the problem of finding optimal trajectories, we provide a model in which the existence of optimal trajectories is guaranteed, and design an algorithm to find approximately optimal trajectories for a kinematic planar robot within this model. We also design an algorithm to build data structures to represent the configuration space, supporting optimal trajectory queries for any given pair of configurations in an obstructed environment. We are also interested in planning paths for expendable robots moving in a threat environment. Since robots are expendable, our goal is to ensure a certain number of robots reaching the goal. We consider a new motion planning problem, maximum k-survivability: given two points in a stochastic threat environment, find n paths connecting two given points while maximizing the probability that at least k paths reach the goal. Intuitively, a good solution should be diverse to avoid several paths being blocked simultaneously, and paths should be short so that robots can quickly pass through dangerous areas. Finding sets of paths with maximum k-survivability is NP-hard. We design two algorithms: an algorithm that is guaranteed to find an optimal list of paths, and a set of heuristic methods that finds paths with high k-survivability