Real-Robot Experiment with Pedestrians
A Clearpath Jackal using our approach navigates among pedestrians while minimizing its Collision Probability (CP) as well as mantaining the CP below a desired threshold. Pedestrians, tracked via motion capture, have their uncertain future motion predicted by a uni-modal Gaussian Distribution.
Abstract
Deploying mobile robots safely among humans requires the motion planner to account for the uncertainty in the other agents' predicted trajectories. This remains challenging in traditional approaches, especially with arbitrarily shaped predictions and real-time constraints.
To address these challenges, we propose a Dynamic Risk-Aware Model Predictive Path Integral control (DRA-MPPI), a motion planner that incorporates uncertain future motions modelled with potentially non-Gaussian stochastic predictions. By leveraging MPPI’s gradient-free nature, we propose a method that efficiently approximates the joint Collision Probability (CP) among multiple dynamic obstacles for several hundred sampled trajectories in real-time via a Monte Carlo (MC) approach.
This enables the rejection of samples exceeding a predefined CP threshold or the integration of CP as a weighted objective within the navigation cost function. Consequently, DRA-MPPI mitigates the freezing robot problem while enhancing safety. Real-world and simulated experiments with multiple dynamic obstacles demonstrate DRA-MPPI’s superior performance compared to state-of-the-art approaches, including Scenario-based Model Predictive Control (S-MPC), Frenét planner, and vanilla MPPI.
Comparisons with Non-Gaussian Predictions
Although not visualized in the video, all pedestrians are predicted to have a small chance of turning left any future time-step. This is modeled with a Gaussian Mixture Model. Below is a video comparison to a state-of-the-art Scenario-Based MPC [de Groot, 2023], more comparisons are provided in the tables and figures of the paper.
Comparisons with Gaussian Predictions
Below are the video comparisons showing a Jackal robot navigating a corridor with an increasing number of pedestrians. Pedestrian motion is simulated with a social forces model, but are predicted by the robot by a Gaussian distribution centered on a constant velocity model. This mismatch is added to make the simulation more realistic, as the prediction will not perfectly match actual motion. Comparisons with several other approaches are presented in the tables and figures in our paper.
DRA-MPPI (Ours)
Four pedestrians
Scenario-Based MPC
Four pedestrians
Predictability Awareness For Efficient and Robust Multi-Agent Coordination
Roman Chiva Gil,
Daniel Jarne Ornia*,
Khaled A. Mustafa*,
Javier Alonso-Mora.
In 24th International Conference on Autonomous Agents and Multiagent Systems (AAMAS),
2025.
To safely and efficiently solve motion planning problems in multiagent settings, most approaches attempt to solve a joint optimization that explicitly accounts for the responses triggered in other agents. This often results in solutions with an exponential computational complexity, making these methods intractable for complex scenarios with many agents. While sequential predict-and-plan approaches are more scalable, they tend to perform poorly in highly interactive environments. This paper proposes a method to improve the interactive capabilities of sequential predict-and-plan methods in multi-agent navigation problems by introducing predictability as an optimization objective. We interpret predictability throughnthe use of general prediction models, by allowing agents to predict themselves and estimate how they align with these external predictions. We formally introduce this behavior through the freeenergy of the system, which reduces (under appropriate bounds) to the Kullback-Leibler divergence between plan and prediction, and use this as a penalty for unpredictable trajectories. The proposed interpretation of predictability allows agents to more robustly leverage prediction models, and fosters a ‘soft social convention’ that accelerates agreement on coordination strategies without the need of explicit high level control or communication. We show how this predictability-aware planning leads to lower-cost trajectories and reduces planning effort in a set of multi-robot problems, including autonomous driving experiments with human driver data, where we show that the benefits of considering predictability apply even when only the ego-agent uses this strategy.
Active Disturbance Rejection Control (ADRC) for Trajectory Tracking of a Seagoing USV: Design, Simulation, and Field Experiments
Jelmer van der Saag,
Elia Trevisan,
Wouter Falkena,
Javier Alonso-Mora.
In IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS),
2025.
Unmanned Surface Vessels (USVs) face significant control challenges due to uncertain environmental disturbances like waves and currents. This paper proposes a trajectory tracking controller based on Active Disturbance Rejection Control (ADRC) implemented on the DUS V2500. A custom simulation incorporating realistic waves and current disturbances is developed to validate the controller's performance, supported by further validation through field tests in the harbour of Scheveningen, the Netherlands, and at sea. Simulation results demonstrate that ADRC significantly reduces cross-track error across all tested conditions compared to a baseline PID controller but increases control effort and energy consumption. Field trials confirm these findings while revealing a further increase in energy consumption during sea trials compared to the baseline.
ROME: Robust Multi-Modal Density Estimator
Anna Mészáros,
Julian F Schumann,
Javier Alonso-Mora,
Arkady Zgonnikov,
Jens Kober.
In 33rd International Joint Conference on Artificial Intelligence (IJCAI),
2024.
The estimation of probability density functions is a fundamental problem in science and engineering. However, common methods such as kernel density estimation (KDE) have been demonstrated to lack robustness, while more complex methods have not been evaluated in multi-modal estimation problems. In this paper, we present ROME (RObust Multi-modal Estimator), a non-parametric approach for density estimation which addresses the challenge of estimating multi-modal, non-normal, and highly correlated distributions. ROME utilizes clustering to segment a multi-modal set of samples into multiple uni-modal ones and then combines simple KDE estimates obtained for individual clusters in a single multi-modal estimate. We compared our approach to state-of-the-art methods for density estimation as well as ablations of ROME, showing that it not only outperforms established methods but is also more robust to a variety of distributions. Our results demonstrate that ROME can overcome the issues of over-fitting and over-smoothing exhibited by other estimators.
Sampling-based Model Predictive Control Leveraging Parallelizable Physics Simulations
Corrado Pezzato,
Chadi Salmi,
Elia Trevisan,
Max Spahn,
Javier Alonso-Mora,
Carlos Hernández Corbato.
In IEEE Robotics and Automation Letters (RA-L),
2025.
We present a method for sampling-based model predictive control that makes use of a generic physics simulator as the dynamical model. In particular, we propose a Model Predictive Path Integral controller (MPPI), that uses the GPU-parallelizable IsaacGym simulator to compute the forward dynamics of a problem. By doing so, we eliminate the need for manual encoding of robot dynamics and interactions among objects and allow one to effortlessly solve complex navigation and contact-rich tasks. Since no explicit dynamic modeling is required, the method is easily extendable to different objects and robots. We demonstrate the effectiveness of this method in several simulated and real-world settings, among which mobile navigation with collision avoidance, non-prehensile manipulation, and whole-body control for high-dimensional configuration spaces. This method is a powerful and accessible tool to solve a large variety of contact-rich motion planning tasks.
TrajFlow: Learning Distributions over Trajectories for Human Behavior Prediction
A. Meszaros,
J. F. Schumann,
J. Alonso-Mora,
A. Zgonnikov,
J. Kober.
In IEEE Intelligent Vehicles Symposium (IV),
2024.
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.
Multi-Modal MPPI and Active Inference for Reactive Task and Motion Planning
Yuezhe Zhang,
Corrado Pezzato,
Elia Trevisan,
Chadi Salmi,
Carlos Hernández Corbato,
Javier Alonso-Mora.
In IEEE Robotics and Automation Letters (RA-L),
2024.
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.
RACP: Risk-Aware Contingency Planning with Multi-Modal Predictions
Khaled A. Mustafa,
Daniel Jarne Ornia,
Jens Kober,
Javier Alonso-Mora.
In IEEE Transactions on Intelliegent Vehicles (T-IV),
2024.
For an autonomous vehicle to operate reliably within real-world traffic scenarios, it is imperative to assess the repercussions of its prospective actions by anticipating the uncertain intentions exhibited by other participants in the traffic environment. Driven by the pronounced multi-modal nature of human driving behavior, this paper presents an approach that leverages Bayesian beliefs over the distribution of potential policies of other road users to construct a novel risk-aware probabilistic motion planning framework. In particular, we propose a novel contingency planner that outputs long-term contingent plans conditioned on multiple possible intents for other actors in the traffic scene. The Bayesian belief is incorporated into the optimization cost function to influence the behavior of the short-term plan based on the likelihood of other agents' policies. Furthermore, a probabilistic risk metric is employed to fine-tune the balance between efficiency and robustness. Through a series of closed-loop safety-critical simulated traffic scenarios shared with human-driven vehicles, we demonstrate the practical efficacy of our proposed approach that can handle multi-vehicle scenarios.
Biased-MPPI: Informing Sampling-Based Model Predictive Control by Fusing Ancillary Controllers
Elia Trevisan,
Javier Alonso-Mora.
In IEEE Robotics and Automation Letters (RA-L),
2024.
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. In this paper, 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.
Interaction-Aware Sampling-Based MPC with Learned Local Goal Predictions
W. Jansma,
E. Trevisan,
A. Serra-Gomez,
J. Alonso-Mora.
In Proc. IEEE International Symposium on Multi-Robot and Multi-Agent Systems,
2023.
Motion planning for autonomous robots in tight, interaction-rich, and mixed human-robot environments is challenging. State-of-the-art methods typically separate prediction and planning, predicting other agents' trajectories first and then planning the ego agent's motion in the remaining free space. However, agents' lack of awareness of their influence on others can lead to the freezing robot problem. We build upon Interaction-Aware Model Predictive Path Integral (IA-MPPI) control and combine it with learning-based trajectory predictions, thereby relaxing its reliance on communicated short-term goals for other agents. We apply this framework to Autonomous Surface Vessels (ASVs) navigating urban canals. By generating an artificial dataset in real sections of Amsterdam's canals, adapting and training a prediction model for our domain, and proposing heuristics to extract local goals, we enable effective cooperation in planning. Our approach improves autonomous robot navigation in complex, crowded environments, with potential implications for multi-agent systems and human-robot interaction.
Probabilistic Risk Assessment for Chance-Constrained Collision Avoidance in Uncertain Dynamic Environments
Khaled A. Mustafa,
Oscar de Groot,
Xinwei Wang,
Jens Kober,
Javier Alonso-Mora.
In IEEE International Conference on Robotics and Automation (ICRA),
2023.
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 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 trajec- tories 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.
Multi-Agent Path Integral Control for Interaction-Aware Motion Planning in Urban Canals
L. Streichenberg,
E. Trevisan,
J. J. Chung,
R. Siegwart,
J. Alonso-Mora.
In , in IEEE Int. Conf. on Robotics and Automation (ICRA),
2023.
Autonomous vehicles that operate in urban envi- ronments shall comply with existing rules and reason about the interactions with other decision-making agents. In this paper, we introduce a decentralized and communication-free interaction-aware motion planner and apply it to Autonomous Surface Vessels (ASVs) in urban canals. We build upon a sampling-based method, namely Model Predictive Path Integral control (MPPI), and employ it to, in each time instance, compute both a collision-free trajectory for the vehicle and a prediction of other agents’ trajectories, thus modeling inter- actions. To improve the method’s efficiency in multi-agent sce- narios, we introduce a two-stage sample evaluation strategy and define an appropriate cost function to achieve rule compliance. We evaluate this decentralized approach in simulations with multiple vessels in real scenarios extracted from Amsterdam’s canals, showing superior performance than a state-of-the- art trajectory optimization framework and robustness when encountering different types of agents.
Sampling-Based MPC Using a GPU-parallelizable Physics Simulator as Dynamic Model: an Open Source Implementation with IsaacGym
C. Pezzato,
C. Salmi,
E. Trevisan,
J. Alonso-Mora,
C. Hernandez Corbato.
In Embracing Contacts Workshop at IEEE Int. Conf. on Robotics and Automation (ICRA),
2023.
We present a method for solving finite horizon optimal control problems using a generic physics simulator as the dynamical model. In particular, we present an open-source implementation of a model predictive path integral controller (MPPI), that uses the GPU-parallelizable IsaacGym simulator as the dynamical model to compute the forward dynamics of the system. This allows one to effortlessly solve complex contact-rich tasks such as for example, non-prehensile manipulation of a variety of objects, or picking with a mobile manipulator. Since there is no explicit dynamic modeling required from a user, the repository is easily extendable to different objects and robots, as we show in the experiments section. This makes this method a powerful and accessible tool to solve a large variety of contact-rich tasks.
TrajFlow: Learning the Distribution over Trajectories
A. Meszaros,
J. Alonso-Mora,
J. Kober.
In 5th Workshop on Long-term Human Motion Prediction at IEEE Int. Conf. on Robotics and Automation (ICRA),
2023.
Predicting the future behaviour of people re- mains an open challenge for the development of risk-aware autonomous vehicles. An important aspect of this challenge is effectively capturing the uncertainty inherent to human behaviour. This paper studies an approach for multi-modal probabilistic motion forecasting of an agent with improved accuracy in the predicted sample likelihoods. Our approach achieves state-of-the-art results on the inD dataset when evalu- ated with the standard metrics employed for motion forecasting. Furthermore, our approach also achieves state-of-the-art results when evaluated with respect to the likelihoods it assigns to its generated trajectories. Evaluations on artificial datasets indicate that the distributions learned by our model closely correspond to the true distributions observed in data and are not as prone to being over-confident in a single outcome in the face of uncertainty.
Visually-Guided Motion Planning for Autonomous Driving from Interactive Demonstrations
R. Perez-Dattari,
B. Brito,
O. de Groot,
J. Kober,
J. Alonso-Mora.
In IFAC Engineering Applications of Artificial Intelligence Journal,
2022.
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.
Learning Interaction-Aware Guidance for Trajectory Optimization in Dense Traffic Scenarios
B. Brito,
A. Agarwal,
J. Alonso-Mora.
In IEEE Transactions on Intelligent Transportation Systems (T-ITS),
2022.
Autonomous navigation in dense traffic scenarios remains challenging for autonomous vehicles (AVs) because the intentions of other drivers are not directly observable and AVs have to deal with a wide range of driving behaviors. To maneuver through dense traffic, AVs must be able to reason how their actions affect others (interaction model) and exploit this reasoning to navigate through dense traffic safely. This paper presents a novel framework for interaction-aware motion planning in dense traffic scenarios. We explore the connection between human driving behavior and their velocity changes when interacting. Hence, we propose to learn, via deep Reinforcement Learning (RL), an interaction-aware policy providing global guidance about the cooperativeness of other vehicles to an optimization-based planner ensuring safety and kinematic feasibility through constraint satisfaction. The learned policy can reason and guide the local optimization-based planner with interactive behavior to pro-actively merge in dense traffic while remaining safe in case the other vehicles do not yield. We present qualitative and quantitative results in highly interactive simulation environments (highway merging and unprotected left turns) against two baseline approaches, a learning-based and an optimization-based method. The presented results demonstrate that our method significantly reduces the number of collisions and increases the success rate with respect to both learning-based and optimization-based baselines.
Regulations Aware Motion Planning for Autonomous Surface Vessels in Urban Canals
J. de Vries,
E. Trevisan,
J. van der Toorn,
T. Das,
B. Brito,
J. Alonso-Mora.
In Proc. IEEE Int. Conf. on Robotics and Automation (ICRA),
2022.
In unstructured urban canals, regulation-aware interactions with other vessels are essential for collision avoidance and social compliance. In this paper, we propose a regulations aware motion planning framework for Autonomous Surface Vessels (ASVs) that accounts for dynamic and static obstacles. Our method builds upon local model predictive contouring control (LMPCC) to generate motion plans satisfying kino-dynamic and collision constraints in real-time while including regulation awareness. To incorporate regulations in the planning stage, we propose a cost function encouraging compliance with rules describing interactions with other vessels similar to COLlision avoidance REGulations at sea (COLREGs). These regulations are essential to make an ASV behave in a predictable and socially compliant manner with regard to other vessels. We compare the framework against baseline methods and show more effective regulation-compliance avoidance of moving obstacles with our motion planner. Additionally, we present experimental results in an outdoor environment.