To efficiently deploy robotic systems in society, mobile robots must move autonomously and safely through complex environments. Nonlinear model predictive control (MPC) methods provide a natural way to find a dynamically feasible trajectory through the environment without colliding with nearby obstacles. However, the limited computation power available on typical embedded robotic systems, such as quadrotors, poses a challenge to running MPC in real time, including its most expensive tasks: constraints generation and optimization. To address this problem, we propose a novel hierarchical MPC scheme that consists of a planning and a tracking layer. The planner constructs a trajectory with a long prediction horizon at a slow rate, while the tracker ensures trajectory tracking at a relatively fast rate. We prove that the proposed framework avoids collisions and is recursively feasible. Furthermore, we demonstrate its effectiveness in simulations and lab experiments with a quadrotor that needs to reach a goal position in a complex static environment. The code is efficiently implemented on the quadrotor's embedded computer to ensure real-time feasibility. Compared to a state-of-the-art single-layer MPC formulation, this allows us to increase the planning horizon by a factor of 5, which results in significantly better performance.

Sim2real, the transfer of control policies from simulation to the real world, is crucial for efficiently solving robotic tasks without the risks associated with real-world learning. How-ever, discrepancies between simulated and real environments, especially due to unmodeled dynamics and latencies, significantly impact the performance of these transferred policies. In this paper, we address the challenges of sim2real transfer caused by latency and asynchronous dynamics in real-world robotic systems. Our approach involves developing a novel framework, REX (Robotic Environments with jaX), that uses a graph-based simulation model to incorporate latency effects while optimizing for parallelization on accelerator hard-ware. Our framework simulates the asynchronous, hierarchical nature of real-world systems, while simultaneously estimating system dynamics and delays from real-world data and implementing delay compensation strategies to minimize the sim2real gap. We validate our approach on two real-world systems, demonstrating its effectiveness in improving sim2real performance by accurately modeling both system dynamics and delays. Our results show that the proposed framework supports both accelerated simulation and real-time processing, making it valuable for robot learning.

Robots will increasingly operate near humans that introduce uncertainties in the motion planning problem due to their complex nature. Optimization-based planners typically avoid humans through collision avoidance chance constraints. This allows the planner to optimize performance while guaranteeing probabilistic safety. However, existing real-time methods do not consider the actual probability of collision for the planned trajectory but rather its marginalization, that is, the independent collision probabilities for each planning step and/or dynamic obstacle, resulting in conservative trajectories. To address this issue, we introduce a novel real-time capable method termed Safe Horizon MPC that explicitly constrains the joint probability of collision with all obstacles over the duration of the motion plan. This is achieved by reformulating the chance-constrained planning problem using scenario optimization and predictive control. Out of sampled realizations of human motion, we identify which cases affect the optimization. This allows us to certify the planned trajectory in real-time. Our method is less conservative than state-of-the-art approaches, applicable to arbitrary probability distributions of the obstacles’ trajectories, computationally tractable and scalable. We demonstrate our proposed approach using a mobile robot and an autonomous vehicle in an environment shared with humans.

As Autonomous Surface Vessels (ASVs) become increasingly prevalent in marine applications, ensuring their safe operation, in the presence of faults, is crucial to human safety. This paper presents a scheme that encompasses the detection and isolation of actuator faults within ASVs to ensure uninterrupted and safe operation. The method primarily addresses the loss of thruster effectiveness as a specific actuator fault. For fault detection, the proposed method leverages residuals generated by nonlinear observers, coupled with adaptive thresholds, enhancing fault detection accuracy. The active fault isolation strategy employs actuator redundancy to insulate specific system states from faults by dynamically reconfiguring the actuation configuration in response to detected faults. Comprehensive simulation results demonstrate the effectiveness of this methodology across diverse marine traffic scenarios where the ASV needs to perform a collision avoidance maneuver.

We propose a novel approach to track the state of charge (SoC) of batteries in mobile robots to improve their capabilities. The batteries' status is critical to accomplish their mission, but limited battery life can be a challenge. Our methodology focuses on modeling and estimating the SoC of batteries through system identification and fractional-order models. These models are flexible and can adjust to transient responses, allowing for accurate estimation of battery characteristics. Specifically, we use cubic spline interpolation to obtain the open-circuit voltage (OCV) and the different resistors of the battery model. To estimate the SoC, we deploy a novel approach based on the moving horizon estimation (MHE) algorithm, which is suitable for handling poor initial estimation and constraints on the battery model. We consider the constraint on the peak discharging current, which can limit the performance of mobile robots in low-battery mode. We validate our approach by applying system identification and MHE to data from a mobile robot. The results show that our method accurately estimates the SoC despite poor initial values, enabling improved performance for mobile robots.

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.

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 relevant to motion planning. We use these rules for traffic situation assessment and to derive traffic-related constraints that are inserted in the optimization problem. Our 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 planning horizon. The ability to plan considering different types of constraints and the system's dynamics, over a long horizon in a unified manner, leads to a proactive motion planner that mimics rule-compliant maneuvering behavior, suitable for navigation in mixed-traffic environments. The efficacy and scalability of the derived algorithm are validated in different simulation scenarios, including complex traffic situations with multiple Obstacle Vessels.

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.

Contingency planning, wherein an agent generates a set of possible plans conditioned on the outcome of an uncertain event, is an increasingly popular way for robots to act under uncertainty. In this work we take a game-theoretic perspective on contingency planning, tailored to multi-agent scenarios in which a robot's actions impact the decisions of other agents and vice versa. The resulting contingency game allows the robot to efficiently interact with other agents by generating strategic motion plans conditioned on multiple possible intents for other actors in the scene. Contingency games are parameterized via a scalar variable which represents a future time when intent uncertainty will be resolved. By estimating this parameter online, we construct a game-theoretic motion planner that adapts to changing beliefs while anticipating future certainty. We show that existing variants of game-theoretic planning under uncertainty are readily obtained as special cases of contingency games. Through a series of simulated autonomous driving scenarios, we demonstrate that contingency games close the gap between certainty-equivalent games that commit to a single hypothesis and non-contingent multi-hypothesis games that do not account for future uncertainty reduction.

This paper presents the design of a research platform for autonomous driving applications, the Delft's Autonomous-driving Robotic Testbed (DART). Our goal was to design a small-scale car-like robot equipped with all the hardware needed for on-board navigation and control while keeping it cost-effective and easy to replicate. To develop DART, we built on an existing off-the-shelf model and augmented its sensor suite to improve its capabilities for control and motion planning tasks. We detail the hardware setup and the system identification challenges to derive the vehicle's models. Furthermore, we present some use cases where we used DART to test different motion planning applications to show the versatility of the platform. Finally, we provide a git repository with all the details to replicate DART, complete with a simulation environment and the data used for system identification.

With increased developments and interest in cooperative driving and higher levels of automation (SAE level 3+), the need for safety systems that are capable to monitor system health and maintain safe operations in faulty scenarios is increasing. A variety of faults or failures could occur, and there exists a high variety of ways to respond to such events. Once a fault or failure is detected, there is a need to classify its severity and decide on appropriate and safe mitigating actions. To provide a solution to this mitigation challenge, in this paper a functional-safety architecture is proposed and an optimization-based mitigation algorithm is introduced. This algorithm uses nonlinear model predictive control (NMPC) to bring a vehicle, suffering from a severe fault, such as a power steering failure, to a safe-state. The internal model of the NMPC uses the information from the fault detection, isolation and identification to optimize the tracking performance of the controller, showcasing the need of the proposed architecture. Given a string of ACC vehicles, our results demonstrate a variety of tactical decision-making approaches that a fault-affected vehicle could employ to manage any faults. Furthermore, we show the potential for improving the safety of the affected vehicle as well as the effect of these approaches on the duration of the manoeuvre.

This letter proposes a distributed strategy to achieve both persistent monitoring and target detection in a rectangular and obstacle-free environment. Each robot has to repeatedly follow a smooth trajectory and avoid collisions with other robots. To achieve this goal, we rely on the time-inverted Kuramoto dynamics and the use of Lissajous curves. We analyze the resiliency of the system to perturbations or temporary failures, and we validate our approach through both simulations and experiments on real robotic platforms. In the letter, we adopt Model Predictive Contouring Control as a low level controller to minimize the tracking error while accounting for the robots' dynamical constraints and the control inputs saturation. The results obtained in the experiments are in accordance with the simulations.

Robots deployed to the real world must be able to interact with other agents in their environment. Dynamic game theory provides a powerful mathematical framework for modeling scenarios in which agents have individual objectives and interactions evolve over time. However, a key limitation of such techniques is that they require a priori knowledge of all players’ objectives. In this work, we address this issue by proposing a novel method for learning players’ objectives in continuous dynamic games from noise-corrupted, partial state observations. Our approach learns objectives by coupling the estimation of unknown cost parameters of each player with inference of unobserved states and inputs through Nash equilibrium constraints. By coupling past state estimates with future state predictions, our approach is amenable to simultaneous online learning and prediction in receding horizon fashion. We demonstrate our method in several simulated traffic scenarios in which we recover players’ preferences, for, e.g. desired travel speed and collision-avoidance behavior. Results show that our method reliably estimates game-theoretic models from noise-corrupted data that closely matches ground-truth objectives, consistently outperforming state-of-the-art approaches.

This work presents a method for multi-robot coordination based on a novel distributed nonlinear model predictive control (NMPC) formulation for trajectory optimization and its modified version to mitigate the effects of packet losses and delays in the communication among the robots. Our algorithms consider that each robot is equipped with an onboard computation unit to solve a local control problem and communicate with neighboring autonomous robots via a wireless network. The difference between the two proposed methods is in the way the robots exchange information to coordinate. The information exchange can occur in a following: 1) synchronous or 2) asynchronous fashion. By relying on the theory of the nonconvex alternating direction method of multipliers (ADMM), we show that the proposed solutions converge to a (local) solution of the centralized problem. For both algorithms, the communication exchange preserves the safety of the robots; that is, collisions with neighboring autonomous robots are prevented. The proposed approaches can be applied to various multi-robot scenarios and robot models. In this work, we assess our methods, both in simulation and with experiments, for the coordination of a team of autonomous vehicles in the following: 1) an unsupervised intersection crossing and 2) the platooning scenarios.

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.

The up-and-coming concept of Industry 5.0 fore-sees human-centric flexible production lines, where collaborative robots support human workforce. In order to allow a seamless collaboration between intelligent robots and human workers, designing solutions for non-expert users is crucial. Learning from demonstration emerged as the enabling approach to address such a problem. However, more focus should be put on finding safe solutions which optimize the cost associated with the demonstrations collection process. This paper introduces a preliminary outline of a system, namely EValueAction (EVA), designed to assist the human in the process of collecting interactive demonstrations taking advantage of simulation to safely avoid failures. A policy is pre-trained with human-demonstrations and, where needed, new informative data are interactively gathered and aggregated to iteratively improve the initial policy. A trial case study further reinforces the relevance of the work by demonstrating the crucial role of informative demonstrations for generalization.

We present a novel Curvature-Aware Model Pre-dictive Contouring Control (CA-MPCC) formulation for mobile robotics motion planning. Our method aims at generalizing the traditional contouring control formulation derived from machining to autonomous driving applications. The proposed controller is able of handling sharp curvatures in the reference path while subject to non-linear constraints, such as lane boundaries and dynamic obstacle collision avoidance. Com-pared to a standard MPCC formulation, our method improves the reliability of the path-following algorithm and simplifies the tuning, while preserving real-time capabilities. We validate our findings in both simulations and experiments on a scaled-down car-like robot.

In multi-agent settings, game theory is a natural framework for describing the strategic interactions of agents whose objectives depend upon one another’s behavior. Trajectory games capture these complex effects by design. In competitive settings, this makes them a more faithful interaction model than traditional “predict then plan” approaches. However, current game-theoretic planning methods have important limitations. In this work, we propose two main contributions. First, we introduce an offline training phase which reduces the online computational burden of solving trajectory games. Second, we formulate a lifted game which allows players to optimize multiple candidate trajectories in unison and thereby construct more competitive “mixed” strategies. We validate our approach on a number of experiments using the pursuit-evasion game “tag.”

Search missions require motion planning and navigation methods for information gathering that continuously replan based on new observations of the robot's surroundings. Current methods for information gathering, such as Monte Carlo Tree Search, are capable of reasoning over long horizons, but they are computationally expensive. An alternative for fast online execution is to train, offline, an information gathering policy, which indirectly reasons about the information value of new observations. However, these policies lack safety guarantees and do not account for the robot dynamics. To overcome these limitations we train an information-aware policy via deep reinforcement learning, that guides a receding-horizon trajectory optimization planner. In particular, the policy continuously recommends a reference viewpoint to the local planner, such that the resulting dynamically feasible and collision-free trajectories lead to observations that maximize the information gain and reduce the uncertainty about the environment. In simulation tests in previously unseen environments, our method consistently outperforms greedy next-best-view policies and achieves competitive performance compared to Monte Carlo Tree Search, in terms of information gains and coverage time, with a reduction in execution time by three orders of magnitude.

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.