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.

@en

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

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.

@en

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.

@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

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.

@en

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.

@en

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.

@en

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.

@en

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.@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

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.

@en

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.

@en

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.

@en

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.”@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

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.@en

This paper presents DeepKoCo, a novel modelbased agent that learns a latent Koopman representation from images. This representation allows DeepKoCo to plan efficiently using linear control methods, such as linear model predictive control. Compared to traditional agents, DeepKoCo learns taskrelevant dynamics, thanks to the use of a tailored lossy autoencoder network that allows DeepKoCo to learn latent dynamics that reconstruct and predict only observed costs, rather than all observed dynamics. As our results show, DeepKoCo achieves a similar final performance as traditional model-free methods on complex control tasks, while being considerably more robust to distractor dynamics, making the proposed agent more amenable for real-life applications.@en

Autonomous cars can reduce road traffic accidents and provide a safer mode of transport. However, key technical challenges, such as safe navigation in complex urban environments, need to be addressed before deploying these vehicles on the market. Teleoperation can help smooth the transition from human operated to fully autonomous vehicles since it still has human in the loop providing the scope of fallback on driver. This paper presents an Active Safety System (ASS) approach for teleoperated driving. The proposed approach helps the operator ensure the safety of the vehicle in complex environments, that is, avoid collisions with static or dynamic obstacles. Our ASS relies on a model predictive control (MPC) formulation to control both the lateral and longitudinal dynamics of the vehicle. By exploiting the ability of the MPC framework to deal with constraints, our ASS restricts the controller’s authority to intervene for lateral correction of the human operator’s commands, avoiding counter-intuitive driving experience for the human operator. Further, we design a visual feedback to enhance the operator’s trust over the ASS. In addition, we propose an MPC’s prediction horizon data based novel predictive display to mitigate the effects of large latency in the teleoperation system. We tested the performance of the proposed approach on a high-fidelity vehicle simulator in the presence of dynamic obstacles and latency.@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