Representing the 3D environment with instance-aware semantic and geometric information is crucial for interaction-aware robots in dynamic environments. Nevertheless, creating such a representation poses challenges due to sensor noise, instance segmentation and tracking errors, and the objects' dynamic motion. This paper introduces a novel particle-based instance-aware semantic occupancy map to tackle these challenges. Particles with an augmented instance state are used to estimate the Probability Hypothesis Density (PHD) of the objects and implicitly model the environment. Utilizing a State-augmented Sequential Monte Carlo PHD (S² MC-PHD) filter, these particles are updated to jointly estimate occupancy status, semantic, and instance IDs, mitigating noise. Additionally, a memory module is adopted to enhance the map's responsiveness to previously observed objects. Experimental results on the Virtual KITTI 2 dataset demonstrate that the proposed approach surpasses state-of-the-art methods across multiple metrics under different noise conditions. Subsequent tests using real-world data further validate the effectiveness of the proposed approach.

We present a sampling-based model predictive control method that uses a generic physics simulator as the dynamical model. In particular, we propose a Model Predictive Path Integral controller (MPPI) that employs the GPU-parallelizable IsaacGym simulator to compute the forward dynamics of the robot and environment. Since the simulator implicitly defines the dynamic model, our method is readily extendable to different objects and robots, allowing one to solve complex navigation and contact-rich tasks. We demonstrate the effectiveness of this method in several simulated and real-world settings, including mobile navigation with collision avoidance, non-prehensile manipulation, and whole-body control for high-dimensional configuration spaces. This is a powerful and accessible open-source tool to solve many contact-rich motion planning tasks.

This study investigates the impact of walking and e-hailing on the scale economies of on-demand mobility services. An analytical framework is developed to i) explicitly characterize the physical interactions between passengers and vehicles in the matching and pickup processes, and ii) derive the closed-form degree of scale economies (DSE) to quantify scale economies. The general model is then specified for conventional street-hailing and e-hailing, with and without walking before pickup and after dropoff. We show that, under a system-optimum fleet size, the market always exhibits economies of scale regardless of the matching mechanism and the walking behaviors, though the scale effect diminishes as passenger demand increases. Yet, street-hailing and e-hailing show different scale economies in their matching process. While street-hailing matching shows a constant DSE of two, e-hailing matching is more sensitive to demand and its DSE diminishes to one when passenger competition emerges. Walking, on the other hand, has mixed effects on the scale economies: while the reduced pickup and in-vehicle times bring a positive scale effect, the extra walking time and possible concentration of vacant vehicles and waiting passengers on streets negatively affect scale economies. All these analytical results are validated through agent-based simulations on Manhattan with real-life demand patterns.

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.

Control Barrier Functions (CBFs) that provide formal safety guarantees have been widely used for safety-critical systems. However, it is non-trivial to design a CBF. Utilizing neural networks as CBFs has shown great success, but it necessitates their certification as CBFs. In this work, we leverage bound propagation techniques and the Branch-and - Bound scheme to efficiently verify that a neural network satisfies the conditions to be a CBF over the continuous state space. To accelerate training, we further present a framework that embeds the verification scheme into the training loop to synthesize and verify a neural CBF simultaneously. In partic-ular, we employ the verification scheme to identify partitions of the state space that are not guaranteed to satisfy the CBF conditions and expand the training dataset by incorporating additional data from these partitions. The neural network is then optimized using the augmented dataset to meet the CBF conditions. We show that for a non-linear control-affine system, our framework can efficiently certify a neural network as a CBF and render a larger safe set than state-of-the-art neural CBF works. We further employ our learned neural CBF to derive a safe controller to illustrate the practical use of our framework.

This paper proposes a decentralized trajectory planning framework for the collision avoidance problem of multiple micro aerial vehicles (MAVs) in environments with static and dynamic obstacles. The framework utilizes spatiotemporal occupancy grid maps (SOGM), which forecast the occupancy status of neighboring space in the near future, as the environment representation. Based on this representation, we extend the kinodynamic A∗ and the corridor-constrained trajectory optimization algorithms to efficiently tackle static and dynamic obstacles with arbitrary shapes. Collision avoidance between communicating robots is integrated by sharing planned trajectories and projecting them onto the SOGM. The simulation results show that our method achieves competitive performance against state-of-the-art methods in dynamic environments with different numbers and shapes of obstacles. Finally, the proposed method is validated in real experiments.

Smart cameras are an essential component in surveillance and monitoring applications, and they have been typically deployed in networks of fixed camera locations. The addition of mobile cameras, mounted on robots, can overcome some of the limitations of static networks such as blind spots or back-lightning, allowing the system to gather the best information at each time by active positioning. This work presents a hybrid camera system, with static and mobile cameras, where all the cameras collaborate to observe people moving freely in the environment and efficiently visualize certain attributes from each person. Our solution combines a multi-camera distributed tracking system, to localize with precision all the people, with a control scheme that moves the mobile cameras to the best viewpoints for a specific classification task. The main contribution of this paper is a novel framework that exploits the synergies that result from the cooperation of the tracking and the control modules, obtaining a system closer to the real-world application and capable of high-level scene understanding. The static camera network provides global awareness of the control scheme to move the robots. In exchange, the mobile cameras onboard the robots provide enhanced information about the people on the scene. We perform a thorough analysis of the people monitoring application performance under different conditions thanks to the use of a photo-realistic simulation environment. Our experiments demonstrate the benefits of collaborative mobile cameras with respect to static or individual camera setups.

When designing a motion planner for autonomous robots there are usually multiple objectives to be considered. However, a cost function that yields the desired trade-off between objectives is not easily obtainable. A common technique across many applications is to use a weighted sum of relevant objective functions and then carefully adapt the weights. However, this approach may not find all relevant trade-offs even in simple planning problems. Thus, we study an alternative method based on a weighted maximum of objectives. Such a cost function is more expressive than the weighted sum, and we show how it can be deployed in both continuous-and discrete-space motion planning problems. We propose a novel path planning algorithm for the proposed cost function and establish its correctness, and present heuristic adaptations that yield a practical runtime. In extensive simulation experiments, we demonstrate that the proposed cost function and algorithm are able to find a wider range of trade-offs between objectives (i.e., Pareto-optimal solutions) for various planning problems, showcasing its advantages in practice.

Integrating on-demand services into public transport networks might be the best way to face the current situation in which these new technologies have increased congestion in most cities. When cooperating with on-demand services rather than competing with them, public transport would not risk losing users, and could attract some passengers from private modes thanks to an increased quality of service. This fact has engendered a growing literature discussing how to design such an integrated system. However, all of that research has imposed that on-demand mobility is to solve the so-called “last-mile problem”, serving only as a feeder that connects the exact origins/destinations with the traditional public transit network. As it induces a large number of transfers and it precludes some scale-effects to be triggered, in this paper we challenge that imposition and investigate if this is the best spatial integration strategy. To do so, we study a simplified linear city in a morning peak situation, where we propose seven different line structures, all of them combining a traditional fixed line with on-demand ride-pooling (ODRP): three direct structures, where ODRP can serve full trips, three semi-direct, where a single ODRP vehicle can serve the largest part of a trip, and a base case in which ODRP is restricted to the first and final legs only. Our results show that the base case is optimal only under very specific demand patterns, or when transfer penalties are disregarded. Our analytical approach reveals relevant operational aspects of such integrated systems: namely, that the base case can help increase directness (diminishing detours), and that ODRP can help shorten the routes of the fixed services to decrease operator costs.

Many problems in robotics seek to simultaneously optimize several competing objectives. A conventional approach is to create a single cost function comprised of the weighted sum of the individual objectives. Solutions to this scalarized optimization problem are Pareto optimal solutions to the original multi-objective problem. However, finding an accurate representation of a Pareto front remains an important challenge. Uniformly spaced weights are often inefficient and do not provide error bounds. We address the problem of computing a finite set of weights whose optimal solutions closely approximate the solution of any other weight vector. To this end, we prove fundamental properties of the optimal cost as a function of the weight vector. We propose an algorithm that greedily adds the weight vector least-represented by the current set, and provide bounds on the regret. We extend our method to include suboptimal solvers for the scalarized optimization, and handle stochastic inputs to the planning problem. Finally, we illustrate that the proposed approach significantly outperforms baseline approaches for different robot planning problems with varying numbers of objective functions.

We study the problem of finding statistically distinct plans for stochastic task assignment problems such as online multi-robot pickup and delivery (MRPD) when facing multiple competing objectives. In many real-world settings robot fleets do not only need to fulfil delivery requests, but also have to consider auxiliary objectives such as energy efficiency or avoiding human-centered work spaces. We pose MRPD as a multi-objective optimization problem where the goal is to find MRPD policies that yield different trade-offs between given objectives. There are two main challenges: 1) MRPD is computationally hard, which limits the number of trade-offs that can reasonably be computed, and 2) due to the random task arrivals, one needs to consider statistical variance of the objective values in addition to the average. We present an adaptive sampling algorithm that finds a set of policies which i) are approximately optimal, ii) approximate the set of all optimal solutions, and iii) are statistically distinguishable. We prove completeness and adapt a state-of-the-art MRPD solver to the multi-objective setting for three example objectives. In a series of simulation experiments we demonstrate the advantages of the proposed method compared to baseline approaches and show its robustness in a sensitivity analysis. The approach is general and could be adapted to other multi-objective task assignment and planning problems under uncertainty.

Risk assessment is a crucial component of collision warning and avoidance systems for intelligent vehicles. Reachability-based formal approaches have been developed to ensure driving safety to accurately detect potential vehicle collisions. However, they suffer from over-conservatism, potentially resulting in false–positive risk events in complicated real-world applications. In this paper, we combine two reachability analysis techniques, a backward reachable set (BRS) and a stochastic forward reachable set (FRS), and propose an integrated probabilistic collision–detection framework for highway driving. Within this framework, we can first use a BRS to formally check whether a two-vehicle interaction is safe; otherwise, a prediction-based stochastic FRS is employed to estimate the collision probability at each future time step. Thus, the framework can not only identify non-risky events with guaranteed safety but also provide accurate collision risk estimation in safety–critical events. To construct the stochastic FRS, we develop a neural network-based acceleration model for surrounding vehicles and further incorporate a confidence-aware dynamic belief to improve the prediction accuracy. Extensive experiments were conducted to validate the performance of the acceleration prediction model based on naturalistic highway driving data. The efficiency and effectiveness of the framework with infused confidence beliefs were tested in both naturalistic and simulated highway scenarios. The proposed risk assessment framework is promising for real-world applications.

Motion planning for autonomous robots in dynamic environments poses numerous challenges due to uncertainties in the robot's dynamics and interaction with other agents. Sampling-based MPC approaches, such as Model Predictive Path Integral (MPPI) control, have shown promise in addressing these complex motion planning problems. However, the performance of MPPI relies heavily on the choice of sampling distribution. Existing literature often uses the previously computed input sequence as the mean of a Gaussian distribution for sampling, leading to potential failures and local minima. We propose a novel derivation of MPPI that allows for arbitrary sampling distributions to enhance efficiency, robustness, and convergence while alleviating the problem of local minima. We present an efficient importance sampling scheme that combines classical and learning-based ancillary controllers simultaneously, resulting in more informative sampling and control fusion. Several simulated and real-world demonstrate the validity of our approach.

As robots shift from industrial to human-centered spaces, adopting mobile manipulators, which expand workspace capabilities, becomes crucial. In these settings, seamless interaction with humans necessitates compliant control. Two common methods for safe interaction, admittance, and impedance control, require force or torque sensors, often absent in lower-cost or lightweight robots. This paper presents an adaption of impedance control that can be used on current-controlled robots without the use of force or torque sensors and shows its application for compliant control of a mobile manipulator. A calibration method is designed that enables estimation of the actuators' current/torque ratios and frictions, used by the adapted impedance controller, and that can handle model errors. The calibration method and the performance of the designed controller are experimentally validated using the Kinova GEN3 Lite arm. Results show that the calibration method is consistent and that the designed controller for the arm is compliant while also being able to track targets with five-millimeter precision when no interaction is present. Additionally, this paper presents two operational modes for interacting with the mobile manipulator: one for guiding the robot around the workspace through interacting with the arm and another for executing a tracking task, both maintaining compliance to external forces. These operational modes were tested in real-world experiments, affirming their practical applicability and effectiveness.Code: https://github.com/tud-amr/mobile-manipulator-compliance

Task and Motion Planning (TAMP) has made strides in complex manipulation tasks, yet the execution robustness of the planned solutions remains overlooked. In this work, we propose a method for reactive TAMP to cope with runtime uncertainties and disturbances. We combine an Active Inference planner (AIP) for adaptive high-level action selection and a novel Multi-Modal Model Predictive Path Integral controller (M3P2I) for low-level control. This results in a scheme that simultaneously adapts both high-level actions and low-level motions. The AIP generates alternative symbolic plans, each linked to a cost function for M3P2I. The latter employs a physics simulator for diverse trajectory rollouts, deriving optimal control by weighing the different samples according to their cost. This idea enables blending different robot skills for fluid and reactive plan execution, accommodating plan adjustments at both the high and low levels to cope, for instance, with dynamic obstacles or disturbances that invalidate the current plan. We have tested our approach in simulations and real-world scenarios.

Predicting the future behavior of human road users is an important aspect for the development of risk-aware autonomous vehicles. While many models have been developed towards this end, effectively capturing and predicting the variability inherent to human behavior still remains an open challenge. This paper proposes TrajFlow - a new approach for probabilistic trajectory prediction based on Normalizing Flows. We reformulate the problem of capturing distributions over trajectories into capturing distributions over abstracted trajectory features using an autoencoder, simplifying the learning task of the Normalizing Flows. TrajFlow outperforms state-of-the-art behavior prediction models in capturing full trajectory distributions in two synthetic benchmarks with known true distributions, and is competitive on the naturalistic datasets ETH/UCY, rounD, and nuScenes. Our results demonstrate the effectiveness of TrajFlow in probabilistic prediction of human behavior.

Ride-sourcing companies have worsened congestion in numerous cities worldwide, as many users are attracted from more sustainable modes. To reverse this trend, it is crucial to leverage the technology of connecting users and vehicles online and use it to strengthen public transport, which can be achieved by integrating on-demand pooled services with existing fixed-line services. We propose an efficient and practical integration idea: namely, to complement fixed bus lines with a fleet of smaller vehicles that follow flexible (on-demand) routes side-by-side with the fixed routes, so that part of the demand that would have used the fixed line can ride the flexible service instead. With this scheme, a smaller bus fleet is required, partially compensating for the increase in operators’ costs stemming from the flexible vehicles. This integration strategy favors mostly two types of users: those traveling in low-demand periods, through lower waiting times, and those located far from the bus stops, because the on-demand vehicles can reduce their access time. We develop simulations in real-world scenarios from Santiago, Chile, and Berlin, Germany, for the cases of human-driven and automated vehicles. Results show that when vehicles are automated: (i) A small number of on-demand vehicles can reduce average walking times from approximately 12 to 2 min while reducing operators’ costs, leading to a Pareto improvement, (ii) A larger number of on-demand vehicles can diminish total costs by 13%–39%, through a reduction in users’ costs, although increasing operators’ costs. If vehicles are not automated, total costs are reduced by more than 10% in all of the scenarios analyzed, but a Pareto improvement is not always possible. In general, this mixed fixed/on-demand system outperforms the use of on-demand ridepooling only. Results are more promising in Berlin, because large buses are cheaper in Santiago and run more crowded, so it is more costly to partially replace them by smaller vehicles.