Ground robots navigating in complex, dynamic environments must compute collision-free trajectories to avoid obstacles safely and efficiently. Nonconvex optimization is a popular method to compute a trajectory in real-time. However, these methods often converge to locally optimal solutions and frequently switch between different local minima, leading to inefficient and unsafe robot motion. In this work, we propose a novel topology-driven trajectory optimization strategy for dynamic environments that plans multiple distinct evasive trajectories to enhance the robot's behavior and efficiency. A global planner iteratively generates trajectories in distinct homotopy classes. These trajectories are then optimized by local planners working in parallel. While each planner shares the same navigation objectives, they are locally constrained to a specific homotopy class, meaning each local planner attempts a different evasive maneuver. The robot then executes the feasible trajectory with the lowest cost in a receding horizon manner. We demonstrate, on a mobile robot navigating among pedestrians, that our approach leads to faster trajectories than existing planners.

@en

Autonomous mobile robots require predictions of human motion to plan a safe trajectory that avoids them. Because human motion cannot be predicted exactly, future trajectories are typically inferred from real-world data via learning-based approximations. These approximations provide useful information on the pedestrian's behavior, but may deviate from the data, which can lead to collisions during planning. In this work, we introduce a joint prediction and planning framework, Partitioned Scenario Replay (PSR), that stores and partitions previously observed human trajectories, referred to as scenarios. During planning, scenarios observed in similar situations are reintroduced (or replayed) as motion predictions. By sampling real data and by building on scenario optimization and predictive control, the planner provides probabilistic collision avoidance guarantees in the real-world. Relying on this guarantee to remain safe, PSR can incrementally improve its prediction and planning performance online. We demonstrate our approach on a mobile robot navigating around pedestrians.

@en

Logistics and transportation can greatly benefit from the use of autonomous robots, such as self-driving vehicles. Robots can help to move goods or people without human supervision. One of the main components that enable autonomous navigation among humans is motion planning. Motion planning is responsible for computing a collision-free trajectory that moves the robot to its destination, based on the perceived information. The motion planner should be efficient, robust, and safe. This thesis contributes towards this goal by investigating the design of motion planning algorithms for autonomous navigation of mobile robots near humans.

Traditional motion planners for dynamic environments have two key limitations that this thesis aims to address. First, they assume that their model of dynamic obstacles (e.g., humans) is exactly correct, capturing it with a single deterministic prediction. In practice, the robot cannot observe human intentions and must account for its uncertainty about the human's future behavior. Second, motion planners usually compute a single trajectory around an obstacle as a result of previously taken decisions without exploring alternative options. They react slowly or even fail to find a solution when unpredicted changes make this path undesirable. This results in poor planning performance in dynamic environments.

The goal of this thesis is to develop motion planners that account for the uncertainty of human motion predictions and that are consistent and robust in their decision-making in order to deal with unpredicted changes in dynamic environments. To accomplish this goal, this thesis proposes two motion planning frameworks: scenario-based and topology-driven trajectory optimization.

The first contribution of this thesis is Scenario-based Model Predictive Contouring Control (S-MPCC), a real-time capable probabilistic planning framework that incorporates any uncertainty associated with the motion predictions of dynamic obstacles. Contrary to existing methods that only account for small variations around a single predicted trajectory (unimodal uncertainty), the proposed planner accounts for multiple possible trajectories (multi-modal uncertainty). The planner therefore safely accounts for several outcomes, for instance, to express that a pedestrian may or may not cross in front of the robot.

S-MPCC bounds the probability of collision in each time step with all obstacles through Chance-Constrained Optimization (CCO). The CCO is reformulated as an optimization without uncertainty by sampling trajectories from the predicted distribution, known as scenarios. Each scenario represents a possible position of all obstacles in one time step, and the planner avoids collisions with all scenarios. This Scenario Program (SP), through a tailored linearization, can be solved efficiently online. S-MPCC therefore plans probabilistic safe trajectories independent of the underlying distribution of the uncertainty.

S-MPCC considers the probability of collision separately for each time instance in the planned trajectory. The second contribution of this thesis, Safe Horizon Model Predictive Control (SH-MPC), builds on S-MPCC to constrain the joint probability of collision with all obstacles over the duration of the planned trajectory. Existing methods that separately constrain the probability of collision in each time step (temporal marginal) and with each obstacle (obstacle marginal) lead to overly cautious motion planning when safety constraints are enforced. SH-MPC formulates a single chance constraint to bound the overall probability of collision. This CCO is reformulated as an SP where each scenario represents a possible trajectory for all obstacles. To certify the joint probability of collision with the SP, the number of scenarios that affect the motion plan needs to be identified. SH-MPC estimates this quantity at a negligible computational cost during optimization. Consequently, SH-MPC plans trajectories in real-time under generic uncertainties that are less cautious than existing methods without compromising on safety.

The probabilistic safety of S-MPCC and SH-MPC is linked to the underlying accuracy of the prediction model of the obstacles that provide the scenarios. As a third contribution, a joint prediction and planning framework, Partitioned Scenario Replay (PSR), is proposed that replays past observations of human motion as scenarios for scenario-based planning. PSR does not fit a distribution on observed data but directly uses the data as empirical evidence of the underlying uncertainty and thereby provides a real-world safety guarantee.

A key limitation of the developed scenario-based planners and other optimization-based planners is that they locally refine an initial trajectory. This initial trajectory largely determines the quality of the final trajectory, while it does not consider other options. The fourth contribution of this thesis is Topology-driven Model Predictive Control (T-MPC) that concurrently optimizes trajectories, each attempting a different way to pass the obstacles. T-MPC is composed of a guidance planner and several parallel local planners. The guidance planner identifies guidance trajectories for several distinct maneuvers, relying on results from topology to distinguish trajectories. Each local planner is composed of an existing optimization-based planner (e.g., a scenario-based planner) and an additional set of constraints that are derived from one of the guidance trajectories. The guidance trajectories are optimized by the local planners in parallel, and the results are compared to determine which trajectory gets executed. T-MPC is faster, more consistent, and safer than several state-of-the-art planners. Contrary to similar existing work, it does not rely on an explicit lane structure and therefore enables both urban driving and mobile robotic applications.

The motion planners developed in this thesis are extensively validated in simulation and in experiments with a small-scale mobile robot and a full-scale self-driving vehicle navigating among pedestrians. The robot-agnostic implementation of the proposed planners that were developed for this thesis is available open source.@en

In this work, we propose two cooperative passivity-based control methods for networks of mechanical systems. By cooperatively synchronizing the end-effector coordinates of the individual agents, we achieve cooperation between systems of different types. The underlying passivity property of our control approaches ensures that cooperation is stable and robust. Neither of the two approaches rely on the modeling information of neighbors, locally, which simplifies the interconnection of applicable systems and makes the approaches modular in their use. Our first approach is a generalized cooperative Interconnection-and-Damping Assignment passivity-based control (IDA-PBC) scheme for networks of fully actuated and underactuated systems. Our approach leverages the definition of end-effector coordinates in existing single-agent IDA-PBC solutions for underactuated systems to satisfy the matching conditions, independently of the cooperative control input. Accordingly, our approach integrates a large set of existing single-agent solutions and facilitates cooperative control between these and fully actuated systems. Our second approach proposes agent outputs composed of their end-effector coordinates and velocities to guarantee cooperative stability for networks of fully actuated systems in the presence of communication delays. We validate both approaches in simulation and experiments. @en

Balancing safety and efficiency when planning in crowded scenarios with uncertain dynamics is challenging where it is imperative to accomplish the robot's mission without incurring any safety violations. Typically, chance constraints are incorporated into the planning problem to provide probabilistic safety guarantees by imposing an upper bound on the collision probability of the planned trajectory. Yet, this results in an overly conservative behavior on the grounds that the gap between the obtained risk and the specified upper limit is not explicitly restricted. To address this issue, we propose a real-time capable approach to quantify the risk associated with planned trajectories obtained from multiple probabilistic planners, running in parallel, with different upper bounds of the acceptable risk level. Based on the evaluated risk, the least conservative plan is selected provided that its associated risk is below a specified threshold. In such a way, the proposed approach provides probabilistic safety guarantees by attaining a closer bound to the specified risk, while being applicable to generic uncertainties of moving obstacles. We demonstrate the efficiency of our proposed approach, by improving the performance of a state-of-the-art probabilistic planner, in simulations and experiments using a mobile robot in an environment shared with humans.@en

Navigating mobile robots through environments shared with humans is challenging. From the perspective of the robot, humans are dynamic obstacles that must be avoided. These obstacles make the collision-free space nonconvex, which leads to two distinct passing behaviors per obstacle (passing left or right). For local planners, such as receding-horizon trajectory optimization, each behavior presents a local optimum in which the planner can get stuck. This may result in slow or unsafe motion even when a better plan exists. In this work, we identify trajectories for multiple locally optimal driving behaviors, by considering their topology. This identification is made consistent over successive iterations by propagating the topology information. The most suitable high-level trajectory guides a local optimization-based planner, resulting in fast and safe motion plans. We validate the proposed planner on a mobile robot in simulation and real-world experiments.@en

This paper presents a rule-compliant trajectory optimization method for the guidance and control of autonomous surface vessels. The method builds on Model Predictive Contouring Control and incorporates the International Regulations for Preventing Collisions at Sea - known as COLREGs - relevant for motion planning. We use these traffic rules to derive a trajectory optimization algorithm that guarantees safe navigation in mixed-traffic conditions, that is, in traffic environments with human operated vessels. The choice of an optimization-based approach enables the formalization of abstract verbal expressions, such as traffic rules, and their incorporation in the trajectory optimization algorithm along with the dynamics and other constraints that dictate the system's evolution over a sufficiently long receding horizon. The ability to plan considering different types of constraints over a long horizon in a unified manner leads to a proactive motion planner that mimics rule-compliant maneuvering behavior. The efficacy of the derived algorithm is validated in different simulation scenarios.

@en

The successful integration of autonomous robots in real-world environments strongly depends on their ability to reason from context and take socially acceptable actions. Current autonomous navigation systems mainly rely on geometric information and hard-coded rules to induce safe and socially compliant behaviors. Yet, in unstructured urban scenarios these approaches can become costly and suboptimal. In this paper, we introduce a motion planning framework consisting of two components: a data-driven policy that uses visual inputs and human feedback to generate socially compliant driving behaviors (encoded by high-level decision variables), and a local trajectory optimization method that executes these behaviors (ensuring safety). In particular, we employ Interactive Imitation Learning to jointly train the policy with the local planner, a Model Predictive Controller (MPC), which results in safe and human-like driving behaviors. Our approach is validated in realistic simulated urban scenarios. Qualitative results show the similarity of the learned behaviors with human driving. Furthermore, navigation performance is substantially improved in terms of safety, i.e., number of collisions, as compared to prior trajectory optimization frameworks, and in terms of data-efficiency as compared to prior learning-based frameworks, broadening the operational domain of MPC to more realistic autonomous driving scenarios.

@en

In this work we consider the problem of cooperative end-effector control between heterogeneous fully actuated agents when varying-time delays and/or packet loss are present. We couple agents via outputs encoded with task-space coordinates and velocities that are transformed into wave-variables to overcome the destabilising effects of the communication network. The scheme poses dynamic requirements on the agents which are locally satisfied with feedback control that integrates subtasks, such as joint-limit avoidance or local tracking, when there are redundant degrees-of-freedom. The proposed approach extends existing methods to task-space control. The approach is robust to network effects, applies to nonlinear systems and is scalable by design. The tuning task is simplified considerably by separation of the cooperative and non-cooperative control terms. We demonstrate the efficacy of the proposed approach experimentally.

@en

We present an optimization-based method to plan the motion of an autonomous robot under the uncertainties associated with dynamic obstacles, such as humans. Our method bounds the marginal risk of collisions at each point in time by incorporating chance constraints into the planning problem. This problem is not suitable for online optimization outright for arbitrary probability distributions. Hence, we sample from these chance constraints to generate scenarios, which translate the probabilistic constraints into deterministic ones. In practice, each scenario represents the collision constraint for a dynamic obstacle at the location of the sample. The number of theoretically required scenarios can be very large. Nevertheless, by exploiting the geometry of the workspace, we show how to prune most scenarios before optimization and we demonstrate how the reduced scenarios can still provide probabilistic guarantees on the safety of the motion plan. Since our approach is scenario based, we are able to handle arbitrary uncertainty distributions. We apply our method in a Model Predictive Contouring Control framework and demonstrate its benefits in simulations and experiments with a moving robot platform navigating among pedestrians, running in real-time.

@en